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

周硙硙

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

1 引言

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

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

目前，雖然關(guān)于UAV路徑規(guī)劃和路徑跟蹤控制律的設(shè)計(jì)相對(duì)成熟，但是大多數(shù)研究都是分開的，沒有將路徑規(guī)劃和路徑跟蹤控制律統(tǒng)籌設(shè)計(jì)，也很少有利用規(guī)劃路徑特點(diǎn)進(jìn)行UAV路徑跟蹤控制律的設(shè)計(jì)。為此，本文提出一種基于三維五次PH曲線［10～12］的UAV協(xié)同路徑規(guī)劃方法，該路徑規(guī)劃算法能夠迅速為UAV規(guī)劃出較優(yōu)的路徑；同時(shí)，針對(duì)UAV三維PH路徑的特點(diǎn)，設(shè)計(jì)出相應(yīng)的跟蹤控制律，實(shí)現(xiàn)對(duì)UAV三維PH路徑的跟蹤控制。

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

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

2.1 三維五次PH曲線

2.2 基于三維五次PH曲線UAV路徑規(guī)劃

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

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

基于三維五次PH路徑的特點(diǎn)，對(duì)UAV三維五次PH路徑的跟蹤控制采用降維轉(zhuǎn)化的思想，即將UAV的三維PH路徑跟蹤控制降維轉(zhuǎn)化為UAV二維路徑跟蹤控制和高度的跟蹤控制。在式（3）～（5）和式（7）～（10）的基礎(chǔ)上，UAV三維五次PH路徑的三個(gè)坐標(biāo)分量都可表示成參數(shù)的多項(xiàng)式形式。即

3.1 二維平面路徑跟蹤控制律設(shè)計(jì)

基于三維PH路徑三個(gè)坐標(biāo)分量可精確已知的特點(diǎn)，采用非線性動(dòng)態(tài)逆控制器設(shè)計(jì)方法進(jìn)行路徑跟蹤控制律設(shè)計(jì)，可有效降低控制律設(shè)計(jì)的復(fù)雜度。在文獻(xiàn)［13］對(duì)UAV二維PH路徑跟蹤控制律設(shè)計(jì)的基礎(chǔ)上，在跟蹤UAV三維PH路徑時(shí)，基于降維跟蹤控制思想，相應(yīng)的二維平面路徑跟蹤控制律設(shè)計(jì)為

3.2 高度跟蹤控制律設(shè)計(jì)

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

4.1 UAV三維五次PH路徑規(guī)劃仿真

設(shè)定UAV的初始位置點(diǎn)Ps和目標(biāo)位置點(diǎn)Pf分 別 為，Ps和Pf處的兩個(gè)方向角分別為且最大曲率約束κmax=0.1，最大撓率約束τmax=0.1。相應(yīng)的Matlab仿真結(jié)果如圖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可飛行路徑的曲率和撓率約束條件。因此，規(guī)劃出的UAV三維五次PH路徑滿足要求。

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

對(duì)規(guī)劃的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°。對(duì)UAV三維PH路徑的跟蹤控制的Matlab仿真結(jié)果如圖3～6所示。

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

圖3 xe誤差曲線

圖4 ye誤差曲線

圖5 ze誤差曲線

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

5 結(jié)語

本文給出一種基于三維五次PH曲線理論的UAV三維路徑規(guī)劃方法，并在路徑規(guī)劃的過程中，考慮UAV的飛行性能約束，使規(guī)劃出的PH路徑安全可飛行；另外針對(duì)規(guī)劃出的UAV三維PH路徑參數(shù)化等特點(diǎn)，采用降維轉(zhuǎn)化并基于非線性動(dòng)態(tài)逆跟蹤控制方法設(shè)計(jì)出UAV的三維PH路徑跟蹤控制律。通過Matlab仿真結(jié)果驗(yàn)證了提出的UAV三維路徑規(guī)劃和路徑跟蹤控制律設(shè)計(jì)方法的有效性和可行性。