一種基于改進Theta*的機器人路徑規劃算法

肖國寶，嚴宣輝

(福建師范大學數學與計算機科學學院，福建 福州 350007)

路徑規劃是智能交通、智能網絡、機器人等人工智能研究領域的重要分支，所謂移動機器人路徑規劃技術，就是機器人根據自身傳感器對環境的感知，在線規劃出一條安全、可靠的運行路徑，同時高效完成作業任務［1-2］.它是一個比較復雜的帶約束條件的優化問題，約束條件包括但不限于:不與障礙物碰撞、運動路徑最短、盡量遠離障礙物、路徑盡量平滑等［3-4］.

未知環境下的機器人路徑規劃是智能體實現其在線規劃的前提，也是移動機器人導航中一個重要的問題.目前，確定性環境的導航控制方法已取得了大量的研究和應用成果［5-6］，在二維未知環境中的導航控制方面已展開了一些研究，并提出了若干方法［7-9］;但隨著人類活動空間的擴張，已有的二維路徑規劃技術越來越滿足不了許多領域的需求，人們迫切需要一套成熟可靠的三維空間路徑規劃技術［7］.

機器人路徑規劃需要考慮很多因素，其中主要包括環境的不確定性和動態特性、規劃算法的優越性、實時能力等.三維環境下的機器人路徑規劃問題由于運動學和動力學約束變得非常復雜，機器人的自由度也大幅度增加，這要求規劃算法的實時性要比較好，防止出現數據爆炸的情況.針對盲目搜索的效率較低，會耗費過多的計算空間與時間，目前用于三維路徑規劃比較常見的算法有 A*［8］、Diskstra、D*-lite、貪婪搜索、四/八叉樹搜索［9］以及這些算法的改進［10-11］.采用的搜索策略如寬度優先、深度優先、啟發式、非啟發式搜索等［7］.

經典的啟發式搜索算法——A*算法是由P.E.Hart等在20世紀60年代提出的，該算法在各個領域得到了廣泛的應用［12］.A*算法也是目前用來解決游戲地圖路徑搜索問題最優的算法，但該算法只能用在格子環境中，這在一定程度上限制了其進一步的推廣.Alex等在2007年提出了一種A*算法的變種——Theta*算法［13］，突破了格子的限制，允許以任意角度改變路徑的方向.

啟發函數是評定候選擴展節點的方法，以便確定哪個節點最有可能在通向目標的最佳路徑上，它將會影響到整個算法的性能和效果.在大部分啟發式搜索算法中的啟發函數只估算到起始點和目標點的代價，沒有合理地利用障礙物信息，這會限制其用于解決機器人路徑規劃問題時的進一步推廣.

本文將局部環境的障礙物對機器人產生的斥力作為懲罰函數引入到啟發函數中，為機器人提供了更加可靠的啟發信息.首先，利用仿真實驗的數據統計合理地選擇懲罰函數權重，以確定啟發函數;然后在此啟發策略的基礎上，改進A*算法的變種——Theta*算法，用PS_Theta*算法對路徑進行平滑處理，優化路徑.

1 算法描述

1.1 三維環境模型

移動機器人環境模型問題就是機器人根據傳感器的感知獲得環境的二維或三維空間模型.單元分解建模是典型的環境模型，其主要思想是將環境離散化為若干個規則的相同大小的基本單元——三維的立方格，通過二值信息便可以對障礙物和自由空間進行標識，因此使用簡單的傳感器即可獲得創建地圖的信息.然而，立方格的大小直接影響著移動機器人環境信息存儲量的大小和規劃時間的長短，立方格選得小，環境分辨率高，但環境信息存儲量大，相應的干擾信號相對增加，使得決策工作量加大，最終導致規劃速度變慢，降低系統的實時性;當立方格變大時，雖然抗干擾能力增強，但環境分辨率下降，在密集障礙物環境中不利于規劃出有效的路徑［14］.

在Matlab 7.6中建立2種常見的模型:1)城市環境模型，如圖1;2)普通環境模型，如圖2.其中，將機器人看作質點，不能碰到環境中的任何障礙物.

圖1 城市環境模型Fig.1 Urban environment model

圖2 普通環境模型Fig.2 Common environment model

任意時刻，在二維地圖中，機器人的運動方向有8個，如圖3所示，機器人可以到達相鄰柵格的各個頂點;在三維地圖中，機器人的運動方向有26個，如圖4所示，機器人可以到達相鄰立方格的各個頂點.

圖3 二維運動方向Fig.3 The direction of movement in 2-D environment

圖4 三維運動方向Fig.4 The direction of movement in 3-D environment

機器人路徑規劃問題就是在圖中尋找一個從起始點S到目標點T之間點的集合，并要求相鄰點之間的線段連接沒有經過障礙物.

1.2 改進啟發函數

在啟發式搜索算法中，引入當前節點x的啟發式估價函數f'(x)，其函數的定義式為

式中:f'(x)是從起點S出發到目標點的最小代價值f(x)的估價函數，g(x)是從起點S到當前節點x的實際代價值，h'(x)是從當前節點x到目標點估計的最小代價值，α、β是2個增益系數.顯然，當h'(x)=0時，啟發式搜索算法就變成了沒有任何啟發信息的盲目搜索算法，如普通Dijkstra算法.

在啟發式搜索算法中，找到最優路徑的關鍵在于估價函數h(x)的選取，如可以取兩節點的歐幾里德距離作為估價值.由式(1)可知，啟發信息與環境中的障礙物無關，但對于機器人路徑規劃問題，障礙物的位置信息是規劃過程中需要考慮的重要信息之一.因此，啟發信息若能夠考慮障礙物的位置信息將更有前瞻性和可靠性，有利于機器人準確地判斷下一步的動作.在保證路徑長度較短的前提下，盡可能地靠近目標點及遠離障礙物是路徑規劃最理想的狀態，此處引入人工勢場法(artificial potential field，APF)的思想［15-16］，讓機器人盡可能地避開障礙物.本文將一定范圍內的障礙物對機器人產生的斥力作為一種懲罰函數添加至啟發函數中.在人工勢場法中斥力場公式為:

式中:η 是正增益系數;X=(xr，yr)和 XG=(xG，yG)分別表示機器人位置向量和目標點位置向量;ρ表示機器人到障礙物的歐式距離;ρ0表示單個障礙物影響的最大距離范圍.相應的斥力為其負梯度，如式(2):

式中:

將式(2)作為懲罰函數可知，障礙物距離機器人越近，其產生的斥力就越大，進而相應的啟發函數值也會增大，即懲罰越嚴重.將其并入到式(1)，得到式(3):

式中:g(x)是從起點S到當前節點x的實際代價值，h'(x)是從當前節點x到目標點估計的最小代價值(用與目標點之間的歐式距離來衡量)，w(x)是當前節點x在局部環境中所受到的斥力總和的模長，α、β、θ是3個增益系數.當 θ=0時，f'(x)為普通的啟發函數，即與式(1)等價.

采用式(3)作為啟發函數，考慮到了機器人從傳感器得到的障礙物位置信息，可以根據局部環境信息的變化動態地修改啟發函數，這將有利于規劃出更優的軌跡.此外，經過改進的啟發函數仍然能夠滿足啟發式搜索算法的可歸納性［17］，即在一些問題的求解過程中，如果存在最短路徑，無論在什么情況之下，該搜索算法都能夠保證找到這條最短路徑.

啟發函數中的系數選擇將會影響到函數的最終結果，從實驗的角度分析系數的選擇.選取α與β的最佳比例［18］為，β與θ最佳的比例從多個指標(路徑長度、方向轉換次數、擴展的節點數和運行時間)進行分析.在500×500的柵格中，采用具有代表性的啟發式搜索算法——A*算法［12］作為測試算法，選擇不同的β與θ比例在500種隨機環境下進行路徑規劃，有效的平均數據統計如表1所示.

表1 不同增益系數比例下規劃的數據統計結果Table 1 The calculation results of path planning in different gain factors

從表1的數據統計可以得出以下結論:1)采用同一比例的不同β與θ值規劃后的軌跡基本一致;2)合理地將障礙物信息引入到啟發函數后，可以得到更優的路徑(當θ=0時，A*算法采用的是普通的啟發策略);3)當比例大于10時，各項數據指標趨于穩定;4)從多個指標衡量，最佳的比例為(θ/β)=(1/7).同樣，采用其他啟發式搜索算法作為測試算法，其數據統計也能夠反應出類似的趨勢.

1.3 Basic Theta*算法及其改進

Basic Theta*算法是一種柵格路徑規劃算法，也是A*算法的一種變種，其最大的改進在于它不要求搜索樹節點的父節點必須是其相鄰的節點，因此所求解路徑的軌跡不限制在柵格的橫向與縱向，理論上允許以任意角度改變路徑的方向［13］.

在估價函數的選取上，Basic Theta*算法與A*算法是一致的，故在使用其解決路徑規劃問題時，式(3)作為其啟發函數能夠獲取更優的路徑.該算法在搜索中設置了2個表:OPEN表和 CLOSE表.OPEN表保存了所有已擴展而未被考察過的節點，CLOSE表記錄了已被考察過的節點.

Basic Theta*算法的步驟如下［19］.

1)初始化OPEN、CLOSE表，并將起始點S放入到OPEN表中.

2)如果OPEN表為空，表示沒有找到路徑，退出.

3)從OPEN表中找出最佳節點，作為當前節點x，并將其從OPEN表移到CLOSE表中.

4)判斷當前節點x是否為目標點，如果是，則通過節點x的父節點，一直遍歷到起點，找到路徑的節點集合，搜索結束;否則，轉至5).

5)開始擴展當前節點x，找到當前節點的所有后續節點集合U并遍歷集合內節點.如果遍歷的節點yi不在OPEN或者CLOSE表中，將該節點yi加入OPEN表中，計算該節點yi的估計值，并記錄其父節點為x;否則，轉至6).

6)判斷當前節點x的父節點x'與遍歷的節點yi是否相通(節點之間的線段沒有經過障礙物):

①若相通，比較當前節點的父節點x'的代價g(x')和OPEN表中的代價g(yi)，如果g(x')較小，則更新表中的代價，并將節點yi的父節點指向x';

②若不相通，直接比較當前節點的代價g(x)和OPEN表中的代價g(yi)，如果g(x)較小，則更新表中的代價及父節點.

7)按照估價值遞增順序，對OPEN表中的節點進行排序，轉向2).

Basic Theta*算法能夠突破格子的限制，找到可行路徑，但該算法存在著一些缺陷，即最先找到的路徑并不能保證路徑最短.如圖5所示，最短路徑應該是E1到B9的線段，但E1不會成為B9的父節點，路徑經過了點C6.分析其原因在于Basic Theta*的擴展方式［13］，即2個頂點之間并不會隨意形成父節點與子節點的關系.

圖5 Theta*算法規劃軌跡演示Fig.5 The path planning demo of Theta*algorithm

對路徑進行平滑處理能夠有效地優化路徑［20］，在經過Basic Theta*算法搜索找到路徑后，中間經過的點會大量減少，對其進一步平滑可以在較短的時間內完成.具體平滑步驟如下.

1)獲取Basic Theta*規劃后的路徑軌跡節點集合.

2)選擇集合中的起始點為平滑的基準點x.

3)判斷節點x與節點z是否相通，其中，節點z為節點y的后續節點，節點y為節點x的后續節點:

①若相通，去除中間節點y，即將節點x的后續節點修改為節點z，相應地，節點z的父節點為節點x;

②否則，將基準點更新為節點y.如此循環，直到節點y為目標點時，循環結束，轉到4).

4)平滑完畢后，從目標點回溯到起始點，重新組成最佳的節點集合.

由Basic Theta*算法與平滑步驟組成PS_Theta*算法，針對圖5中Basic Theta*算法規劃的軌跡，PS_Theta*算法能夠進一步優化路徑，如圖6所示.光滑度是路徑軌跡優越性的重要指標，光滑的軌跡能夠有效提高機器人到達目標點的效率，可以盡可能地滿足非完整性約束的條件.

圖6 PS_Theta*算法規劃軌跡演示Fig.6 The path planning demo of PS_Theta*algorithm

2 仿真與結果分析

2.1 二維隨機環境下的路徑規劃問題

本文在Matlab 7.6環境下對算法進行驗證和比較.在二維環境下，采用本文提出的改進啟發式搜索策略，對 A*［12］、Basic Theta*［13］與 PS_Theta*算法進行對比實驗.在柵格模型中，選擇2種規格進行測試，分別為100×100與500×500，障礙物按不同比例隨機生成，并隨機初始化起始點與目標點，統計500次仿真實驗，平均數據如表2所示，圖7和8分別表示2種規格下3種算法規劃的平均路徑方向變化頻率曲線圖.

表2 二維隨機環境的仿真實驗結果Table 2 The simulation experimental results in 2-D random environment

圖7 路徑方向變化頻率曲線(100×100)Fig.7 The graph of heading changes(100 ×100)

圖8 路徑方向變化頻率曲線(500×500)Fig.8 The graph of heading changes(500 ×500)

由表2的數據及圖7和8的曲線圖可以表明，使用Basic Theta*算法能夠規劃出較短的路徑，PS_Theta*算法利用較少的額外運行時間代價使得路徑更優，主要體現在2點:1)路徑長度更短;2)路徑方向變化大量降低.路徑方向變化頻率能夠體現路徑的光滑程度，是路徑優越性的重要指標.

2.2 三維隨機環境下的路徑規劃問題

將采用啟發式搜索算法解決路徑規劃問題的環境規模擴展至三維隨機環境中.在2種環境模型下采用不同啟發式搜索算法進行路徑規劃，2種隨機的城市環境模型的規劃軌跡如圖9和10所示，從多個角度觀察普通環境模型的規劃軌跡如圖11和12所示.3種啟發式搜索算法均能夠快速地找到目標點，從規劃后的對比效果可以表明，PS_Theta*算法規劃的路徑軌跡長度最短.

圖9 城市環境模型規劃演示1Fig.9 Urban path 1

圖10 城市環境模型規劃演示2Fig.10 Urban path 2

圖11 普通環境模型規劃結果Fig.11 The path 1 in common environment

圖12 普通環境模型規劃結果Fig.12 The path 2 in common environment

最后，通過多次實驗的統計數據對比算法的性能.在三維立方格模型中，選擇2種規格進行測試，分別為50×50×50與100×100×100，按不同比例隨機生成障礙物，并隨機初始化起始點與目標點的位置.統計500次仿真實驗，平均數據如表3所示，圖13和14分別表示2種規格下3種算法規劃的平均路徑方向變化頻率曲線圖.

圖13 路徑方向變化頻率曲線(50×50×50)Fig.13 The graph of heading changes(50 ×50 ×50)

圖14 路徑方向變化頻率曲線(100×100×100)Fig.14 The graph of heading changes(100×100×100)

表3 三維隨機環境的仿真實驗結果Table 3 The simulation experimental results in 3-D random environment

由表3的數據及圖13和14的曲線圖可以表明，在三維隨機復雜的環境下，3種啟發式搜索算法均能夠快速地找到路徑.相比其他2種算法，PS_Theta*算法利用較少的額外時間代價，大幅度降低了路徑方向改變的頻率，保證了軌跡的平滑性，并且能有效地縮短路徑長度.相比二維環境，PS_Theta*算法的優勢更加明顯，提高了路徑方向變化的區分度.

3 結束語

啟發式搜索是一種有序高效的搜索策略，其相對簡單的時間復雜度能夠保證規劃的實時性，本文將障礙物對機器人產生的斥力作為懲罰函數加入到啟發函數中，合理性主要體現在以下3點:1)提高啟發函數的前瞻性與可靠性;2)考慮到了障礙物對機器人的影響，能夠較好地選擇節點，減少擴展的節點數量;3)有效地減少規劃時間，提高效率.而將Theta*算法應用在三維隨機環境解決機器人路徑規劃問題，相比A*算法，體現出了其算法的優越性，即有效縮短了路徑長度.利用改進算法PS_Theta*對路徑進行平滑處理，能夠大幅度地降低路徑方向變化的頻率，提高軌跡的平滑性.

總之，將局部環境中的障礙物信息引入到啟發函數中，為機器人提供更加可靠的啟發信息，以提高其下一步動作的準確性，使用改進算法大幅度降低了規劃的路徑方向變化，這在實際應用中能夠提高機器人動作連貫性，加快到達目的地.

［1］朱大奇，顏明重.移動機器人路徑規劃技術綜述［J］.控制與決策，2010，25(7):961-967.ZHU Daqi，YAN Mingzhong.Survey on technology of mobile robot path planning［J］.Control and Decision，2010，25(7):961-967.

［2］肖國寶，嚴宣輝.一種動態不確定環境中機器人路徑規劃方法［J］.計算機系統應用，2012，21(4):92-98.XIAO Guobao，YAN Xuanhui.Path panning of mobile robot in dynamic nondeterministic environments［J］.Computer Systems and Applications，2012，21(4):92-98.

［3］張捍東，鄭睿，岑豫皖.移動機器人路徑規劃技術的現狀與展望［J］.系統仿真學報，2005，17(2):439-443.ZHANG Handong，ZHENG Rui，CEN Yuwan.Present situation and future development of mobile robot path planning technology［J］.Acta Simulata Systematica Sinica，2005，17(2):439-443.

［4］劉華軍，楊靜宇，陸建峰，等.移動機器人運動規劃研究綜述［J］.中國工程科學，2006，8(1):85-94.LIU Huajun，YANG Jingyu，LU Jianfeng，et al.Research on mobile robots motion planning:a survey［J］.Engineering Science，2006，8(1):85-94.

［5］禹建麗，李曉燕，王躍明，等.一種基于神經網絡的機器人路徑規劃算法［J］.洛陽工學院學報，2001，22(1):31-34.YU Jianli，LI Xiaoyan，WANG Yueming，et al.An algorithm of path planning for car-like robots based on neural network［J］.Journal of Luoyang Institute of Technology，2001，22(1):31-34.

［6］HU Yanrong，YANG S X.A knowledge based genetic algorithm for path planning of a mobile robot［C］//Proceedings of the 2004 IEEE International Conference on Robotics and Automation.New Orleans，USA，2004:4350-4355.

［7］陳洋，趙新剛，韓建達.移動機器人3維路徑規劃方法綜述［J］.機器人，2010，32(4):568-576.CHEN Yang，ZHAO Xin'gang，HAN Jianda.Review of 3D path planning methods for mobile robot［J］.Robot，2010，32(4):568-576.

［8］SATHYARAJ B M，JAIN L C，FINN A，et al.Multiple UAVs path planning algorithms:a comparative study［J］.Fuzzy Optimization and Decision Making，2008，7(3):257-267.

［9］WU X J，TANG J，LI Q，et al.Development of a configuration space motion planner for robot in dynamic environment［J］.Robotics and Computer-Integrated Manufacturing，2009，25(1):13-31.

［10］CARSTEN J，FERGUSON D，STENTZ A.3D field D:improved path planning and replanning in three dimensions［C］//2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.Beijing，China，2006:3381-3386.

［11］DOLGOV D，THRUN S，MONTEMERLO M，et al.Practical search techniques in path planning for autonomous driving［C］//Proceedings of the First International Symposium on Search Techniques in Artificial Intelligence and Robotics.Chicago，USA，2008:1-6.

［12］HART P E，NILSSON N J，RAPHAEL B.A formal basis for the heuristic determination of minimum cost paths［J］.IEEE Transactions on Systems Science and Cybernetics，1968，4(2):100-107.

［13］NASH A，DANIEL K，KOENIG S，et al.Theta*:anyangle path planning on grids［C］//Proceedings of the Twenty-Second AAAI Conference on Artificial Intelligence.Vancouver，Canada:AAAI Press，2007:1177-1183.

［14］蔡自興，徐光祐.人工智能及其應用［M］.3版.北京:清華大學出版社，2004.

［15］SABATTINI L，SECCHI C，FANTUZZI C.Arbitrarily shaped formations of mobile robots:artificial potential fields and coordinate transformation［J］.Autonomous Robots，2011，30(4):385-397.

［16］SHENG Junwen，HE Gaoqi，GUO Weibin，et al.An improved artificial potential field algorithm for virtual human path planning［C］//Proceedings of the Entertainment for Education，and 5th International Conference on E-learning and Games.Berlin/Heidelberg，Germany:Springer-Verlag，2010:592-601.

［17］周小鏡.基于改進 A*算法的游戲地圖尋徑的研究［D］.重慶:西南大學，2011:22-23.ZHOU Xiaojing.Research of routing in the game map based on improved A*algorithm［D］.Chongqing:Southwest University，2011:22-23.

［18］De FILIPPIS L，GUGLIERI G，QUAGLIOTTI F.Path planning strategies for UAVS in 3D environments［J］.Journal of Intelligent＆ Robotic Systems，2011，65(1/2/3/4):247-264.

［19］NASH A，KOENIG S，LIKHACHEV M.Incremental Phi*:incremental any-angle path planning on grids［C］//Proceedings of the 21st International Joint Conference on Artificial Intelligence.San Francisco，USA:Morgan Kaufmann Publishers Inc.，2009:1824-1830.

［20］BOTEA A，MULLER M，SCHAEFFER J.Near optimal hierarchical path-finding［J］.Journal of Game Development，2004，1(1):7-28.