基于改進Informed-RRT* 算法的機器人路徑規劃

代軍，李志明，李艷琴，趙俊偉

（河南理工大學 機械與動力工程學院，河南 焦作 454000）

0 引 言

近年來，移動機器人相關技術研究取得了較大突破，廣泛應用于消防救援、醫療服務、餐飲服務、病毒消殺等領域。市場對移動機器人路徑規劃的精度和效率要求也逐步提高，路徑規劃技術作為移動機器人領域的核心技術之一，已成為科研人員關注的熱點。

傳統的路徑規劃算法包括蟻群算法［1］、遺傳算法［2］、深度學習算法［3-4］、Dijkstra算法［5］、A*算法［6］等。T.Lung等［7］基于隨機采樣原理提出了Rapid-exploring Random Tree（RRT）算法，RRT算法在空間中隨機提取采樣點進行搜索，不需構建顯式的任務空間，因而計算簡單，且其具有概率完備性，即在規劃中一定有解。戶望力［8］針對RRT算法盲目采樣的缺點，通過約束采樣方向使節點樹向目標點生長；李洋等［9］提出一種通過自適應采樣步長使節點樹快速覆蓋采樣區域的RRT算法；為解決RRT算法的路徑非最優問題，S.Karaman等［10］提出RRT*算法，RRT*算法在規劃過程中對新節點進行局部的剪枝重新布線，使路徑接近最優解；劉猛等［11］將RRT*算法與三次曲線規劃相結合，通過三次曲線規劃使路徑趨向平滑，減少汽車自主泊車時的橫向移動；曹凱等［12］將人工勢場法與RRT*算法融合，并通過人工勢場引導節點樹向目標方向生長；其他算法也相繼被提出，如基于D*的Field D*［13］，基于RRT*的Informed-RRT*［14-15］等算法。

Informed-RRT*算法不需要構建顯式的采樣空間，但路徑規劃目的性差、路徑優化效率低。為此，本文提出一種改進的Informed-RRT*路徑規劃算法。首先，在路徑規劃過程中引入貪心算法思想，當找到一個可以加入節點樹的新采樣點后，判斷該節點能否直接與目標點連線，若條件成立，則結束路徑規劃過程進入路徑優化過程，否則，繼續采樣；其次，將路徑優化過程中搜索潛在最優父節點的搜索對象由構建的節點樹替換為路徑，減少路徑優化過程中搜索潛在最優父節點時消耗的時間。通過融合兩種優化方案，進一步提高路徑規劃的效率。

1 RRT*算法

RRT算法從起點開始，在空間中隨機采樣，并從路徑樹上找到與采樣點最接近且能與它無障礙連接的父節點，然后連接父節點與采樣點，將采樣點加入路徑樹，直至終點附近區域被搜索到。RRT*算法在RRT算法的基礎上重選父節點和剪枝重布線。首先，當采樣點Xnew加入節點樹后，以采樣點為圓心畫一個小圈，考慮在圈內是否有更好的父節點使起點到該節點的距離更短。若存在更加合適的父節點，就將它們連接起來，并去除原來的連線。然后，判斷在圈內是否存在某種節點，該節點經過Xnew節點到初始節點的代價比該節點到初始節點原路徑的代價小，若存在該類型節點，則將Xnew節點作為該節點的父節點，并斷開該節點與原父節點的連接。

RRT*算法的偽代碼如圖1所示，M為實驗地圖，T為節點樹，V為節點樹的節點，E為節點樹中節點之間的連線。圖2（a）對應圖1第4行，RRT*算法在圖中采樣得到節點Xrand。圖1（第5-6行）Xrand節點在節點樹中找到距離最近的節點Xnear（圖2（a）中為Xnew），若Xrand節點與Xnear 節點的距離小于StepSize，Xrand節點即為Xnew節點。若Xrand節點與Xnear節點的距離大于StepSize，則依據步長StepSize，沿Xnear到Xrand方向上的截取一段等于Step Size的距離得到一個新的Xnew節點。圖2（b）對應圖1第7-8行，以Xnew為圓心，StepSize為半徑畫圓，在圓內搜索節點樹中的潛在最優父節點，并保存到Ls之中。然后在Ls列表中找到Xmin節點，該節點保證從Xinit到Xmin到Xnew節點代價最小。圖2（c），（d）對應圖1第9～12行將Xnew節點與Xmin的關系加入到節點樹中，然后根據Ls列表進行剪枝重布線，該過程首先檢查Ls中每個節點x，如果從Xinit到Xnew到x的成本小于原來從Xinit到x的成本，并且可以將Xnew和x相連，則斷開x和原來父節點的連接，將Xnew作為x的父節點連接到樹中，并發布新的節點樹T。圖1（第13行）檢測是否到達目標點。

圖1 RRT*算法偽代碼Fig.1 RRT*algorithm pseudocode

圖2 RRT*算法重選父節點和重布線示意圖Fig.2 RRT*algorithm reselect parent node and rewiring schematic diagram

2 Informed-RRT*算法

Informed-RRT*算法在RRT*算法基礎上對采樣空間進行優化，偽代碼如圖3所示。圖3中，Cmin為起點到終點的距離，Cbest為規劃路徑的長度，初始化狀態下Cbest為0。如圖4所示，當得到第一條路徑后，計算路徑的長度Cbest，如式（1）～（2）所示，Cbest為橢圓兩個頂點的距離2×a，Cmin為橢圓兩個焦點的距離2×c。Informed-RRT*算法根據Cbest和Cmin兩個參數，得到一個橢圓的采樣空間。

圖3 Informed-RRT*算法偽代碼Fig.3 Informed-RRT*algorithm pseudo-code

圖4 Informed-RRT*采樣區域示意圖Fig.4 Schematic diagram of the Informed-RRT*sampling area

圖3第3行中，Informed-RRT*算法在路徑優化過程中的采樣空間為橢圓區域，而RRT*算法的采樣空間為整個區域，因此，Informed-RRT*算法隨著路徑不斷被優化，Cbest不斷縮短，采樣空間會不斷縮小，收斂速度不斷加快。

3 Informed-RRT*算法的優化

Informed-RRT*算法以當前節點到目標點距離的閾值判斷是否與目標點連線，然后結束路徑規劃進入路徑優化過程。實際環境中，存在雖然與目標點的距離大于閾值但仍然可以直接與目標點連線的新采樣點，面對此種情況，Informed-RRT*路徑規劃算法會繼續采樣，直到小于閾值的采樣節點出現時才進入路徑優化過程。相對而言，貪心算法會直接將新采樣點與目標點連線，然后結束路徑規劃進入路徑優化過程。除此之外，Informed-RRT*算法在路徑優化過程中，搜索潛在最優父節點時的搜索對象是整個節點樹，搜索過程中會消耗大量時間，嚴重影響路徑規劃效率。

3.1 貪心算法的引入

貪心算法以當前情況為基礎，根據某個優化目的做最優選擇，不考慮各種可能的整體情況，因此省去了為尋找最優解必須耗費的大量時間。

圖5對應圖6第13行，節點Goal為目標點，節點New_node為新得到的采樣點，Dalte為節點搜索障礙物的搜索半徑，Dis為采樣點到目標點的距離。如圖5（a）所示，原算法判斷找到目標點的依據為：New_node與目標點的距離Dis小于Dalte。規劃出第一條路徑后，算法結束路徑規劃過程，進入路徑優化過程。如圖5（b）所示，引入貪心算法的改進Informed-RRT*算法判斷是否找到目標點的依據為：新節點New_node能否直接與目標點連線。

圖5 搜索目標示意圖Fig.5 Schematic diagram of searching target

圖6 改進Informed-RRT*算法偽代碼Fig.6 Improved Informed-RRT*algorithm pseudo-code

3.2 搜索對象的優化

在Informed-RRT*算法的路徑優化過程中對潛在最優父節點的搜索對象是路徑規劃過程中構成的節點樹，潛在的最優父節點數量只有少數幾個，檢測過程中需要檢測所有節點，因此存在檢測多余節點會造成路徑規劃效率低下。本文考慮將搜索潛在最優父節點的搜索對象從節點樹替換成路徑。

代碼如圖6（第19～29行）所示。（第21行）定義規劃出的待優化路徑為path，在空間內采樣得到采樣點Xrand。若檢測出Xrand點在障礙物上，則該節點被舍棄，結束本次迭代。（第22行）若Xrand點不在障礙物上，則在path列表中查找離Xrand點最近的節點Xnear。（第23行）查到Xnear 后記為path（c），然后計算Xrand到path（c）節點的距離記為delta，若delta大于SizeStep，則沿path（c）到Xrand的方向截取SizeStep長度選取一個新的點記為Xnew，若delta小于SizeStep，則Xrand節點即為要更新的節點Xnew。（第24行）得到Xnew節點后，檢測path（c）是否在path列表的頭部或尾部，若在path列表的頭部或尾部，則Xmin為空；若不在，則Xmin非空。圖7對應圖6第26～27行，若Xmin非空，則比較Path（b）-Xnew-Path（d）的長度與Path（b）-Path（c）-Path（d）的長度，若Path（b）-Xnew-Path（d）長度小于Path（b）-Path（c）-Path（d）長度，則用Xnew節點代替Path中的Path（c）節點，剪枝布線獲得新路徑，然后重新計算Cbest，進一步縮小采樣空間。

圖7 改進Informed-RRT*算法路徑優化示意圖Fig.7 Schematic diagram of improved Informed-RRT*path optimization

4 模擬仿真實驗

為了驗證改進算法的有效性，本文采用MATLAB軟件進行路徑規劃仿真實驗。實驗仿真區域為900 cm×700 cm的矩形，起始位置Start為（100 cm，240 cm），目標位置Goal為（800 cm，150 cm）。圖8中方框表示環境內的障礙物，圓圈表示路徑規劃過程中采樣得到的節點。實驗環境根據障礙物的數量分為簡單，較復雜，復雜3個等級。從規劃路徑的長度與時間兩方面對改進前后兩種路徑規劃算法的實驗結果進行對比。

由圖8可以看出，細實線為第一次規劃出的路徑，粗實線為在第一條路徑基礎上的優化路徑。仿真結果表明，由于改進Informed-RRT*算法融合了貪心算法思想，因此在采樣點數量上有大幅減少。

圖8 改進前后兩種算法在不同環境下規劃的路徑Fig.8 Paths planned by the two algorithms before and after improving in different environments

表1為改進前后兩種路徑規劃算法在3種實驗環境下規劃路徑的長度。表中實驗1，2，3分別為簡單環境，較復雜環境，復雜環境下的仿真實驗。路徑1為改進前后兩種算法規劃出的第一條路徑，路徑2為改進前后兩種算法在路徑優化過程中得到的路徑。實驗結果表明，在3種不同復雜程度的仿真實驗環境下，兩種路徑規劃算法規劃出的第二條路徑均比第一條路徑縮短1～2 m。對比改進前后兩種算法規劃路徑的長度，改進Informed-RRT*算法在路徑規劃與路徑優化過程中得到的兩條路徑的長度均比原算法規劃出的兩條路徑縮短10%～20%。

表1 改進前后兩種算法在不同環境中路徑長度的實驗結果Tab.1 Experimental results of path length of the two algorithms before and after improving in different environments

表2為改進前后路徑規劃算法在3次實驗中規劃路徑消耗的時間。在簡單實驗環境下，改進算法在路徑規劃過程中消耗的時間比原算法消耗的時間縮短了88.93%；在較復雜實驗環境下，改進算法在路徑規劃過程中消耗的時間比原算法消耗的時間縮短了89.53%；在復雜實驗環境下，改進算法路徑規劃消耗的時間比原算法路徑規劃消耗的時間縮短了83.22%。實驗結果表明，在3種實驗環境下改進Informed-RRT*路徑規劃算法在路徑規劃過程中消耗的時間比原算法消耗的時間縮短80%～90%。

表2 改進前后兩種算法在不同環境中路徑規劃耗時的實驗結果Tab.2 Experimental results of path planning time of the two algorithms before and after improving in different environments

5 結 語

本文針對Informed-RRT*路徑規劃算法存在的問題，從路徑規劃和路徑優化兩個方面對Informed-RRT*路徑規劃算法進行優化。在3種不同復雜度的環境下進行仿真實驗，結果表明，改進Informed-RRT*算法在規劃路徑長度與規劃路徑時間兩方面均優于原算法，這為提高機器人自主導航效率的研究提供了理論基礎。