基于改進A＊算法的無人機避障路徑規(guī)劃①

李曉輝,苗 苗,冉保健,趙 毅,李 剛

(長安大學 電子與控制工程學院,西安 710064)

無人機作為我國科技創(chuàng)新的重要產業(yè),正處于井噴式發(fā)展時期.它的技術日漸成熟,能夠執(zhí)行復雜的危險任務.多用于軍事偵查、電力巡檢、貨物運輸、物流配送等,在航空領域占有一席之地.隨著自動控制技術、傳感技術、導航技術、實時監(jiān)控技術、計算機技術的飛速發(fā)展,無人機的功能也越來越完善,應用領域不斷擴大.因此,研究無人機路徑規(guī)劃問題有著重要意義[1,2].我國物流運輸行業(yè)迅猛發(fā)展,已經成為我國國民生產的重要組成部分,與人們的生活密切相關.而隨著市場需求的增大,配送成本也在不斷升高.合理有效的運輸方案,可以幫助企業(yè)降低運輸成本,提高運輸質量,同時還能實現(xiàn)節(jié)能減排的環(huán)保目標[3].無人機因其高效、靈活、低成本的特點,漸漸成為各大物流公司的寵兒.無人機本身是科技的產物,應該合理正確的使用,不應該再出現(xiàn)無人機碰撞飛機、使用無人機偷窺等負面新聞,這些都是十分危險的行為.再加上我國嚴格的航空管制,設置禁飛區(qū)顯得十分有必要.因此,無人機路徑規(guī)劃問題的研究常常與禁飛區(qū)避障問題相結合.

路徑規(guī)劃問題一直是國內外學者研究的熱點,無人機的路徑規(guī)劃問題最初是由旅行商問題(Traveling Salesman Problem,TSP)[4]和車輛路徑問題(Vehicle Routing Problem,VRP)[3]衍變而來.無人機避障路徑規(guī)劃問題分為二維和三維兩種情況.本文主要研究二維平面的路徑規(guī)劃問題.目前大多數(shù)的二維平面路徑規(guī)劃問題中,禁飛區(qū)的類型都只是單一的,比如只存在多邊形障礙物[2],或者只存在圓形障礙物[5,6].文獻[2]提出了一種改進的A*算法來規(guī)劃無人機路線,但只針對單一的矩形禁飛區(qū).文獻[5]提出順序凸規(guī)劃問題逼近非凸零件的算法來規(guī)劃無人機航線,是只針對圓形禁飛區(qū)設計的.文獻[6]將融合簡化稀疏A*算法與模擬退火算法相結合,規(guī)劃出了一條代價較低的航跡,但禁飛區(qū)的類型也是單一的圓形.本文研究的是多類型禁飛區(qū)共存的情況,即同時包含有圓形、多邊形障礙物,航跡也可以貼著禁飛區(qū)的某條邊飛行,這大大提高了算法設計的難度.這類問題最常用到的算法包括模擬退火算法[6]、改進的粒子群優(yōu)化算法(Particle Swarm Optimization)[7]、蟻群算法(Ant Colony Algorithm)、幾何法[8]等.文獻[6]提出了將A*算法與模擬退火算法相結合來解決無人機避障的路徑規(guī)劃問題,能夠利用較少內存,快速的得到一條綜合代價較低且較為平滑的航跡.文獻[7]提出了一種改進的粒子群算法來規(guī)劃無人機航向路線,相比較傳統(tǒng)的粒子群算法,改進后的算法適應性更強,環(huán)境規(guī)劃策略更加靈活,但是集中式計算負擔過大,有時不能滿足需要.文獻[8]提出了一種基于幾何法的路徑規(guī)劃方法,在航跡部分加入了螺旋線航跡.本文采用的是改進后的A*算法來解決多類型禁飛區(qū)下無人機路徑規(guī)劃的問題.

1 問題描述

本文研究的問題是通過算法找出無人機從任意起點到任意終點運輸代價最小的安全避障路線.假設某指定區(qū)域內有多個不同類型的禁飛區(qū),假設物流無人機均為充電旋翼式無人機,對貨物載重、航行時間、電池能耗等有約束限制.為了確保貨物能夠安全準時運輸至指定客戶點,需要對無人機進行路徑規(guī)劃.本文假設僅對物流無人機航行階段路徑進行規(guī)劃,則無人機的運動可簡化為二維平面運動.

無人機路徑規(guī)劃需要滿足以下原則:

①無人機飛行起始點記為S,終止點記為T.

②任務規(guī)劃應該得到一條從任務起點到任務終點的路徑.

③要在無人機避過禁飛區(qū)的同時,保證規(guī)劃的路徑應盡量最優(yōu)化,滿足無人機航程最短的要求.

無人機路徑規(guī)劃的實現(xiàn)主要包括兩個步驟:(1)環(huán)境建模.根據無人機所在的實際環(huán)境,在仿真系統(tǒng)中建立合適的模型,將實際的物理空間抽象成能用數(shù)學模型求解的虛擬空間.(2)路徑搜索[9].根據任務條件規(guī)劃出一條從任意起點S到任意終點T的任務路線,在滿足任務條件約束、滿足避障要求的同時使路徑代價達到最短.

本文想要實現(xiàn)的目標是:無人機能在準確避過多類型障礙物的同時,最小化航行里程.

2 算法設計

A*算法是一種靜態(tài)路網中求解最短路徑最有效的直接搜索算法,它通過啟發(fā)函數(shù)來引導算法的搜索方向.針對本文研究的問題,對A*算法做了一定的改進.首先輸入無人機飛行起始點S,終止點T.建立兩個數(shù)組C1、C2,用來存放無人機所經過節(jié)點的信息.

上式中,fi表示起點S到節(jié)點i的有效已走距離,這里的有效已走距離是指已經順利避過障礙物的路線距離;gi表示節(jié)點i到終止點T的直線距離,這里的直線距離是指不考慮障礙物時節(jié)點i到終止點T的直線距離,即剩余最小距離,這是一個最小估算值.Di是fi與gi的和,表示節(jié)點i目前的最小總代價值.

算法重要步驟:

Step 1.輸入起始點和終止點的坐標,將起點存入C1 中.

Step 2.遍歷C1,找到最小值Dmin對應的節(jié)點對它進行展開,即找到距離該節(jié)點最近的障礙物,獲取該障礙物的節(jié)點信息,將該障礙物的可用節(jié)點添加到C1 中,并把C1 中已展開的點移至C2 中.

Step 3.重復Step 2,直到當前展開點與終點連線不再經過任何障礙物,可直接到達終點時停止,或者直到C1為空時停止循環(huán).

Step 4.對得到的最優(yōu)路徑再次檢查修復.

Step 4是對路徑的修復,如圖1所示,假設我們已經找到S到T點的最優(yōu)路徑,圖中可以看出S點到V4點的連線不穿過禁飛區(qū),可以直達,這種情況下就可以刪除S到V4之間的多余節(jié)點;而S到V6點之間的連線穿過了禁飛區(qū),則不可修復.為了最終結果更加完美,代碼中對每個節(jié)點都進行了檢測,測試它們到其他節(jié)點是否可直達,若可達則刪除多余節(jié)點來修復路徑[10].

圖1 路徑修復圖

重點算法部分的偽代碼見算法1和算法2.

算法1.重點算法輸入:起點S 坐標,終點T的坐標,障礙物信息輸出:最優(yōu)解best_solution//C1,C2 保存點的信息,包括每個點的f 值、g 值、坐標、上層節(jié)點讀取障礙物點信息,起始點信息;設best_solution的初始f 值為inf;判斷S→T是否闖過障礙物,不穿過障礙物可直接返回;若穿過障礙物,則將S 加入到C1 中,進入while 循環(huán);While 停止機制:當前迭代的節(jié)點與終點T 連線不再穿過任何障礙物,或者C1為空找C1 中找到min(f+g)對應的節(jié)點minPoint,對它展開;找到距離minPoint 最近的障礙物的頂點信息(若是圓形障礙物,找左右兩個切點);If 頂點信息輸出為空(即已經不穿過任何障礙物)更新best_solution,跳出循環(huán);else 繼續(xù)尋找更新best_solution;更新C1 (刪去C1 中已展開的點,加入最近障礙物頂點);end修復best_solution 中的路線(具體代碼見算法2);畫出路線;結束.

算法2.修復部分算法輸入:best_solution輸出:new_best_solution//num 表示最優(yōu)解best_solution 中所經過節(jié)點的個數(shù)for i=1,2,…,num–2 for j=i+2,i+3,…,num–1判斷best_solution 中第i 個點到第j 個點的連線是否闖過障礙物;if 沒有穿過障礙物將點i和點j 間所有的多余節(jié)點刪去;end end end更新best_solution 中的路線即最終距離;new_best_solution=best_solution;結束.

舉例說明:

假設現(xiàn)有多邊形障礙物3 個,圓形障礙物3 個.如圖2所示,起始點為S,終止點為T,創(chuàng)建C1、C2,它們保存的節(jié)點信息包括:f值、g值、節(jié)點坐標、上層節(jié)點坐標.從起始點S出發(fā),將S保存到C1 中,遍歷C1,找到Dmin對應的節(jié)點S,對S展開,找到距離S最近的障礙物節(jié)點P4、P5、P6,將P4、P5、P6中可用的節(jié)點添加到元胞數(shù)組C1 中,并將C1 中已展開的點S移動到C2 中;再次遍歷C1,找到Dmin所對應的節(jié)點P6,再繼續(xù)尋找距離P6點最近的障礙物,并將該障礙物的可用節(jié)點信息存入C1 中,這里注意,當找到的距離P6點最近的障礙物是圓形時,求P6與終點T之間的左右兩條切線,將可用切點當作備選點存入C1 中;再次遍歷重復上述步驟,直到當前展開點與終點T的連線不再穿過任何障礙物時,就可得出S到T的最優(yōu)路徑.

尋優(yōu)的結果是要經過多次循環(huán)產生的,圖3完整的復現(xiàn)了這一過程,圖中數(shù)值表示的是由公式1 得出的D值.算法中的每次循環(huán)都要遍歷C1,找出最小值Dmin對應的節(jié)點,繼續(xù)往下分支.圖3中P12下的分支3B表示節(jié)點P12和終點T關于圓形障礙物3 所做切線得出的切點,P11下的分支表示節(jié)點P11和終點T關于圓形障礙物3 所做切線得出的切點1和切點2.這里注意,可用切點不一定都能找到兩個,有的切點嚴重偏離當前圓形障礙物時,就屬于不可用切點,不予以考慮.圖3中可以看出障礙物點P4、P5、P6多次出現(xiàn),但它們對應的上層節(jié)點有所不同,所以它們的f值、對應的D值也都不相同,代表著各自的意義.圖中有向下箭頭分支的節(jié)點表示在某次循環(huán)中已經被展開過的點,這類點要及時移動到C2 中.

圖2 路徑規(guī)劃圖

3 仿真分析

我們對改進的A*算法進行仿真實驗,對實驗結果進行了分析.實驗中用到的PC 系統(tǒng)為Windows10,處理器型號為Intel(R)Core(TM)i7-8700,主頻率為3.19 GHZ,開發(fā)環(huán)境為Matlab 2016b,仿真實驗過程通過Matlab 編程語言實現(xiàn),使用Matlab 對規(guī)劃環(huán)境及規(guī)劃出的航跡進行作圖.實驗中的環(huán)境模型大小為800 km×900 km,同時包含了多種類型的禁飛區(qū),共14 個禁飛區(qū),其中6 個圓形禁飛區(qū),8 個多邊形禁飛區(qū).

通過仿真實驗,模擬了4 條路徑,如圖4所示,每條路線的途徑點信息如表1所示.可以清楚的看到我們設計的最優(yōu)路線允許貼著禁飛區(qū)的邊飛行,我們的算法也能解決多種形狀并存的壁障問題.

圖3 路徑分支圖

圖4 路徑規(guī)劃圖

表1 最優(yōu)路徑信息

4 結束語

本文改進了A*算法,解決了圓形、多邊形禁飛區(qū)共存情況下的避障路徑規(guī)劃問題,對比只有單一類型禁飛區(qū)的問題有一定的實用性,得到的路線更接近最優(yōu).但也有一些不足之處,比如沒有應對突發(fā)威脅時的實時航跡規(guī)劃手段.本文僅討論無人機避障路徑規(guī)劃的問題,后續(xù)會對綜合的多倉庫、多約束條件下的物流無人機路徑規(guī)劃問題進行研究,在算法自主實時性方面尋求突破,完善無人機航跡規(guī)劃策略.