融合改進RRT和動態(tài)窗口法的路徑規(guī)劃算法

辛鵬，馬希青

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

0 前言

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

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

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

1 改進RRT算法

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

1.1 增加評價函數(shù)

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

(1)

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

圖1 無障礙下改進RRT算法與傳統(tǒng)RRT算法對比

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

圖2 有障礙下改進RRT算法與傳統(tǒng)RRT算法對比

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

1.2 路徑優(yōu)化

改進RRT算法規(guī)劃的路徑中仍存在大量的冗余節(jié)點及路段，在路徑中提取關(guān)鍵節(jié)點，去除冗余節(jié)點，并重新規(guī)劃路徑。實現(xiàn)步驟為：

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

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

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

圖3 優(yōu)化步驟

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

表1 改進RRT算法與優(yōu)化后路徑對比

2 改進動態(tài)窗口法

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

2.1 機器人運動學(xué)模型

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

(2)

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

2.2 速度采樣

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

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

ω∈[ωmin，ωmax]}

(3)

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

(4)

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

(5)

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

2.3 改進評價函數(shù)

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

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

(6)

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

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

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

(7)

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

傳統(tǒng)動態(tài)窗口法與改進動態(tài)窗口法的路徑規(guī)劃結(jié)果如圖4—圖5所示，對比結(jié)果如表2所示?？梢钥闯觯合噍^于傳統(tǒng)算法，改進算法路徑長度減少了0.462%，沒有較大變化；路徑規(guī)劃時間縮短了19.021%，改進評價函數(shù)提升了搜索效率。

圖4 傳統(tǒng)動態(tài)窗口法 圖5 改進動態(tài)窗口法路徑

表2 傳統(tǒng)動態(tài)窗口法與改進動態(tài)窗口法時間對比

3 融合算法

傳統(tǒng)動態(tài)窗口法在搜索過程中沒有指引，容易陷進局部最優(yōu)。改進的RRT算法規(guī)劃的路徑不夠平滑，機器人在移動過程中容易與障礙物發(fā)生接觸。

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

圖6 融合算法實現(xiàn)流程

4 仿真與分析

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

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

圖7 傳統(tǒng)RRT算法路徑 圖8 改進RRT算法路徑

圖9 傳統(tǒng)動態(tài)窗口法路徑 圖10 融合算法路徑

表3 4種算法對比

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

實驗二：在優(yōu)化改進RRT算法規(guī)劃的路徑中添加臨時障礙物，臨時障礙物為圖11所示的空心柵格，以驗證融合算法是否能夠?qū)崿F(xiàn)避障。

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

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

圖13 機器人整體運行軌跡

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

圖16 機器人姿態(tài)變化

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

5 結(jié)論

在有障礙物與無障礙物的環(huán)境下分別將優(yōu)化改進RRT算法與傳統(tǒng)RRT算法進行比對，驗證了改進算法的高效性。為了使算法能夠?qū)崿F(xiàn)避障，將優(yōu)化改進RRT算法與改進動態(tài)窗口法相結(jié)合，經(jīng)過實驗驗證算法的有效性，并且在規(guī)劃的路徑中加入臨時障礙物時，移動機器人也能很好地避開。