單提曉，蔣 蓁

（上海大學機械工程與自動化學院，上海 200072）

0 引言

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

1 問題描述

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

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

圖1 雷達分布圖

2 路徑規(guī)劃方法

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

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

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

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

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

圖3 Voronoi圖

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

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

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

表1 距離矩陣部分數(shù)據(jù)

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

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

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

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

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

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

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

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

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

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

3 仿真結(jié)果

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

3.1 section取值對所得路徑影響

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

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

3.2 各階段路程長度對比

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

圖7 section取值不同時的路徑

圖8 總路程對比

圖9 各階段總路程對比

3.3 計算時間對比

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

圖10 計算時間對比

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

4 結(jié)論

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

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

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

［3］陳理榮.數(shù)學建模導論［M］.北京:北京郵電學院出版社，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.