一種基于分割法的無人機(jī)路徑規(guī)劃新方法*

單提曉，蔣 蓁

（上海大學(xué)機(jī)械工程與自動化學(xué)院，上海 200072）

0 引言

無人機(jī)的路徑規(guī)劃是無人機(jī)研究的一個重要領(lǐng)域，其目的是根據(jù)任務(wù)目標(biāo)在有障礙環(huán)境中規(guī)劃出一條滿足約束條件的最優(yōu)無碰撞飛行軌跡。文中提出的無人機(jī)路徑規(guī)劃方法基于Voronoi圖法和Dijkstra算法。Voronoi圖，又叫泰森多邊形或Dirichlet圖，是獲取無人機(jī)安全飛行路徑的一種常用方法，并且在計(jì)算機(jī)中容易實(shí)現(xiàn)，但是通過Voronoi圖得到的路徑往往比較曲折，并不是最優(yōu)路徑，很多情況下還存在多條可選路徑。Dijksta算法解決的是有向圖中單個源點(diǎn)到其他頂點(diǎn)的最短路徑問題。結(jié)合Voronoi圖法生成的可選路徑，可以大大減少Dijkstra算法中需要計(jì)算的節(jié)點(diǎn)數(shù)量，從而提高算法執(zhí)行效率。

1 問題描述

在無人機(jī)的路徑規(guī)劃過程中，存在的威脅主要有影響飛行安全的雷達(dá)、山峰、建筑等。在實(shí)際的路徑規(guī)劃中，為了減少計(jì)算量，需要對這些威脅進(jìn)行簡化。假設(shè)在10 km×10 km的飛行區(qū)域內(nèi)分布有11個位置已知的敵方探測雷達(dá)，雷達(dá)的作用方式完全相同且相互獨(dú)立，每部雷達(dá)的探測范圍均為半徑1 km。如圖1所示為雷達(dá)分布圖，其中圓圈表示每個探測雷達(dá)所能探測的最大范圍，圓圈中點(diǎn)表示探測雷達(dá)所在位置。無人機(jī)在某一固定飛行高度相對于雷達(dá)做等速規(guī)避運(yùn)動。無人機(jī)安全飛行區(qū)為圖中所示圓圈以外的區(qū)域。

圖1中左下方坐標(biāo)為（0，2）的點(diǎn)為無人機(jī)飛行路徑的起點(diǎn)，右上方坐標(biāo)為（10，9）的點(diǎn)為飛行路徑的終點(diǎn)。路徑規(guī)劃的目的就是在雷達(dá)探測范圍以外的區(qū)域找到一條連接起點(diǎn)和終點(diǎn)并且滿足約束條件、距離最短的路徑。

圖1 雷達(dá)分布圖

2 路徑規(guī)劃方法

文中提出的路徑規(guī)劃方法基于Matlab軟件平臺，首先建立關(guān)于飛行區(qū)域內(nèi)雷達(dá)點(diǎn)的Voronoi圖以獲取安全飛行路徑。利用Dijkstra算法結(jié)合Voronoi圖法獲得的飛行路徑計(jì)算出一條可連通并且長度最短的安全路徑。若無法獲得此路徑，則算法結(jié)束。若可得到此路徑，則使用文中提出的分割法對經(jīng)Dijkstra算法得到的路徑進(jìn)行優(yōu)化得到拐點(diǎn)數(shù)量更少的飛行路徑。最后使用spcrv函數(shù)對所得路徑進(jìn)行最終優(yōu)化得到平滑的最終安全飛行路徑。如圖2所示為文中路徑規(guī)劃算法工作流程圖。

圖2 路徑規(guī)劃算法工作流程圖

2.1 建立關(guān)于雷達(dá)點(diǎn)的Voronoi圖

在Matlab中利用Voronoi函數(shù)可以很方便的生成關(guān)于雷達(dá)點(diǎn)的Voronoi圖，并返回相應(yīng)節(jié)點(diǎn)的坐標(biāo)位置［1］。

其中voronoi函數(shù)可作出Voronoi圖，voronoin函數(shù)返回相應(yīng)數(shù)據(jù);Radar_X為11×1維矩陣，代表雷達(dá)點(diǎn)在該區(qū)域中位置的橫坐標(biāo);Radar_Y為11×1維矩陣，代表雷達(dá)點(diǎn)在該區(qū)域中位置的縱坐標(biāo);返回值V為n×2維矩陣，代表Voronoi圖垂直平分線交點(diǎn)在圖中的坐標(biāo)位置，n值由雷達(dá)的數(shù)量和雷達(dá)的位置決定;C為11×1維矩陣，代表相應(yīng)雷達(dá)點(diǎn)對應(yīng)的V向量中的節(jié)點(diǎn)下標(biāo)。

圖3 Voronoi圖

圖3為voronoi函數(shù)生成的Voronoi圖，從圖中可以看出每條線段即相鄰兩個雷達(dá)點(diǎn)的垂直平分線。無人機(jī)按照此線飛行即可以安全執(zhí)行飛行任務(wù)。因此，采用Voronoi圖生成的初始飛行路徑就能夠很好的回避危險(xiǎn)區(qū)域，為之后的進(jìn)一步路徑優(yōu)化提供了便利［2］。

2.2 利用Dijkstra算法篩選最短路徑

要在 Matlab中實(shí)現(xiàn) Dijkstra算法需要求得Voronoi圖中節(jié)點(diǎn)對應(yīng)的距離矩陣Distance_M［3］。對于無人機(jī)飛行路徑的起點(diǎn)、終點(diǎn)與節(jié)點(diǎn)的連接方法，文中選擇起點(diǎn)與終點(diǎn)分別和與此兩點(diǎn)可連通的距離最近的節(jié)點(diǎn)對應(yīng)的兩雷達(dá)點(diǎn)的中點(diǎn)連接，利用式（2）中得到的矩陣C可以求得距離矩陣，如表1所示為所得距離矩陣部分?jǐn)?shù)據(jù)。表中數(shù)值為inf表示兩點(diǎn)不可連通，如Distance_M（1，3）=inf表示節(jié)點(diǎn)1和節(jié)點(diǎn)3無法連通，數(shù)值不為inf表示兩點(diǎn)可連通，如Distance_M（1，2）=2.304 9表示節(jié)點(diǎn)1和節(jié)點(diǎn)2可連通并且其相距 2.304 9 km［4－5］。

表1 距離矩陣部分?jǐn)?shù)據(jù)

利用距離矩陣Distance_M并結(jié)合Dijkstra算法即可求得基于Voronoi圖所得節(jié)點(diǎn)的最短路徑:

其中:distance表示返回經(jīng)Dijkstra算法計(jì)算得到的最短路徑長度;path為1×m維矩陣，表示最短路徑從起點(diǎn)到終點(diǎn)的節(jié)點(diǎn)下標(biāo)。所得路徑如圖4粗實(shí)線所示。

圖4 利用Dijkstra算法得到的最短路徑

2.3 利用分割法優(yōu)化最短路徑

由圖4可以看出經(jīng)過Voronoi圖法和Dijkstra算法得到的路徑在拐點(diǎn)處的曲率變化非常大，這就要求無人機(jī)在經(jīng)過每個拐點(diǎn)時都要急劇改變控制角以改變飛行路徑，這對于無人機(jī)來說是難以從技術(shù)上實(shí)現(xiàn)的。

文中提出采用分割法將圖3中路徑的每條線段進(jìn)行等長分割，分割段數(shù)用section表示，定義Min_D為航跡到雷達(dá)點(diǎn)的最小距離，顯然有:

依次連接起點(diǎn)與之后的分割點(diǎn)成一條直線，直到直線（無人機(jī)飛行路徑）與圓（雷達(dá)掃描范圍）的最小距離小于Min_D，即最短飛行路徑所能達(dá)到的極限位置。此時記錄下上一分割點(diǎn)，并以此分割點(diǎn)為新的起點(diǎn)依次連接此點(diǎn)以后的分割點(diǎn)。重復(fù)以上過程直到最后連接點(diǎn)為終點(diǎn)。如圖5所示，圖中虛線即section=50時，經(jīng)過分割法得到的無人機(jī)飛行路徑（考慮到無人機(jī)飛行的安全性，在此取Min_D=1.2）。

2.4 平滑優(yōu)化路徑

雖然經(jīng)過分割法優(yōu)化后得到的路徑拐點(diǎn)減少，但是仍存在前后曲率變化過大的拐點(diǎn)。采用B樣條函數(shù)對路徑進(jìn)行平滑可以很好的解決所得路徑在曲率及保持原路徑的一致性之間的矛盾。Matlab中的spcrv函數(shù)為生成均勻劃分的B樣條函數(shù)，利用spcrv函數(shù)對Dijkstra算法生成的路徑進(jìn)行均勻劃分，即可得到平滑的飛行路徑。如圖6中曲線所示為經(jīng)spcrv函數(shù)計(jì)算所得的路徑，不僅能夠保證偏離原路徑的幅度較小，而且能夠滿足無人機(jī)安全飛行的要求。

圖5 利用分割法得到的優(yōu)化路徑

3 仿真結(jié)果

文中基于Voronoi圖和Dijkstra算法得到無人機(jī)飛行的初始路徑，利用分割法對所得路徑進(jìn)行優(yōu)化以減少拐點(diǎn)，最后對所得路徑進(jìn)行平滑，得到滿足約束條件的最優(yōu)飛行路徑。

3.1 section取值對所得路徑影響

為了獲知采用分割法時section的取值對所得無人機(jī)路徑的影響，文中分別對section不同取值進(jìn)行仿真實(shí)驗(yàn)。如圖7中a、b、c、d依次為section取值為5，10，50，500時得到的路徑。實(shí)驗(yàn)結(jié)果表明，當(dāng) section＜50時，隨著section取值的增大，所得最優(yōu)路徑逐漸趨于平滑，改變明顯。當(dāng)section≥50時，所得的最優(yōu)路徑的改變不再明顯。

圖8所示為本次仿真section不同取值情況下所得路徑的總路程，從圖中可以看出，當(dāng)section取值小于50時，所得最優(yōu)路徑的路程長度優(yōu)化效果明顯;當(dāng)section取值大于500時，隨著section的增大并不能明顯降低最優(yōu)路徑的路程長度。

3.2 各階段路程長度對比

如圖9所示為某次仿真實(shí)驗(yàn)section取值為50時，所得路徑長度依次經(jīng)Dijkstra算法、分割法、spcrv平滑后所得數(shù)據(jù)，由圖可以看出采用文中提出的分割法對經(jīng)Dijkstra算法得到的路徑進(jìn)行優(yōu)化，可以有效縮短飛行路徑，經(jīng)spcrv函數(shù)對路徑進(jìn)行平滑后，可進(jìn)一步縮短飛行路徑。

圖7 section取值不同時的路徑

圖8 總路程對比

圖9 各階段總路程對比

3.3 計(jì)算時間對比

圖10所示為section依次取值為5，10，50，500，5 000，50 000時，程序執(zhí)行所需的時間。為了減少其他隨機(jī)性因素對程序執(zhí)行時間的影響，將section每種取值的情況都仿真10次并取平均值。從圖中可以看出，當(dāng)section取值小于50時，程序執(zhí)行時間變化不大，均在0.379 3 s以內(nèi)。當(dāng)section取值大于5 000時，程序執(zhí)行時間呈指數(shù)式增長。當(dāng)section=50 000時，程序執(zhí)行時間為14.718 5 s。

圖10 計(jì)算時間對比

綜合以上實(shí)驗(yàn)結(jié)果表明，當(dāng)section取值在50左右時，所得最優(yōu)路徑的路程長度就可達(dá)到滿意效果，并且計(jì)算所需時間非常短。在實(shí)際應(yīng)用中，可根據(jù)需要在50左右對section取值。

4 結(jié)論

為解決無人機(jī)路徑規(guī)劃問題，文中在Matlab軟件平臺下，基于Voronoi圖并結(jié)合Dijkstra算法得到初始飛行路徑。利用文中提出的分割法對初始飛行路徑進(jìn)行優(yōu)化可進(jìn)一步減少初始路徑中的拐點(diǎn)并縮短路徑路程，最后利用spcrv函數(shù)對路徑進(jìn)行平滑得到能夠滿足無人機(jī)安全飛行要求的最優(yōu)路徑。仿真結(jié)果證明了文中算法的可行性，并且驗(yàn)證了其時間性能。在下一步的工作中，仍需對獲取最優(yōu)路徑的算法做進(jìn)一步的改良。

［1］張志涌。Matlab教程［M］.北京:北京航空航天大學(xué)出版社，2010.

［2］許松清，吳海彬，林宜，等.基于Voronoi圖法的移動機(jī)器人路徑規(guī)劃［J］.中國工程機(jī)械學(xué)報(bào)，2005，3（3）:336－340.

［3］陳理榮.數(shù)學(xué)建模導(dǎo)論［M］.北京:北京郵電學(xué)院出版社，1999.

［4］DongKai Fan，Ping Shi.Improvement of Dijktra’s algorithm and its application in route planning［C］//2010 Seventh International Conference on Fuzzy Systems and Knowledge Discovery Vol.4，2010，4:1901 －1904.

［5］Yin Chao，Wang Hongxia.Developed Dijkstra shortest path search algorithm and simulation［C］//2010 International Conference on Computer Design and Applications Vol.1，2010:116－119.