無人機航跡優化與跟蹤技術研究

賈文濤，李春濤

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

0 引言

近年來，隨著無人機技術的飛速發展，其在軍事及民用領域發揮著越來越重要的作用[1]。例如，軍事中利用無人機在復雜環境下的偵察能力，完成對威脅目標的精確打擊任務；在民用領域，無人機正逐步代替人類完成繁瑣、枯燥和危險的工作。無人機的總體系統架構可分為5層[2]：航點規劃器、動態航跡平滑器、航跡跟蹤器、飛行控制器和無人機本體。無人機的航跡跟蹤能力是其安全完成飛行任務的保障，本文利用航跡優化和航跡跟蹤技術，提出了一種精確可靠的航跡跟蹤方法。

在航跡優化領域，Dubins提出的由直線段和常曲率圓弧段組成的Dubins曲線被廣泛應用于機器人和無人機的航跡平滑中。為解決Dubins曲線在直線段和圓弧段連接處的曲率不連續問題，基于變曲率弧段的Clothoid曲線被提出，可使無人機得到連續的制導指令[3]。由于多項式樣條曲線滿足曲率連續的要求，ANASTASIOS M Lekkas利用Hermite三次樣條曲線實現對離散航點的平滑，完成了無人機對目標航點的跟蹤[4]。ZHAO Shulong利用B樣條曲線，實現了對變曲率航跡的擬合，提高了無人機的跟蹤精度[5]。畢達哥拉斯矢端曲線(PH曲線)擁有等距線的優良性質，根據這一特性，可以在PH曲線周圍定義一段安全距離來防止碰撞，用于解決多架無人機的集群飛行問題[6]。

目前的航跡跟蹤方法可分為兩大類，基于幾何計算的方法和基于控制理論的方法[7]。傳統的幾何計算法由導彈制導技術演變而來，包括純追蹤法、視線法(LOS)[4]以及二者相結合的方法(PLOS)。這些方法可實現對簡單航跡的跟蹤，但對于復雜的變曲率航跡跟蹤性能較差。MIT的SANGHYUK Park利用虛擬目標點，設計了一種L1非線性制導律(NLGL)[8-9]，該方法設計簡單，易于實現，跟蹤性能依賴于虛擬探測長度的選取。利用控制理論進行航跡跟蹤問題研究，易于分析制導律的穩定性和魯棒性。基于比例-積分-微分(PID)的經典控制理論可實現對簡單航跡的跟蹤[10]。現代控制理論技術如：線性二次型調節器(LQR)[11]、模型預測控制(MPC)[12-13]、自適應控制[14]等，也被用于解決航跡跟蹤問題，提高了抗風干擾能力，使航跡跟蹤具有較好的魯棒性。

為滿足實際應用中對航點的跟蹤，本文利用Bezier曲線對離散航點進行動態平滑，形成一條曲率連續的航跡。在期望航跡上引入一個具有動態約束的虛擬目標點作為跟蹤對象[15]，設計非線性制導律，實現對期望航跡的跟蹤。

1 無人機的航跡優化

1.1 無人機質點運動方程

本文主要研究無人機在二維平面的航跡跟蹤問題。假設無人機在二維平面內定高、定速飛行，采用文獻[12]中的協調飛機動力學(CFV)模型。CFV模型由常微分方程描述，復雜度較小，將無人機運動簡化為質點運動，其動力學表達式如式(1)所示。

(1)

1.2 航跡優化技術

無人機若以離散航點兩兩連接形成的直線段航跡進行跟蹤，往往會在航點切換時無法滿足自身的機動約束造成較大的跟蹤誤差，所以需要利用動態航跡平滑器將離散航點優化為一條滿足無人機運動約束特別是機動性條件的可飛行航跡。n次貝塞爾曲線的伯恩斯坦(Bernstein)的表達式[18]如下：

(2)

式中：bj為曲線的控制點；Bj,n(t)為伯恩斯坦基函數，其k階導矢為：

(3)

式中Δkbj為k階向前差分矢量，它可由k-1階向前差分矢量遞推得到，其定義如式(4)所示。

(4)

本文采用組合貝塞爾曲線來完成航跡的平滑，引入航跡分段序號，將貝塞爾曲線作為每個航段的樣條曲線，可得到式(5)中用局部參數表示的組合n次貝塞爾航跡曲線。貝塞爾曲線具有很多優良和寶貴的性質[18]，使其更利于航跡形狀設計。

(5)

式中：i為航段序號，表明整條航路由一個個航段組成；控制頂點bni+j中的bni表示各個離散航跡點，每段航路曲線由n+1個控制點控制。當第i-1段與第i段兩相鄰航路連接時，要求他們在連接點處具有直到r階相等的左右導數，稱曲線在該點Cr連續。航段連接點的左右導矢方程如下：

(6)

(7)

聯立式(6)和式(7)得到航段連接點處的Cr連續條件：

Δkbni-k=Δkbni，k=0,1,…,r

(8)

對于給定的航跡點使用組合3次貝塞爾曲線進行航跡平滑得到C2連續的航跡，可實現航路曲率的連續性，由式(8)可推出在連接點的1階導矢連續確立了無人機速度的連續性，2階導矢連續確立了無人機側向加速度的連續性，得到式(9)。

(9)

引入輔助點di(i=0,1,…,m)，如圖1所示，由4個航跡點q0、q1、q2、q3、q4組成一條航路，將航路分成4段，每條航路由4個控制點控制，首尾2個控制點與航跡點重合，得到如下關系：

圖1 組合3次貝塞爾曲線示意圖

(10)

聯立式(9)和式(10)可得關系式(11)：

(11)

通過求解輔助點，進而確定所有的控制點，帶入式(5)得到航跡平滑曲線。該方法求解貝塞爾曲線不涉及基函數的運算，通過控制點就可得到航跡曲線，相比于Hermite三次樣條曲線計算量大大減小。

在實際的無人機航跡跟蹤問題中，一般是根據無人機執行的任務類型，設計任務目標點，形成一個離散的期望航點序列。期望航點如表1所示，利用貝塞爾曲線進行航跡平滑，得到的曲線如圖2所示。可以看出經過航跡平滑后的航跡曲率連續，更加符合無人機的機動飛行約束，為制導律的設計奠定了基礎。

表1 航點信息

圖2 航跡平滑仿真示意圖

2 非線性制導律設計

2.1 動態虛擬目標跟蹤點

對于一般的直線和圓弧航跡，可將無人機在航跡上的垂直投影點作為目標跟蹤點，求取無人機與目標航跡的側偏距和航向角偏差，利用這些信息進行制導律的設計。但對于一條變曲率航跡，由于其曲率的無規則變化，利用垂直投影求出的目標參考點可能存在多個，同時目標航向與側偏也可能在短時間內存在較大波動。

為了解決這一問題，本文提出了一種動態虛擬目標點的設計方法。在目標航跡上定義一個動態的虛擬目標點，該目標點的移動速度同無人機地速存在如下關系：

(12)

其中：Vt為虛擬動態目標點在目標航線上移動的速度；Vg為無人機地速；L為無人機距虛擬目標點的距離；參數R*為設定的虛擬目標點與無人機的最小距離，它的選取可根據無人機滾轉角的響應時間確定。

由公式(12)可以看出，當無人機遠離目標航跡時，虛擬點移動速度很小，幾乎不動，此時無人機追蹤航跡上的一個固定點快速靠近航跡。當無人機靠近航跡時(L=R*)，虛擬目標點的速度增加并與無人機的速度相等，無人機與虛擬目標點在航路長度上始終相差R*，并緊跟虛擬目標點飛行。與傳統的航跡跟蹤方法相比，該方法并沒有將目標航跡和無人機獨立開來，而是將無人機與動態虛擬目標點通過相對距離及速度建立起聯系。

虛擬目標點在每個制導律解算周期內進行更新，確定下一時刻無人機需要跟蹤的虛擬目標點，對于直線航跡，更新方法如式(13)所示。

(13)

其中：xt、yt為虛擬目標點位置；Vt、ΔT、ψref分別為虛擬目點速度、制導律解算周期及航跡方位角。

跟蹤半徑為Rc、圓心坐標為[xo,yo]的圓弧航跡時，定義沿順時針方向旋轉為正方向，虛擬目標點的更新如下：

(14)

其中Δψ、ψi-1分別為單位制導律周期內虛擬點相對圓心轉動的角度和上一時刻虛擬目標點相對圓心的方位角。

跟蹤一般的離散航點時，首先利用上節的離散航點平滑方法，得到目標航跡的參數曲線如式(15)所示。

(15)

虛擬目標點的更新可利用下式求出：

(16)

其中：ui-1、ui分別為上一時刻和當前時刻虛擬目標點在航跡上的參數值；ΔS為單位制導律解算周期內虛擬目標點行進的距離。可以利用組合辛普森定積分公式和二分迭代法求取參數ui，當前航段剩余距離<ΔS，需要進行航段切換，采用分航段積分確定參數ui的值。

2.2 非線性制導律設計

常規的導彈目標攔截模型，導彈只需要擊中目標即可完成任務，而無人機跟蹤虛擬目標點的制導模型中，無人機并不會與虛擬目標點交會，而是一直跟蹤下去，直到到達最后的任務航點。基于這一特點，本文設計的制導律，在考慮無人機航向角和視線角的偏差基礎上，加入了虛擬目標點的航向角和視線角的偏差，制導律方法如式(17)所示。

(17)

其中acmd、L、γa、γt、λ分別為側向加速度指令、無人機與虛擬目標點的距離、無人機航向角、虛擬目標點航向角、無人機與虛擬目標點之間的視線角。

當無人機跟蹤直線航跡，到達航跡上的某點時，若實際速度方向與該點的虛擬速度方向不同，會立刻脫離直線航跡，造成擺振現象，航跡跟蹤收斂較慢，跟蹤圓弧或變曲率航跡問題會更加突出。將虛擬目標點的航向信號引入制導律的設計中，這樣在跟蹤航跡時，無人機會調整航向飛向虛擬目標點。同時，在到達虛擬目標點前調整無人機航向，使其在到達目標航跡時與虛擬目標點速度方向一致，使實際航跡快速收斂到目標航跡。

2.3 制導律分析

當評價一個制導算法的好壞時通常的依據是其航跡跟蹤誤差的收斂速度。定義航跡跟蹤誤差d為無人機距航跡最近的距離，即通過投影算法得到的側偏距的大小，無人機在期望航跡右邊側偏距為正。下面從直線航跡跟蹤和圓弧航跡跟蹤兩個方面來進行制導律的分析。

1)直線航跡跟蹤

基于小擾動假設，當無人機跟蹤直線段，L→R*時，虛擬目標點的速度等于無人機速度，如圖3所示，可得到如下關系：

圖3 直線航跡跟蹤示意圖

(18)

同時，側向加速度

(19)

聯立式(17)-式(19)得：

(20)

其特征多項式的形式是一個簡單的二階系統：

(21)

該二階系統的自然頻率和阻尼比為：

(22)

航跡跟蹤誤差在時域上的響應曲線如式(23)所示。

(23)

(24)

可以看出當時間t→，航跡跟蹤誤差以的速度收斂到0。

2)圓弧航跡跟蹤

假設無人機與虛擬目標點以相距R*的距離在跟蹤圓弧航跡，如圖4所示。理想情況下，無人機應沿半徑為Rc的圓弧由A點在軌跟蹤飛行到C點，但實際由于受到外界環境很小的擾動，無人機偏離圓弧軌道位于B點，進行脫軌跟蹤，基于此小擾動假設：

(25)

式中：η為航向偏差角；δ為在軌跟蹤和脫軌跟蹤的視線角偏差。圖4中α為線段AC的弦切角。

圖4 圓弧航跡跟蹤示意圖

側向加速度可以表示如下：

(26)

由圓的幾何關系可以得到：

(27)

基于小角度假設并且當d?2Rc時：

(28)

聯立式(17)和式(27)，得

(29)

將式(25)、式(26)和式(28)帶入式(29)：

(30)

化簡，得

(31)

可以看出它同樣是一個2階系統的形式,跟蹤誤差的收斂速度如式(32)所示。

(32)

其收斂速度與圓弧半徑無關，說明了該制導律在跟蹤圓弧和直線時具有一致性，無需進行制導律邏輯切換，應用起來更加簡單。

2.4 制導律性能對比

從文獻[8]可得出L1非線性制導律，其側向加速度指令：

(33)

式中L1為一個固定的探測步長。當無人機與期望航跡距離>L1時，無法確定虛擬跟蹤點，該制導律失效，需要額外的制導邏輯來彌補這一缺陷，而本文提出的R*制導律不存在這個缺點。為了對比兩者的收斂速度，令L1=R*，可以得到非線性制導律在跟蹤直線航跡時的收斂速度：

(34)

跟蹤圓弧航跡時的收斂速度：

(35)

與式(32)對比可以看出，R*制導律的跟蹤誤差收斂速度是L1非線性制導律的2倍。在跟蹤圓弧航跡時，其跟蹤誤差收斂速度受到圓弧半徑的影響，R*制導律不受圓弧半徑的影響，可以將這一性質應用到變曲率航跡的跟蹤上。

3 仿真驗證

為了驗證論文提出的航跡跟蹤控制方法的優劣，本節進行3種情況的仿真，直線航跡跟蹤、圓弧航跡跟蹤和離散航跡點跟蹤。在前兩個情況中加入L1非線性制導律的跟蹤對比仿真。仿真時采用CFV模型，給定樣例無人機定常飛行速度為Vg=30 m/s，無人機運動狀態約束最大側向加速度amax=10 m/s2。

3.1 直線跟蹤仿真

無人機初始位置為(0 m,0 m),初始航向角為90°，參考航跡起點為(100 m,0 m),期望航跡角90°。分別取2種制導律的參數L1和R*為100 m和150 m。仿真結果如圖5所示。從圖5中可以看出，L1制導律在到達期望航跡后會出現超調，而R*制導律可以無超調地收斂到期望航跡。L1制導律在參數為100 m和150 m的收斂時間分別為14.43 s和21.09 s,R*制導律在參數為100 m和150 m的收斂時間分別為7.22 s和10.51 s,R*制導律的誤差收斂速度大約是L1制導律的2倍，這與理論分析一致。

圖5 直線航跡跟蹤仿真示意圖

3.2 圓弧跟蹤仿真

無人機初始位置為(500 m,200 m)，初始航向角為45°，參考航跡起點為(400 m,200 m)，跟蹤圓心為(200 m,200 m)，半徑為200 m的圓弧航跡，其中2種制導律參數選取L1=R*=50 m,對于L1制導律，當初始位置距離參考航跡較遠時，選取無人機在參考航跡上的投影點作為虛擬參考點，從圖6可以看出，L1制導律在收斂到參考航跡之前，存在超調和擺振現象，而R*制導律可以無超調地收斂到期望航跡。從圖7的側向加速度指令可以看出，相比于L1制導律，R*制導律中加入了動態虛擬目標點的航向角，得到的制導律指令更加平緩，有利于控制回路的跟蹤，避免了無人機進行大機動飛行，加快了航跡跟蹤誤差的收斂速度。

圖6 圓弧航跡跟蹤仿真示意圖

圖7 圓弧航跡跟蹤側向加速度指令示意圖

3.3 離散航跡點跟蹤

使用R*制導律跟蹤1.2節中經過航跡平滑后的期望航跡。無人機初始位置為(50 m,10 m)，初始航向角為45°，R*=20 m,仿真結果如圖8所示。可以看出，R*制導律在跟蹤經過參數化的變曲率曲線時，幾乎與期望航跡重合，并且在航段切換過程中跟蹤效果良好，跟蹤誤差在0.5 m以內。從圖9的仿真結果可以看出，無人機航向角始終緊跟視線角及虛擬點航向角，達到了精確跟蹤的效果。

圖8 離散航點跟蹤仿真示意圖

圖9 離散航點跟蹤航向角收斂示意圖

4 結語

本文針對無人機航跡跟蹤問題，提出了一種基于動態虛擬目標點的R*非線性制導律。在跟蹤離散航點時，采用貝塞爾曲線進行航跡優化，并提出一種貝塞爾曲線的簡單求解方法，得到了曲率連續的參數曲線，解決了航點切換時出現較大跟蹤誤差的問題。該制導律可以利用無人機與期望航跡的距離，建立無人機與動態虛擬目標點的聯系。通過小擾動線性化，分析了制導律的跟蹤性能，在跟蹤直線和圓弧航跡時，該制導律跟蹤誤差的響應是一個2階系統，與L1制導律相比，跟蹤誤差收斂速度是其2倍。當無人機初始點距離期望航跡較遠時，不需要設計額外的制導邏輯進行引導。同時利用R*制導律不受曲線曲率影響的優點，對參數化的變曲率航跡進行跟蹤，仿真結果顯示R*制導律可以實現對變曲率航跡的精確跟蹤。