改進(jìn)Informed-RRT＊的移動(dòng)機(jī)器人路徑規(guī)劃算法研究

靳午煊，馬向華，趙金良

1.上海應(yīng)用技術(shù)大學(xué) 電氣與電子工程學(xué)院，上海 201418

2.上海電氣電力電子有限公司，上海 201916

現(xiàn)如今移動(dòng)機(jī)器人已廣泛應(yīng)用于交通導(dǎo)航、特種作業(yè)、醫(yī)療餐飲服務(wù)等行業(yè)中[1]。路徑規(guī)劃作為移動(dòng)機(jī)器人的核心技術(shù)，一直是學(xué)者們研究的熱門話題。移動(dòng)機(jī)器人全局路徑規(guī)劃的目的是能夠在已知環(huán)境中快速高效地找到一條最短路徑。對(duì)于傳統(tǒng)路徑規(guī)劃算法，有基于搜索的Dijkstra[2]和A*[3]算法也有遺傳算法[4]和模擬退火法[5]等智能優(yōu)化算法，但傳統(tǒng)的全局路徑規(guī)劃算法規(guī)劃效率過低且不適合如今復(fù)雜多變的環(huán)境。1998年，提出了基于采樣的快速搜索樹（rapidly-exploring random tree，RRT）[6]由于其隨機(jī)性強(qiáng)，規(guī)劃效率快的特點(diǎn)得到了廣泛關(guān)注。由于RRT算法規(guī)劃出的路徑并不是最優(yōu)的，2010年，提出了RRT*[7]算法，在原RRT算法基礎(chǔ)上加入重選父節(jié)點(diǎn)進(jìn)行重布線的策略，使其擁有了漸進(jìn)最優(yōu)性。2014 年，在RRT*的基礎(chǔ)上加入橢圓狀態(tài)子集的概念提出了Ⅰnformed-RRT*[8]算法，加快了算法的收斂速度。為了追求路徑搜索和規(guī)劃的效率，近年來國內(nèi)外對(duì)Ⅰnformed-RRT*算法的改進(jìn)也有很多。其中比較突出的改進(jìn)是將Ⅰnformed-RRT*算法與一些較完備的避障或者規(guī)劃算法相融合。文獻(xiàn)[9]將動(dòng)態(tài)窗口法融入Ⅰnformed-RRT*，使其更好地適用于移動(dòng)機(jī)器人并證明了此方法對(duì)所有RRT 類算法的適用性。文獻(xiàn)[10]將Ⅰnformed-RRT*與人工勢(shì)場(chǎng)法融合，加入自適應(yīng)步長方法在靜態(tài)和動(dòng)態(tài)障礙物的適應(yīng)性。文獻(xiàn)[11]采用融合有向D*的策略，使規(guī)劃出的路徑在轉(zhuǎn)彎處得到優(yōu)化，且更適合連續(xù)小洞和狹窄通道。此類融合算法在提升規(guī)劃效率的同時(shí)將耗費(fèi)過多內(nèi)存，需要更高性能的移動(dòng)機(jī)器人才能滿足規(guī)劃需求。還有一些學(xué)者對(duì)Ⅰnformed-RRT*算法本身進(jìn)行改進(jìn)。其中文獻(xiàn)[12]采用混合抽樣方法代替隨機(jī)抽樣，使移動(dòng)機(jī)器人在復(fù)雜環(huán)境中的規(guī)劃效果得到了提高。但對(duì)于較簡(jiǎn)單的環(huán)境算法效果提升并不明顯。文獻(xiàn)[13]將參考路徑劃分為一定數(shù)量的路徑段并逐個(gè)優(yōu)化這些段來解決在多曲線路徑中的收斂問題，但此方法并不適合所有現(xiàn)實(shí)或者仿真環(huán)境。文獻(xiàn)[14]的引入改進(jìn)的Metropolis接受準(zhǔn)則篩選每次迭代生成的新節(jié)點(diǎn)使算法更短時(shí)間找到最優(yōu)解但會(huì)使算法喪失一定的隨機(jī)性。文獻(xiàn)[15]采用反向擴(kuò)展策略，當(dāng)遇到障礙時(shí)快速更新采樣區(qū)間，在障礙物的邊界區(qū)域獲得接近最優(yōu)路徑成本的可行路徑，此算法保留了原算法的隨機(jī)性但缺乏規(guī)劃時(shí)的目的性。

針對(duì)現(xiàn)如今Ⅰnformed-RRT*算法研究中存在的問題，采用節(jié)點(diǎn)優(yōu)化方法對(duì)此算法進(jìn)行改進(jìn)。從隨機(jī)節(jié)點(diǎn)分布，新節(jié)點(diǎn)的偏置生成對(duì)路徑規(guī)劃環(huán)節(jié)進(jìn)行優(yōu)化，然后對(duì)規(guī)劃出的路徑采取節(jié)點(diǎn)減冗余策略，并在節(jié)點(diǎn)轉(zhuǎn)折處采用對(duì)稱極多項(xiàng)式曲線法進(jìn)行平滑處理。通過與原算法的對(duì)比，驗(yàn)證改進(jìn)算法的可行性。

1 Informed-RRT＊算法

1.1 RRT＊算法

RRT*算法在RRT算法規(guī)劃好的隨機(jī)樹上加入了重選父節(jié)點(diǎn)和重布線的操作。重選父節(jié)點(diǎn)的原理如圖1所示，以Qnew（節(jié)點(diǎn)J）為圓心，以定義好的半徑畫圓，圓內(nèi)包含了原始父節(jié)點(diǎn)（節(jié)點(diǎn)F）在內(nèi)的4個(gè)潛在最優(yōu)父節(jié)點(diǎn)，分別為節(jié)點(diǎn)E、節(jié)點(diǎn)F、節(jié)點(diǎn)Ⅰ和節(jié)點(diǎn)G。由圖定義好每段路徑的代價(jià)，其中原始路徑A-C-F-J的代價(jià)為4+6+4=14；以節(jié)點(diǎn)Ⅰ為潛在最有父節(jié)點(diǎn)的=路徑A-C-F-Ⅰ-J的代價(jià)為4+6+6+4=20，比原始路徑代價(jià)高，則不考慮。同理可知以G節(jié)點(diǎn)作為父節(jié)點(diǎn)的A-E-G-J代價(jià)為18；以節(jié)點(diǎn)E為父節(jié)點(diǎn)的路徑A-E-J代價(jià)最低，為11，則選擇節(jié)點(diǎn)E作為最優(yōu)父節(jié)點(diǎn)。如圖1（b）便是重選父節(jié)點(diǎn)后的結(jié)果。

圖1 重選父節(jié)點(diǎn)策略Fig.1 Reselecting parent node policy

RRT*算法的隨機(jī)樹重布線原理：為了進(jìn)一步降低收斂的路徑成本，為Qnew節(jié)點(diǎn)選擇可連接的下一個(gè)臨近節(jié)點(diǎn)。如圖2 所示Qnew的臨近節(jié)點(diǎn)為E，F(xiàn)，G，Ⅰ。其中到達(dá)F節(jié)點(diǎn)的A-E-J-F代價(jià)為7+4+4=15。而原本的路徑A-C-F代價(jià)為4+6=10，所以此路徑不做重新布線。對(duì)于到達(dá)Ⅰ的A-E-J-Ⅰ的代價(jià)為7+4+4=15；而原始路徑AC-F-Ⅰ的代價(jià)為4+6+6=16。所以用A-E-J-Ⅰ代替A-C-F-Ⅰ進(jìn)行重布線，同理可知A-E-J-G代價(jià)大于原始路徑A-EG，故不做重布線。構(gòu)成新的路徑圖2（b）所示。

1.2 Informed-RRT＊算法

Ⅰnformed-RRT*算法引入了橢圓狀態(tài)子集的概念，如圖3 所示，繼承了RRT*算法的概率完備性和漸進(jìn)最優(yōu)性，通過橢圓狀態(tài)子集限制采樣空間，加快了收斂過程的速度。其中Cmin作為橢圓的焦距，長度為起始點(diǎn)到終止點(diǎn)的距離。Cbest為橢圓的長軸，長度為算法規(guī)劃的路徑長度。Ⅰnformed-RRT*算法根據(jù)Cbest和Cmin兩個(gè)參數(shù)建立橢圓采樣空間。Cbest隨著重選父節(jié)點(diǎn)操作不斷減小，橢圓短軸也不斷減小，此時(shí)采樣空間會(huì)不斷縮小，收斂速度不斷加快。

圖3 橢圓采樣區(qū)示意圖Fig.3 Diagram of elliptic sampling area

Ⅰnformed-RRT*算法為在橢圓狀態(tài)子集U中的隨機(jī)采樣，采樣樣本是由單位圓中的均勻分布的采樣樣本經(jīng)過L轉(zhuǎn)換矩陣變換得到。

這種變換可以由超橢球矩陣S的Cholesky分解得到。

對(duì)于橢圓，可以僅從長軸和焦距計(jì)算變換，長軸對(duì)齊的坐標(biāo)系中的超橢球體矩陣是對(duì)角矩陣。分解得到的超橢球體對(duì)角矩陣為：

由上式可得：

另外從橢圓采樣坐標(biāo)系到圓形采樣坐標(biāo)系的變換可以通過Wahba問題的求解方式解決：

其中，U∈?n×n，V∈?n×n并且兩者都是酉矩陣可進(jìn)行奇異值分解:U∑VT=M，M矩陣可由長軸的外積和單位矩陣第一列相乘得到。

綜上橢圓狀態(tài)子集內(nèi)由圓形采樣區(qū)轉(zhuǎn)換后的采樣點(diǎn)：

2 改進(jìn)Informed-RRT＊算法

2.1 自適應(yīng)t-分布的采樣節(jié)點(diǎn)優(yōu)化

Ⅰnformed-RRT*算法采用與RRT*算法相同的完全隨機(jī)的采樣方法，目的是避開障礙物找尋一條最短的路徑。其完全隨機(jī)性導(dǎo)致大量節(jié)點(diǎn)分布在非必要的空白地區(qū)，浪費(fèi)了內(nèi)存并且降低了采樣的效率。為克服以上缺點(diǎn)，引入t-分布的概率密度函數(shù)對(duì)隨機(jī)樹的采樣環(huán)節(jié)進(jìn)行優(yōu)化，具體要求是在距離障礙物較遠(yuǎn)的地方降低隨機(jī)采樣節(jié)點(diǎn)Qf的分布概率，在距離障礙物較近的地方增加隨機(jī)采樣節(jié)點(diǎn)Qf的分布概率，對(duì)于更為復(fù)雜的障礙物環(huán)境自適應(yīng)地進(jìn)行調(diào)整。這樣在避免浪費(fèi)節(jié)點(diǎn)的同時(shí)增加節(jié)點(diǎn)重選過程中障礙物附近的節(jié)點(diǎn)數(shù)量，以獲取更為精致的路徑。

t-分布又稱為Student分布，其概率密度表達(dá)式為：

式中，n為t-分布的自由度，當(dāng)自由度n較小時(shí)，概率密度函數(shù)較為溫和，中間極值較低，兩端值較高，當(dāng)n較大時(shí)，概率密度函數(shù)較為陡峭，中間極值較高，兩端值較低，不同自由度的t-分布的概率密度函數(shù)如圖4所示。

圖4 t-分布概率密度函數(shù)Fig.4 t-distribution probability density function

設(shè)定L為采樣節(jié)點(diǎn)Qf到障礙物的距離，L>0 。可以看出采樣節(jié)點(diǎn)距離障礙物越遠(yuǎn)，L越大，采樣概率越低，反之采樣概率越大。故t-分布優(yōu)化的采樣概率分布函數(shù)為：

采樣概率分布函數(shù)如圖5 所示，當(dāng)n較大，如n為∞時(shí)，t-分布為高斯分布，采樣概率分布函數(shù)較為陡峭，在距離障礙物較近時(shí)，分布函數(shù)值較小，對(duì)采樣點(diǎn)的增加量較小，適合較為簡(jiǎn)單的障礙物環(huán)境。對(duì)于更加復(fù)雜的障礙物形狀，自適應(yīng)地降低自由度n的大小，如n=1時(shí)，t-分布為柯西分布，在距離障礙物越近的情況下，采樣點(diǎn)的分布概率增加量有所提高。引入t-分布對(duì)采樣環(huán)節(jié)進(jìn)行優(yōu)化，自適應(yīng)地調(diào)整t-分布自由度的大小，可以避免單一分布函數(shù)對(duì)不同環(huán)境下的同一處理，降低規(guī)劃效率。

圖5 采樣概率分布函數(shù)Fig.5 Sample probability distribution function

對(duì)采樣環(huán)節(jié)的優(yōu)化，使更多的采樣點(diǎn)分布在障礙物周圍，首先算法會(huì)根據(jù)基礎(chǔ)的快速搜索樹算法規(guī)劃出一條初始路徑。在t-分布優(yōu)化下，初始路徑會(huì)貼近障礙物。在節(jié)點(diǎn)重選環(huán)節(jié)，有更多的備選節(jié)點(diǎn)分布在障礙物附近，而在空白區(qū)域備選節(jié)點(diǎn)分布較少，將會(huì)在提高算法精度的同時(shí)降低節(jié)點(diǎn)浪費(fèi)從而提高算法的規(guī)劃速率。

2.2 橢圓焦距節(jié)點(diǎn)偏置策略

Ⅰnformed-RRT*算法的隨機(jī)樹的生長過程是完全隨機(jī)的，在近幾年的研究中，有學(xué)者采用以目標(biāo)點(diǎn)Qgoal為偏置點(diǎn)的方法使隨機(jī)樹的生長具備了目的性，但這一策略會(huì)產(chǎn)生一定的弊端，較大的單一偏置量會(huì)讓整體算法喪失隨機(jī)性，從而有陷入局部最優(yōu)的可能。

為了避免這一問題，已知橢圓狀態(tài)子集中焦距Cmin為理想的最優(yōu)路徑，故將偏置擴(kuò)展為整個(gè)橢圓焦距，以橢圓焦距作為偏置點(diǎn)的集合，橢圓的焦距Cmin是初始節(jié)點(diǎn)Qinit到目標(biāo)節(jié)點(diǎn)Qgoal的直線距離，每個(gè)采樣隨機(jī)點(diǎn)概率的選取橢圓焦距上的偏置點(diǎn)作為新生長節(jié)點(diǎn)的偏置，直到以Qgoal為偏置點(diǎn)結(jié)束。采用橢圓焦距偏置，使隨機(jī)樹的生長貼近于最優(yōu)路徑，在避免局部最優(yōu)的同時(shí)增加了算法隨機(jī)樹生長的目的性，加快橢圓采樣空間收斂速度。

以節(jié)點(diǎn)作為原點(diǎn)，以橢圓焦距方向作為x軸，節(jié)點(diǎn)Qgoal方向作為x軸的正方向，垂直于焦距的方向作為y軸建立直角坐標(biāo)系，如圖6所示。在橢圓焦距上等距取m個(gè)點(diǎn)作為新節(jié)點(diǎn)Qnew生長的可選偏置點(diǎn)，分別記為x1,x2,…,xm-1,xm，其中xm為節(jié)點(diǎn)Qgoal。當(dāng)隨機(jī)采樣節(jié)點(diǎn)Qf的x坐標(biāo)位于xn-1點(diǎn)到xn點(diǎn)之間時(shí)，只考慮xn-1之后的點(diǎn)，選取xn,xn+1,xn+2,…,xm-1,xm作為偏置點(diǎn)。隨機(jī)采樣節(jié)點(diǎn)Qf1和Qf2在橢圓焦距上的可選偏置點(diǎn)如圖6所示。

圖6 焦距偏置點(diǎn)分布圖Fig.6 Distribution diagram of bias points at focal length

為了在規(guī)劃時(shí)避免陷入局部最優(yōu)，應(yīng)保留隨機(jī)樹無偏置隨機(jī)生長的本領(lǐng)，故以K為偏置概率系數(shù)，以ρ1和ρ2作為向隨機(jī)采樣點(diǎn)Qf和偏置點(diǎn)前進(jìn)的步長。若Qf的x坐標(biāo)位于xn-1點(diǎn)到xn點(diǎn)之間，此時(shí)新節(jié)點(diǎn)Qnew以xn+1為偏置點(diǎn)生長的概率為：

其中，概率服從以均值為μ=xn，方差為σ的高斯分布函數(shù)。

此時(shí)新節(jié)點(diǎn)Qnew擴(kuò)展點(diǎn)可表示為：

兩個(gè)方向產(chǎn)生的步長組成的平行四邊形如圖7 所示，對(duì)角線的方向便為新節(jié)點(diǎn)Qnew的方向，Qnewp為原始無偏置的新節(jié)點(diǎn)。

圖7 焦距偏置隨機(jī)樹生長圖Fig.7 Growth diagram of focal length bias random tree

2.3 重選祖輩節(jié)點(diǎn)策略

Ⅰnformed-RRT*的隨機(jī)樹生長環(huán)節(jié)中會(huì)產(chǎn)生較多的節(jié)點(diǎn)，過多的節(jié)點(diǎn)會(huì)在節(jié)點(diǎn)重選環(huán)節(jié)使規(guī)劃出的路徑產(chǎn)生較多的轉(zhuǎn)折，并不適合實(shí)際機(jī)器人的運(yùn)行。在節(jié)點(diǎn)重選的過程中，每一代節(jié)點(diǎn)本身都記錄著順序（輩分）和代價(jià)，這給重選祖輩節(jié)點(diǎn)提供了支撐。相比于計(jì)算節(jié)點(diǎn)到Cmin的距離進(jìn)行比較重連接的Dougla-Peucker減冗余算法，采用重選祖輩節(jié)點(diǎn)的方法直接調(diào)用算法中已知的節(jié)點(diǎn)信息，路徑彎折概率較小且效率更高。

在算法規(guī)劃出最優(yōu)路徑后從節(jié)點(diǎn)Qgoal開始再次向節(jié)點(diǎn)Qinit進(jìn)行選擇重新布線，操作如下：

（1）對(duì)從Qgoal到Qinit的中間節(jié)點(diǎn)進(jìn)行標(biāo)號(hào)，記為Qn,Qn-1,…,2,1。由于Qn并不是Qgoal的父節(jié)點(diǎn)，故先只將節(jié)點(diǎn)Qn加入重選集Z中。

（2）已知Qn-1為Qn的父節(jié)點(diǎn)，則Qn-2為Qn的祖輩節(jié)點(diǎn)，連接Qn和Qn-2，判斷連接線是否有障礙物，若有障礙物則將Qn-1加入Z中，然后以Qn-1向前重選；若連接線無障礙物，則向前連接Qn和Qn-3，再次判斷是否有障礙物，依次類推，直到重選至Qinit節(jié)點(diǎn)停止。

（3）依次連接重選集Z內(nèi)的節(jié)點(diǎn)，最終路徑與Qgoal相連，即為減冗余后的路徑。

2.4 轉(zhuǎn)折節(jié)點(diǎn)平滑法

經(jīng)過節(jié)點(diǎn)減冗余后的路徑，不夠平滑且路徑角度偏轉(zhuǎn)過大，不適合實(shí)際移動(dòng)機(jī)器人運(yùn)行。需要采取適當(dāng)?shù)穆窂狡交呗浴Ｗ鳛榻?jīng)典的路徑平滑方法，圓弧-直線法在平滑后生成的路徑曲線的曲率和移動(dòng)機(jī)器人運(yùn)行時(shí)的向心加速度會(huì)發(fā)生跳變，影響機(jī)器人的精度和效率。而對(duì)于對(duì)稱極多項(xiàng)式曲線，其與直線連接時(shí)曲率連續(xù)，且曲率有界，路徑滿足幾何學(xué)特征，并能滿足機(jī)器人運(yùn)行的動(dòng)力學(xué)要求。

對(duì)稱極多項(xiàng)式曲線在路徑轉(zhuǎn)折點(diǎn)的平滑原理如圖8所示，此時(shí)生成路徑P0→P1→P2，路徑P0→P1為對(duì)稱極多項(xiàng)式曲線，路徑P1→P2為直線。

圖8 對(duì)稱極多項(xiàng)式曲線平滑路徑Fig.8 Smoothing path of symmetric polar polynomial curves

P0是P1關(guān)于MN的對(duì)稱點(diǎn)。x-y是直角坐標(biāo)系r-φ為極坐標(biāo)系，確定對(duì)稱極多項(xiàng)式曲線方程為：

求解平滑路徑的過程就是確定系數(shù)R和α的過程，α=θ2-θ1，P點(diǎn)的坐標(biāo)為：

初始曲率半徑：

由于移動(dòng)機(jī)器人具有一定的動(dòng)力學(xué)特征，在轉(zhuǎn)彎時(shí)，為了避免機(jī)器人與地面打滑的現(xiàn)象，必須要有一定的向心力。這樣當(dāng)機(jī)器人以一定速度v行駛時(shí)，就會(huì)存在一個(gè)最小的拐彎半徑Rmin，此時(shí)路徑的曲率半徑應(yīng)大于移動(dòng)機(jī)器人運(yùn)行時(shí)的最小轉(zhuǎn)彎半徑，也就是生成路徑的最大曲率需滿足：

機(jī)器人實(shí)際運(yùn)行過程中，會(huì)有多段直線連接的情況，需在平滑路徑時(shí)添加若干個(gè)過度狀態(tài)Ptrans，用多段直線和多條對(duì)稱極多項(xiàng)式曲線來連接初始點(diǎn)和目標(biāo)點(diǎn)。其中初始位姿與目標(biāo)位姿相交平行和轉(zhuǎn)交大于的多段對(duì)稱極多項(xiàng)式曲線連接，如圖9所示。

圖9 多段對(duì)稱極多項(xiàng)式平滑路徑生成Fig.9 Multi-symmetrical polar polynomial for smooth path generation

3 仿真實(shí)驗(yàn)與分析

3.1 平面空間仿真

采用PyCharm2019 環(huán)境，設(shè)置障礙物環(huán)境大小為300×300。起始點(diǎn)坐標(biāo)為（10，10），由圖中綠色點(diǎn)表示；目標(biāo)點(diǎn)坐標(biāo)為（250，250），用紅色點(diǎn)表示。規(guī)劃出的路徑由深藍(lán)色線或粉色線表示障礙物用黑色塊狀物表示。

如圖10 和圖11 分別為簡(jiǎn)單障礙物和復(fù)雜障礙物環(huán)境下原算法和改進(jìn)算法對(duì)比實(shí)驗(yàn)。其中圖10（b）和圖11（b）中連接起始點(diǎn)和目標(biāo)點(diǎn)的紅色虛線為改進(jìn)算法中橢圓焦距。

圖10 簡(jiǎn)單障礙物環(huán)境下仿真對(duì)比Fig.10 Simulation comparison in simple obstacle environment

圖11 復(fù)雜障礙物環(huán)境下仿真對(duì)比Fig.11 Simulation comparison in complex obstacle environment

Ⅰnformed-RRT*算法和改進(jìn)Ⅰnformed-RRT*算法在簡(jiǎn)單障礙物環(huán)境下仿真的對(duì)比結(jié)果如表1所示，節(jié)點(diǎn)數(shù)目減少了4.18%，表明保證在規(guī)劃時(shí)改進(jìn)算法在提高節(jié)點(diǎn)利用率的同時(shí)能有效地節(jié)省使用的內(nèi)存。在搜索時(shí)間和路徑長度上分別減少了22.84%和10.98%，可以看出在簡(jiǎn)單障礙物環(huán)境下改進(jìn)后的算法效率更高，規(guī)劃出的路徑更優(yōu)。

表1 簡(jiǎn)單障礙物環(huán)境30次仿真平均值Table 1 Average values of 30 simulations of simple obstacle environment

復(fù)雜障礙物環(huán)境下Ⅰnformed-RRT*算法和本文改進(jìn)Ⅰnformed-RRT*算法仿真對(duì)比結(jié)果如表2所示，在復(fù)雜障礙物環(huán)境下改進(jìn)算法比原算法節(jié)點(diǎn)數(shù)目少了17.12%，在節(jié)點(diǎn)利用率上有了明顯的提升，并且搜索時(shí)間上減少了25.65%，路徑長度上縮短了8.50%。可以看出改進(jìn)算法對(duì)復(fù)雜障礙物環(huán)境也有著較強(qiáng)的適應(yīng)性。

表2 復(fù)雜障礙物環(huán)境30次仿真平均值Table 2 Average values of 30 simulations of complex obstacle environment

路徑平滑環(huán)節(jié)首先采用重選祖輩節(jié)點(diǎn)的方式對(duì)整條路徑上的節(jié)點(diǎn)減冗余，減少路徑的轉(zhuǎn)折。復(fù)雜障礙物環(huán)境下減冗余后的結(jié)果如圖12所示。其路徑節(jié)點(diǎn)數(shù)量由圖11（b）的86 減少為10。然后對(duì)減冗余后的路徑采用對(duì)稱極多項(xiàng)式曲線法進(jìn)行轉(zhuǎn)折處的平滑，最終平滑后的效果如圖13所示。采用以上兩種方法處理出的路徑滿足機(jī)器人運(yùn)動(dòng)時(shí)的動(dòng)力學(xué)要求，且可以提高運(yùn)行時(shí)的穩(wěn)度和精度。

圖12 路徑節(jié)點(diǎn)減冗余Fig.12 Reducing redundancy of path nodes

圖13 路徑轉(zhuǎn)折處平滑F(xiàn)ig.13 Smoothing of path bends

3.2 ROS平臺(tái)實(shí)驗(yàn)

將路徑規(guī)劃優(yōu)化和路徑平滑在機(jī)器人操作系統(tǒng)（robot operating system，ROS）下的Rviz 可視化工具下進(jìn)行仿真實(shí)驗(yàn)。首先在Gazebo 中建立機(jī)器人仿真環(huán)境，使用turtlebot3模型作為仿真機(jī)器人，如圖14所示。

圖14 Gazebo仿真環(huán)境搭建Fig.14 Gazebo simulation environment construction

在Rviz可視化工具下，首先采用Gmapping的SLAM方法建立地圖。設(shè)置起始點(diǎn)坐標(biāo)為（24，15，0），目標(biāo)點(diǎn)坐標(biāo)為（255，225，0），障礙物和邊界用黑色表示，規(guī)劃出的路徑用綠色實(shí)線表示，彩色不規(guī)則線段表示鄰近障礙物或邊界的代價(jià)地圖。如圖15（a）表示采用Ⅰnformed-RRT*算法作為全局規(guī)劃器規(guī)劃出的路徑，圖15（b）為采用改進(jìn)Ⅰnformed-RRT*算法作為全局規(guī)劃器規(guī)劃的路徑。改進(jìn)算法在搜索環(huán)節(jié)時(shí)更加快速，路徑偏向起始點(diǎn)和目標(biāo)點(diǎn)的連線，且較平滑，更加適合實(shí)際機(jī)器人運(yùn)行。

圖15 Rviz環(huán)境下兩種全局規(guī)劃器對(duì)比Fig.15 Comparison of two global planners in Rviz environment

4 結(jié)束語

改進(jìn)Ⅰnformed-RRT*算法，采用t-分布對(duì)隨機(jī)采樣點(diǎn)的分布概率進(jìn)行優(yōu)化，然后采用橢圓焦距偏置策略對(duì)新節(jié)點(diǎn)的生成進(jìn)行偏置引導(dǎo)，在路徑平滑環(huán)節(jié)采用節(jié)點(diǎn)減冗余策略，并在轉(zhuǎn)折節(jié)點(diǎn)上采用對(duì)稱極多項(xiàng)式曲線進(jìn)行平滑處理。通過平面空間和Gazebo環(huán)境下的實(shí)驗(yàn)仿真可以看出，改進(jìn)算法比原算法規(guī)劃出路徑速度更快，規(guī)劃出的路徑更優(yōu)，且能在更復(fù)雜的障礙物環(huán)境中滿足機(jī)器人的動(dòng)力學(xué)需求。