基于三維PH曲線的UAV在線路徑規劃及跟蹤控制研究?

周硙硙

（海軍裝備部駐南京地區第一軍事代表室 南京 210001）

1 引言

隨著無人飛行器（Unmanned Aerial Vehicle，UAV）技術的快速發展［1］，UAV的航程、機動性能都得到了大幅提升，使得UAV更加智能化、自動化，使得UAV在目標識別［2］、目標跟蹤［3］等方面有著越來越廣泛的應用。而日益復雜化的應用環境卻對UAV執行任務的能力提出了更高要求。因此，UAV執行任務體系的形成需要對參與執行任務的UAV進行實時在線路徑規劃，規劃的路徑需要滿足相關性能約束條件，使得執行任務的UAV能夠協同工作；同時UAV在執行任務過程中需要準確地跟蹤規劃的路徑，使UAV實際飛行軌跡較小地偏離期望路徑。

一些學者對UAV的在線三維路徑規劃算法進行了研究，文獻［4］為保證無人機以最小被發現概率和較小的航程到達目標點，提出一種基于改進蟻群算法的無人機三維路徑規劃方法；文獻［5］針對固定目標規劃問題，提出一種動態稀疏A*算法和voronoi圖改進算法融合的無人機三維路徑規劃方法；文獻［6］通過柵格化方法對無人機飛行環境進行三維建模，并主要以最短路徑為目標，提出了基于改進蟻群算法的無人機三維路徑規劃方法，但未考慮無人機的性能約束條件；文獻［7］綜合考慮無人機飛行高度、航跡長度等權重因子，提出了基于改進A*算法的三維無人機路徑規劃方法，并對規劃的路徑進行優化，以滿足無人機的俯仰角、偏航角、轉彎半徑等性能約束條件。在路徑規劃的基礎上，設計相關的路徑跟蹤控制律，從而實現對規劃路徑的跟蹤。文獻［8］提出一種基于切換平面的自適應非線性制導方法跟蹤規劃的三維Dubins路徑；文獻［9］通過模型參考自適應與動態逆相結合的方法設計路徑跟蹤控制器，實現對四旋翼無人機三維路徑的跟蹤控制。

目前，雖然關于UAV路徑規劃和路徑跟蹤控制律的設計相對成熟，但是大多數研究都是分開的，沒有將路徑規劃和路徑跟蹤控制律統籌設計，也很少有利用規劃路徑特點進行UAV路徑跟蹤控制律的設計。為此，本文提出一種基于三維五次PH曲線［10～12］的UAV協同路徑規劃方法，該路徑規劃算法能夠迅速為UAV規劃出較優的路徑；同時，針對UAV三維PH路徑的特點，設計出相應的跟蹤控制律，實現對UAV三維PH路徑的跟蹤控制。

2 基于三維五次PH曲線的UAV路徑規劃

在三維空間中，當UAV接受任務后，需要在初始位置點 ps(xs,ys,zs)和目標位置點 pf(xf,yf,zf)之間規劃出一條滿足UAV性能約束的路徑r(t)，在ps和 pf處的偏航角和俯仰角分別為(?s,ψs)、(?f,ψf)，即初始點和目標點處的位姿信息分別為poses(xs,ys,zs,?s,ψs)、posef(xf,yf,zf,?f,ψf)。那么，UAV路徑規劃可表述為

2.1 三維五次PH曲線

2.2 基于三維五次PH曲線UAV路徑規劃

2.3 三維五次PH路徑約束條件

3 三維五次PH路徑的跟蹤控制

基于三維五次PH路徑的特點，對UAV三維五次PH路徑的跟蹤控制采用降維轉化的思想，即將UAV的三維PH路徑跟蹤控制降維轉化為UAV二維路徑跟蹤控制和高度的跟蹤控制。在式（3）～（5）和式（7）～（10）的基礎上，UAV三維五次PH路徑的三個坐標分量都可表示成參數的多項式形式。即

3.1 二維平面路徑跟蹤控制律設計

基于三維PH路徑三個坐標分量可精確已知的特點，采用非線性動態逆控制器設計方法進行路徑跟蹤控制律設計，可有效降低控制律設計的復雜度。在文獻［13］對UAV二維PH路徑跟蹤控制律設計的基礎上，在跟蹤UAV三維PH路徑時，基于降維跟蹤控制思想，相應的二維平面路徑跟蹤控制律設計為

3.2 高度跟蹤控制律設計

4 UAV三維五次PH路徑規劃及跟蹤控制仿真

4.1 UAV三維五次PH路徑規劃仿真

設定UAV的初始位置點Ps和目標位置點Pf分 別 為，Ps和Pf處的兩個方向角分別為且最大曲率約束κmax=0.1，最大撓率約束τmax=0.1。相應的Matlab仿真結果如圖1和圖2所示，圖1為UAV三維PH路徑，圖2為UAV三維PH路徑的曲率和撓率曲線。

圖1 UAV三維PH路徑

圖2 UAV三維PH路徑的曲率和撓率曲線

從圖2中可以得出UAV三維PH路徑的曲率最大值κmax和撓率最大值τmax分別為0.015、0.075，均小于約束條件值0.1，能夠滿足UAV可飛行路徑的曲率和撓率約束條件。因此，規劃出的UAV三維五次PH路徑滿足要求。

4.2 UAV三維PH路徑的跟蹤控制仿真

對規劃的UAV三維五次PH路徑跟蹤控制的初始化條件：UAV的速度vB=28m/s，阻尼比ξ=0.707，自然頻率ωn=0.12rad/s，UAV路徑初始曲率κ0=0，初始跟蹤誤差d0=0.6m，初始偏航角角θ0=-20°，UAV初始高度h0=0，初始俯仰角ψ0=15°。對UAV三維PH路徑的跟蹤控制的Matlab仿真結果如圖3～6所示。

圖3～5分別給出了UAV三維PH路徑跟蹤的xe誤差曲線、ye誤差曲線和ze誤差曲線；圖6為UAV三維五次PH路徑跟蹤圖。從三條誤差曲線上可以看出，三維PH路徑的跟蹤誤差被控制在較小值上，可以得出相應的UAV三維PH路徑跟蹤控制律設計的有效性和可行性，表明基于三維PH曲線規劃的UAV路徑可跟蹤。

圖3 xe誤差曲線

圖4 ye誤差曲線

圖5 ze誤差曲線

圖6 三維五次PH路徑跟蹤圖

5 結語

本文給出一種基于三維五次PH曲線理論的UAV三維路徑規劃方法，并在路徑規劃的過程中，考慮UAV的飛行性能約束，使規劃出的PH路徑安全可飛行；另外針對規劃出的UAV三維PH路徑參數化等特點，采用降維轉化并基于非線性動態逆跟蹤控制方法設計出UAV的三維PH路徑跟蹤控制律。通過Matlab仿真結果驗證了提出的UAV三維路徑規劃和路徑跟蹤控制律設計方法的有效性和可行性。