動態(tài)環(huán)境中的無人機路徑規(guī)劃方法

劉 洋 章衛(wèi)國 李廣文 史靜平

(西北工業(yè)大學(xué) 自動化學(xué)院，西安 710129)

路徑規(guī)劃是無人機任務(wù)規(guī)劃中最基本的部分.由于無人機路徑規(guī)劃時需要考慮威脅、動態(tài)約束等問題，使得規(guī)劃的復(fù)雜性顯著增加.為了解決這些問題，文獻［1］提出了概率地圖法(PRM，Probability Road Maps).概率地圖法可以構(gòu)造自由空間中的連通圖，將連續(xù)空間的規(guī)劃問題轉(zhuǎn)化為拓?fù)淇臻g的規(guī)劃問題，使路徑規(guī)劃問題的復(fù)雜度與環(huán)境的復(fù)雜程度和規(guī)劃空間的維數(shù)無關(guān)，而主要依賴于路徑搜索復(fù)雜度.

在實際規(guī)劃環(huán)境中不但有靜止的障礙物，還有運動的障礙物.因此，路徑規(guī)劃不僅需要考慮與靜止障礙物的碰撞，還需要研究如何防止與運動障礙物碰撞的問題.對于動態(tài)環(huán)境中的路徑規(guī)劃問題已有很多研究.文獻［2］通過對環(huán)境進行模糊描述，將距離信息轉(zhuǎn)換為相應(yīng)位置受障礙物影響的程度，表示為模糊信息，解決了動態(tài)障礙物的表示問題.文獻［3］修改了傳統(tǒng)勢場函數(shù)，通過增大運動障礙物的威脅程度的方法表示動態(tài)障礙物的勢場，并應(yīng)用于規(guī)劃中.文獻［4］中通過引入碰撞危險度概念進行路徑規(guī)劃，成功解決了動態(tài)障礙物的避碰問題.文獻［5］中把動態(tài)障礙物都看作勻速運動的障礙物，采用遺傳算法研究了動態(tài)環(huán)境中的規(guī)劃問題.文獻［6］提出了一種基于分層強化學(xué)習(xí)的路徑規(guī)劃算法，但運行速度較慢.文獻［7］采用PRM方法構(gòu)建路線圖，同時將時間離散化，然后按每個時間點對動態(tài)的障礙物進行碰撞檢測.這些研究都是通過預(yù)測某一時刻運動障礙物的運動范圍或計算運動障礙物在某一位置出現(xiàn)的概率來解決這一問題的，這就存在一個缺陷，即這些方法都只能表示運動障礙物在某一時刻的情況，而不能表示其所有時刻的情況.由于無人機的飛行是連續(xù)的，經(jīng)過不同位置的時刻是不同的，這樣的方法只能保證得到的路徑是可行的，不能保證是最優(yōu)的.要想解決這個問題，就需要表示出運動障礙物在所有時刻的位置.而概率地圖法在空間維數(shù)增加時，不會顯著增加采樣點的數(shù)量.因此本文通過引入時間軸，將構(gòu)型空間擴展為構(gòu)型-時間(CT，Configuration-Time)空間.在CT空間中可以表示運動障礙物在所有時刻的情況，這樣就可以得到更優(yōu)的結(jié)果，更好地解決含有動態(tài)障礙物環(huán)境的路徑規(guī)劃問題.

完成規(guī)劃環(huán)境的表示后就可以搜索可行的路徑.在路徑生成階段，本文采用了一種改進的蟻群算法，通過將方向信息作為啟發(fā)信息引入蟻群算法，使螞蟻在算法運行開始階段的搜索更有目的性，提高了算法的收斂速度.

1 規(guī)劃環(huán)境的構(gòu)建

在實際的規(guī)劃環(huán)境中，不但存在著靜止障礙，還存在著一些運動的障礙，它們在規(guī)劃環(huán)境中的位置隨時間改變而改變，如何在規(guī)劃中考慮運動障礙的碰撞檢測是一個研究的重點.將算法從靜態(tài)環(huán)境擴展到動態(tài)環(huán)境的研究已有很多，但是只有有限的幾種方法可以很好地處理動態(tài)環(huán)境的規(guī)劃問題.概率地圖法通過隨機采樣構(gòu)建概率地圖的方法表示環(huán)境信息，可以處理動態(tài)環(huán)境的規(guī)劃問題.

為了在構(gòu)型空間中表示運動障礙，本文在構(gòu)型空間中引入時間軸，將障礙物所有時刻的位置表示出來，從而把構(gòu)型空間擴展為CT空間，然后在CT空間中進行碰撞檢測并生成地圖，可以解決動態(tài)障礙的問題.對于一個運動趨勢已知的障礙物，在未來任意時刻其位置是已知的，因此可以把這種運動障礙物的所有時刻的位置在CT空間中表示出來.對于運動規(guī)律未知的障礙物，當(dāng)其最大速度已知時，則可以確定其在任意時刻可能的位置范圍，若把所有可能的范圍都標(biāo)注為碰撞的，則可以保證規(guī)劃得到的路徑無碰撞.基于這種思想，本文將所有障礙物都在CT空間中表示并采用PRM方法構(gòu)建環(huán)境地圖.

如圖1所示為規(guī)劃環(huán)境，其中障礙物1為運動規(guī)律未知的障礙物，其最大運動速度已知，只能給定其運動范圍;障礙物2與障礙物3為運動方向已知但速度未知的障礙物，它們沿箭頭所指方向運動;障礙物4為運動規(guī)律已知的障礙物，其沿箭頭所指方向勻速運動;障礙物5為運動軌跡已知，速度未知的障礙物.它們在CT空間中的表示如圖2所示.

圖1 規(guī)劃環(huán)境

圖2 運動障礙物在CT空間中的表示

對于傳統(tǒng)概率地圖法，首先從構(gòu)型空間中選取一個點，運用碰撞檢測程序檢測采樣得到的點的位置，若碰撞，則舍棄;若位于自由空間中，則把這個點作為結(jié)點加入到路線圖中.隨后，嘗試將新獲得的結(jié)點與路線圖中的結(jié)點連接在一起，如此循環(huán)直到完成路線圖的構(gòu)建.這種方法可以自然地擴展到CT空間，在CT空間中隨機得到的每個采樣點都包含兩個信息:一個是采樣點的位置信息，另一個是采樣點的時間信息.這種方法會得到一部分無用的采樣點，這些無用的采樣點無人機肯定不會經(jīng)過，可以在構(gòu)建地圖時忽略.例如，在初始點附近但時間坐標(biāo)值較大的采樣點是肯定不會經(jīng)過的，這就使算法性能有部分浪費.為此，改進了采樣點的獲得方法，只給定其位置坐標(biāo)而不給定其時間坐標(biāo)，在路徑生成過程中對于經(jīng)過的采樣點動態(tài)地添加時間坐標(biāo)并進行碰撞檢測.時間坐標(biāo)的值由當(dāng)前結(jié)點的前向結(jié)點的時間坐標(biāo)和兩個結(jié)點之間的距離確定.時間坐標(biāo)添加方法如圖3所示.圖中給出了兩條在概率地圖基礎(chǔ)上得到的出發(fā)點cs和目標(biāo)點c2之間帶時間坐標(biāo)的路徑示意圖，圖中Δt為從cs到c1所需的時間.當(dāng)無人機從cs出發(fā)到達(dá)c1點時，c1點的時間坐標(biāo)由cs點的時間坐標(biāo)與無人機從cs到c1點所需時間相加得到.

圖3 時間坐標(biāo)的添加方法

2 基于概率地圖的蟻群算法

基于概率地圖的路徑規(guī)劃的難點在于，當(dāng)概率地圖規(guī)模較大的時候，會存在搜索時間過長的問題.因此，構(gòu)建快速有效的優(yōu)化搜索算法是合理解決路徑規(guī)劃問題的關(guān)鍵.蟻群算法作為一種智能算法，能夠在較短的時間內(nèi)至少得到一個次優(yōu)解，可以有效解決這個問題.

傳統(tǒng)的蟻群算法［8］在開始階段由于各個路徑段上的信息素是相同的，螞蟻經(jīng)過各個路徑段的概率是相同的，這樣就會使螞蟻搜索到的路徑較差，如果在初始時刻給螞蟻一定的指引，就能使螞蟻更早地尋找到較好的路徑.文獻［9］通過計算目標(biāo)吸引度引導(dǎo)螞蟻搜索，但是目標(biāo)吸引度計算復(fù)雜，運算效率較低.文獻［10］則是采用了自適應(yīng)調(diào)整的啟發(fā)函數(shù)，實時計算啟發(fā)函數(shù)引導(dǎo)螞蟻搜索.本文將方向信息作為新的啟發(fā)信息引入蟻群算法中，使螞蟻在搜索路徑時受到指引，加快蟻群算法的收斂速度，能夠以更快的速度得到解.

2.1 改進的狀態(tài)轉(zhuǎn)移規(guī)則

蟻群算法中螞蟻按照環(huán)境中的信息素分布和啟發(fā)信息進行其狀態(tài)轉(zhuǎn)移，啟發(fā)信息由候選的解元素的評價值決定.在路徑規(guī)劃中蟻群算法的狀態(tài)轉(zhuǎn)移是指螞蟻由一個采樣點經(jīng)過一條邊移動到采樣點，構(gòu)成路徑的解元素就是概率地圖中的邊.螞蟻k在結(jié)點i選擇結(jié)點j的概率為

式中，τij是邊(i，j)上的信息素軌跡強度;ηij為邊(i，j)的能見度，反映由結(jié)點i轉(zhuǎn)移到結(jié)點j的啟發(fā)程度，這個量在蟻群算法的運行中不變;Φk表示螞蟻k下一步可以選擇的結(jié)點.由式(1)可知，轉(zhuǎn)移概率與成正比.α和β為兩個參數(shù)，分別反映了螞蟻在運動過程中所積累的信息素和啟發(fā)信息在螞蟻選擇路徑中的相對重要性.

在基本蟻群算法中，螞蟻開始路徑搜索時，由于各條路徑上的信息素含量是相等的，文獻［8］中啟發(fā)信息選用了路徑段長度的倒數(shù)，不能很好地引導(dǎo)螞蟻選擇路徑，降低了算法的效率.為了在初始時刻對螞蟻的搜索做出指引，本文對啟發(fā)信息做了改進，引入了方向信息，使螞蟻在尋找路徑時受到方向信息的影響，提高了算法的效率.改進算法的啟發(fā)信息函數(shù)公式如下:

式中θij為可選結(jié)點j與目標(biāo)結(jié)點的連線與初始終止點連線的夾角.可見轉(zhuǎn)移概率不但與成正比，而且還與夾角θ的余弦值成正比.ij夾角θij越小，cosθij值越大，螞蟻選擇結(jié)點j的可能性就越大.

2.2 基于約束條件的候選集合策略

在蟻群算法中，構(gòu)建螞蟻狀態(tài)轉(zhuǎn)移的候選集合策略是提高算法性能的一個重要途徑.為了能夠更好地表達(dá)環(huán)境信息，構(gòu)建的概率地圖含有的采樣點數(shù)量往往較多，這就導(dǎo)致無人機路徑規(guī)劃問題規(guī)劃空間的規(guī)模比較大，因此設(shè)計一個合適的候選集合策略可以提高算法的運行效率，具有很重要的意義.

在基于蟻群算法的無人機路徑規(guī)劃中，螞蟻的狀態(tài)轉(zhuǎn)移候選集合的構(gòu)造需要考慮環(huán)境約束以及無人機的性能約束，有下面幾個方面的因素:①概率地圖中結(jié)點之間的連通關(guān)系限制;②無人飛機的最小航線段長度限制，飛機在經(jīng)過轉(zhuǎn)彎后需要一定的直飛距離來修正轉(zhuǎn)彎造成的航線偏移并調(diào)整自己的飛行姿態(tài)，這有利于飛機對航線的精確跟蹤;③無人飛機的最大轉(zhuǎn)彎角度的限制，這與飛機的機動性能相關(guān);④飛機最大航程限制，這與飛機的載油量相關(guān).在考慮這些限制之后，蟻群算法中螞蟻可以選擇的結(jié)點范圍大幅減少，有利于算法性能的提升.

2.3 基于排序的信息素更新機制

為了防止蟻群算法過早地收斂，陷入局部最優(yōu)，在信息素更新時采用了基于排序的更新機制.

基于排序的信息素更新機制的局部信息素更新規(guī)則與基本蟻群算法相同，路徑段(r，s)上的信息素更新如下所示:

式中，ρ為揮發(fā)系數(shù);Δτrs為路徑段上信息素的增量.

全局信息素更新中在每次循環(huán)完成后，對螞蟻搜索得到的路徑按照長度進行排序，并對排在前m名的螞蟻進行相應(yīng)的信息素更新，更新規(guī)則如下所示:

式中，Δτij為當(dāng)前路徑段上走過的螞蟻按照排名得到的信息素更新量的總和;λ為揮發(fā)系數(shù).

2.4 基于排序的信息素更新機制

基于排序的蟻群算法步驟如下所示:

初始化

Repeat

For k=1:n//對每一只螞蟻

Pi={ps};//將螞蟻置于起始點

Repeat//查找路徑

For所有候選路徑點

根據(jù)式(2)，計算啟發(fā)信息ηij

根據(jù)式(3)進行局部信息素更新

Until到達(dá)目標(biāo)點.

根據(jù)式(4)進行全局信息素更新;

Until滿足終止條件.

3 仿真實驗

基于上述算法設(shè)計，本文針對如圖1所示的動態(tài)規(guī)劃環(huán)境進行仿真實驗，規(guī)劃范圍為200×200，障礙物為圓盤形，最大轉(zhuǎn)彎角為60°，構(gòu)建概率地圖的采樣點數(shù)量為600個.圖4為改進蟻群算法得到的路徑.圖5為無人機沿規(guī)劃得到的路徑飛行時在部分時刻與障礙物的相對位置圖.其中菱形為無人機當(dāng)前時刻的位置，灰色圓盤為障礙物的可能位置.由仿真結(jié)果可見，無人機與障礙物是不碰撞的，驗證了方法的可行性.

圖4 改進蟻群算法得到的路徑

圖5 無人機在部分時刻與障礙物的相對位置圖

為了驗證算法的有效性，本文對蟻群算法和改進蟻群算法分別進行了仿真驗證.圖6為路徑長度的迭代曲線對比圖，由仿真結(jié)果可見，改進蟻群算法可以在算法運行初期得到更好的解，這說明由于引入了方向信息作為啟發(fā)信息，螞蟻在算法開始階段尋找路徑時，受到了更好的引導(dǎo)，可以更早地尋找到較短的路徑，使算法更快地收斂.

圖6 路徑長度的迭代曲線對比

為了驗證本文提出的蟻群算法在不同規(guī)模的概率地圖中的有效性，本文在圖1所示環(huán)境中對采樣點數(shù)量為600，800和1 000的情況分別進行了仿真實驗，圖7為路徑長度的迭代曲線對比圖.由迭代曲線對比可見，算法在不同采樣點數(shù)量的情況下都可以在算法開始階段找到較好的結(jié)果，并且算法能夠快速地收斂到最終結(jié)果.

圖7 不同采樣點數(shù)量條件下的路徑長度迭代曲線對比

4 結(jié) 束 語

本文針對動態(tài)環(huán)境中的路徑規(guī)劃問題，提出了一種引入時間軸擴展構(gòu)型空間的方法.仿真結(jié)果表明，這種方法可以解決動態(tài)環(huán)境中的無人機避障問題.在路徑生成階段，本文提出了一種改進的蟻群算法，通過引入方向信息作為新的啟發(fā)信息，改進蟻群算法可以更快地收斂到最優(yōu)解，提高了蟻群算法的運算效率，更好地滿足了無人機規(guī)劃的實時性要求.

References)

［1］ Kavraki L E，Svestka P，Latombe JC，et al.Randomized preprocessing of configuration space for fast path planning［C］//IEEE International Conference on Robotics and Automation.San Diego:IEEE，1994:3020 －3026

［2］曾碧，楊宜民.動態(tài)環(huán)境下基于蟻群算法的實時路徑規(guī)劃方法［J］.計算機應(yīng)用研究，2010，27(3):860 －863 Zeng Bi，Yang Yimin.Method of real time path planning based on ant colony algorithm in dynamic environment［J］.Application Research of Computers，2010，27(3):860 －863(in Chinese)

［3］朱毅，張濤，程農(nóng)，等.動態(tài)環(huán)境下基于子目標(biāo)的移動機器人路徑規(guī)劃方法［J］.系統(tǒng)仿真學(xué)報，2010，22(增刊1):254－257 Zhu Yi，Zhang Tao，Cheng Nong，et al.Sub-goal based path planning method for mobile robot under dynamic environment［J］.Journal of System Simulation，2010，22(Supplement 1):254 －257(in Chinese)

［4］肖本賢，齊東流，劉海霞，等.動態(tài)環(huán)境中基于模糊神經(jīng)網(wǎng)絡(luò)的AGV路徑規(guī)劃［J］.系統(tǒng)仿真學(xué)報，2006，18(9):2401－2404 Xiao Benxian，Qi Dongliu，Liu Haixia，et al.AGV path planning in the dynamic environment based-on fuzzy neural network［J］.Journal of System Simulation，2006，18(9):2401 － 2404(in Chinese)

［5］劉國棟，謝宏斌，李春光.動態(tài)環(huán)境中基于遺傳算法的移動機器人路徑規(guī)劃的方法［J］.機器人，2003，25(7):327－330 Liu Guodong，Xie Hongbin，Li Chunguang.Method of mobile robot path planning in dynamic environment based on genetic algorithm［J］.Robot，2003，25(7):327 －330(in Chinese)

［6］沈晶，顧國昌，劉海波.未知動態(tài)環(huán)境中基于分層強化學(xué)習(xí)的移動機器人路徑規(guī)劃［J］.機器人，2006，28(5):544－547 Shen Jing，Gu Guochang，Liu Haibo.Mobile robot path planning based on hierarchical reinforcement learning in unknown dynamic environment［J］.Robot，2006，28(5):544 － 547(in Chinese)

［7］ Van Den Berg J，Overmars M.Kinodynamic motion planning on road maps in dynamic environments［C］//Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems.San Diego:IEEE，2007:4253 －4258

［8］ Colorni A，Dorigo M，Maniezzo V.Distributed optimization by ant colonies［C］//The 1st European Conference on Artificial Life.Paris:Elsevier Publishing，1991:134 －142

［9］張曉勇，吳敏，彭軍，等.機器人救援的目標(biāo)吸引動態(tài)路徑規(guī)劃蟻群算法［J］.系統(tǒng)仿真學(xué)報，2011，23(9):1854 －1859 Zhang Xiaoyong，Wu Min，Peng Jun，et al.Target attraction based ant colony for dynamic path planning of rescue robot［J］.Journal of System Simulation，2011，23(9):1854 －1859(in Chinese)

［10］柳長安，鄢小虎，劉春陽，等.基于改進蟻群算法的移動機器人動態(tài)路徑規(guī)劃方法［J］.電子學(xué)報，2011，39(5):1220－1224 Liu Chang’an，Yan Xiaohu，Liu Chunyang，et al.Dynamic path planning for mobile robot based on improved ant colony optimization algorithm［J］.Acta Electronica Sinica，2011，39(5):1220－1224(in Chinese)