李 妍，徐向榮，徐 浩，李 雙

(安徽工業(yè)大學(xué)機(jī)械工程學(xué)院，安徽馬鞍山243032)

基于蟻群算法的空中機(jī)器人三維軌跡規(guī)劃

李 妍，徐向榮，徐 浩，李 雙

(安徽工業(yè)大學(xué)機(jī)械工程學(xué)院，安徽馬鞍山243032)

軌跡規(guī)劃是機(jī)器人任務(wù)規(guī)劃系統(tǒng)的一個(gè)重要部分。在MATLAB平臺(tái)下，建立空中機(jī)器人飛行三維環(huán)境模型，基于此模型利用蟻群算法對(duì)空中機(jī)器人飛行軌跡進(jìn)行規(guī)劃研究。首先根據(jù)限制條件對(duì)建立的環(huán)境模型進(jìn)行預(yù)處理和柵格化處理，然后將蟻群算法應(yīng)用于三維已知環(huán)境模型中，得到1條由起始點(diǎn)到目標(biāo)點(diǎn)的最短路徑,以此作為軌跡規(guī)劃的最優(yōu)路徑?？罩袡C(jī)器人按照求得的最優(yōu)路徑進(jìn)行運(yùn)動(dòng)，完成飛行任務(wù)。在路徑尋優(yōu)過(guò)程中蟻群算法表現(xiàn)出優(yōu)良的有效性和使用性。

空中機(jī)器人；蟻群算法；軌跡規(guī)劃；柵格化

空中機(jī)器人軌跡規(guī)劃是對(duì)空中機(jī)器人在有障礙物的環(huán)境下自主避開障礙物從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)的規(guī)劃研究。軌跡規(guī)劃可分為離線軌跡規(guī)劃和在線軌跡規(guī)劃，這2種規(guī)劃方法在軍事和民用方面均具有重要應(yīng)用價(jià)值。在初始階段，空中機(jī)器人運(yùn)用人工規(guī)劃的方法尋找路徑，但人工規(guī)劃的航跡過(guò)于粗糙，達(dá)不到軌跡規(guī)劃任務(wù)的要求。對(duì)此，國(guó)內(nèi)外學(xué)者提出了一系列軌跡規(guī)劃算法，包括模擬退火算法、遺傳算法、蟻群算法、粒子群優(yōu)化算法等[1-4]，且均在軌跡規(guī)劃任務(wù)中取得了顯著成果。蟻群算法是意大利學(xué)者Dorigo M通過(guò)觀察螞蟻的覓食習(xí)性提出的一種仿生優(yōu)化算法[3]。此后諸多學(xué)者對(duì)其進(jìn)行了改進(jìn)以提高蟻群算法的性能[5-9]。相對(duì)于其他算法，蟻群算法具有正反饋、分布式計(jì)算等優(yōu)點(diǎn)，可保證算法運(yùn)行的快速性，避免全局優(yōu)化中早熟性收斂。文中利用蟻群算法對(duì)三維環(huán)境模型進(jìn)行離線軌跡規(guī)劃，將蟻群算法路徑規(guī)劃原理與三維環(huán)境模型相結(jié)合，說(shuō)明蟻群算法不僅能求解一維、簡(jiǎn)單問(wèn)題，而且在求解復(fù)雜、多維問(wèn)題中也有很好應(yīng)用。

1 三維環(huán)境模型的建立

空中機(jī)器人在已知的三維環(huán)境模型中飛行，完成環(huán)境監(jiān)測(cè)、低空偵察、定點(diǎn)干擾等任務(wù)。環(huán)境模型的建立方式有多種，文中選用山峰形式的環(huán)境模型對(duì)空中機(jī)器人進(jìn)行軌跡規(guī)劃[10]，模型如下式

其中：x，y為在水平面上的投影坐標(biāo)；z為對(duì)應(yīng)于x，y坐標(biāo)處山峰的高度；i為第i個(gè)山峰；x(i)，y(i)為山峰i的輪廓形狀；h(i)為控制山峰i高度的參數(shù)；xi0，yi0為山峰i最高點(diǎn)所在的位置坐標(biāo)。

利用式(1)建立的仿真環(huán)境模型如圖1，平滑曲面為山地表面，曲面以下部分為障礙物，各坐標(biāo)值為量綱為一的量?？罩袡C(jī)器人具有一定的飛行速度，不能緊貼環(huán)境模型中的障礙物飛行，要與障礙物有一定的安全距離。為保證空中機(jī)器人飛行的安全性，需對(duì)三維環(huán)境模型進(jìn)行預(yù)處理，擴(kuò)大障礙物的范圍。

1.1 環(huán)境模型預(yù)處理

將飛行中存在危險(xiǎn)的區(qū)域作為障礙物區(qū)域處理，禁止空中機(jī)器人在此區(qū)域飛行?；诖朔N思想對(duì)已知環(huán)境模型進(jìn)行擴(kuò)展。

1)設(shè)機(jī)器人安全通過(guò)障礙物的距離為d1，d1越大，機(jī)器人安全通過(guò)障礙物的可能性就越大；d1越小，則機(jī)器人安全通過(guò)障礙物的可能性就越小。d1根據(jù)空中機(jī)器人的飛行速度合理取值。

2)空中機(jī)器人具有一定的體積，設(shè)機(jī)器人軸心到機(jī)器人實(shí)體最遠(yuǎn)點(diǎn)的長(zhǎng)度為d2。

3)根據(jù)空中機(jī)器人的大小，安全距離擴(kuò)張障礙物的范圍d為

利用式(2)對(duì)已知環(huán)境模型進(jìn)行擴(kuò)展處理，得到的環(huán)境模型如圖2。由圖2可以看出，環(huán)境模型經(jīng)過(guò)擴(kuò)展后山峰的高度和體積均有所增加。在此環(huán)境模型中空中機(jī)器人可在非障礙物區(qū)域安全飛行。

1.2 柵格化環(huán)境模型

柵格化環(huán)境建模是將空中機(jī)器人進(jìn)行軌跡規(guī)劃的空間分解成一系列尺寸相同的網(wǎng)格(柵格)單元，每個(gè)網(wǎng)格為1個(gè)像元，像元為柵格化環(huán)境模型的基本單位[11]，如圖3。利用柵格將環(huán)境模型進(jìn)行均勻劃分時(shí)，要保證機(jī)器人能夠在每個(gè)柵格中自由活動(dòng)。圖3中有的柵格完全由障礙物填充，有的柵格中無(wú)障礙物，有的柵格中部分是障礙物。根據(jù)柵格與障礙物區(qū)域的交集，可將柵格分為完全可行柵格、完全不可行柵格、不完全可行柵格。

為保證機(jī)器人的飛行安全，把不完全可行柵格歸為完全不可行柵格(障礙物柵格)，如圖4。每個(gè)柵格都用數(shù)值0或1表示柵格的存在狀態(tài)。完全可行柵格取值為0，完全不可行柵格取值為1。柵格的像元值發(fā)生變化，說(shuō)明環(huán)境模型發(fā)生改變。在MATLAB仿真環(huán)境中，經(jīng)過(guò)柵格化處理的已知環(huán)境模型可用只有0或1的三維矩陣表示。

1.3 柵格的標(biāo)識(shí)

經(jīng)柵格化的環(huán)境模型由多個(gè)柵格組成，每個(gè)柵格都有1個(gè)固定的位置坐標(biāo)，對(duì)柵格位置坐標(biāo)的標(biāo)識(shí)有2種方法可以采用。

1)直角坐標(biāo)法。以柵格左下角作為坐標(biāo)原點(diǎn)，水平向右方向?yàn)閤軸正方向、水平向上方向?yàn)閥軸正方，豎直方向?yàn)閦軸正方向，每個(gè)柵格都可用直角坐標(biāo)(x，y，z)唯一標(biāo)識(shí)。

2)序號(hào)法。對(duì)豎直方向進(jìn)行由下到上的標(biāo)注層次劃分，按照水平向右，水平向上的順序，從柵格陣左下角第一個(gè)柵格開始，給每個(gè)柵格1個(gè)序號(hào)(1～N)，序號(hào)與柵格一一對(duì)應(yīng)。先對(duì)z方向0～1內(nèi)的柵格進(jìn)行標(biāo)識(shí)，再對(duì)z方向1～2內(nèi)的柵格進(jìn)行標(biāo)識(shí)，依次類推，完成對(duì)所有柵格的標(biāo)識(shí)。

以上2種標(biāo)識(shí)方法可用式(3)，(4)進(jìn)行轉(zhuǎn)換：

其中：n為用序號(hào)標(biāo)識(shí)法表示的柵格位置坐標(biāo)；Nx為xoy平面內(nèi)每行的柵格數(shù)；Nxy為xoy平面內(nèi)所有柵格的總數(shù)；mod為n/Nxy的余數(shù)；ceil為n/Nxy的整數(shù)，并且取整的方向?yàn)檎裏o(wú)窮大[12]。

2 基于蟻群算法的三維軌跡規(guī)劃

2.1 最優(yōu)路徑

路徑規(guī)劃得到路徑是否最優(yōu)，根據(jù)空中機(jī)器人飛行路徑的長(zhǎng)短進(jìn)行判斷，空中機(jī)器人由起始點(diǎn)到目標(biāo)點(diǎn)飛行的距離越短，則走過(guò)的路徑越優(yōu)化。設(shè)從起始點(diǎn)開始，空中機(jī)器人到達(dá)目標(biāo)點(diǎn)共經(jīng)過(guò)w個(gè)位置點(diǎn)，其中第p個(gè)位置點(diǎn)坐標(biāo)Wp=(xp,yp,zp)，這個(gè)過(guò)程中空中機(jī)器人飛行的距離用L表示。

空中機(jī)器人由起始點(diǎn)到目標(biāo)點(diǎn)的路徑不是唯一的，假設(shè)可行路徑的條數(shù)為l。利用式(5)計(jì)算可得各路徑的長(zhǎng)度集合Lall，l條路徑中最短的1條即為最優(yōu)路徑minL。

2.2 蟻群算法的應(yīng)用

圖4所示，環(huán)境模型中有8 000個(gè)柵格，用序號(hào)法對(duì)每個(gè)柵格進(jìn)行標(biāo)識(shí)，每個(gè)柵格對(duì)應(yīng)1個(gè)(1～8 000)坐標(biāo)值。設(shè)螞蟻數(shù)為M，并且每只螞蟻必須具備如下條件。

1)螞蟻的爬行速度恒定，不考慮轉(zhuǎn)彎、上下、左右移動(dòng)時(shí)的速度變化。螞蟻在柵格化環(huán)境模型中運(yùn)動(dòng)，每走1步就是從一個(gè)完全可行柵格的中心移動(dòng)到另一個(gè)完全可行柵格的中心，其移動(dòng)時(shí)可選擇的下一柵格為與當(dāng)前位置相鄰接的完全可行柵格。對(duì)于三維環(huán)境模型，1個(gè)柵格的鄰接?xùn)鸥裼?6個(gè)，將1個(gè)柵格與其他各柵格間是否可以相互運(yùn)動(dòng)的狀態(tài)用一個(gè)(0，1)鄰接矩陣表示，則可形成一個(gè)8 000×8 000的二維矩陣Gd。

環(huán)境模型為無(wú)向圖，第i列和第i行都表示柵格i與其他各柵格間的可運(yùn)動(dòng)狀態(tài)。若值為1，則與對(duì)應(yīng)的柵格相連接，并且與對(duì)應(yīng)柵格之間可相互運(yùn)動(dòng)；若值為0，則與對(duì)應(yīng)的柵格不連接或與對(duì)應(yīng)的柵格之間相連接，但對(duì)應(yīng)的柵格為完全不可行柵格。

2)每只螞蟻在爬行過(guò)程中，都會(huì)在爬過(guò)的路徑上釋放信息素，假設(shè)初始狀態(tài)下信息素均勻分布，表示為τij(0)=C(C為常數(shù))。

3)在完成1次循環(huán)前，每只螞蟻不會(huì)重復(fù)選擇已經(jīng)走過(guò)的柵格為下一柵格[13]。將走過(guò)的柵格坐標(biāo)儲(chǔ)存到T中，其中Tm(m=1，2，…，M)用來(lái)記錄螞蟻m目前已經(jīng)走過(guò)的位置。

4)每只螞蟻選擇下一個(gè)柵格的概率，與其相連路徑上信息素的濃度及下一柵格與目標(biāo)點(diǎn)柵格的距離有關(guān)。信息素濃度越高，距離目標(biāo)點(diǎn)柵格越近的柵格被選中的概率就越大，其他柵格被選擇的概率較小。

設(shè)螞蟻m(m=l，2，…，M)在t時(shí)刻由柵格i轉(zhuǎn)移到柵格j的概率為

式中：Dm表示螞蟻m位于柵格i時(shí)下一步允許選擇的柵格；τij表示柵格i到柵格j的信息素強(qiáng)度；α表示控制信息素的相對(duì)重要程度，是常數(shù)；β表示控制路徑長(zhǎng)度的相對(duì)重要程度，是常數(shù)；ηij表示柵格i到柵格j的能見度，反映由柵格i轉(zhuǎn)移到柵格j的啟發(fā)程度[14]。

其中djE表示柵格j到目標(biāo)點(diǎn)柵格E的距離。Pmij(t)越大，柵格j被選的概率就越大。

5)路徑上存留信息素的量隨著時(shí)間的推移在不斷揮發(fā)、減少。用參數(shù)ρ(0≤ρ＜1)表示信息素的持久性，則1-ρ表示信息素的消失程度。u時(shí)刻后，所有螞蟻完成1次循環(huán)，各路徑上信息素的量就會(huì)發(fā)生變化，可用式(9)，(10)調(diào)整：

其中：Δτij表示本次循環(huán)中所有螞蟻留在路徑由位置點(diǎn)i到位置點(diǎn)j上的信息量；表示第m只螞蟻在本次循環(huán)中留在路徑由位置點(diǎn)i到位置點(diǎn)j上的信息量。

式中：Q表示第m只螞蟻完成1次循環(huán)后釋放在經(jīng)過(guò)路徑上的信息素總量；Lm表示第m只螞蟻在本次循環(huán)中經(jīng)過(guò)路徑的總長(zhǎng)度[15]。

6)M只螞蟻經(jīng)過(guò)K迭代循環(huán)，形成M×K條由起始點(diǎn)到目標(biāo)點(diǎn)的路徑。求解M×K條路徑中的最短路徑即為最優(yōu)路徑。

其中，Lkm表示第k次循環(huán)中第m只螞蟻由起始點(diǎn)到目標(biāo)點(diǎn)走過(guò)的路徑長(zhǎng)度。利用式(12)，(13)求解出最短路徑，在MATLAB中利用plot3命令將最短路徑與已知環(huán)境模型相結(jié)合繪制直觀圖。圖5為蟻群算法求解過(guò)程流程圖。

3 規(guī)劃結(jié)果

在MATLAB中利用蟻群算法進(jìn)行離線軌跡規(guī)劃，環(huán)境模型為20×20×20的柵格模型，各柵格的存在狀態(tài)可用20×20×20的三維矩陣表示，矩陣中0表示自由柵格，1表示障礙物柵格，下一步可移動(dòng)位置點(diǎn)用矩陣Gd表示。將蟻群算法路徑尋優(yōu)原理與三維環(huán)境模型相結(jié)合求出最優(yōu)路徑minL。利用蟻群算法進(jìn)行軌跡規(guī)劃初始時(shí)可產(chǎn)生多條不同路徑，但迭代到一定次數(shù)后螞蟻的覓食路徑會(huì)趨于收斂。圖6為三維環(huán)境模型中，用蟻群算法求解路徑時(shí)K次迭代后得出的最優(yōu)路徑。S表示起始點(diǎn)；E表示終止點(diǎn)。圖7為其俯視圖，圖8為用蟻群算法求解路徑時(shí)每次迭代最短路徑的集合。圖9為用蟻群算法求解路徑時(shí)迭代次數(shù)與最短路徑、平均路徑的關(guān)系曲線，路徑長(zhǎng)度為量綱為一的量。由圖9可知，迭代次數(shù)較少時(shí)，每次迭代后得到的最短路徑各不相同，迭代到一定次數(shù)后最優(yōu)路徑趨于一個(gè)確定值，同時(shí)平均路徑長(zhǎng)度也在不斷減小。由此可知，隨著迭代次數(shù)的增加，蟻群算法求解的最短路徑值趨于穩(wěn)定，表明遺群算法能夠很好地解決三維環(huán)境模型的軌跡規(guī)劃問(wèn)題。

4 結(jié) 論

在柵格化三維環(huán)境模型中，利用蟻群算法對(duì)空中機(jī)器人的軌跡進(jìn)行全局規(guī)劃設(shè)計(jì)，能較好地找到一條理想的規(guī)劃路徑，體現(xiàn)出蟻群算法在解決路徑優(yōu)化問(wèn)題中的使用價(jià)值。但是蟻群算法在求解過(guò)程中需要多次迭代，花費(fèi)較長(zhǎng)時(shí)間。當(dāng)?shù)欢ù螖?shù)后，算法可能在某個(gè)或某些局部最優(yōu)區(qū)域內(nèi)發(fā)生停滯，或隨著問(wèn)題規(guī)模變大得到一個(gè)部分最優(yōu)解而非全局最優(yōu)解，因此，蟻群算法還需進(jìn)一步改進(jìn)，以找到快捷可行的路徑優(yōu)化方法。

[1]Kirkpatrick S,Jr Gelatt C D,Vecchi M P.Optimization by simulated annealing[J].Science,1983,220(11):650-671.

[2]Holland J H.Genetic algorithm and the optima allocations of trials[J].SIAM Journal of Computing,1973,2:88-105.

[3]Colorni A，Dorigo M，Maniezzo V.Distributed optimization by ant colonies[C]//Proc of the First European Conference on Artificial Life.Paris,France,1991:134-142.

[4]Kennedy J.The particle swarm:Social adaptation of knowledge[C]//Proceedings of the IEEE International Conference on Evolutionary Computation.Indianapolis:IEEE,1997:303-308.

[5]Gutjahr W J.Agraph-based ant system and its convergence[J].Future Generation Computer Systems,2000,16(8):873-888.

[6]Stützle T,Hoos H H.MAX-MIN ant system[J].Future Generation Computer Systems,2000,16(8):889-914.

[7]喬茹，章小兵，趙光興.基于改進(jìn)蟻群算法的移動(dòng)機(jī)器人全局路徑規(guī)劃[J].安徽工業(yè)大學(xué)學(xué)報(bào)(自然科學(xué)版)，2009,26(1):77-80.

[8]陳崚，沈杰，秦玲，等.基于分布均勻度的自適應(yīng)蟻群算法[J].軟件學(xué)報(bào)，2003,14(8):1379-1387.

[9]高尚，江新姿，湯可宗.蟻群算法與遺傳算法的混合算法[C]//第26屆中國(guó)控制會(huì)議論文集.湖南：張家界，2007:701-704.

[10]胡志忠，沈春林.基于數(shù)字地圖預(yù)處理的實(shí)時(shí)航跡規(guī)劃[J].南京航空航天大學(xué)學(xué)報(bào)，2002,34(4):382-385.

[11]朱慶保，張玉蘭.基于柵格法的機(jī)器人路徑規(guī)劃蟻群算法[J].機(jī)器人，2005,27(2):132-136.

[12]黃力，張?jiān)龇?，朱亞?基于二叉決策的機(jī)器人路徑規(guī)劃研究[J].機(jī)械設(shè)計(jì)與制造，2008(3):156-158.

[13]陳英俏.改進(jìn)蟻群算法及其在移動(dòng)機(jī)器人路徑規(guī)劃中的研究[D].沈陽(yáng)：東北大學(xué)，2010.

[14]盛忠起，李洪峰，段曉艷，等.基于蟻群算法的一類生產(chǎn)鏈調(diào)度問(wèn)題求解[J].機(jī)械設(shè)計(jì)與制造，2008(7):176-178.

[15]趙開新，魏勇，王東署.改進(jìn)蟻群算法在移動(dòng)機(jī)器人路徑規(guī)劃中的研究[J].計(jì)算機(jī)測(cè)量與控制，2014,22(11):3725-3727.

責(zé)任編輯：何莉

Three-dimensional Trajectory Planning ofAerial Robot Based onAnt ColonyAlgorithm

LI Yan，XU Xiangrong，XU Hao，LI Shuang
(1.School of Mechanical Engineering,Anhui University of Technology,Ma'anshan 243032，China)

Trajectory planning is an important part of the robot mission planning system.With MATLAB platform,establish 3D environment model of the aerial robot.Based on the model,use ant colony algorithm for aerial robot trajectory planning.Firstly,preprocess and rasterize process the established environment model according to the constraints.Then,apply the ant colony algorithm to 3D environment model to get a shortest path from the starting point to the target point as the optimal path.Aerial robot accords the obtained optimal path to complete the mission.Ant colony algorithm shows the validity and practicability during the path optimization.

aerial robots;ant colony algorithm;trajectory planning;rasterisation

TP311

A

10.3969/j.issn.1671-7872.2015.04.012

2015-03-17

科技部中歐(塞爾維亞)政府間科技合作項(xiàng)目((2013)2-5);安徽省教育廳高等學(xué)校自然科學(xué)研究項(xiàng)目(KJ2013Z022)

李妍(1987-)，女，山東濱州人，碩士生，研究方向?yàn)闄C(jī)器人技術(shù)及應(yīng)用。

徐向榮(1962-)，男，安徽蕪湖人，教授，主要研究方向?yàn)闄C(jī)器人運(yùn)動(dòng)軌跡規(guī)劃、生物力學(xué)。

1671-7872(2015)-04-0360-06