地形跟隨中的航跡規(guī)劃與跟蹤

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

(南京航空航天大學自動化學院，南京 211106)

0 引言

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

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

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

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

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

1.1 遞推三次樣條曲線設計

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

(1)

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

(2)

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

(3)

可見，對于二階導數k區(qū)間內為L的線性函數，只要確定各節(jié)點參數hi,si,ki即可確定三次樣條曲線形狀。

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

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根據式(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é)點參數si及hi，取決于各節(jié)點的二階導數值ki(i=1,2,…,n)和初值h0,s0,k0。

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

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

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

(8)

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

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

(9)

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

smin≤s≤smax

(10)

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

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

kmin≤k≤kmax

(11)

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

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

根據上節(jié)推導，式(9)約束可以寫成

(12)

式(10)約束可以寫成

(13)

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

x=Δ2(k′-kminη)

(14)

(15)

(16)

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

(17)

式中，

(18)

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

(19)

略去常數項，地形跟隨最優(yōu)航跡規(guī)劃問題轉化為求解標準二次型的數學規(guī)劃問題，即

(20)

s.t.Ax≥bx≥0

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

2 矢量場法航跡跟蹤

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

2.1 問題描述

對于地形跟隨任務來說，只考慮飛行器的縱垂質心運動，保持飛行器速度恒定。以法向過載為輸入的固定翼飛行器縱垂質心運動模型為

(21)

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

2.2 矢量場制導算法

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

圖1 直線矢量場

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

根據上述構造條件，構造如下矢量場[13]

(22)

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

矢量場所給的航跡角指令不僅與矢量場有關，還取決于航路段的方向角γL，因此制導指令為

(23)

2.3 可行矢量場

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

(24)

當飛行器沿矢量場飛行時有γ-γL=γd，即

(25)

(26)

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

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

(27)

矢量場的可行性保證了無論初始距離ep有多大，只要飛行器的航跡角已與矢量場方向對齊，飛行器就可沿著式(22)矢量場確定的方向飛行。根據定義，若矢量場可行，則必須滿足

(28)

綜上，在飛行器速度恒定的情況下，可以得出調整矢量場參數以滿足可行性條件的方法。

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

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

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

圖2 矢量場角度變化率

3 仿真驗證

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

為驗證最優(yōu)航跡規(guī)劃算法的有效性，本文取目標區(qū)域地形二維剖面離散數據，全程50 km，步長50 m，結果如圖3所示。圖4為航路點與地形相對高度，最小值為43 m，無撞地風險，均值約為68.8 m。

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

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

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

圖5 坡度與過載

3.2 跟蹤結果

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

(29)

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

圖6 航跡跟蹤結果

圖7 跟蹤誤差

圖8 過載與航跡傾斜角速率

4 結束語

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