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

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

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

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

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

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

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

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

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

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

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

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

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

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

將飛行中存在危險的區(qū)域作為障礙物區(qū)域處理，禁止空中機器人在此區(qū)域飛行。基于此種思想對已知環(huán)境模型進行擴展。

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

2)空中機器人具有一定的體積，設機器人軸心到機器人實體最遠點的長度為d2。

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

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

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

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

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

1.3 柵格的標識

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

1)直角坐標法。以柵格左下角作為坐標原點，水平向右方向為x軸正方向、水平向上方向為y軸正方，豎直方向為z軸正方向，每個柵格都可用直角坐標(x，y，z)唯一標識。

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

以上2種標識方法可用式(3)，(4)進行轉(zhuǎn)換：

其中：n為用序號標識法表示的柵格位置坐標；Nx為xoy平面內(nèi)每行的柵格數(shù)；Nxy為xoy平面內(nèi)所有柵格的總數(shù)；mod為n/Nxy的余數(shù)；ceil為n/Nxy的整數(shù)，并且取整的方向為正無窮大[12]。

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

2.1 最優(yōu)路徑

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

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

2.2 蟻群算法的應用

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

在柵格化三維環(huán)境模型中，利用蟻群算法對空中機器人的軌跡進行全局規(guī)劃設計，能較好地找到一條理想的規(guī)劃路徑，體現(xiàn)出蟻群算法在解決路徑優(yōu)化問題中的使用價值。但是蟻群算法在求解過程中需要多次迭代，花費較長時間。當?shù)欢ù螖?shù)后，算法可能在某個或某些局部最優(yōu)區(qū)域內(nèi)發(fā)生停滯，或隨著問題規(guī)模變大得到一個部分最優(yōu)解而非全局最優(yōu)解，因此，蟻群算法還需進一步改進，以找到快捷可行的路徑優(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]喬茹，章小兵，趙光興.基于改進蟻群算法的移動機器人全局路徑規(guī)劃[J].安徽工業(yè)大學學報(自然科學版)，2009,26(1):77-80.

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

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

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

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

[12]黃力，張增芳，朱亞超.基于二叉決策的機器人路徑規(guī)劃研究[J].機械設計與制造，2008(3):156-158.

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

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

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

責任編輯：何莉

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

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

李妍(1987-)，女，山東濱州人，碩士生，研究方向為機器人技術及應用。

徐向榮(1962-)，男，安徽蕪湖人，教授，主要研究方向為機器人運動軌跡規(guī)劃、生物力學。

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