靳午煊，馬向華，趙金良

1.上海應用技術大學 電氣與電子工程學院，上海 201418

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

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

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

1 Informed-RRT＊算法

1.1 RRT＊算法

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

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

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

1.2 Informed-RRT＊算法

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

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

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

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

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

由上式可得：

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

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

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

2 改進Informed-RRT＊算法

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

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

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

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

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

設定L為采樣節(jié)點Qf到障礙物的距離，L>0 ?？梢钥闯霾蓸庸?jié)點距離障礙物越遠，L越大，采樣概率越低，反之采樣概率越大。故t-分布優(yōu)化的采樣概率分布函數為：

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

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

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

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

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

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

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

圖6 焦距偏置點分布圖Fig.6 Distribution diagram of bias points at focal length

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

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

此時新節(jié)點Qnew擴展點可表示為：

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

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

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

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

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

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

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

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

2.4 轉折節(jié)點平滑法

經過節(jié)點減冗余后的路徑，不夠平滑且路徑角度偏轉過大，不適合實際移動機器人運行。需要采取適當的路徑平滑策略。作為經典的路徑平滑方法，圓弧-直線法在平滑后生成的路徑曲線的曲率和移動機器人運行時的向心加速度會發(fā)生跳變，影響機器人的精度和效率。而對于對稱極多項式曲線，其與直線連接時曲率連續(xù)，且曲率有界，路徑滿足幾何學特征，并能滿足機器人運行的動力學要求。

對稱極多項式曲線在路徑轉折點的平滑原理如圖8所示，此時生成路徑P0→P1→P2，路徑P0→P1為對稱極多項式曲線，路徑P1→P2為直線。

圖8 對稱極多項式曲線平滑路徑Fig.8 Smoothing path of symmetric polar polynomial curves

P0是P1關于MN的對稱點。x-y是直角坐標系r-φ為極坐標系，確定對稱極多項式曲線方程為：

求解平滑路徑的過程就是確定系數R和α的過程，α=θ2-θ1，P點的坐標為：

初始曲率半徑：

由于移動機器人具有一定的動力學特征，在轉彎時，為了避免機器人與地面打滑的現象，必須要有一定的向心力。這樣當機器人以一定速度v行駛時，就會存在一個最小的拐彎半徑Rmin，此時路徑的曲率半徑應大于移動機器人運行時的最小轉彎半徑，也就是生成路徑的最大曲率需滿足：

機器人實際運行過程中，會有多段直線連接的情況，需在平滑路徑時添加若干個過度狀態(tài)Ptrans，用多段直線和多條對稱極多項式曲線來連接初始點和目標點。其中初始位姿與目標位姿相交平行和轉交大于的多段對稱極多項式曲線連接，如圖9所示。

圖9 多段對稱極多項式平滑路徑生成Fig.9 Multi-symmetrical polar polynomial for smooth path generation

3 仿真實驗與分析

3.1 平面空間仿真

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

如圖10 和圖11 分別為簡單障礙物和復雜障礙物環(huán)境下原算法和改進算法對比實驗。其中圖10（b）和圖11（b）中連接起始點和目標點的紅色虛線為改進算法中橢圓焦距。

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

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

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

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

復雜障礙物環(huán)境下Ⅰnformed-RRT*算法和本文改進Ⅰnformed-RRT*算法仿真對比結果如表2所示，在復雜障礙物環(huán)境下改進算法比原算法節(jié)點數目少了17.12%，在節(jié)點利用率上有了明顯的提升，并且搜索時間上減少了25.65%，路徑長度上縮短了8.50%?？梢钥闯龈倪M算法對復雜障礙物環(huán)境也有著較強的適應性。

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

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

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

圖13 路徑轉折處平滑Fig.13 Smoothing of path bends

3.2 ROS平臺實驗

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

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

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

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

4 結束語

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