王 彪， 董 超， 黃 毅， 唐超穎

(南京航空航天大學(xué)自動(dòng)化學(xué)院，南京 211106)

0 引言

實(shí)現(xiàn)地形跟隨的方案之一是基于地形數(shù)據(jù)，采用航跡規(guī)劃算法確定期望航跡曲線，再通過(guò)航跡跟蹤控制完成。高精度的地形數(shù)據(jù)為該方案的實(shí)現(xiàn)增加了可行性。

航跡規(guī)劃是綜合考慮多種因素所得最優(yōu)或可行的飛行航跡[1]。通常的航跡規(guī)劃分為初始航跡生成和航跡光順兩步。國(guó)內(nèi)外一般采用Voronoi圖法[2]、概略圖法[3]等結(jié)合Dijkstra,A*[4]等最短路徑搜索算法生成初始折線航跡，再通過(guò)合適的曲線光順?biāo)惴ǐ@得滿足飛行性能約束的光滑航跡。利用B樣條[5-6]、Dubins曲線[7]和Reeds-Shepp曲線[8]光順?biāo)煤桔E不利于后續(xù)跟蹤。本文綜合考慮現(xiàn)有規(guī)劃算法的研究弊端、地形跟隨問(wèn)題中航跡規(guī)劃的需求，采用文獻(xiàn)[9]最優(yōu)航跡方法，通過(guò)最小化一個(gè)最優(yōu)性能函數(shù)，借鑒文獻(xiàn)[10]設(shè)計(jì)滿足飛行器機(jī)動(dòng)性能約束的三次樣條函數(shù)，再施以高階連續(xù)性約束，將航跡生成和航跡光順一步完成。

經(jīng)典的Pure Pursuit，LOS及其各變種的跟蹤方法[11-12]對(duì)飛行器的機(jī)動(dòng)能力有較高要求，針對(duì)該問(wèn)題提出了許多非線性制導(dǎo)律。其中:NELSON等[13]提出的矢量場(chǎng)(Vector Field,VF)制導(dǎo)律逐漸成為無(wú)人機(jī)制導(dǎo)領(lǐng)域的著名工具，它們分別構(gòu)造了直線與圓弧矢量場(chǎng)生成航向角指令，在有風(fēng)條件下仿真有效;朱欣華等[14]針對(duì)兩輪驅(qū)動(dòng)機(jī)器人的路徑跟蹤問(wèn)題，通過(guò)適量場(chǎng)法精確跟蹤較為復(fù)雜的正弦路徑;XU等[15]在自動(dòng)水面艦艇控制系統(tǒng)中仿真驗(yàn)證矢量場(chǎng)法比LOS算法有更好的跟蹤性能;文獻(xiàn)[16]研究證明矢量場(chǎng)路徑跟蹤依賴更少的控制量。盡管如此，目前在地形跟隨中應(yīng)用矢量場(chǎng)路徑跟蹤技術(shù)的研究仍然較少。本文基于矢量場(chǎng)方法研究面向地形跟隨的航跡跟蹤策略，針對(duì)參考路徑構(gòu)造矢量場(chǎng)，根據(jù)最大航跡角變化率給出矢量場(chǎng)全局可行的條件，用于指導(dǎo)調(diào)整矢量場(chǎng)參數(shù)，再考慮無(wú)人機(jī)的輸入約束，設(shè)計(jì)以過(guò)載為控制量的跟蹤控制器，有效完成地形跟隨任務(wù)。

1 地形跟隨最優(yōu)航跡規(guī)劃

地形跟隨中的最優(yōu)航跡規(guī)劃是對(duì)于給定區(qū)域的三維地形剖面，一條從初始位置到終點(diǎn)的滿足預(yù)定安全高度和飛行器機(jī)動(dòng)極限的飛行軌跡。

1.1 遞推三次樣條曲線設(shè)計(jì)

在地形跟隨航跡規(guī)劃問(wèn)題中，航跡曲線至少滿足二階可導(dǎo)才能滿足飛行器自身航跡角和過(guò)載等機(jī)動(dòng)性能約束。本節(jié)以如下三次樣條函數(shù)[10]描述區(qū)間(Li-1,Li)(i=1,2,…,n)上期望地形跟隨航跡，即

(1)

式中：hi-1為區(qū)間(Li-1,Li)左節(jié)點(diǎn)高程信息;si-1為該點(diǎn)一階導(dǎo)數(shù)，即斜率；ki-1為二階導(dǎo)數(shù)，即等效曲率；pi=(ki-ki-1)/Δ為區(qū)間內(nèi)三階導(dǎo)數(shù)的差分近似，Δ=Li-Li-1為地形區(qū)間步長(zhǎng)。對(duì)式(1)逐次進(jìn)行空間求導(dǎo)可得

(2)

k(L)=ki-1+pi(L-Li-1)

(3)

可見，對(duì)于二階導(dǎo)數(shù)k區(qū)間內(nèi)為L(zhǎng)的線性函數(shù)，只要確定各節(jié)點(diǎn)參數(shù)hi,si,ki即可確定三次樣條曲線形狀。

特別地，若取固定步長(zhǎng)，則節(jié)點(diǎn)Li處曲線參數(shù)hi,si,ki之間的遞推關(guān)系及與初值h0,s0,k0的關(guān)系為

hi=h0+Δ(s0+s1+…+si-2+si-1)+Δ2(2k0+3k1+…+3ki-1+ki)/6

(4)

si=s0+Δ(k0+2k1+…+2ki-1+ki)/2

(5)

式(4)中的s1+…+si-2+si-1根據(jù)式(5)又可改寫為

s1+…+si-2+si-1=(i-1)s0+Δ[(i-1)k0+

(2i-3)k1+(2i-5)k2+…+3ki-2+ki-1]/2

(6)

從而

(7)

可見全程曲線各節(jié)點(diǎn)參數(shù)si及hi，取決于各節(jié)點(diǎn)的二階導(dǎo)數(shù)值ki(i=1,2,…,n)和初值h0,s0,k0。

于是，在預(yù)知初值h0,s0,k0的前提下，確定各節(jié)點(diǎn)上的ki值可推全程航跡，由h0,s0,k0遞推而來(lái)的節(jié)點(diǎn)參數(shù)保證了全程曲線的光滑性。

1.2 最優(yōu)航跡建模

在執(zhí)行地形跟隨飛行任務(wù)中，通常希望盡量貼近地形飛行，以充分利用地形掩蔽。因此，最優(yōu)航跡就是航路點(diǎn)與地形點(diǎn)之間的高度差盡可能小，同時(shí)滿足飛行安全和飛行器機(jī)動(dòng)約束條件。定義性能指標(biāo)函數(shù)J度量航跡與地形輪廓的貼合程度，即

(8)

式中：qi≥0,li≥0,qi+li>0；ei表示航路點(diǎn)與理想高度的誤差ei=hi-Ti-H0，hi為航跡高程，Ti為地形高程，H0是設(shè)定的安全高程。

地形跟隨的航跡不僅要滿足式(9)高程偏差要求，還要滿足飛行器航跡傾斜角θ與法向加速度aN的限制，即

(9)

航跡傾斜角的正切即航跡曲線的斜率，因此航跡傾斜角限制可以轉(zhuǎn)化為航跡曲線的斜率約束，即

smin≤s≤smax

(10)

式中：smin=tanθmin;smax=tanθmax。

在給定飛行速度V的條件下，法向加速度與航跡曲率K滿足關(guān)系K=aN/V2，同時(shí)，曲率K與其二階導(dǎo)數(shù)k滿足K=k|cos3θ|，在航跡傾斜角較小時(shí)曲率值略等于二階導(dǎo)數(shù)為

kmin≤k≤kmax

(11)

式中：kmin=aNmin/V2；kmax=aNmax/V2。

于是，在地形區(qū)間(L0,Ln)上地形跟隨最優(yōu)航跡模型可以完整描述為：在式(9)～式(11)約束下，使式(8)性能指標(biāo)函數(shù)最小的航跡。

根據(jù)上節(jié)推導(dǎo)，式(9)約束可以寫成

(12)

式(10)約束可以寫成

(13)

定義中間變量xi=Δ2(ki-kmin)，表示各節(jié)點(diǎn)的二階導(dǎo)數(shù)值與極值關(guān)系，并整理成向量形式，即

x=Δ2(k′-kminη)

(14)

(15)

(16)

式(8)度量函數(shù)寫成矩陣向量形式為

(17)

式中，

(18)

根據(jù)推導(dǎo)，誤差向量e=Dx-d，式(17)等效為

(19)

略去常數(shù)項(xiàng)，地形跟隨最優(yōu)航跡規(guī)劃問(wèn)題轉(zhuǎn)化為求解標(biāo)準(zhǔn)二次型的數(shù)學(xué)規(guī)劃問(wèn)題，即

(20)

s.t.Ax≥bx≥0

式中:R=DTQD；gT=lTD-dTQD;A=(DF-F-I)T；b=(dfNfpkδη)T。

2 矢量場(chǎng)法航跡跟蹤

矢量場(chǎng)航跡跟蹤分為建立矢量場(chǎng)模型和計(jì)算控制量使飛行器沿矢量場(chǎng)回到期望路徑兩步。

2.1 問(wèn)題描述

對(duì)于地形跟隨任務(wù)來(lái)說(shuō)，只考慮飛行器的縱垂質(zhì)心運(yùn)動(dòng)，保持飛行器速度恒定。以法向過(guò)載為輸入的固定翼飛行器縱垂質(zhì)心運(yùn)動(dòng)模型為

(21)

式中：p=(x′h)T是飛行器位置;γ為航跡傾斜角；n為鉛垂面法向過(guò)載。

2.2 矢量場(chǎng)制導(dǎo)算法

航跡規(guī)劃算法所得最優(yōu)航跡為期望位置的航路點(diǎn)，各點(diǎn)之間路徑為直線段，如圖1所示，黑色實(shí)線WiWi+1為期望路段，路段方向角γL，構(gòu)造如圖矢量場(chǎng)，γd為矢量場(chǎng)中點(diǎn)與航路段的夾角，γd表示期望的飛行方向。

圖1 直線矢量場(chǎng)

對(duì)于飛行平面一點(diǎn)p，ep表示p到期望路段的垂直距離，矢量場(chǎng)的構(gòu)造滿足以下3個(gè)條件：1) 當(dāng)ep→∞時(shí)，矢量以角度γ∞指向路徑；2) 當(dāng)ep=0時(shí)，矢量與路徑方向相同；3) 當(dāng)ep∈(0,∞)時(shí)，矢量方向連續(xù)變化。

根據(jù)上述構(gòu)造條件，構(gòu)造如下矢量場(chǎng)[13]

(22)

式中：γ∞與kL>0為矢量場(chǎng)參數(shù)；γ∞決定指向路徑的角度；kL影響矢量場(chǎng)從γ∞到0的變化率。

矢量場(chǎng)所給的航跡角指令不僅與矢量場(chǎng)有關(guān)，還取決于航路段的方向角γL，因此制導(dǎo)指令為

(23)

2.3 可行矢量場(chǎng)

根據(jù)上述定義推導(dǎo)滿足矢量場(chǎng)可行性條件。由式(22)矢量場(chǎng)可得γd的時(shí)間導(dǎo)數(shù)為

(24)

當(dāng)飛行器沿矢量場(chǎng)飛行時(shí)有γ-γL=γd，即

(25)

(26)

當(dāng)γ∞在(0,π/2]時(shí)，0≤(2γ∞/π)arctan(kLep)<π/2，式(26)可寫為

(2γ∞/π)arctankLep=arctan (γ∞/kLπep)。

(27)

矢量場(chǎng)的可行性保證了無(wú)論初始距離ep有多大，只要飛行器的航跡角已與矢量場(chǎng)方向?qū)R，飛行器就可沿著式(22)矢量場(chǎng)確定的方向飛行。根據(jù)定義，若矢量場(chǎng)可行，則必須滿足

(28)

綜上，在飛行器速度恒定的情況下，可以得出調(diào)整矢量場(chǎng)參數(shù)以滿足可行性條件的方法。

1) 確定γ∞。由于γ∞∈(0,π/2]，為計(jì)算方便，γ∞可取為π的倍數(shù)，如π/3。

2) 將γ∞代入式(27)，求出ep 0(kL)。

3) 將ep 0(kL)代入式(25)求出|f(ep 0)|，根據(jù)式(28)可行性條件算出kL。

圖2 矢量場(chǎng)角度變化率

3 仿真驗(yàn)證

3.1 最優(yōu)航跡規(guī)劃

為驗(yàn)證最優(yōu)航跡規(guī)劃算法的有效性，本文取目標(biāo)區(qū)域地形二維剖面離散數(shù)據(jù)，全程50 km，步長(zhǎng)50 m，結(jié)果如圖3所示。圖4為航路點(diǎn)與地形相對(duì)高度，最小值為43 m，無(wú)撞地風(fēng)險(xiǎn)，均值約為68.8 m。

圖3 地形跟隨最優(yōu)航跡

圖4 最優(yōu)航跡相對(duì)高度

圖5進(jìn)一步驗(yàn)證規(guī)劃航跡的機(jī)動(dòng)能力的限制。航跡曲線正向坡度最大值為13.38°，負(fù)向坡度最小值為-9.754°，均在預(yù)設(shè)的飛行航跡角極限值[-10°,15°]范圍內(nèi)，過(guò)載最大為3.997g，最小0.5g，也在預(yù)設(shè)過(guò)載限制[0.2g,4.8g]內(nèi)。

圖5 坡度與過(guò)載

3.2 跟蹤結(jié)果

f(ep)max=f(ep 0)≈0.385VkL。

(29)

跟蹤結(jié)果如圖6所示，飛行器持續(xù)緊密地跟蹤參考航跡。圖7為跟蹤誤差ep，最大約10 m。航跡角控制器的控制量如圖8所示，圖8中藍(lán)色實(shí)線和虛線分別為過(guò)載上、下限，飛行器全程過(guò)載均未超限。

圖6 航跡跟蹤結(jié)果

圖7 跟蹤誤差

圖8 過(guò)載與航跡傾斜角速率

4 結(jié)束語(yǔ)

為實(shí)現(xiàn)飛行器地形跟隨任務(wù)，本文設(shè)計(jì)了一種固定翼飛行器地形跟隨實(shí)現(xiàn)方案。首先采用一種基于三次樣條函數(shù)的最優(yōu)規(guī)劃算法，直接生成光順可飛的參考航跡，然后采用矢量場(chǎng)法推導(dǎo)制導(dǎo)律，給出矢量場(chǎng)全局可行條件和參數(shù)設(shè)計(jì)規(guī)則，使得所構(gòu)造的矢量場(chǎng)符合飛行器機(jī)動(dòng)能力約束。對(duì)真實(shí)地形數(shù)據(jù)進(jìn)行仿真飛行，所得結(jié)果滿足預(yù)定要求，飛行器完成了地形跟隨任務(wù)，表明了本文所研究方案及算法的有效性。