分級(jí)隨機(jī)采樣弱隨機(jī)RRT算法及在移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃中的應(yīng)用

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

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

1 引 言

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

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

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

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

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

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

(1)

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

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

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

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

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

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

3 分級(jí)隨機(jī)采樣弱隨機(jī)RRT算法

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

3.1 分級(jí)隨機(jī)采樣

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

(2)

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

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

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

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

(3)

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

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

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

3.2 獲取子節(jié)點(diǎn)

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

(4)

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

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

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

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

3.3 搜索臨近節(jié)點(diǎn)

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

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

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

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

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

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

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

3.4 弱隨機(jī)RRT算法

弱隨機(jī)RRT算法具體步驟如下：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4.1 非線性反饋控制器

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

圖7 差動(dòng)輪式移動(dòng)機(jī)器人模型Fig.7 Model of the differential wheeled mobile robot

(5)

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

(6)

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

(7)

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

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

(8)

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

(9)

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

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

(10)

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

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

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

采用文獻(xiàn)[25]中將控制器得到的平移速度與角速度作為期望值，考慮差速移動(dòng)機(jī)器人的平移速度公式與角速度公式。

(11)

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

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

(12)

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

(13)

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

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

(14)

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

(15)

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

圖8 移動(dòng)機(jī)器人全向運(yùn)動(dòng)軌跡Fig.8 Omnidirectional motion trajectory of the mobile robot

5 仿 真

5.1 機(jī)器人運(yùn)動(dòng)軌跡及運(yùn)動(dòng)狀態(tài)

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

圖9 采用非線性反饋控制器后機(jī)器人運(yùn)動(dòng)軌跡Fig.9 Motion trajectory of the mobile robot with nonlinear feedback controller

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

圖10 采用快速限幅非線性反饋控制器后 機(jī)器人運(yùn)動(dòng)軌跡Fig.10 Motion trajectory of the mobile robot with fast limiting amplitude nonlinear feedback controller

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

對(duì)基于分級(jí)隨機(jī)采樣的弱隨機(jī)RRT算法在上述狹窄通道室內(nèi)環(huán)境中的原始路徑進(jìn)行冗余節(jié)點(diǎn)裁剪與轉(zhuǎn)向節(jié)點(diǎn)附近多次采樣，降低原始路徑的長(zhǎng)度與節(jié)點(diǎn)數(shù)。根據(jù)移動(dòng)機(jī)器人動(dòng)態(tài)約束，移差速輪速度與加速度均不能超過最大值，且移動(dòng)機(jī)器人僅通過調(diào)節(jié)兩動(dòng)力輪來控制其運(yùn)動(dòng)狀態(tài)，為了更接近移動(dòng)機(jī)器人實(shí)際的運(yùn)動(dòng)狀態(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)境狹窄通道運(yùn)動(dòng)規(guī)劃仿真結(jié)果。

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

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

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

6 結(jié) 論

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