高曉冬，楊秀霞，張 毅

（1.海軍裝備部軍械裝備局，北京 100081；2.海軍航空大學，山東 煙臺 264001）

0 引言

目前，無人飛行器（UAV）無論在軍事上還是民用上都得到了越來越廣泛的應用。為提高UAV在復雜環(huán)境下執(zhí)行任務的能力，通常采用多UAV協(xié)同，此時也對多UAV到達目的地的時間和飛行角度提出了要求。對UAV的路徑規(guī)劃主要有航路點規(guī)劃及曲線規(guī)劃兩種。路徑點規(guī)劃只考慮路徑點處UAV的性能約束，而沒有在整個航跡上考慮，航跡的曲率變化會比較大，這樣會導致實際飛行航跡與規(guī)劃所得到的航跡之間存在較大誤差。因此，要得到真正可飛行的航跡，必須考慮路徑的曲率。

一些學者提出應用曲線來直接規(guī)劃整個航跡，如文獻[1]提出利用Dubins曲線來規(guī)劃多UAV航跡。而從微分幾何的原理來看，要得到平滑的飛行器運動軌跡，路徑的前兩階導數(shù)至少是存在的，即曲線的曲率是連續(xù)的，而Dubin路徑是C1類曲線，但不是C2類曲線。文獻[2]提出將Bézier曲線應用于機器人的路徑規(guī)劃中，胡峰[3]將Bézier曲線應用于多UAV路徑規(guī)劃。Bézier曲線可基于起始終止點的位置和切向量使UAV即時生成路徑，并且在路徑的任意點處都可以進行路徑重規(guī)劃，并到達目標位置。

目前，Pythagorean Hodograph（PH）曲線[4-6]被廣泛應用于飛行器的在線航跡規(guī)劃中。PH曲線規(guī)劃航跡為C2類曲率連續(xù)航跡，用于航跡規(guī)劃相比其他曲線有好多優(yōu)點：1）整體曲率比較?。?）曲線的長度、曲率、彎曲能量能以閉合形式求出；3）起始和終止點的位置和方向直接被用于邊界條件；4）長度和曲率容易協(xié)調(diào)。PH曲線生成可基于Bézier樣條曲線，因此，其還擁有Bézier曲線規(guī)劃的優(yōu)點。

文獻[7-8]給出了多UAV同時到達的PH曲線生成方法，多UAV協(xié)同時，選最長的航跡作為參考航跡；考慮航跡的曲率等性能約束，若不滿足要求，考慮將PH控制點的切向量增長，此時整體路徑長度會增加，文中沒有對二者的最優(yōu)協(xié)調(diào)曲線參數(shù)選擇進行研究，并且沒有考慮復雜威脅環(huán)境下的多UAV協(xié)同。文獻[9]提出一種復雜環(huán)境下的多UAV路徑規(guī)劃算法，該算法基于速度障礙圓弧法，采用PH曲線對UAV進行避障路徑重規(guī)劃，但沒有考慮多UAV之間的協(xié)同。

1 基于PH曲線的可飛行路徑生成及控制參數(shù)研究

1.1 PH路徑的生成

采用五次PH曲線進行航跡規(guī)劃，它具有彎曲點且能為飛行航跡提供足夠靈活的最低階次PH曲線[5]。UAV進行路徑規(guī)劃時，給出初始點P0和目標點P5處的位姿信息，，就可以規(guī)劃出滿足UAV性能約束的可飛行路徑r（t）：

其中，（x0，y0）為出發(fā)點P0的坐標，（x5，y5）為目標點P5的坐標，θ0和θ5分別表示為出發(fā)點P0和目標點P5處的方向角，t為控制參數(shù)，表示UAV的性能約束條件。

文獻[8]給出了二維五次 PH 路徑r（t）基于控制點的表達形式

其中，u（t）和 v（t）可設置為

1.2 PH路徑約束條件研究

首先，為了滿足UAV的動態(tài)性能要求，所規(guī)劃的PH路徑需要滿足UAV的最大曲率約束，即。

另外，為了能夠?qū)崿F(xiàn)多UAV路徑規(guī)劃在目標點P5處的角度協(xié)同，需要在PH路徑規(guī)劃時能夠在目標點處考慮UAV的飛行角度約束。而五次PH路徑規(guī)劃的初始化條件中，目標點處UAV的位姿信息包含了UAV的飛行方向角θ5，因此，可將多UAV在目標點處協(xié)同的飛行角度作為規(guī)劃UAV可飛行PH路徑初始化條件中的位姿信息，從而實現(xiàn)多UAV在目標點處的方向角協(xié)同。

1.3 PH路徑長度的控制參數(shù)研究

1.3.1 PH路徑長度與控制參數(shù)關系的數(shù)學推導

PH曲線滿足勾股條件，矢端曲線是曲線的切線也即曲線的一階導數(shù)，因此，與速度矢量等價。以t為參數(shù)的r（t）的長度可寫為如下形式：

PH航跡的長度、曲率和等距線均具有閉合的多項式表達式。選擇合適的多項式σ（t），使其滿足

則可通過對多項式σ（t）的積分算出航跡長度 s（t），而 σ（t）本身即為速度。曲線的路徑長度為：

式（13）中，均勻變化的t可以得到沿曲線均勻變化的路徑長度。

由式（2）、式（3）和式（12）、式（13）可得

再將式（4）和式（5）代入式（14），可確定 σ（t）的二次多項式形式為

即

將 u0，u1，u2，v0，v1和 v2看作變量，此時多項式σ（t）的階次為

由文獻[8]可知，變量 u0，u1，u2，v0，v1，v2都可以表達成為和一個函數(shù)的乘積。所以，多項式σ（q）可表示為

由于 θ0，θ5，θP0P5為設定的路徑規(guī)劃初始化條件，根據(jù)式（19）可得，PH路徑的長度s與參數(shù)k和ε有關。而當ε的值較小時，ε對PH路徑長度的影響較小，可認為PH路徑長度s與出發(fā)點和目標點處切向量長度k具有‘類線性’關系。

1.3.2 PH路徑長度與控制參數(shù)關系的仿真驗證

將UAV路徑規(guī)劃條件進行初始化，對PH路徑長度s與切向量長度k之間的‘類線性’關系進行仿真驗證。仿真圖1給出了其中一種路徑規(guī)劃初始化條件下的仿真結(jié)果，從圖1中可以看出規(guī)劃的PH路徑長度s與切向量長度k之間為‘類線性’關系。

1.4 時間和角度約束下的多UAV協(xié)同路徑規(guī)劃

對于多UAV協(xié)同路徑規(guī)劃，可充分利用PH路徑長度s與切向量長度k之間的‘類線性’關系，可快速規(guī)劃出多UAV路徑長度相差不大的PH路徑，再通過簡單的速度控制即可實現(xiàn)多UAV到達目標點處的時間協(xié)同。另外，可將目標點P5的飛行角度協(xié)同融入到PH路徑規(guī)劃的初始化條件中。

2 多UAV自主避障協(xié)同路徑規(guī)劃

2.1 復雜環(huán)境下UAV自主避障規(guī)劃

UAV在復雜環(huán)境飛行過程中，可能會遭遇未知動態(tài)障礙物，但UAV可利用自身攜帶的傳感器裝置探測到周圍環(huán)境中的障礙物，并獲取它們的位置和速度信息，UAV探測到未知動態(tài)障礙物的情況示意圖如下頁圖2所示。為簡化UAV對未知動態(tài)障礙物的避碰，將UAV當作一個點，同時根據(jù)障礙物與UAV之間的相對大小，將障礙物‘膨化’為一個半徑為R的圓。那么，過UAV位置坐標點作障礙圓的切線l1和l2，則UAV與障礙物之間形成了障礙錐CC（Collision Cone）。障礙物的位置坐標為，速度矢量為；UAV的位置坐標為，速度矢量為。另外，設定UAV攜帶的傳感器探測距離為d0。

首先，需要根據(jù)相應的初始化條件，判斷UAV探測到的障礙物威脅性。當α≥α0時，障礙物不具有威脅性；當α＜α0時，障礙物具有威脅性。其中，α為相對速度矢量vu0與位置矢量之間的夾角，α0為障礙錐的半頂角。α和α0可通過下列關系進行求解，

對威脅障礙物的避碰可通過速度障礙VO進行求解，速度障礙示意圖如圖3所示。圖3中的速度障礙VO是將示意圖2中的障礙錐CC平移障礙物的速度矢量vo得到。為實現(xiàn)UAV對威脅障礙物的避碰，需要將UAV的速度矢量vu旋轉(zhuǎn)出速度障礙VO，圖3給出了相應的旋轉(zhuǎn)示意圖。

假設UAV的速度大小不變，將速度矢量末端旋轉(zhuǎn)至速度障礙VO的邊界上，對應的UAV與障礙物之間相對速度矢量vuo位于速度障礙邊界l1和l2上，從而可實現(xiàn)UAV對威脅障礙物的避碰。根據(jù)示意圖3中的幾何關系，可對UAV避碰的期望速度矢量vu進行求解，

又

2.2 復雜環(huán)境下多UAV避障協(xié)同路徑規(guī)劃

復雜環(huán)境下，UAV在飛行過程中可能探測到周圍存在的威脅障礙物，需對其進行避碰，此時多UAV協(xié)同路徑規(guī)劃需考慮因避障路徑重規(guī)劃產(chǎn)生的影響。為實現(xiàn)復雜環(huán)境下的多UAV協(xié)同，設定以最長避障重規(guī)劃路徑作為多UAV協(xié)同的參考路徑。

3 多UAV自主避障協(xié)同路徑規(guī)劃仿真

3.1 多UAV協(xié)同路徑規(guī)劃仿真

對時間和角度約束下基于PH曲線的多UAV協(xié)同路徑規(guī)劃方法進行仿真驗證。各UAV的仿真初始化條件如表1所示。

表1 多UAV協(xié)同路徑規(guī)劃初始化條件及路徑長度

下頁圖4給出了多UAV協(xié)同路徑規(guī)劃仿真圖。通過計算，規(guī)劃出的各UAV的PH路徑長度分別為 559.3、560.0、559.6、559.8，則各 UAV 的 PH 路徑長度相差不大，再通過簡單的速度控制，即可實現(xiàn)各UAV到達目標點的時間協(xié)同。另外，各UAV到達目標點處的飛行角度為初始化條件的角度，到達目標點處的角度協(xié)同也可實現(xiàn)。

3.2 多UAV避障協(xié)同路徑規(guī)劃仿真

在3.1節(jié)仿真過程中，假設UAV2探測到在處存在一動態(tài)障礙O，‘膨化’半徑為R=45，其速度大小為V0=40，速度方向與OX軸的夾角分別為θo=195°，各UAV速度Vu=55。

通過計算，UAV2需要對障礙物O進行避障，仿真結(jié)果如圖5所示。從圖5中可以看出，UAV2成功實現(xiàn)了避障，此時各PH路徑的長度分別為559.3，560.7，559.6，559.8，各 PH 路徑長度相差也不大，仍可以通過簡單的速度控制實現(xiàn)時間協(xié)同。

4 結(jié)論

通過PH曲線路徑規(guī)劃實現(xiàn)多UAV到達目標點的時間和角度協(xié)同。將多UAV到達目標點處的飛行角度協(xié)同融入到規(guī)劃PH路徑的初始化條件中。推導并仿真驗證了PH路徑長度與控制參數(shù)之間的關系，得出了PH路徑長度s與切向量長度k之間‘類線性’關系，據(jù)此可快速確定多UAV路徑長度近似相等的控制參數(shù)k，避免了為實現(xiàn)多UAV時間協(xié)同而進行的迭代過程，大大地降低了算法的復雜度和協(xié)同規(guī)劃時間。另外，對于復雜環(huán)境下的多UAV協(xié)同，首先采用速度障礙法進行避障，采用PH曲線進行路徑重規(guī)劃，通過UAV速度及PH曲線生成參數(shù)的控制，也可實現(xiàn)多UAV到達目標點處的時間和角度協(xié)同。