固定翼巡線無(wú)人機(jī)實(shí)時(shí)蟻群算法路徑規(guī)劃

鄭武略+尚濤+金釗

摘 要：本文針對(duì)傳統(tǒng)蟻群算法局部早熟等問(wèn)題對(duì)算法進(jìn)行了改進(jìn)，并對(duì)無(wú)人機(jī)路徑規(guī)劃及重規(guī)劃多條件約束問(wèn)題進(jìn)行了研究，提高蟻群算法航跡規(guī)劃計(jì)算速度及全局性來(lái)滿足實(shí)時(shí)蟻群算法航跡規(guī)劃要求。文章針對(duì)傳統(tǒng)蟻群算法的早熟問(wèn)題采用全局與局部信息素互補(bǔ)衰減法，來(lái)高效完成蟻群算法尋優(yōu)問(wèn)題，并當(dāng)無(wú)人機(jī)偏離航線時(shí)能及時(shí)根據(jù)無(wú)人機(jī)所在位置重新規(guī)劃。通過(guò)實(shí)驗(yàn)結(jié)果得出蟻群算法具有快速的計(jì)算能力，能在短時(shí)間內(nèi)對(duì)更改的路徑進(jìn)行響應(yīng)，完成無(wú)人機(jī)自校正航跡規(guī)劃。

關(guān)鍵詞：無(wú)人機(jī)；實(shí)時(shí)路徑規(guī)劃；蟻群算法

中圖分類(lèi)號(hào)：TP242 文獻(xiàn)標(biāo)識(shí)碼：A

Abstract：Because of premature in traditional ACO（Ant Colony Optimal）， this article comes up with a improved algorithm and use this algorithm to realize the multifactor control in UAV online path planning to improve the speed and global search. By combing local evaporation with global pheromone update， the ACO has a higher speed in finding optimal planning path. Whats more， its suit for the self adjust UAV path planning. Through the improving ACO， a more precise and efficiency path can be found.

Keywords： unmanned aerial vehicle（UAV）； path planning online； ant colony optimal（ACO）

1 引言

近年來(lái)，作為新興航空機(jī)器人的無(wú)人機(jī)，由于體積小、質(zhì)量輕及高機(jī)動(dòng)性等因素讓其在各行各業(yè)都得到了廣泛的應(yīng)用于認(rèn)可[1]。其在電力巡線、區(qū)域勘測(cè)、無(wú)損檢測(cè)等作業(yè)中優(yōu)秀的性能及靈活的操縱性使其收到電力行業(yè)廣泛認(rèn)可，并且由于無(wú)人機(jī)可搭載多樣傳感器，使無(wú)人機(jī)的功能拓展與應(yīng)用得到廣泛的拓展與關(guān)注。而在固定翼無(wú)人機(jī)自主巡線作業(yè)中，由于電力巡檢部分輸電走廊處于地勢(shì)危險(xiǎn)地區(qū)，這給無(wú)人機(jī)自主巡線造成很大的困難，同時(shí)對(duì)無(wú)人機(jī)航跡規(guī)劃精確性有了更高的要求，使無(wú)人機(jī)自校正重新路徑重規(guī)劃算法成為無(wú)人機(jī)航跡規(guī)劃的研究者爭(zhēng)相探討研究的技術(shù)難點(diǎn)。

目前航跡規(guī)劃方向已經(jīng)涌現(xiàn)出大量的研究性成果，其中‘The Visibility Graph 法針對(duì)衛(wèi)星地圖序列，通過(guò)鳥(niǎo)瞰視角針對(duì)無(wú)人機(jī)作業(yè)區(qū)域利用直線段標(biāo)定規(guī)劃路徑[2]。四叉樹(shù)路徑規(guī)劃法通過(guò)將無(wú)人機(jī)巡航環(huán)境分解成四叉樹(shù)結(jié)構(gòu)，通過(guò)四叉樹(shù)結(jié)構(gòu)分叉來(lái)遍歷整個(gè)巡航環(huán)境來(lái)生成無(wú)人機(jī)航行最優(yōu)航跡[3]。Jitin Kumar Goyal等人針對(duì)A*算法計(jì)算搜索細(xì)胞尺寸進(jìn)行改進(jìn)，并利用啟發(fā)因子來(lái)提高搜索最優(yōu)航跡的效率，對(duì)無(wú)人機(jī)與搜索環(huán)境之間的比例進(jìn)行考慮來(lái)提高航跡規(guī)劃算法的安全性[4]。

蟻群算法最早由Dorigo 等[5]作為一種多條件約束優(yōu)化的計(jì)算方法展開(kāi)研究，因?yàn)槠淇梢匀值牟⑿杏?jì)算并且具有良好的動(dòng)態(tài)性及魯棒性，因此在復(fù)雜的組合優(yōu)化問(wèn)題十分適用。由于在收斂后期基本蟻群算法早熟局的缺點(diǎn)，有大量研究人員將蟻群算法與其他算法相結(jié)合來(lái)提高蟻群算法搜索能力，但同時(shí)大大增加了算法的復(fù)雜性與計(jì)算速度[6-8]。本文以信息素更新等方面作為改進(jìn)點(diǎn)，來(lái)提高蟻群算法全局搜索能力，與計(jì)算速度，進(jìn)而實(shí)現(xiàn)無(wú)人機(jī)自校正航跡規(guī)劃。

2 改進(jìn)蟻群算法

2.1 蟻群優(yōu)化算法

蟻群算法基于是螞蟻從出發(fā)點(diǎn)沿地形尋找食物，找到食物后原路折返，并在走過(guò)的路徑上留下信息素。因此，所走路徑越短的螞蟻，其所走路徑上包含有較高的信息素含量，而后尋找食物的螞蟻會(huì)有較大的可能選擇這條路線，由此組成一個(gè)逐漸逼近最優(yōu)解的正反饋尋優(yōu)過(guò)程。

1） 蟻群算法轉(zhuǎn)移策略

蟻群算法迭代初始，置 （h為常數(shù)）并設(shè)定蟻群規(guī)模為m （1，2，…，M）。其中，第t次迭代i點(diǎn)到j(luò)點(diǎn)間所含的信息濃度用 來(lái)表示。在蟻群尋優(yōu)過(guò)程中，螞蟻通過(guò)可行路徑上的信息素作為下一方向搜索依據(jù)，并將轉(zhuǎn)移概率與輪盤(pán)賭注法相結(jié)合來(lái)對(duì)下一節(jié)點(diǎn)進(jìn)行搜索。 為第k只螞蟻在第t次的迭代時(shí)從點(diǎn)i轉(zhuǎn)移到j(luò)的概率，其計(jì)算公式如（1）所示：

其中， 為蟻群算法中螞蟻的可到達(dá)點(diǎn)， 為禁止清單，記錄螞蟻k代所走過(guò)的節(jié)點(diǎn)及螞蟻禁止通行的節(jié)點(diǎn)，并針對(duì)當(dāng)前路徑及時(shí)動(dòng)態(tài)調(diào)整。 表示尚未搜索節(jié)點(diǎn)，此外式（1）中的 代表信息素濃度因子， 代表啟發(fā)因子， 表示引導(dǎo)因子端的重要性權(quán)重，且 與 的取值大小酸谷算法收斂速度。

2）啟發(fā)因子設(shè)計(jì)

通過(guò)對(duì)啟發(fā)因子進(jìn)行設(shè)計(jì)，可以影響蟻群算法轉(zhuǎn)移概率，進(jìn)而引導(dǎo)螞蟻搜索方向，對(duì)螞蟻尋優(yōu)進(jìn)行引導(dǎo)。其計(jì)算公式如式（2）所示。

其中， 為點(diǎn)i與點(diǎn)j之間的距離。且當(dāng)下一搜索節(jié)點(diǎn)距當(dāng)前位置越近，導(dǎo)引因子則越大。此外，無(wú)人機(jī)巡線飛行過(guò)程由于姿態(tài)控制會(huì)對(duì)無(wú)人機(jī)自身機(jī)能造成一定的損耗，因此無(wú)人機(jī)在巡線過(guò)程中具有能量消耗上限即 ，因此每一航程段距離 應(yīng)滿足：

2.2 基于蟻群算法的航跡規(guī)劃

本文基于固定翼無(wú)人機(jī)作業(yè)路徑情況考慮，一般而言無(wú)人機(jī)巡線作業(yè)飛行軌跡都是從起點(diǎn)出發(fā)遍歷各個(gè)作業(yè)目標(biāo)點(diǎn)完成任務(wù)后折返的過(guò)程。本文通過(guò)考慮在無(wú)時(shí)間條件約束時(shí)給定無(wú)人機(jī)飛行起點(diǎn)和所需遍歷的各個(gè)目標(biāo)點(diǎn)，并設(shè)定無(wú)人機(jī)巡線環(huán)境限制來(lái)將路徑規(guī)劃問(wèn)題視為 TSP（Traveling Salesman Problem） 問(wèn)題求解[12，13]。其偽代碼如下所示：

Step1 設(shè)定蟻群規(guī)模，并將蟻群隨機(jī)分配到各個(gè)目標(biāo)點(diǎn)中，并將目標(biāo)點(diǎn)置于禁忌表中。

Step2 對(duì)蟻群算法迭代次數(shù)進(jìn)行設(shè)定，當(dāng)達(dá)到最大迭代次數(shù)時(shí)結(jié)束循環(huán)，否則循環(huán)執(zhí)行步驟3、4，并在迭代結(jié)束后進(jìn)行全局信息素更新。

Step3 對(duì)螞蟻 k（1≤k≤M），進(jìn)行如下指示：

if （查找可達(dá)目標(biāo)點(diǎn)i）

若找到，則記錄i為下一目標(biāo)點(diǎn)；

else

記錄本次循環(huán)路徑。

Step4 更新路徑上的信息素。

Step5 輸出結(jié)果。

Step6 程序結(jié)束。

2.3 無(wú)人機(jī)自校正航跡規(guī)劃

在無(wú)人機(jī)航跡規(guī)劃歷程中，系統(tǒng)會(huì)根據(jù)用戶給定的目標(biāo)點(diǎn)自動(dòng)生成最優(yōu)航跡，可以控制無(wú)人機(jī)根據(jù)蟻群算法所給出的目標(biāo)點(diǎn)來(lái)遍歷其他作業(yè)點(diǎn)。但是當(dāng)需要臨時(shí)更改重新生成路徑時(shí)，需要用戶手動(dòng)更改新目標(biāo)點(diǎn)，然后利用蟻群算法對(duì)無(wú)人機(jī)航跡路線進(jìn)行校正。

無(wú)人機(jī)路徑規(guī)劃系統(tǒng)主要包括兩類(lèi)規(guī)劃策略，一種是基于蟻群算法的靜態(tài)無(wú)人機(jī)路徑規(guī)劃，另一種是根據(jù)無(wú)人機(jī)真實(shí)環(huán)境而重新設(shè)定無(wú)人機(jī)航跡的動(dòng)態(tài)無(wú)人機(jī)航跡規(guī)劃。如果無(wú)人機(jī)由于環(huán)境威脅及作業(yè)目標(biāo)變更時(shí)，固定翼無(wú)人機(jī)會(huì)偏移原航跡。因此，系統(tǒng)需要以當(dāng)前無(wú)人機(jī)坐標(biāo)位置為起點(diǎn)，對(duì)原航跡規(guī)劃路線進(jìn)行校正，然后令無(wú)人機(jī)按新生成的路徑來(lái)對(duì)作業(yè)點(diǎn)進(jìn)行遍歷。 自動(dòng)控制系統(tǒng)是無(wú)人機(jī)自主飛行系統(tǒng)中的關(guān)鍵環(huán)節(jié)，因此無(wú)人機(jī)需要跟隨蟻群算法所生成的航跡來(lái)執(zhí)行飛行命令。當(dāng)無(wú)人機(jī)規(guī)劃航跡變更后，無(wú)人機(jī)飛行控制將會(huì)收到來(lái)自地面站的目標(biāo)點(diǎn)變更指令及導(dǎo)入校正后的規(guī)劃路徑，并通過(guò)PWM信號(hào)對(duì)無(wú)人機(jī)姿態(tài)進(jìn)行控制[14，15]。

3 仿真實(shí)例

3.1 基于蟻群算法的無(wú)人機(jī)路徑規(guī)劃

設(shè)定無(wú)人機(jī)起點(diǎn)為（0，10，4），并設(shè)定目標(biāo)點(diǎn)為（20，8，5）， 設(shè) =1.5， =2， =0.1， =0.01， =0.1，M=30，K=100，其中M為蟻群規(guī)模，K為迭循環(huán)次數(shù)。仿真得到的路徑曲線分別如圖1所示。

由于將起始點(diǎn)、終止點(diǎn)及地圖信息輸入到蟻群算法搜索中，算法時(shí)長(zhǎng)301毫秒，并且從算法生成路徑中可以看出算法在避開(kāi)障礙物的同時(shí)可以沿著地形走勢(shì)飛行，切實(shí)提高無(wú)人機(jī)巡線效率。

3.2 無(wú)人機(jī)蟻群算法的路徑重規(guī)劃

設(shè)定無(wú)人機(jī)巡線作業(yè)目標(biāo)點(diǎn)分別為{10，50，20}，{25，44，25}，{34，38，36}，{48，47，31}，{62，37，41}，{75，48，34}，{64，60，39}，{45，68，37}，{34，59，30}，{22，52，28}，其蟻群算法參數(shù)與上文航跡規(guī)劃所設(shè)參數(shù)保持一直，得到無(wú)人機(jī)遍歷目標(biāo)點(diǎn)的路徑如圖4紅線所示航跡，其算法運(yùn)行時(shí)間為353毫秒，最短路徑長(zhǎng)為170.209。當(dāng)無(wú)人機(jī)航跡遍歷目標(biāo)更改時(shí)，生成路徑如圖2藍(lán)線航跡所示，最短路徑長(zhǎng)度為162.641，航跡校正用時(shí)261ms。

4 結(jié)論

本文主要針對(duì)巡線無(wú)人機(jī)基于蟻群算法無(wú)人機(jī)實(shí)時(shí)航跡規(guī)劃方法進(jìn)行研究。根據(jù)實(shí)驗(yàn)結(jié)果可以看出，改進(jìn)蟻群算法高效的迭代能力和快速的計(jì)算速度可以在目標(biāo)點(diǎn)更改后快速做出相應(yīng)，并計(jì)算出改變后的無(wú)人機(jī)航跡目標(biāo)點(diǎn)后無(wú)人機(jī)新的航跡路線，滿足無(wú)人機(jī)實(shí)時(shí)航跡規(guī)劃準(zhǔn)確性和實(shí)時(shí)性的要求，讓無(wú)人機(jī)實(shí)時(shí)做出反應(yīng)。

參考文獻(xiàn)

[1]. 王連波. 淺談無(wú)人機(jī)的發(fā)展現(xiàn)狀及發(fā)展趨勢(shì)研究[J]. 科技與企業(yè). 2013（14）：349

[2]. Christos Alexopoulos and Paul M. Griffin. Path Planning for a Mobile Robot. IEEE transactions on Systems[J]. Man and Cybernetics. 1992， 22（2）： 318 – 322

[3]. Kambhampati S. and Davis L.Multiresolution path planning for mobile robots[J]. IEEE Journal of Robotics and Automation. 1986， 2（3） ： 135-145

[4]. Jitin Kumar Goya， K.S. Nagla. A New Approach of Path Planning for Mobile Robots[J]. IEEE. 2014：863-867

[5]. Dorigo M， Maniezzo V， Colorni A. Ant system： optimization by a colony of cooperating agent [J]. IEEE Transactions on Systems ，Man ， and Cybernetics ， 1996， 26（1）： 29- 41

[6]. 左洪浩.蟻群優(yōu)化算法及其應(yīng)用研究 [D].合肥： 中國(guó)科學(xué)技術(shù)大學(xué)合肥智能機(jī)械研究所，2006.

ZUO Hong Hao. Research on the Ant Colony Optimization Algorithm and Its Applications[D]. Hefei： Institute of Intelligent Machines，University of Science and Technology of China，2006

[7]. Yang Yu .Research on Path Planning for Mobile Robot Based on Ant Colony Algorithm in Dynamic Environment[J].IEEE，2008：497-499

[8]. TANG L.，F(xiàn)ANG T .J .，”P(pán)ath Planning Method based on Improved Ant Colony Algorithm”， Journal of university of science and technology of china. China，Vol.39（9）， pp. 77-79， 2009.

[9]. Gu Ping， Xiu Chunbo， Cheng Yi， Luo Jing， Li Yanqing. Adaptive Ant Colony Optimization Algorithm[J]. IEEE. 2014：95-98.

[10]. 王緒芝.基于蟻群算法的無(wú)人機(jī)航跡規(guī)劃及其動(dòng)態(tài)仿真[D].南京航空航天大學(xué)自動(dòng)化學(xué)院，2012

WANG Xu-zhi. Path Planning for UAV Based on Ant Colony Algorithm and Dynamic Simulation[D].Nanjing University of Aeroautics and Astronautics，2012

[11]. Ruey-Maw Chen. Heuristics Based Ant Colony Optimization for Vehicle Routing Problem[J] .IEEE Conference on Industrial Electronics and Applications，2011：1039-1043

[12]. Wen zhi-qiang， Cai zi-xing. Global path planning approach based on ant colony optimization algorithm. J. cent. South univ. technol， 2006， 13（6）：707-712

[13]. Wu Bin， Shi Zhong-zhi， An Ant Colony Algorithm Based Partition Algorithm for TSP， Chinese J. Computers， Vol. 24， No. 12， 1328-1333，2001

[14]. Path Planning and Obstacle Avoidance for Vision Guided quadrotor UAV Navigation[J].12th IEEE International Conference on control & Automation（ICCA），2016：984-989

[15]. S. S. Ge and Y. J. Cui， ”Dynamic motion planning for mobile robots using potential field method”， Autonomous Robots， Vol. 13， pp. 207-222， 2002