基于改進Bi-RRT的移動機器人路徑規劃算法
2022-06-01 13:17:28崔春雷陳詩豪沈超航
計算機測量與控制 2022年5期
崔春雷，陳詩豪，沈超航，李 鋒
(廣東交通職業技術學院，廣州 510650)
0 引言
隨著科技水平的進步，移動機器人越來越多的被應用到日常生活和工作的多種場景中，如目標的移動、探測和清潔等。 在移動機器人的研究領域中，路徑規劃算法屬于重點研究方向。移動機器人路徑規劃問題可定義為: 在一個存在障礙物的空間里，給定移動機器人的起點和終點，在遵循時間最短、路徑最優以及機器人運動學規律等一系列約束條件下，找到一條與障礙物無碰撞的路徑。其中環境完全已知的規劃問題屬于全局路徑規劃，環境部分已知的規劃問題為局部路徑規劃。常見的路徑規劃算法包括：A*算法、人工勢場法、可視圖法、概率路圖算法、模擬退火算法、粒子群算法、蟻群算法、遺傳算法等。以上算法往往需要事先對狀態空間內的障礙物進行環境建模，導致計算復雜高，收斂速度過于緩慢。
LaValle等人在1998年提出了經典的快速搜索隨機樹 (RRT，rapid-exploring random tree)算法。RRT算法的搜索過程通過隨機采樣的方式把搜索樹導向空白區，通過對空間中的采樣點進行碰撞檢測，從而避免了對空間的建模，該算法具有概率完備、計算量小、效率高等優點，能有效地解決高維空間以及復雜環境下的路徑規劃。然而 RRT 算法也存在一些不足，如需要在全圖進行采樣與搜索，會產生較多不相關的節點，從而增加了算法對內存的需求，并且也增加了相應的搜索時間，導致收斂速率較緩慢。針對RRT算法存在的缺陷，國內外眾多學者紛紛進行了研究，提出了多種改進的RRT算法，如C.Urmson等在文獻[16]發表了一個基于啟發式的偏向算法，它將搜索樹向目標點的位置進行了偏移，引導搜索樹向目標區域生長，使路徑搜索時間進一步優化。針對原始RRT算法中搜索得到的路徑并非最優的問題，Karaman等在文獻[17]中提出了具有漸進最優性的RRT*算法，該算法增加了父節點的重選和剪枝兩個優化過程，但此算法也存在收斂速度較慢的缺點。為了提高搜索速度，Kuffne等人先后提出了RRT-connect算法和雙向搜索樹(Bi-RRT)算法。Bi-RRT算法從起點和終點同時出發，并行生成兩棵RRT樹，直至兩棵樹相遇，相較于RRT算法，Bi-RRT算法的收斂速度更快，但Bi-RRT算法采用的仍是RRT算法的隨機節點擴展思想，導致路徑搜索過程存在無目標導向性等缺點。
為了解決雙向搜索樹(Bi-RRT)算法在路徑搜索時無目標導向性所導致的搜索效率過低的問題，本文提出了一種基于高斯采樣的改進Bi-RRT算法。該算法在雙向搜索的基礎上，引入啟發式搜索思想，采樣點以一定概率以高斯分布的方式被約束在起點與終點周邊，從而降低搜索的盲目性，提高了搜索的效率。通過仿真實驗表明：在多種類型的復雜環境中，相對于基本的Bi-RRT算法，該算法搜索過程中擴展節點更少、收斂速度更快、路徑相對更優。
1 Bi-RRT算法
RRT算法中隨機樹的生長缺乏目標導向性，導致大量冗余節點出現，算法的收斂速度較慢，Kuffner提出的雙向RRT算法(Bi-RRT)則較好的解決了這個問題。Bi-RRT算法的主要過程為：以起始點q
和目標點q
為根節點分別構建兩棵隨機搜索樹T
和T
，樹T
以q
為樹的根節點進行擴展，樹T
以q
為樹的根節點進行擴展，直到兩棵樹相遇。具體過程如圖1所示，算法首先在整個搜索空間中生成隨機擴展節點g
，然后遍歷隨機樹T
和T
，找出兩棵樹中距離q
最近的節點并記為q
，接著由q
出發向q
延伸步長δ
得到新節點q
，之后對新生成的節點q
進行碰撞檢測，若檢測到q
與障礙物碰撞則舍棄該節點,反之將q
添加到樹中，此時q
是的q
父節點；通過對上述過程進行迭代，使兩棵搜索樹不斷向著對方擴展，直到兩棵樹各自的q
之間的距離小于設定的閾值，此時認為兩棵樹相遇，路徑規劃成功。
圖1 Bi-RRT算法節點擴展過程
相對于RRT算法，Bi-RRT算法效率更高搜索速度更快，但是Bi-RRT算法采用的仍然是 RRT算法的隨機擴展節點思想，這也導致Bi-RRT算法和RRT算法一樣存在著構型無目標導向性的缺點。
2 基于高斯采樣的改進Bi-RRT算法
由于RRT算法和Bi-RRT算法在路徑搜索過程中缺乏目標導向性，導致搜索過程的隨機性較大，搜索樹往往會擴展到遠離我們所期待的目標區域的‘無用區域’，浪費了大量的計算資源，需要較長時間才能找到可行路徑，且路徑的代價往往較大。為此，本文提出了一種基于高斯采樣的Bi-RRT算法，該算法的核心思想在于引入啟發式搜索思想，隨機擴展節點q
不再以均勻分布的形式在搜索空間內隨機出現，而是以一定概率以高斯分布的方式出現在目標點周邊，這樣搜索過程不但可以引導搜索樹盡可能朝著目標區域前進，同時也保留了RRT算法的空間搜索能力，算法不僅提高了搜索效率，得到的路徑也相對更優。2.1 二維高斯分布性質分析
(1)
其中:μ
,σ
為分量X
的期望值與標準差，μ
,σ
為分量Y
的期望值與標準差，ρ
值決定了X
和Y
的線性相關程度。這里假定μ
=μ
=0，σ
=σ
=5，來觀察ρ
取不同值時二維隨機變量(X
，Y
)概率密度函數的圖像。
圖2 ρ值與二維高斯分布概率密度函數圖像關系
利用二維高斯分布的上述特性，我們引入啟發式搜索策略，隨機擴展采樣點q
不再以均勻分布的形式出現搜索空間內，而是被二維高斯分布函數約束在起點與終點周邊區域，并引導搜索樹朝著該區域方向生長。采用這種策略，在越接近目標點的空間，采樣點q
的出現概率越大，但是又不會把概率完全鎖死在目標點本身，這樣不但可以啟發隨機搜索樹向著目標區域生長，提高搜索的效率，同時又能以一定的概率繞過障礙物。2.2 高斯分布隨機采樣點的生成方法
本文算法采用雙采樣點策略，即用采樣點q
來引導隨機樹T
的生長，用采樣點q
來引導隨機樹T
的生長，而q
和q
則分別由以起點q
和終點q
為中心的兩個二維高斯分布函數來分別來約束其生成。
圖3 坐標系轉換示意圖
(2)
其中式(2)中的(x
，y
)為q
在坐標系OXY
中的坐標。按相同方法，我們可以生成以q
中心點隨機采樣點q
(x
，y
)。由圖4可以看出隨機采樣點q
主要分布在起點附近區域，且越接近點，其出現的概率越大，q
則主要分布在終點q
附近區域，越接近點q
，其出現的概率也越大。
圖4 隨機采樣點的概率分布示意圖
2.3 改進Bi-RRT算法實現
算法1給出了改進Bi-RRT算法的偽代碼，算法過程如下：首先以起點q
和終點q
為根節點分別構建隨機樹T
和T
(第1-2行)； 然后先以q
為目標通過算法2生成隨機點q
用來引導T
的生長，找到隨機樹T
中離q
最近的點q
，以q
為起點向q
方向延伸步長δ
生成葉子節點q
，判斷q
與q
之間是否存在障礙物，若不存在障礙物則將q
作為T
的子節點，并添加邊(q
,q
)(第4-9行)；接著以q
為目標通過算法2生成隨機點q
用來引導T
的生長，按照相同的規則生成葉子節點q
和邊(q
,q
)(第10～15行)；最后如果q
和q
之間的距離小于設定的閾值s
，且兩者之間沒有障礙物，則判定兩棵樹相互連接，路徑規劃完成(第16～17行)。算法1：改進Bi-RRT算法偽代碼
1)V
←{q
};E
←Φ
;T
←(V
,E
);2)V
←{q
};E
←Φ
;T
←(V
,E
);3)fori
=1 toN
do;4)q
←sanple(q
);5)q
←Nearest(T
,q
);6)q
←Steer(q
,q
);7)if ObstacleFree(q
,q
)then8)V
←V
∪{q
};9)E
←E
∪{q
,q
};10)q
←Sample(q
);11)q
←Nearest(T
,q
);12)q
←Steer(q
,q
);13)if ObstacleFree(q
,q
)then;14)V
←V
∪{q
};15)E
←E
∪{q
,q
};16)if (Dis tan ce(q
;q
)q
)) then17)Return(T
,T
)。為了平衡搜索過程中的隨機性與目標導向性，隨機點q
的產生由3種機制共同決定，見公式(3)。設置兩個目標偏置概率p
和p
，且滿足0<p
<p
<1。
(3)
公式(3)中p
為0～1之間的一個隨機數；當p
≤p
時，q
由2.2中的方法產生，即隨機點q
滿足二維高斯分布特性；當p
≥p
時，隨機采樣點q
為目標點q
或q
本身，這樣可以充分利用目標點的信息，使得搜索樹以一定概率朝著目標點方向快速前進；當p
<p
<p
時，q
符合均勻分布，這樣可以讓一部分隨機點保留采樣時的隨機性，確保搜索過程在概率上的完備性，保障搜索樹能跳出局部最優；算法2給出了公式(3)的偽代碼。算法2：隨機點生成的偽代碼
P
() //生成0～1之間的隨機數p
ifp
≤p
q
=q
. //q
可取q
或q
else ifp
>p
q
=q
;//此時q
按均勻分布elseq
=q
;//此時q
按二維高斯分布end if return(q
)3 仿真與分析
為了驗證算法的有效性，在配置window10系統，主頻3.40 GHz，內存16 GB的PC機上，采用Matlab 2020a對算法進行了編程仿真實驗。仿真時設置的參數如下：仿真空間尺寸為500×500，起點q
坐標為[1，1]，終點q
坐標為[500，500]，步長δ
=15，兩樹之間的距離閾值s
=30，目標偏置概率p
=0.6，p
=0.9，二維高斯分布的參數為σ
=σ
=0.
25d
，ρ
=0.
5。3.1 算法性能分析
仿真時按照障礙物的分布類型設置了3種有代表性的環境：簡單環境、復雜環境、迷宮環境。分別在每一種環境下對Bi-RRT算法和基于高斯采樣的改進Bi-RRT算法的性能進行對比。具體過程為：在每一種環境下，分別對兩種算法各進行50次仿真測試，并使用路徑長度(L
)、擴展節點數目(n
)、路徑規劃時間(t
) 這3個參數作為性能指標，并求得這3個指標的50次仿真測試結果的均值，最后通過這3個指標對算法性能進行定量分析。
圖5 環境1:簡單環境
圖6 環境2:復雜環境
圖7 環境3:迷宮環境
圖5為簡單環境中兩種算法的表現，圖6為充滿障礙物的復雜環境中兩種算法的表現，圖7為通道狹窄曲折的迷宮環境下兩種算法的表現。從圖5～7可以明顯看出，基本Bi-RRT算法采樣過程因為缺乏目標導向，采樣點分布過于隨機，導致搜索樹往往會在“無用區域”浪費過多資源，產生過多的無用節點，而本文改進算法引入啟發式搜索思想，充分利用了目標點的位置信息，讓隨機點不再“盲目”出現，而是以高斯分布的形式出現在目標點附近的空間，搜索過程所需的隨機采樣點的數量更少，效率也更高。
如表1所示，對圖5這種簡單環境，本文算法的額外的擴展節點更少，路徑也更平滑，路徑長度也更小。對圖6這種充滿密集障礙物的復雜環境，本文算法的優勢更加明顯，相對于基本Bi-RRT算法，平均規劃時間縮短了43.9%，平均擴展節點數目減少了41.4%，路徑長度優化了8.1%。對圖7這種富有挑戰性的充滿狹窄通道的迷宮環境，相對于基本Bi-RRT算法，改進算法的平均規劃時間縮短了30.9%，平均擴展節點數目減少了27.2%，路徑長度優化了2%。通過定量分析可知，本文提出的改進Bi-RRT算法的路徑搜索時間更短、擴展節點更少、路徑更優。
表1 不同環境下算法性能對比
3.2 目標偏置概率對算法性能的影響
改進算法中目標偏置概率p
,p
對算法的運行效率有著重要影響。由公式(3)可知采樣點由3種機制共同產生，分別在概率p
=p
下按二維高斯分布出現，在概率p
=p
-p
下按均勻分布出現，以概率p
=1-p
把起點(或終點)作為采樣點，顯然p
+p
+p
=1。為了簡化問題，保持p
=0.
1不變，此時p
=0.
9-p
，這里p
顯然表示的是按高斯分布出現的采樣點占總的采樣點數的百分比。下面來分析概率p
的值對算法性能的影響，這里仍然以圖6中的復雜環境作為測試環境，在保持其他仿真參數不變的情況下，測試p
取不同值時算法性能的表現。圖8為仿真測試得到的平均節點數目隨概率p
變化的曲線，從圖中可以看出，平均節點數目隨著概率p
的增加而減少，當p
=0.
7附近時達到極小值。圖9則為平均規劃時間隨概率p
變化的曲線，與圖8的規律一致，這里規劃時間也隨著概率p
的增加而減少，當p
=0.
7時規劃時間達到最小值，之后隨著p
的增加規劃時間又有所上升。可見，概率p
較小時，即按照高斯分布出現的采樣點占比少時，算法效率會降低；另一方面概率p
如果取到0.
9，即所有隨機點都按照高斯采樣時，則又因為缺少了自由采樣帶來的隨機性，算法效率也會降低，只有當p
取到合適值時，算法的效率才最高。
圖8 平均節點數目隨概率變化曲線
圖9 平均規劃時間隨概率變化曲線
4 結束語
為了改進Bi-RRT算法的效率，本文提出了一種改進Bi-RRT算法，該算法分別以機器人的起點和終點為概率分布的中心，構建二維高斯分布概率密度函數，并用該函數約束隨機采樣點的生成，同時也保留一部分均勻分布的采樣點，通過這些具有目標啟發性的采樣點來引導兩棵搜索樹快速向目標點生長并相遇。該算法在保證搜索過程概率完備性的前提下，提高了尋徑的效率和質量，相對于基本的Bi-RRT算法，本文算法在應對復雜環境、迷宮環境等環境時的表現更佳，如在復雜環境下規劃時間縮短了43.9%，擴展節點數目減少了41.4%，路徑長度優化了8.1%，最后分析了目標偏置概率對算法性能的影響，找到了使算法效果最優時對應的高斯分布采樣點的占比。未來可以考慮將本文算法結合RRT*算法，并應用到3維以上的高維空間中的路徑規劃問題。
猜你喜歡
中老年保健(2021年12期)2021-08-24 03:30:40
中國傳媒大學學報(自然科學版)(2021年1期)2021-06-09 08:43:00
甘肅教育(2020年14期)2020-09-11 07:57:42
中國生殖健康(2020年6期)2020-02-01 06:28:50
中國生殖健康(2019年11期)2019-01-07 01:28:02
領導決策信息(2018年50期)2018-02-22 06:17:16
商周刊(2017年5期)2017-08-22 03:35:26
中國衛生(2016年2期)2016-11-12 13:22:16
中國工程咨詢(2016年4期)2016-02-14 07:28:28
時代英語·高二(2015年1期)2015-03-16 00:08:11
