基于矢量場法的無人機三維路徑跟蹤控制

顏宗淵，王道波，王博航，周晨昶，姜 燕

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

0 引言

無人機路徑跟蹤問題可以由3個任務來描述：跟蹤移動目標、跟蹤航點路徑以及在固定航點處運動[1]。路徑跟蹤最新發展主要可以分為3類：基于動力學和運動學模型的算法[2-3]、基于虛擬目標點的算法[4]和基于矢量場的算法[5]。盡管基于動力學和運動學模型的路徑跟蹤算法可以保證足夠的跟蹤性能和穩定性，但是其制導命令依賴于模型并且復雜，從而增加了計算復雜度[6]。虛擬目標點制導算法源自導彈制導系統中使用的視線制導方法，現已發展為非線性制導律[4,7-8]，但是，與其他制導律方法相比，該方法引入了更多的簡便性和更少的控制工作，因為它沒有考慮到無人機的運動學和動力學約束，這些約束在實際應用中會降低制導律的性能。基于矢量場的路徑跟蹤算法的基本思想是要構造一個理想的航向場將無人機移動到路徑上，因此，制導律的計算取決于路徑的類型[9]。直線和圓路徑是最常見的路徑，直線和圓也可以組合構造更復雜的路徑。研究發現基于矢量場的路徑跟蹤算法具有較大優勢，可以不依賴模型，且計算復雜度較小，可以加入無人機的運動學和動力學約束，跟蹤性能好[10]。本文將二維矢量場法推廣到三維空間，實現固定翼無人機的三維路徑跟蹤控制。

針對航段切換問題，采用基于圓角幾何的切換準則，根據無人機是否通過半平面作為切換策略，保證無人機可以平滑過渡2條相鄰航段。仿真結果表明，本文提出的三維路徑跟蹤算法具有極佳的跟蹤性能。

1 無人機運動學模型

本文主要研究外回路產生的制導指令使無人機跟隨空間三維路徑。建立無人機的運動學模型為

(1)

由于風對無人機影響的效應還未研究清楚，一般將風的影響作為擾動輸入，由無人機的飛行控制系統實時處理。因此在進行路徑跟蹤時，通常不會將風的影響考慮進去，認為UAV的空速等于地速，航向角ψ認為等于航線角(假設側滑角β等于0)[9]。圖1展示了無人機空速V、航向角ψ和航跡角γ。

圖1 無人機運動模型圖示

2 矢量場法

制導律將采用在文獻[11]提出的矢量場法。需要跟蹤的在三維空間中的路徑是2個二維流形的交線，其中二維流形分別由α1(r)=0和α2(r)=0定義，α1和α2為有界的二階偏微分，r∈R3。一個基本的假設是交線給出的路徑是連通的，并且是一維的。定義函數為

(2)

給出V(r)的一階導數為

(3)

式(3)存在的一個問題是計算得到的u′或許不會等于無人機的空速V，因此,u′需要標準化為

(4)

當u′被標準化為式(4)的值時，由文獻[11]證明得到的穩定性依然是有效的。

將無人機在慣性坐標系的速度分量與式(4)得到的速度u=(u1,u2,u3)T進行等價得到

(5)

對式(5)進行求解可以得到期望的航跡γc和期望的航向角ψd，表達式為

(6)

飽和函數被定義為

(7)

假設通過協調轉彎方程對內環橫向動力學方程進行了精確建模，則可以從式(8)獲得產生理想轉彎性能的滾轉角命令值，即

(8)

kφ為一個正常數。

2.1 直線路徑

直線可以由直線的方向和直線上的一個點描述。讓cL=(cn,ce,cd)T表示直線上任意的一個點，直線的方向可以由在東北坐標系下的相對正北方向的航向角ψL和相對水平面的航跡角γL表示。因此，直線的方向可以表示為

(9)

qL為一個指向期望直線的方向單位向量。這條直線可以表示為

(10)

垂直于由qL定義的縱向平面的單位向量定義為

(11)

相似地，垂直于由qL定義的橫向平面的單位向量為

(12)

因此，pline由以下定義的平面的交線給定

(13)

(14)

因此，在標準化前，期望的速度矢量可以表示為

(15)

圖2表示了qL、cL以及由αlon(r)=0和αlat(r)=0定義的平面。

圖2 由2個平面定義的空間直線

2.2 螺旋線路徑

時間參數化的螺旋路徑為

(16)

(17)

Rh為半徑；λ=+1為螺旋線順時針旋轉；λh=-1為螺旋線逆時針旋轉；γh為沿著螺旋線的航跡角。

為了找到2個面可以定義螺旋線路徑，式(16)中的時間參數需要消除，式(16)可以表示為

(18)

除此之外，將式(16)的東向分量除以北向分量可以得到

(19)

求解t，并將其代入到式(16)中的第3個表達式中得到

(20)

除以Rh將這些等式標準化可以得到

(21)

(22)

用除以Rh去進行歸一化可以使得控制策略的增益不會隨著軌道的大小變化而變化。αcyl和αpl的梯度可以表示為

(23)

(24)

在標準化前，期望的速度矢量可以表示為

(25)

3 三維路徑跟蹤算法

3.1 基于圓角幾何的切換策略

圖3 圓角幾何策略

三維空間里2條線段的切點的計算如下所述：

a.將空間中相鄰的3個航點Wi-1、Wi和Wi+1投影到二維平面，得到wi-1、wi和wi+1。

b.通過下式計算出二維平面內表示相鄰航段方向的向量：

(26)

(27)

c.通過圖3右側的表達式計算得到a、b、c點。

d.a、b、c是A、B、C在二維平面的投影點，且A、B在航段上，由此可以計算得出A、B切點;C是螺旋線的圓心，起始高度為A切點高度，由此得到C的坐標。

3.2 切換策略的算法步驟

在切換策略算法中，pathstate是一個切換標志，如果pathstate=1，則代表無人機將沿著直線路徑運動；pathstate=2，則代表無人機將沿著螺旋線運動；ψpath代表直線路徑的航向，對于螺旋線來說，代表其起始點的航向；γpath是所走路徑相對水平面的夾角；對于直線路徑，λpath=0、λpath=1代表螺旋線是順時針方向，λpath=-1代表螺旋線是逆時針方向；ρ是螺旋線的半徑；pathflag是一個路徑標志，輸出用于下一層路徑跟蹤算法，pathflag=1代表無人機將跟隨直線路徑，pathflag=2代表無人機將跟隨螺旋線路徑；C是螺旋線路徑的圓心位置坐標，對于直線，是起始航點的坐標。

輸入是航點數據pathwaypoit={w1,w2,…,wn}、無人機的位置p=(pn,pe,pd)和最大航跡角γmax，根據無人機的位置實時更新輸出值，算法流程如圖4所示。

圖4 基于圓角幾何的切換策略算法流程

切換策略算法初始化設置pathstate=1，計算出由3個航點構成的2條相鄰航段方向，采用螺旋線擬合相鄰航段，因此，形成了2條直線路徑和1條螺旋線路徑。無人機先沿著直線路徑運動，計算得到直線路徑參數，用于輸出給路徑跟蹤模塊，當到達半平面H1，無人機開始處于航段過渡階段，pathstate=2，將沿著螺旋線路徑運動，計算得到螺旋線路徑參數。當無人機到達半平面H2，說明航段過渡階段已經運動完，pathstate=1，無人機將開始沿著直線路徑運動，重復以上步驟，計算路徑跟隨所需參數。

3.3 路徑跟蹤算法步驟

圖5 基于矢量場法的路徑跟蹤算法流程

路徑跟蹤算法根據pathflag參數確定將要跟隨路徑的參數。當pathflag=1時，無人機將要跟隨直線路徑，采用式(15)計算出跟隨直線路徑所需的速度矢量u′=(u1,u2,u3)T；當pathflag=2時，無人機將要跟隨螺旋線路徑，采用式(25)計算出跟隨螺旋線路徑所需的速度矢量u′=(u1,u2,u3)T。由得到的速度矢量u′，計算得到期望的空速Vc、期望的航跡角γc、期望的航向角ψc和期望的滾轉角φc。將這些期望參數輸入給無人機控制回路中，從而實現無人機的路徑跟蹤。

4 仿真實驗

對本文提出的制導方案進行仿真驗證，以評估算法的性能和魯棒性，無人機的詳細模型詳見文獻[9]，使用硬件是個人計算機Intel Core i7-8750H CPU @2.20 GHz 16 GB RAM。將無人機的起始點和初始航向分別設置為不同的初始值，對直線、螺旋線路徑和由此組合的復雜路徑進行跟蹤，進行多次仿真實驗，驗證算法的跟蹤性能。

4.1 直線路徑

參數kφ會影響無人機在橫向平面的收斂性能和跟蹤表現，kφ的取值范圍需在0.2～0.6之間，如果kφ大于0.7，無人機跟隨路徑可能會出現振蕩現象，如果小于0.2，跟蹤效果比較差，本文算法將其取為0.3。矩陣K1=diag([k11k22k33])是一個對角矩陣；k11和k22會影響無人機在橫向平面上對路徑的收斂性能，k33會對其在縱向平面的收斂性能。k11和k22的取值范圍在5～20之間，本文取9；k33的取值范圍在5～35之間，過大會使得無人機的俯仰角出現振蕩現象，本文取15；K2會影響無人機對路徑的跟蹤性能，取值范圍在140～180之間，本文取150。改變無人機的起始點和初始航向，對2個航點直線軌跡進行跟蹤，仿真結果如圖6所示，可以看到無人機能夠快速跟蹤設定的直線，而且跟蹤誤差很小。

圖6 直線路徑跟蹤結果

4.2 螺旋線路徑

kφ、K1、K2這些參數無人機對螺旋線路徑跟蹤的影響和直線路徑類似，將kφ取值為1.5，k11和k22都取值為20，k33取值為25，K2取值為150。改變無人機的起始點和初始航向，對螺旋線路徑進行跟蹤，仿真結果如圖7所示。初始時刻無人機對于螺旋線跟蹤誤差相對較大，這是因為無人機的初始航向與預設路徑的航向偏差較大，且距離相對較遠，但無人機能夠迅速消除誤差，跟蹤螺旋線路徑，而且之后跟蹤誤差很小。

圖7 螺旋線路徑跟蹤結果

4.3 組合復雜路徑

在本節中，無人機要跟蹤根據航點設定的一系列軌跡，在2個航段的交接處，無人機將會對飛行螺旋線進行平滑過渡，仿真結果如圖8所示。從圖8可以看到，無人機很好地跟蹤所有路徑，且在切換點處沒有比較大的超調，跟蹤誤差極小。期望俯仰角、滾轉角、空速與實際對比的結果和誤差值的結果如圖9和圖10所示。在各過渡位置處，無人機的俯仰角和期望滾轉角會出現波動，因為無人機需要調整姿態來跟蹤下一段路徑；期望值與實際值在過渡位置存在一定偏差，這是因為無人機模型存在動力學和運動學約束，導致在過渡點實際值無法等于期望值，不過誤差很快就會趨于0，這也驗證了本文提出算法的可行性。

圖8 組合路徑跟蹤三維結果

圖9 期望俯仰角、滾轉角、空速與實際對比

圖10 期望與實際俯仰角、滾轉角、空速誤差值

5 結束語

本文提出了一種基于矢量場法的三維航點制導算法，只需要任意給定合理的空間航點，算法將依據空間航點順序將航點連接成各航段，使用直線和螺旋線對航段進行近似擬合得到適合無人機飛行的路徑。根據路徑類型，通過矢量場法計算得到無人機運動的期望空速、期望航向角和期望航跡角，由期望的航向角和航跡角信息可以得到期望滾轉角和俯仰角信息，用于無人機的縱向和橫向通道的控制，將無人機引導到預定的軌跡上。此外，采用一種基于圓角幾何的切換策略，實現各航段的平滑過渡，使用螺旋線擬合相鄰航段，螺旋線僅在切點處與2條線段相交，該切點定義了切換半平面的位置，當無人機通過了切換半平面，無人機會跟蹤下一航段，故此方法還可以減少路徑長度實現路徑優化。采用本文提出的算法進行仿真實驗，仿真結果表明無人機可以很好地跟蹤直線、螺旋線及組合的復雜路徑，驗證了算法的有效性。