復(fù)雜環(huán)境下多UAV協(xié)同可飛行路徑規(guī)劃研究*

高曉冬，楊秀霞，張 毅

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

0 引言

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

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

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

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

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

1.1 PH路徑的生成

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

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

文獻(xiàn)[8]給出了二維五次 PH 路徑r（t）基于控制點(diǎn)的表達(dá)形式

其中，u（t）和 v（t）可設(shè)置為

1.2 PH路徑約束條件研究

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

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

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

1.3.1 PH路徑長度與控制參數(shù)關(guān)系的數(shù)學(xué)推導(dǎo)

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

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

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

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

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

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

即

將 u0，u1，u2，v0，v1和 v2看作變量，此時(shí)多項(xiàng)式σ（t）的階次為

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

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

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

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

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

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

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

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

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

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

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

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

又

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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