移動機器人實時采樣路徑重規劃

涂 睿，王文格，盧成陽

湖南大學 機械與運載工程學院，長沙 410082

路徑規劃技術是實現移動機器人導航功能的關鍵技術[1]，影響著移動機器人導航過程中的效率和安全性。移動機器人的路徑規劃實質上是找到起點到終點的無碰撞路徑[2]，其路徑質量和規劃時間是評價規劃算法性能的重要指標[3]。

基于采樣的路徑規劃算法是近年來最具影響力的算法類型之一，通過隨機分布的采樣點作為啟發信息來逐步獲取地圖中的連通信息，直至獲取可行路徑解。其中最具代表性的是快速搜索隨機樹（RRT）[4]。RRT因其快速的節點拓展機制和強大的搜索性能而被廣泛應用到眾多領域[5-7]，其改進算法在靜態環境中的應用已比較成熟[8-10]。

移動機器人的工作環境往往是動態的（伴隨障礙物移動、突然出現和消失），移動機器人需要在工作環境中實時重規劃路徑以避開動態障礙物，但基于RRT的算法具有強隨機性，應用于動態環境會出現嚴重的路徑抖動問題。文獻[11]提出了一種具有路徑緩存功能的實時重規劃算法ERRT，每次重規劃會重新生長隨機樹并將之前的規劃路徑點作為啟發信息進行偏置采樣。文獻[12]提出了一種反向生長隨機樹的實時重規劃算法DRRT，將規劃終點作為隨機樹樹根，在路徑受阻時對與障礙物發生碰撞的樹枝進行修剪去除，并在此基礎上重新生長來獲取重規劃路徑。文獻[13]結合文獻[11-12]并增加了引力分量，提出了一種動態路徑規劃方法，減弱了重規劃路徑的隨機性。文獻[14]通過人工勢場引導加速隨機樹向目標區域生長并遠離障礙物。上述算法通過保留連通信息和增加引導信息來減少路徑抖動程度，但執行路徑的質量往往不是最優的。文獻[15]提出的RRT*算法具有概率完備性和漸近最優性，被用于搜索最優路徑。其路徑質量會隨著采樣次數的增加而不斷提升，但是其收斂速率會隨著采樣點增多而不斷下降。文獻[16]對RRT*算法的隨機采樣點進行了優化約束，加快了路徑收斂速率。為保證算法的實時規劃性能，需要對重規劃和路徑執行過程每步長的優化采樣次數進行限制，這會導致實時優化效果變差。文獻[17]將動態規劃問題轉化成了類局部規劃問題，在執行路徑被阻斷后，會重新規劃一段路徑繞開障礙物并連接到原路徑未阻斷部分的最近路徑點上，這會導致在從當前位置的規劃的路徑并不是最優的。

針對上述問題，本文提出了一種具備實時優化功能的重規劃算法DRT-RRT*。在路徑執行過程中首先采用路徑剪枝策略減少路徑的拐點數量，并通過組合采樣和局部終點跳動策略來優化當前待執行路徑段，聚焦優化目標來提高采樣點的利用率和路徑收斂速率；然后在路徑重規劃時結合目標偏置采樣和組合采樣策略提高路徑搜索速度和穩定程度；最后基于MATLAB進行對比仿真實驗，結果表明DRT-RRT*執行路徑代價更短，路徑質量更加穩定，實時優化效果更好。

1 反向生長RRT＊算法

1.1 基本RRT＊算法

RRT*算法在RRT算法基礎上增加了重選父節點和重新連接兩個步驟，使得算法具備了漸近最優性。圖1解釋了RRT*算法的漸近優化原理，圖中節點中的數字代表節點拓展順序，樹枝上的數字代表節點間的路徑代價。

圖1（a）表示當前新拓展節點xnew為節點7，其父節點xparent為節點4。圖1（b）表示在一定半徑范圍內獲取xnew的所有相鄰節點集合Xnear，并重新選取Xnear中使xnew到xstart路徑代價最低的節點2作為其新父節點xparent，以此來降低xnew的路徑代價。圖1（c）對xnew及所有鄰節點Xnear進行重新連接。如果從xstart經由xnew（節點7）到達Xnear（節點3～6）中的任意節點的總代價比xstart到Xnear中節點的當前代價要低，則將該節點的父節點重新指定為xnew，以此來降低Xnear中節點的代價。重新連接的有效半徑范圍由公式（1）決定：

圖1 RRT*漸近優化原理Fig.1 Asymptotic optimality principle of RRT*

式中，η為拓展步長，γRRT*是一個常數（它取決于規劃環境），n代表當前隨機樹中的節點個數，d代表規劃環境的維度。

隨著迭代次數不斷增加，重新連接半徑會逐漸減小并穩定下來。RRT*會在每一個新拓展節點生成后進行父節點重選和重新連接這兩個步驟，使路徑逐步向最優解收斂，但其收斂速率會因采樣點的增加而逐步下降。

1.2 反向生長隨機樹的剪枝和重新生長

移動機器人在動態環境中執行規劃路徑時，規劃路徑會隨著環境地圖的改變而發生變化。當待執行路徑受到阻斷時，從機器人位置重新生成一棵隨機樹并進行優化會占用較多計算資源，影響實時性能。于是將路徑終點作為隨機樹的樹根進行反向生長，如圖2（a）所示，起點為機器人當前位置，終點為隨機樹樹根。當規劃環境發生改變后，僅對少量被障礙物干涉的樹枝進行修剪，如圖2（b）、（c）所示。并以剩余隨機樹為基礎重新生長隨機樹，獲取重規劃路徑，如圖2（d）所示。這樣可以保留更多的隨機樹信息，減少未受影響部分路徑的抖動程度[12]。

圖2 隨機樹的剪枝和重新生長Fig.2 Pruning and regrowth of random tree

2 DRT-RRT＊算法

2.1 路徑剪枝處理

RRT*算法規劃的路徑由多段樹枝組成且會包含多個冗余樹節點，這會使得移動機器人在執行路徑的過程中頻繁轉向。文獻[18]提出了一種路徑剪枝的處理方式，通過三角不等式逐段比較，減少路徑長度和路徑中節點數量的同時減少路徑抖動程度。由于路徑拐點大量減少，因此可以將后續的路徑優化過程由對整條路徑的采樣優化轉變為對于拐點位置的優化，這為局部終點跳動策略提供了基礎。

如圖3所示，在初始路徑生成后，該方法會從終點開始向起點進行回溯，將當前節點node、其父節點nodeparent及其祖父節點nodeparent構建為三角形逐步進行修剪。如未與障礙物發生碰撞，則將nodeparent從路徑中刪除，將nodegrandparent作為當前節點node的新父節點nodeparent，同時將回溯的下一個路徑點作為新nodegrandparent繼續構建三角形進行新一輪的修剪。如與障礙物發生碰撞后，則將nodeparent作為當前節點node，回溯兩個路徑點作為其父節點nodeparent和祖父節點nodegrandparent重新構建三角形進行修剪，直到將起點添加進三角形作為nodegrandparent。

圖3 基于三角不等式的路徑剪枝Fig.3 Path pruning based on triangular inequality

2.2 組合采樣

在動態環境中重規劃發生的時間點是不確定的，如果沿用RRT*的隨機采樣策略優化整條執行路徑，很大概率會出現多段路徑未執行就已經發生變化的情況，這會導致對未執行路徑的優化失效，而且在全局范圍內進行隨機采樣會稀釋優化效果。而且為滿足實時性，動態環境中的路徑規劃會限制優化采樣次數，因此全局隨機采樣的實時優化效果會變得很不明顯。

為提高執行路徑的質量，將優化目標從整條待執行路徑轉變為機器人位置至最近路徑拐點的路徑段會更加有效。組合采樣通過增加當前路徑段和其局部終點附近的隨機樹節點密度來提高附近隨機樹的收斂程度，從而縮短當前路徑的長度和降低路徑抖動程度。隨機采樣點xrand的生成方式如公式（2）所示：

組合采樣的第一部分Around(xlocal_goal)會在以局部終點xlocal_goal為中心的圓內進行隨機采樣。在執行路徑優化過程中會對拐點附近的隨機樹進行優化以提高拐點質量，降低路徑代價。在進行重規劃時，局部終點附近的密集采樣點會增加擴展速度，加快重規劃路徑的生成；第二部分Bounded稱為有界采樣，會在機器人和局部終點的連線方向的區域內進行采樣，用于補充重規劃后執行路徑段附近因障礙物碰撞而被修剪掉的大量樹枝，同時提高沿途隨機樹枝的質量，對執行路徑進行實時修正，加速路徑收斂；第三部分Random為全局隨機采樣，用于獲取隨機樹稀疏區域的連通信息。由于稀疏區域拓展新節點時鄰節點較少，因此拓展速度會相對較快。拓展的樹枝會均勻地分布在地圖的自由空間中，為下一次重規劃提供部分樹枝基礎。同時隨機采樣還可以保證算法的概率完備性。

三種采樣方式的權重根據規劃環境以概率α和β進行分配。后續的仿真實驗中α取0.3，β取0.6。

2.3 局部終點跳動策略

RRT*算法的收斂速率會因節點聚集效應而逐漸下降，在接近最優解時收斂速率變得會非常緩慢，較小的質量提升都會花費很大的時間代價。局部終點跳動策略將局部終點模擬為微粒，使其在以自身為中心的一定半徑范圍內進行隨機運動，通過調整局部終點的位置來獲取更優的路徑解。這個過程無需新的采樣點的加入，減少了內存消耗的同時提高了路徑收斂的速率。圖4表示在規劃路徑質量較差時，移動機器人在路徑執行過程中通過局部終點跳動策略來實時修正路徑。

圖4 路徑實時修正過程Fig.4 Real-time path modification process

2.4 DRT-RRT＊實現

DRT-RRT*算法在規劃初始執行路徑時沿用了RRT*算法的節點拓展策略，同時引入了目標偏置采樣，一旦偏置采樣開始，只有在碰到障礙物或者到達終點才會停止，這加快了初始路徑的搜索。然后在隨機樹盡量布滿環境地圖后對執行路徑進行剪枝處理，分割為多段直線路徑，減少路徑拐點，如圖5（a）所示。

從圖5（b）～（i）可以看出，當移動機器人的待執行路徑被突然出現的障礙物阻斷時，DRT-RRT*會先對受影響的樹枝進行修剪，然后通過組合采樣策略快速修復隨機樹，獲取并優化重規劃路徑。為保證實時性，會對重規劃生成后的優化采樣次數進行限制，因此此時重規劃路徑不是最優路徑。DRT-RRT*會將優化任務分配到執行過程中，通過組合采樣和局部終點跳動策略實時修正當前執行路徑段，使其快速向最優路徑收斂。

圖5（j）、（k）表示環境地圖未改變時，執行路徑的實時優化過程。可以看出在圖5（i）中重規劃路徑之后，移動機器人會在路徑執行過程中不斷對未執行路徑進行實時優化，不斷提升待執行路徑段的質量。圖5（l）表示最終的執行路徑。

圖5 DRT-RRT*規劃過程Fig.5 Planning process of DRT-RRT*

DRT-RRT*通過基于三角不等式的路徑剪枝將多段曲折的執行路徑修剪為僅在障礙物處發生轉折的直線路徑，減少了大量的無效拐點，降低了組合采樣第二部分有界采樣和局部終點跳動策略的復雜度。組合采樣第一部分與局部終點跳動策略共同作用，對拐點位置進行逐步優化，以提高當前執行路徑的質量。相比于RRT*算法采用隨機采樣優化的策略，組合采樣結合局部終點跳動策略對于當前執行路徑段的優化效果更為明顯，在重規劃路徑較差時通過調整拐點位置來快速修正路徑，以此更好的保證執行路徑的質量一致性。DRT-RRT*算法流程如圖6所示。

圖6 DRT-RRT*算法流程圖Fig.6 Algorithm flow chart of DRT-RRT*

3 仿真實驗分析

為了驗證DRT-RRT*算法的性能，分別在存在陷阱的未知靜態障礙物環境1、需要進行連續重規劃的未知靜態障礙物環境2、存在未知動態障礙物的環境3進行了仿真分析，考慮到移動機器人的自身的尺寸問題，對障礙物的體積進行了膨脹處理。并將仿真結果與RRT*與本文優化的增加了三角不等式剪枝策略的RRT*-Pruning進行比較。仿真實驗程序在64 bit MATLAB 2016平臺中運行，電腦運行環境為Windows 10，配置為Intel i5-8400U@2.80 GHz CPU和4 GB RAM。

3.1 未知靜態障礙物環境規劃過程

未知靜態障礙物環境1中，障礙物為未知位置的靜態圓形障礙物。移動機器人需要隨著環境地圖的改變重新規劃執行路徑，如圖7（b）所示。地圖右邊界設置了陷阱，當移動機器人即將通過地圖右側邊界的通道到達終點時，將通道關閉，如圖7（c）所示。該環境用于測試各算法在隨機樹被大量剪枝后的重規劃性能。

圖7 環境1規劃過程Fig.7 Planning process in environment 1

在未知靜態障礙物環境2中，障礙物為連續放置的靜態圓形障礙物，用于頻繁地觸發重規劃，測試算法的抗抖動性能，如圖8所示。

圖8 環境2規劃過程Fig.8 Planning process in environment 2

3.2 未知動態障礙物環境規劃過程

未知動態障礙物環境3中，障礙物為2個橫向移動的未知圓形障礙物，如圖9（b）、（c）所示。在終點附近放置了兩個靜態障礙物用于大范圍阻斷隨機樹，用于測試各算法的重規劃性能，如圖9（d）所示。

圖9 環境3規劃過程Fig.9 Planning process in environment 3

3.3 性能比較

各算法進行規劃的環境均為[500，500]，各環境中規劃的過程如圖7～9所示。在靜態環境中初始路徑的采樣次數為1 000次，在動態環境中初始路徑的采樣次數為500次，執行路徑過程中每步長優化采樣次數為20次，各算法在對應環境中均獨立運行20次，取平均值作為性能參數。

圖10中地圖右側的通道關閉，移動機器人附近的隨機樹枝都會被修剪掉。RRT*和RRT*-Pruning會因隨機樹信息的缺失而導致重規劃路徑的質量較差，且在優化采樣次數相同的情況下，實時優化效果不明顯，規劃的路徑會偏離最優路徑較遠。而DRT-RRT*能在隨機數信息較少的情況下快速獲取較高質量的重規劃路徑，并在執行過程中對路徑進行實時修正。

圖10 環境1規劃結果比較Fig.10 Comparison of planning results in environment 1

圖11中連續放置的障礙物會頻繁觸發重規劃。RRT*算法會出現嚴重的路徑抖動，路徑質量會變得很差。RRT*-Pruning因增加了路徑剪枝策略，路徑抖動程度會相對減小，但由于比較貼近障礙物，會被修剪掉更多的隨機樹枝，導致重規劃時間變長。DRT-RRT*會不斷對當前執行路徑段進行優化，以降低路徑的抖動程度，使執行路徑不斷向最優收斂。

圖11 環境2規劃結果比較Fig.11 Comparison of planning results in environment 2

圖12中較大的自由空間使RRT*和RRT*-Pruning的隨機采樣優化策略對于路徑的實時優化效果不明顯。而DRT-RRT*能更好地捕捉地圖信息，利用好障礙物離開后的自由空間以規劃出更優的執行路徑。

圖12 環境3規劃結果比較Fig.12 Comparison of planning results in environment 3

圖13為RRT*、RRT*-Pruning和DRT-RRT*在圖10～12所示環境中進行20次實驗后的路徑長度對比結果。結合表1中平均執行路徑長度參數可以看出DRT-RRT*在3種環境下的平均執行路徑長度均低于其他兩種算法。在規劃環境1中，相比于RRT*，平均路徑長度縮短了17.2%，相比于RRT*-Pruning，平均路徑長度縮短了5.1%。在規劃環境2中，相比于RRT*，平均路徑長度縮短了22.8%，相比于RRT*-Pruning，平均路徑長度縮短了3.7%。在規劃環境3中，相比于RRT*，平均路徑長度縮短了15.4%，相比于RRT*-Pruning，平均路徑長度縮短了5.1%。從圖13中各算法規劃路徑長度的波動程度和表1中的路徑長度標準差參數可以看出DRT-RRT*在3種環境下規劃路徑的穩定程度均優于其他兩種算法。由此得知DRT-RRT*所規劃的執行路徑質量更好且更加穩定，路徑執行過程的實時優化效果更好。

表1 路徑質量和穩定性參數對比Table 1 Comparison of path quality and stability parameters

圖13 不同環境下路徑長度對比Fig.13 Comparison of path length in different environment

組合采樣策略會在發生重規劃時將采樣點分布在移動機器人周圍，從而加快隨機樹向機器人位置的生長。相比于RRT*和RRT*-Pruning在地圖中進行隨機采樣，組合采樣的采樣點會在終點附近聚集，得到更多能與終點連通的潛在節點，從而提高重規劃效率，同時有利于后續的路徑優化過程。

通過表2可知，采用了組合采樣策略的DRT-RRT*在不同環境下的平均重規劃采樣數均低于其他兩種算法，提高了節點利用率。在規劃環境1中，相比于RRT*，平均重規劃采樣數降低了39.9%，相比于RRT*-Pruning，平均重規劃采樣數降低了43.2%。在規劃環境2中，相比于RRT*，平均重規劃采樣數降低了23.3%，相比于RRT*-Pruning，平均重規劃采樣數降低了40.3%。在規劃環境3中，相比于RRT*，平均重規劃采樣數降低了69.6%，相比于RRT*-Pruning，平均重規劃采樣數降低了70.8%。由于組合采樣需要對路徑進行剪枝和分割，會增加部分重規劃時間，但采樣數量明顯減少，使得整體上平均重規劃時間低于其他兩種算法。由此得知，DRT-RRT*的實時性更好，重規劃效率更高。

表2 實時性能參數對比Table 2 Comparison of real-time performance parameters

4 結束語

針對傳統的采樣規劃算法在動態環境中進行重規劃時出現的路徑質量差，抖動嚴重，實時優化效果不明顯等問題，提出了一種基于反向生長最優快速搜索隨機樹的實時重規劃算法DRT-RRT*。組合采樣和局部終點跳動策略增強了重規劃性能和執行路徑的實時優化性能，提高了路徑質量的穩定性。仿真實驗數據表明，提出的DRT-RRT*算法規劃效率更高，路徑質量更優，實時優化效果更好。