動態環境中的無人機路徑規劃方法

劉 洋 章衛國 李廣文 史靜平

(西北工業大學 自動化學院，西安 710129)

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

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

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

1 規劃環境的構建

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

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

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

圖1 規劃環境

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

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

圖3 時間坐標的添加方法

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

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

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

2.1 改進的狀態轉移規則

蟻群算法中螞蟻按照環境中的信息素分布和啟發信息進行其狀態轉移，啟發信息由候選的解元素的評價值決定.在路徑規劃中蟻群算法的狀態轉移是指螞蟻由一個采樣點經過一條邊移動到采樣點，構成路徑的解元素就是概率地圖中的邊.螞蟻k在結點i選擇結點j的概率為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

初始化

Repeat

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

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

Repeat//查找路徑

For所有候選路徑點

根據式(2)，計算啟發信息ηij

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

Until到達目標點.

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

Until滿足終止條件.

3 仿真實驗

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

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

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

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

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

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

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

4 結 束 語

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

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］曾碧，楊宜民.動態環境下基于蟻群算法的實時路徑規劃方法［J］.計算機應用研究，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］朱毅，張濤，程農，等.動態環境下基于子目標的移動機器人路徑規劃方法［J］.系統仿真學報，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］肖本賢，齊東流，劉海霞，等.動態環境中基于模糊神經網絡的AGV路徑規劃［J］.系統仿真學報，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］劉國棟，謝宏斌，李春光.動態環境中基于遺傳算法的移動機器人路徑規劃的方法［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］沈晶，顧國昌，劉海波.未知動態環境中基于分層強化學習的移動機器人路徑規劃［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］張曉勇，吳敏，彭軍，等.機器人救援的目標吸引動態路徑規劃蟻群算法［J］.系統仿真學報，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］柳長安，鄢小虎，劉春陽，等.基于改進蟻群算法的移動機器人動態路徑規劃方法［J］.電子學報，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)