分級隨機采樣弱隨機RRT算法及在移動機器人運動規劃中的應用

鄭 維，張 濤，王洪斌，田亞靜，王洪瑞

(1.燕山大學 電氣工程學院，河北 秦皇島 066004；2.國網石家莊市藁城區供電公司， 河北 石家莊 052160; 3.河北大學 電子信息工程學院，河北 保定 071000)

1 引 言

自主機器人的運動規劃一直是熱門的研究領域，其目的是為機器人在空間中規劃出一條帶有控制序列的運動軌跡，使機器人能夠完成給定的運動行為。實現運動規劃的方法主要有兩類：基于圖論的方法[1,2]和基于采樣的方法?；诓蓸拥囊巹澐椒ū苊饬孙@式的構造構型空間，從而避免了“維度災難”問題，首先從狀態空間中隨機抽取樣本，然后檢查樣本的可行性，并選擇接受或拒絕該樣本?？焖贁U展隨機樹(rapidl expansion random tree,RRT)算法以其簡單高效的算法流程以及概率完備性的特點，在基于采樣的規劃方法中有較大優勢。

1998年，Lavalle S M[3]首先提出RRT算法;文獻[4]嚴格地分析隨機采樣算法返回的解的代價隨樣本數量的增加的漸近行為；文獻[5]針對動態環境，提出了一種基于采樣算法的實時動態規劃框架；文獻[6]在RRT算法的基礎上，提出了一種基于增量采樣的運動規劃算法，解決了集群環境下的近似最優運動規劃問題；文獻[7]結合方向相似性得多步擴展與貝塞爾曲線擬合生成初始解；文獻[8]提出了基于“懶惰”的最優快速探索隨機樹算法，能夠在靜態和動態環境中使用更新的信息進行實時重新規劃；文獻[9]引入自適應引力場以獲取采樣點，加快算法得收斂速度；文獻[10]提出了一種漸進優化的采樣算法，得到初始路徑后劃定子區域進行重接線，從而得到最優路徑；文獻[11]提出了Informed RRT*-Connect算法，在獲取初始解后進行重新抽樣，并僅接受更好的解，以此獲取最終解；文獻[12]使用雙樹搜索以加快搜索速度，獲取初始解后，對雙樹進行知情采樣，優化初始解；文獻[13]根據人工勢場法建立的勢場進行采樣，并使兩棵樹同時向前推進，直至相交,減少算法的迭代次數；文獻[14]引入了回歸機制，以防止過度搜索配置空間，提升算法規劃的成功率；文獻[15]所提出的算法中，采樣節點是由目標偏差采樣策略以可變概率確定的，可以在不同障礙物密度的區域中平衡搜索時間和安全性；文獻[16]使用神經網絡來預測成本函數，并設計了一種新的隨機搜索樹重構方法，以在配置空間中尋找接近最優的解決方案；文獻[17]結合了P-RRT*和Q-RRT*的優勢，確保快速收斂到最佳解決方案，并生成更好的初始解決方案；文獻[18]擴大了節點可能存在的父節點集合，從而生成更好的初始解；文獻[19]采用了更集中地偏向RRT樹形增長的方法，加快了算法的收斂速度；文獻[20]結合雙樹結構提出了一種RRT*的新穎方法，以分離擴展和優化過程；文獻[21]引入了雙向變體和啟發式搜索，提高了收斂速度，并具有更高的內存利用率；文獻[22]提出自適應混合動態步長和目標吸引力RRT，在縮短路徑的長度的同時加快了算法的求解速度。

基于上述研究內容可知，盡管加快算法求解速度，減少算法迭代次數已經成為RRT算法改進的重點，但大多數工作仍然是針對多樹搜索及減少迭代次數等方面進行的，而造成RRT算法求解速度慢的根本原因在于其擴展時進行的遍歷尋找最近節點所產生的大量計算。

針對移動機器人運動規劃過程中，RRT算法采樣效率低，尋找最近鄰節點計算量大和非線性反饋控制器不受系統模型動態約束等問題，提出一種新的基于分級隨機采樣與擴展的RRT算法，同時設計了新的快速限幅反饋控制器。最后，使用3種不同類型的地圖對移動機器人的運動規劃過程進行仿真驗證。結果表明：本文方法有效地縮短了RRT算法迭代后期遍歷尋找臨近節點的消耗時間，提高了系統的收斂速度，同時保證運動規劃過程中反饋控制器能夠始終滿足系統模型動態約束。

2 RRT算法

RRT算法迭代完成樹結構示意圖如圖1所示。

圖1 RRT算法迭代完成樹結構示意圖Fig.1 Schematic diagram of the iteration completion tree for the RRT algorithm

隨機樹結構初始化為

T=[ni|i=1,2,3,…,N]

(1)

式中：ni=(x,y,qparent)∈χfree表示節點,(x,y)表示ni節點的坐標，qparent表示ni節點的父節點。

ni是直接從配置空間中隨機選取，采用實值函數ρ:χ×χ→[0,∞)計算配置空間χ中點對點的距離來尋找ni最近的節點qnearest。采用碰撞檢測函數判斷自由空間χfree中是否存在xi；設D表示碰撞檢測函數輸出結果，當D為true時，表示χfree中存在xi，當D為false時，表示χobs中不存在xi，結束循環，進行下一次采樣與檢測。采用碰撞檢測函數判斷xi是否位于自由空間χfree中；當D為true時，表示xi處在χfree中，當D為false時，表示xi位于χobs中，結束循環，進行下一次采樣與檢測。

經過分析可得出：

(1)節點ni所攜帶的信息僅僅包含坐標與父節點之間的信息，并未攜帶更多有利信息，如當前節點與障礙物的相對關系、該區域節點數量、與目標區域的相對距離等。

(2)新節點是通過在配置空間χ中均勻采樣得到的，當配置空間中存在大量的采樣點時，才能保證采樣點在配置空間中的覆蓋率趨于1。但是如果T中存在大量的節點，尤其是在計算配置空間中點對點之間的距離時，會導致計算量激增。

(3)在RRT算法中，碰撞檢測函數與搜索父節點的過程都要通過實值函數ρ:χ×χ→[0,∞)來獲得距離參數。將新的采樣節點連接到樹中時需要遍歷采樣節點與樹中節點之間的距離，獲取最近節點作為父節點。隨著迭代次數n不斷增加時，執行該函數的次數呈指數型增長，從而導致計算量增加。

3 分級隨機采樣弱隨機RRT算法

本文提出了一種新的基于分級隨機采樣與擴展的弱隨機快速搜索隨機樹(RRT)算法，同時設計快速限幅非線性反饋控制器，在保證概率完備性的同時提高算法在迭代后期的搜索速度，進而提高算法在面對狹窄空間的搜索效率。

3.1 分級隨機采樣

針對RRT算法中由于均勻采樣引起的采樣結果不確定性問題，本文采用分級隨機選取的采樣方法，在保證概率完備性的同時減弱采樣點的隨機性，弱隨機樹結構初始化為

(2)

式中：先選擇樹中一節點作為子節點的父節點,priority_1,priority_2表示候選擴展節點集合；score_ni表示T中ni節點分數；a，b，c表示劃分不同優先級的分數節點；else表示節點不作為候選擴展節點。

比較公式(1)和公式(2)，本文提出的算法將T中節點ni按照節點分數劃分為不同的擴展優先級，priority_0,priority_1,priority_2。同時保證在劃分優先級時T中節點ni最多只能存在于一個優先級集合中。

與RRT算法相比，本文提出的方法進行擴展時，并不是先選擇隨機點再尋找其父節點，而是先分級隨機選取樹中父節點作為待擴展的節點，再弱隨機選擇子節點的擴展方向，最后通過計算確定子節點的位置坐標。在選取待擴展父節點時遵循分級隨機選取的原則，即若高優先級集合不為空，則在高優先級集合內隨機選取待擴展節點，若高優先級集合為空，則判斷中優先級集合是否為空。需要說明的是，每次迭代后更新待擴展點集合，當出現高中低3個優先級集合全部為空的情況，則表示此時采樣點已經在狀態空間中均勻分布，無需再次進行隨機擴展，else集合中的節點不作為待擴展節點的候選節點。

為了使選取的待擴展節點能夠順利的完成擴展任務，本文為隨機樹中的節點預分配了8個擴展方向：上、下、左、右、左上、右上、左下、右下。每個節點不再作為質點進行空間搜索，而是存在一定的搜索范圍，搜索范圍是以每個節點為中心，半徑為1/2R(R為步長值)的區域。與RRT算法相比，上述搜索方法提高了搜索效率。然而預分配的擴展方向并不全是可以擴展的，為了記錄節點擴展方向是否可以擴展，本文采用2×4維矩陣dir_mat記錄擴展方向，定義如下：

(3)

式中：dir_mat表示節點的方向屬性矩陣，矩陣元素1，2，3，4，5，6，7，8分別對應節點的8個擴展方向，不可擴展的方向包括兩種情況，第1種是位于障礙物邊界或地圖邊界的節點，第2種是節點附近已存在節點。

方向屬性矩陣與節點方向的對應關系如圖2所示。

圖2 方向屬性矩陣與節點方向對應關系Fig.2 Correspondence of direction attribute matrix and nodes direction

3.2 獲取子節點

為了獲取子節點，計算出各子節點的坐標，給出子節點的計算公式如下：

(4)

式中：ni_childj,j=1,2,3,…,8表示ni節點各方向子節點；x、y分別表示節點橫縱坐標；R為步長值。

依照待擴展集合優先級，優先在高優先級的擴展節點集合中隨機選取ni節點，確定ni的dir_mat屬性矩陣，隨機選取子節點的擴展方向，兩次隨機選取保證探索方向的隨機性。圖3給出ni節點方向子節點示意圖。

圖3 ni各個方向子節點示意圖Fig.3 Schematic diagram of the sub nodes in all directions for ni

圖3中ni節點可擴展的8個方向，ni節點坐標為(x,y)，步長值設置為R，α=45°，β=R·sinα進而根據公式(4)計算出各個子節點的坐標。

3.3 搜索臨近節點

在搜索臨近節點的過程中，由于子節點qchild方向的弱隨機性，會出現兩種臨近節點過度搜索的現象，如圖4所示。

圖4 臨近節點過度搜索示意圖Fig.4 Schematic diagram of the excessive search for the adjacent nodes

為了解決上述問題，本文對生成的qchild點進行臨近搜索判斷，同時為了避免大量的距離計算，提出一種新的判斷策略，將距離計算問題轉變為判斷qchild鄰域矩陣是否為0陣的問題，步驟如下：

第1步：初始化階段，將與地圖匹配的節點位置矩陣map_array初始化為0陣。

第2步：進行重復點判斷時，選擇合適的鄰域矩陣半徑，獲取以qchild點為中心的鄰域矩陣。

第3步：判斷qchild的鄰域矩陣是否為0陣，若為0陣，表示qchild點附近不存在節點，返回0，若不為0陣，表示qchild點附近已經存在節點，則不返回0。

第4步：成功擴展子節點后，更新節點位置矩陣map_array。

3.4 弱隨機RRT算法

弱隨機RRT算法具體步驟如下：

第1步：程序開始時，對程序進行初始化，包括樹的結構體與節點位置矩陣map_array。

第2步：依據節點待選集合的優先級，選取待擴展節點qparent。

第3步：查找待擴展節點的方向矩陣，隨機選擇待擴展節點qparent的可擴展的方向。

第4步：依照公式(3)，計算子節點qchild的位置，并對qchild進行臨近搜索判斷。若qchild附近沒有已存在的節點，則進行第5步，相反則返回第2步。

第5步：更新節點的待選集合與節點位置矩陣map_array

第6步：將子節點添加到樹的結構體中。判斷子節點是否位于目標區域，若位于目標區域，停止迭代，若子節點仍然位于目標區域之外，返回第2步。

圖5表示分級隨機采樣弱隨機RRT算法迭代示意圖，從圖中可以看出：采用分級隨機采樣弱隨機RRT算法進行空間搜索，能夠以較少的點探索到更多的區域，并且節點在空間中的分布均勻。

圖5 分級隨機采樣弱隨機RRT算法迭代示意圖Fig.5 Iteration schematic diagram of hierarchical random sampling weak random RRT algorithm

圖5(a)中迭代次數k=50，說明在迭代初期，新算法中樹結構向著未探索空間進行搜索。

圖5(b)中迭代次數k=200，說明在迭代中期，新算法能夠使用較少的點探索到較大的區域，如果地圖中障礙物較少，則此時能夠獲得初始解。

圖5(c)中迭代次數k=500，說明在迭代后期，絕大部分空間均被探索，此時樹中節點進入臨近飽和狀態。

圖6表示分級隨機采樣弱隨機RRT算法與RRT算法迭代次數與時間對比圖。從圖中可以看出當算法在迭代次數小于400時，兩個算法消耗時間相差不大，但是隨著迭代次數的增加，RRT算法消耗的時間要遠遠超出分級隨機采樣弱隨機RRT算法消耗的時間。這是因為當隨機點均勻分布在配置空間中時，分級隨機采樣弱隨機RRT算法待擴展點集合均為空，即使繼續迭代也不會再增加新的節點，縮短了消耗時間。

圖6 分級隨機采樣弱隨機RRT算法與傳統RRT 算法迭代次數與時間對比圖Fig.6 Comparison results of the iteration times and time between hierarchical random sampling weak random RRT algorithm and RRT algorithm

綜上所述，本文提出的基于分級隨機采樣的弱隨機RRT算法在降低了節點搜索過程中的時間消耗，與RRT算法相比，具有一定的優越性。

4 快速限幅非線性反饋控制器設計

文獻[23]基于非線性反饋控制器，提出了跨越多個路徑點時保持勻速來優化運動軌跡的方法。該方法可生成平滑的運動軌跡，且計算量低。非線性反饋控制器利用運動學模型可以引導機器人通過特定的路徑點從起點移動到終點，但存在不受系統模型動態約束的問題，故不能保證機器人的速度、加速度嚴格滿足機器人動態特性。針對上述問題，本文以差速輪式移動機器人為應用對象，采用分級隨機采樣弱隨機RRT算法，設計快速限幅非線性反饋控制器，保證機器人在運動規劃過程中滿足物理模型的動態約束。

4.1 非線性反饋控制器

差速輪式移動機器人模型如圖7所示，該機器人通過調節兩個動力輪的加速度控制兩動力輪的速度，實現機器人的前進、后退，左右轉向等運動狀態。該差速輪式移動機器人的運動學模型如下：

圖7 差動輪式移動機器人模型Fig.7 Model of the differential wheeled mobile robot

(5)

基于上述模型，設計非線性反饋控制器如下：

(6)

為了保證機器人運動的穩定性，非線性反饋控制器應滿足以下條件[23]:

(7)

由公式(6)可知，在運動開始時，由于機器人位于初始位置，故此時ρ為最大值，導致機器人的水平速度v在下一時刻瞬間達到速度最大值。然而移動機器人的水平速度受其水平加速度的約束，所以會出現機器人在起始時刻擺脫其物理模型約束的行為。對于這一問題，本文提出了將控制器中的平移速度與角速度作為機器人當前期望速度方法，并考慮機器人的物理模型約束[24]，機器人的實際平移速度與角速度關系如下:

v=v0+aΔt,ω=ω0+ΩΔt

(8)

式中：v0表示機器人當前時刻速度；a表示機器人當前時刻水平加速度；ω0表示為機器人當前時刻角速度；Ω表示為機器人當前時刻角加速度；Δt表示控制序列時間間隔。進而計算平移加速度a(t)：

(9)

式中：amax表示機器人最大水平加速度；v(t)表示機器人t時刻水平速度；vref(t)=Kρtanh (Kvρ(t))。

同理可以得到角加速度Ω，如下

(10)

式中：Ωmax表示機器人最大角加速度；ω(t)表示機器人t時刻角速度；ωref(t)=Kαα(t)+Kφφ(t)。

非線性反饋控制器雖然考慮了移動機器人的運動學動態約束，即移動機器人的平移速度與角速度均受其對應的加速度影響，卻忽視了移動機器人的運動狀態受兩差速輪運動狀態的因素[24,25]。兩個動力輪在運動過程中會出現加速度大于最大加速度，從而使機器人擺脫其物理模型約束的情況。基于上述問題，考慮兩個動力輪的加速度變化，使移動機器人的運動更加接近于真實環境中的運動狀態，同時對控制器進行限幅處理,進而設計了快速限幅非線性反饋控制器。

4.2 快速限幅非線性反饋控制器

采用文獻[25]中將控制器得到的平移速度與角速度作為期望值，考慮差速移動機器人的平移速度公式與角速度公式。

(11)

式中：vr、vl分別表示移動機器人右輪和左輪速度；b表示兩差速輪之間的輪距。

已知平移速度v與角速度ω，計算可得到兩個差速輪的速度vr和vl：

(12)

由控制器可得到移動機器人期望平移速度與期望角速度，再根據式(12)進一步得到移動機器人差速輪的期望速度。進而計算差速輪期望加速度：

(13)

式中：ar、al分別表示右輪和左輪加速度；vr_old，vl_old分別表示右輪和左輪上一時刻速度。

移動機器人在運動的起始時刻速度與角速度均為零，而期望的速度與角速度較大，會導致移動機器人的差速輪加速度超出最大加速度amax，所以需要對差速輪的加速度進行限幅處理：

(14)

若根據公式(14)對差速輪的加速度進行限幅，在移動機器人運動伊始，ar與al均大于amax，但是由于角加速度的存在，即ar≠al,而通過公式(14)進行限幅后會導致ar=al=amax，從而使角加速度為零，會使移動機器人的角速度無法收斂到目標角速度。由于存在上述問題，對加速度進行歸一化后限幅：

(15)

通過式(15)對差速輪的加速度進行限幅處理，可在保證角加速度的同時確保差速輪滿足移動機器人的運動學動態約束。此外，快速限幅非線性反饋控制律同樣保證了機器人有平滑的全向運動軌跡，見圖8。

圖8 移動機器人全向運動軌跡Fig.8 Omnidirectional motion trajectory of the mobile robot

5 仿 真

5.1 機器人運動軌跡及運動狀態

圖9給出了采用非線性反饋控制器后機器人運動軌跡與運動狀態關系。

圖9 采用非線性反饋控制器后機器人運動軌跡Fig.9 Motion trajectory of the mobile robot with nonlinear feedback controller

圖10給出了采用快速限幅非線性反饋控制器后機器人運動軌跡與運動狀態關系。從圖9和圖10可以看出采用快速限幅非線性反饋控制器后機器人具有更加平滑的全向運動軌跡。

圖10 采用快速限幅非線性反饋控制器后 機器人運動軌跡Fig.10 Motion trajectory of the mobile robot with fast limiting amplitude nonlinear feedback controller

5.2 機器人路徑規劃

為了驗證提出算法的可擴展性能。本文采用與RRT算法相關的改進算法相同的擴展方式，算法編號與對應算法見表1。

表1 算法編號與對應算法Tab.1 Algorithm number and corresponding algorithm

表1中的算法1～4為RRT算法及其改進算法，算法5～8為本文提出的算法及在其基礎上擴展的算法(有*標記)。同時設計3種不同場景地圖環境對相關算法進行仿真驗證,地圖大小為200 m×200 m。

環境(1):簡易障礙物場景

圖11表示簡易障礙物環境下仿真結果。

從圖11(a)可以看出,在簡易障礙物測試場景中，原始RRT與分級隨機采樣的弱隨機RRT算法均規劃出了一條無碰撞的路徑。

從圖11(b)可以看出，采用分級隨機采樣弱隨機RRT算法求解得到的時間區間更小，穩定性更好。

圖11 簡易障礙物環境仿真結果Fig.11 Simulation results in simple obstacle environment

環境(2)：雜亂障礙物場景

圖12表示雜亂障礙物環境仿真結果。

從圖12(a)中可以看出，各算法在面對雜亂障礙物場景，仍然可以完成規劃任務。由于障礙物的增加，規劃任務難度的增強，需要迭代的次數也隨之增加，分級隨機采樣弱隨機RRT算法效率更高。

從圖12(b)中可以看出，采用分級隨機采樣弱隨機RRT算法求解區間更小，同時縮短了求解時間。

圖12 雜亂障礙物環境仿真結果Fig.12 Simulation results in messy obstacle environment

環境(3)：室內狹窄通道場景

圖13表示室內環境狹窄通道仿真結果。

圖13 室內環境狹窄通道仿真結果Fig.13 Simulation results in narrow channel of the indoor environment

從圖13(a)中可以看出，各個算法完成了指定的規劃任務，但由于通道狹窄，算法迭代次數增加，RRT的求解速度降低;分級隨機采樣弱隨機RRT算法縮短了求解時間，提高了迭代速度。

從圖13(b)中可以看出，采用分級隨機采樣的弱隨機RRT算法求解速度更快，效率更高。

對基于分級隨機采樣的弱隨機RRT算法在上述狹窄通道室內環境中的原始路徑進行冗余節點裁剪與轉向節點附近多次采樣，降低原始路徑的長度與節點數。根據移動機器人動態約束，移差速輪速度與加速度均不能超過最大值，且移動機器人僅通過調節兩動力輪來控制其運動狀態，為了更接近移動機器人實際的運動狀態，僅設置最大速度為vmax=1 m/s，最大加速度為amax=2 m/s2。參數設置為Kρ=1,Kφ=-1,Kα=6,Kv=2。

圖14為室內環境狹窄通道運動規劃仿真結果。

圖14 室內環境狹窄通道運動規劃仿真結果Fig.14 Simulation results of the motion planning in narrow channel of the indoor environment

從圖14(a)可以看出，采用快速限幅非線性反饋控制器無需將原始軌跡轉進行光滑處理，就可以對移動機器人進行位姿規劃，最終生成連續光滑的軌跡，且誤差范圍更小。經過轉向后，移動機器人收斂于終點，且速度衰減值0。

從圖14(b)、圖14(c)和圖14(d)中可以看出，采用快速限幅非線性反饋控制器在對機器人進行轉向和角度控制時，機器人始終滿足物理模型動態約束，其原因是兩動力輪的速度和加速度都不超過物理模型規定的最大值。

6 結 論

本文將RRT算法先產生隨機點，再搜索父節點的擴展方式，改進為先選擇擴展的父節點，再隨機確定擴展方向，最后計算得到子節點，完成迭代。避免了算法迭代后期，搜索父節點產生的計算量大的問題，獲得了更小的求解區間與更快的求解速度。此外，針非線性反饋控制器在移動機器人運動規劃過程中，忽視差速輪加速度，致使差速輪的加速度在轉向時不受移動機器人物理模型約束的問題，提出將非線性反饋控制器得到的差速輪速度作為差速輪期望速度，通過快速限幅得到實際的差速輪加速度，進而保證運動規劃過程中轉向函數始終滿足系統模型動態約束。最后通過仿真驗證提出方法的有效性。仿真結果表明分級隨機采樣弱隨機RRT算法具有更快的搜索速度與收斂速度。后續工作將考慮提升算法初始解的質量，實現動態環境下移動機器人的運動規劃。