融合改進RRT和動態窗口法的路徑規劃算法

辛鵬，馬希青

(河北工程大學機械與裝備工程學院，河北邯鄲 056038)

0 前言

移動機器人可以完成自主移動，自行從起始點到目標點完成相應工作，能夠為工業生產和人們的生活提供較大的便利，因此，移動機器人具有較為廣泛的研發前景。路徑規劃在開發中占有主導方向。如何高效地找到從起始點到終點的安全路徑是路徑規劃的重點[1-3]。常見的規劃算法有：遺傳算法[4]、A*算法[5]、人工魚群算法[6]、蟻群算法[7]等。

快速搜索隨機樹(Rapid-exploration Random Tree，RRT)是較為常見的一種路徑規劃算法，適用于二維和三維空間。但是由于搜索過程中隨機樹的擴展方向是全方位的，因此不能很好地向目標點靠近，搜索效率較低。目前，對于RRT算法已經有了較多的研發和探索。文獻[8]提出將跳點思想融入到RRT*算法中，縮小搜索節點數量，提高搜查效率，并且在ROS中構建仿真環境，驗證算法高效性。文獻[9]使用雙向RRT思想，并對改進算法生成的路徑設置評價函數，從中選出最優路徑，并對選出的路徑平滑調整，通過仿真驗證算法有效性。文獻[10]針對隨機樹生長的隨機性進行改進，引導它向目標點生長且能夠根據周圍環境做出調整，算法搜索具有很強的目標性和偏向性。文獻[11]調整了搜索方向并約束搜索角度，對改進后規劃的節點使用B樣條優化。

針對RRT算法低效，規劃的路徑并不能實現路徑最優且不適合機器人移動的問題，本文作者將改進RRT算法與動態窗口法融合。在全局路徑中，調整擴展方向，提取路徑中關鍵節點，并進行重新規劃，提高搜索效率，減少路徑長度。在局部路徑中，通過改進RRT算法引導動態窗口法，防止陷入局部最優。

1 改進RRT算法

RRT算法可應用于二維與三維空間中。通過在地圖中不斷隨機采樣，增加樹上的葉子從而逐漸擴展隨機樹，當樹葉擴展到目標點或者樹葉距離目標點在一定范圍之內，可以在隨機樹中找到一條從起始到目標點的無碰撞路徑。因為RRT算法擴展點是隨機在地圖上生成，因此在搜索路徑中存在較大隨機性，不能高效找到路徑，并且路徑中有大量的冗余路段和冗余節點。針對這些問題，提出以下兩點改進。

1.1 增加評價函數

傳統RRT算法在擴張時是隨機的，會有很大的不確定性，因此，應對擴展方向進行限制。如式(1)所示：

(1)

其中：Ppoint為實際生成的隨機點；S為以起點和目標點為對角線的矩形面積;Nobs為矩形中障礙物的數量;Prand為地圖中的隨機點；Pgoal為目標點。在搜索過程中，當起始點與目標點之間沒有阻礙時能夠快速搜索到目標點，無障礙時與傳統RRT算法對照如圖1所示。

圖1 無障礙下改進RRT算法與傳統RRT算法對比

當路徑存在障礙物時，按照公式擴展隨機點；當路徑中障礙物過少時，路徑偏向目標點，會存在找不到路徑的情況，此時以當前點為起點，進行隨機搜索；存在障礙物的情況下，改進算法與傳統算法的對照如圖2所示。

圖2 有障礙下改進RRT算法與傳統RRT算法對比

通過圖2可以清晰地看出：在起點與終點相同的情況下，改進RRT算法生成的路徑長度更短，隨機樹的分支更少，在搜索路徑的過程中更有目的性。

1.2 路徑優化

改進RRT算法規劃的路徑中仍存在大量的冗余節點及路段，在路徑中提取關鍵節點，去除冗余節點，并重新規劃路徑。實現步驟為：

(1)將所有節點依次擺放進集合中{P1,P2,P3，…，Pn}。

(2)從起點開始依次直線連接路徑中的節點，直到連線點Pt+1中存在障礙物，此時Pt為關鍵點，并以它為起始點，繼續尋找下一個關鍵點，直到關鍵點與目標點連接中無障礙物。

(3)將目標點與起始點都放入關鍵節點中，使用關鍵節點重新規劃出路徑。優化步驟如圖3所示。

圖3 優化步驟

從表1中可得：優化后，路徑長度從28.970 6減少到25.200 3，優化13.014%；路徑拐點從15個減少到3個，優化了80%。但優化后路徑不夠平滑，不適合移動機器人的移動，且不能在局部路徑中實現避障。因此，將全局算法與動態窗口法相結合，實現全局最優。

表1 改進RRT算法與優化后路徑對比

2 改進動態窗口法

動態窗口法的思想在于對采集到的多組速度數據進行提取，并根據提取出的速度按照分辨率模擬出下一個時間節點內的所有路徑，按照一定的評價體系對路徑進行評分。傳統動態窗口法評價函數固定且單一，不能使機器人快速向目標點移動，因此對動態窗口法的系數進行動態調整。

2.1 機器人運動學模型

機器人在地圖中Δt時間內的運動一般分為圓弧和直線，為了方便計算，假定路徑為直線。機器人移動中角速度為ω，線速度為v，機器人在Δt內的運行軌跡如式(2)所示：

(2)

根據在速度空間中采集到的線速度和角速度，預測在下一個時間段內機器人的運行軌跡。

2.2 速度采樣

在速度空間存在大量速度數據，但考慮到機器人自身的組成以及環境對機器人的限制，因此對機器人的角速度和線速度進行限制，如式(3)：

vm={(v,ω)|v∈[vmin，vmax],

ω∈[ωmin，ωmax]}

(3)

機器人在運行中受到電機力矩等的限制，會對機器人的移動速度造成約束，因此在真實環境中的速度為

(4)

為使機器人在碰到障礙物之前停止，對機器人的移動速度設置上限：

(5)

式中：dist(v,ω)表示在當前運行速度移動機器人的軌跡中與障礙物最短的距離。

2.3 改進評價函數

在采集的速度空間中存在多個數據，不同速度對應不同的軌跡，對軌跡按照標準進行評價，評價函數如式(6)所示:

G(v,ω)=σ[α·head(v,ω)+β·dist(v,ω)+γ·vel(v,ω)]

(6)

其中：head(v,ω)表示當前移動下，移動機器人預估方向與目標方向的偏差；vel(v,ω)用來評價移動機器人當前移動速度；σ為平滑函數；α、β、γ為系數。

式(6)中α與γ和β比例值固定，機器人在移動時不能更具有目的性。此時對系數比例進行調整，使它在移動過程中動態改變:當距離目標點距離較遠時，提高方向占比，快速向目標點行駛；當與障礙物接近時，提高速度占比，快速繞過阻礙。改進評價函數為式(7)：

[2-dist(v,ω)/2Rγ·vel(v,ω)+2β·dist(v,ω)]}

(7)

其中：R為障礙物半徑。為了防止路徑對無障礙物的判定占比過大，對dist(v,ω)進行限定，dist(v,ω)∈(0,2R)。

傳統動態窗口法與改進動態窗口法的路徑規劃結果如圖4—圖5所示，對比結果如表2所示。可以看出：相較于傳統算法，改進算法路徑長度減少了0.462%，沒有較大變化；路徑規劃時間縮短了19.021%，改進評價函數提升了搜索效率。

圖4 傳統動態窗口法 圖5 改進動態窗口法路徑

表2 傳統動態窗口法與改進動態窗口法時間對比

3 融合算法

傳統動態窗口法在搜索過程中沒有指引，容易陷進局部最優。改進的RRT算法規劃的路徑不夠平滑，機器人在移動過程中容易與障礙物發生接觸。

針對這2個問題，將改進RRT算法規劃的路徑分段使用動態窗口法。在整體路徑中，使用改進RRT算法引導動態窗口法進行規劃。在局部路徑中，因為分成小段，因此很好地避免了路徑陷進局部最優。融合算法的實現如圖6所示。

圖6 融合算法實現流程

4 仿真與分析

為了驗證融合算法高效性，在MATLAB中構建柵格地圖，黑色區域表示不可通行，白色區域表示可通行。起始點(4,3)，目標點(24,17)。設計2組實驗進行驗證。

實驗一：驗證優化改進RRT算法的搜索效率和融合算法的可行性，如圖7—圖10所示。4種算法性能對比如表3所示。

圖7 傳統RRT算法路徑 圖8 改進RRT算法路徑

圖9 傳統動態窗口法路徑 圖10 融合算法路徑

表3 4種算法對比

如表3所示：對比傳統RRT算法，優化改進算法路徑縮短了40.54%，規劃時間減少59.68%，路徑中拐點減少了86.36%；傳統動態窗口法陷入局部最優未能找到路徑，融合算法由改進RRT算法規劃的路徑指引，能夠有效到達目標點，且融合算法規劃的路徑更短。

實驗二：在優化改進RRT算法規劃的路徑中添加臨時障礙物，臨時障礙物為圖11所示的空心柵格，以驗證融合算法是否能夠實現避障。

在原有路徑中加入臨時障礙物后，移動機器人的移動以及避障情況如圖11—圖13所示。移動中角速度、線速度和姿態如圖14—圖16。

圖11 移動機器人避開第一個障礙物 圖12 移動機器人避開第二個障礙物

圖13 機器人整體運行軌跡

圖14 機器人運行線速度 圖15 機器人運行角速度

圖16 機器人姿態變化

由圖11—圖13可知：在路徑中加入臨時障礙物后，移動機器人成功從起始點到達目標點，且能夠很好地繞過臨時障礙物，實現避障。

5 結論

在有障礙物與無障礙物的環境下分別將優化改進RRT算法與傳統RRT算法進行比對，驗證了改進算法的高效性。為了使算法能夠實現避障，將優化改進RRT算法與改進動態窗口法相結合，經過實驗驗證算法的有效性，并且在規劃的路徑中加入臨時障礙物時，移動機器人也能很好地避開。