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

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

1 引 言

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

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

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

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

2 RRT算法

RRT算法迭代完成樹結(jié)構(gòu)示意圖如圖1所示。

圖1 RRT算法迭代完成樹結(jié)構(gòu)示意圖Fig.1 Schematic diagram of the iteration completion tree for the RRT algorithm

隨機樹結(jié)構(gòu)初始化為

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

(1)

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

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

經(jīng)過分析可得出：

(1)節(jié)點ni所攜帶的信息僅僅包含坐標(biāo)與父節(jié)點之間的信息，并未攜帶更多有利信息，如當(dāng)前節(jié)點與障礙物的相對關(guān)系、該區(qū)域節(jié)點數(shù)量、與目標(biāo)區(qū)域的相對距離等。

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

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

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

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

3.1 分級隨機采樣

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

(2)

式中：先選擇樹中一節(jié)點作為子節(jié)點的父節(jié)點,priority_1,priority_2表示候選擴展節(jié)點集合；score_ni表示T中ni節(jié)點分?jǐn)?shù)；a，b，c表示劃分不同優(yōu)先級的分?jǐn)?shù)節(jié)點；else表示節(jié)點不作為候選擴展節(jié)點。

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

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

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

(3)

式中：dir_mat表示節(jié)點的方向?qū)傩跃仃?，矩陣元?，2，3，4，5，6，7，8分別對應(yīng)節(jié)點的8個擴展方向，不可擴展的方向包括兩種情況，第1種是位于障礙物邊界或地圖邊界的節(jié)點，第2種是節(jié)點附近已存在節(jié)點。

方向?qū)傩跃仃嚺c節(jié)點方向的對應(yīng)關(guān)系如圖2所示。

圖2 方向?qū)傩跃仃嚺c節(jié)點方向?qū)?yīng)關(guān)系Fig.2 Correspondence of direction attribute matrix and nodes direction

3.2 獲取子節(jié)點

為了獲取子節(jié)點，計算出各子節(jié)點的坐標(biāo)，給出子節(jié)點的計算公式如下：

(4)

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

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

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

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

3.3 搜索臨近節(jié)點

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

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

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

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

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

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

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

3.4 弱隨機RRT算法

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

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

第2步：依據(jù)節(jié)點待選集合的優(yōu)先級，選取待擴展節(jié)點qparent。

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

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

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

第6步：將子節(jié)點添加到樹的結(jié)構(gòu)體中。判斷子節(jié)點是否位于目標(biāo)區(qū)域，若位于目標(biāo)區(qū)域，停止迭代，若子節(jié)點仍然位于目標(biāo)區(qū)域之外，返回第2步。

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

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

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

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

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

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

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

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

4 快速限幅非線性反饋控制器設(shè)計

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

4.1 非線性反饋控制器

差速輪式移動機器人模型如圖7所示，該機器人通過調(diào)節(jié)兩個動力輪的加速度控制兩動力輪的速度，實現(xiàn)機器人的前進、后退，左右轉(zhuǎn)向等運動狀態(tài)。該差速輪式移動機器人的運動學(xué)模型如下：

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

(5)

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

(6)

為了保證機器人運動的穩(wěn)定性，非線性反饋控制器應(yīng)滿足以下條件[23]:

(7)

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

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

(8)

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

(9)

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

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

(10)

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

非線性反饋控制器雖然考慮了移動機器人的運動學(xué)動態(tài)約束，即移動機器人的平移速度與角速度均受其對應(yīng)的加速度影響，卻忽視了移動機器人的運動狀態(tài)受兩差速輪運動狀態(tài)的因素[24,25]。兩個動力輪在運動過程中會出現(xiàn)加速度大于最大加速度，從而使機器人擺脫其物理模型約束的情況?；谏鲜鰡栴}，考慮兩個動力輪的加速度變化，使移動機器人的運動更加接近于真實環(huán)境中的運動狀態(tài)，同時對控制器進行限幅處理,進而設(shè)計了快速限幅非線性反饋控制器。

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

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

(11)

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

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

(12)

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

(13)

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

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

(14)

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

(15)

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

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

5 仿 真

5.1 機器人運動軌跡及運動狀態(tài)

圖9給出了采用非線性反饋控制器后機器人運動軌跡與運動狀態(tài)關(guān)系。

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

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

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

5.2 機器人路徑規(guī)劃

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

表1 算法編號與對應(yīng)算法Tab.1 Algorithm number and corresponding algorithm

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

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

圖11表示簡易障礙物環(huán)境下仿真結(jié)果。

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

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

圖11 簡易障礙物環(huán)境仿真結(jié)果Fig.11 Simulation results in simple obstacle environment

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

圖12表示雜亂障礙物環(huán)境仿真結(jié)果。

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

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

圖12 雜亂障礙物環(huán)境仿真結(jié)果Fig.12 Simulation results in messy obstacle environment

環(huán)境(3)：室內(nèi)狹窄通道場景

圖13表示室內(nèi)環(huán)境狹窄通道仿真結(jié)果。

圖13 室內(nèi)環(huán)境狹窄通道仿真結(jié)果Fig.13 Simulation results in narrow channel of the indoor environment

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

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

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

圖14為室內(nèi)環(huán)境狹窄通道運動規(guī)劃仿真結(jié)果。

圖14 室內(nèi)環(huán)境狹窄通道運動規(guī)劃仿真結(jié)果Fig.14 Simulation results of the motion planning in narrow channel of the indoor environment

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

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

6 結(jié) 論

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