劉紫燕，張 杰

貴州大學(xué) 大數(shù)據(jù)與信息工程學(xué)院，貴陽(yáng)550025

1 引言

隨著科學(xué)技術(shù)的不斷發(fā)展，導(dǎo)航技術(shù)已成為機(jī)器人領(lǐng)域的一個(gè)研究熱點(diǎn)，而路徑規(guī)劃算法是實(shí)現(xiàn)移動(dòng)機(jī)器人導(dǎo)航的重要保障[1]。路徑規(guī)劃是指機(jī)器人按照一定的準(zhǔn)則，在環(huán)境中搜索一條從起始點(diǎn)到目標(biāo)點(diǎn)、可以避開(kāi)障礙物的最優(yōu)路徑[2]。傳統(tǒng)的路徑規(guī)劃算法有人工勢(shì)場(chǎng)法、模擬退火算法、模糊控制法、神經(jīng)網(wǎng)絡(luò)算法等[3]，但這些算法的計(jì)算復(fù)雜度較大，在真實(shí)場(chǎng)景下的應(yīng)用存在缺陷。遺傳算法、蟻群算法、粒子群算法等智能仿生學(xué)算法需要大量的迭代來(lái)求解最優(yōu)路徑，因此收斂速度慢，而且極易陷入局部最優(yōu)值[4-5]。

快速擴(kuò)展隨機(jī)樹（Rapidly-exploring Random Tree，RRT）由于具有快速高效的優(yōu)點(diǎn)，近幾年在移動(dòng)機(jī)器人路徑規(guī)劃中被廣泛采用[6]。RRT 算法通過(guò)隨機(jī)采樣，把搜索導(dǎo)向空白區(qū)域，從而搜索到一條起點(diǎn)到終點(diǎn)的路徑，適合解決復(fù)雜環(huán)境下多自由度機(jī)器人的路徑規(guī)劃問(wèn)題[7]。但是由于傳統(tǒng)RRT 算法擴(kuò)展點(diǎn)具有隨機(jī)性，使得算法搜索過(guò)于平均、規(guī)劃結(jié)果偏離最優(yōu)解[8]。針對(duì)以上缺陷，文獻(xiàn)[9]提出的RRT2connect算法同時(shí)從起點(diǎn)和終點(diǎn)生長(zhǎng)兩棵隨機(jī)樹，在搜索效率方面有了顯著提高，但在組態(tài)空間存在窄道時(shí)其性能會(huì)顯著下降。Karaman和Frazzoli[10]提出了RRT*算法，用以改進(jìn)由RRT 算法產(chǎn)生非最優(yōu)解的問(wèn)題，但存在收斂速度慢、結(jié)果不穩(wěn)定的缺點(diǎn)。同時(shí)莫棟成等人[11]提出了任意時(shí)間RRT算法，通過(guò)減少搜索節(jié)點(diǎn)數(shù)來(lái)提高搜索效率，該算法的缺點(diǎn)是啟發(fā)函數(shù)沒(méi)有達(dá)到最優(yōu)。劉成菊等人[12]提出了動(dòng)態(tài)RRT算法，并將其應(yīng)用于NAO 機(jī)器人，該算法能適應(yīng)復(fù)雜障礙物環(huán)境，但是生成的路徑并非理想平滑曲線。Wang[13]提出基于學(xué)習(xí)的LM-RRT（Learning-based Multi-RRT）算法，用于窄通道中的機(jī)器人路徑規(guī)劃，提高了局部探索能力。Du等人[14]將RRT 算法和人工勢(shì)場(chǎng)法相結(jié)合，提出一種啟發(fā)式采樣方法，具有更高的采樣效率和收斂速度。Chang 等人[15]提出了SARRT（Struc‐ture-Aware Rapidly-exploring Random Tree）算法，通過(guò)構(gòu)建溫度場(chǎng)使隨機(jī)樹生長(zhǎng)偏向于目標(biāo)區(qū)域，取得良好的效果，然而求解擴(kuò)散方程計(jì)算成本昂貴，耗費(fèi)了大量時(shí)間。

為了解決上述問(wèn)題，本文結(jié)合目標(biāo)偏向策略和氣味擴(kuò)散法對(duì)RRT算法加以改進(jìn)，并對(duì)生成的路徑進(jìn)行優(yōu)化處理，從而提高算法效率和路徑質(zhì)量。通過(guò)基于ROS的仿真實(shí)驗(yàn)和實(shí)驗(yàn)室環(huán)境下的Turtlebot2 實(shí)驗(yàn)，證實(shí)了改進(jìn)RRT算法的優(yōu)越性、實(shí)用性和可靠性。

2 機(jī)器人運(yùn)動(dòng)模型

機(jī)器人運(yùn)動(dòng)模型如圖1 所示，可將運(yùn)動(dòng)視為通過(guò)旋轉(zhuǎn)平移兩種方式完成從一點(diǎn)到另一點(diǎn)的操作。假設(shè)機(jī)器人沿曲線運(yùn)動(dòng)，t 時(shí)刻機(jī)器人的線速度為vt，角速度為ωt。在(t-1,t]時(shí)刻內(nèi)（時(shí)間間隔為Δt），機(jī)器人以r=v/ω為半徑的圓形軌跡上移動(dòng)，其瞬時(shí)速度為：

圖1 機(jī)器人運(yùn)動(dòng)模型

令t-1 時(shí)刻機(jī)器人的位姿Xt-1=(xt-1,yt-1,θt-1)T，Δt 時(shí)間內(nèi)以速度常量(v,ω)T進(jìn)行運(yùn)動(dòng)，則t 時(shí)刻機(jī)器人的位姿為：

機(jī)器人所有時(shí)刻的位姿構(gòu)成了機(jī)器人實(shí)際行走的路徑。

3 基本RRT算法

RRT 算法由Lavalle 于1998 年提出，它是一種多維空間的高效規(guī)劃方法[16]。RRT 算法復(fù)雜度小，可直接應(yīng)用于非完整約束系統(tǒng)的規(guī)劃。設(shè)機(jī)器人的位姿空間為C，初始節(jié)點(diǎn)為Qstart，目標(biāo)節(jié)點(diǎn)為Qgoal，機(jī)器人最小步長(zhǎng)為s，初始隨機(jī)樹為T。算法以Qstart為根節(jié)點(diǎn)，在位姿空間C 中隨機(jī)產(chǎn)生采樣點(diǎn)Qrand，然后遍歷T 中的所有節(jié)點(diǎn)，尋找離Qrand最近的節(jié)點(diǎn)Qnear作為擴(kuò)展節(jié)點(diǎn)。此時(shí)判斷Qnear與Qrand的連接線是否與障礙物相交，若相交則在位姿空間C 中重新生成隨機(jī)采樣點(diǎn)Qrand；否則，機(jī)器人由Qnear向Qrand運(yùn)動(dòng)最小步長(zhǎng)s到達(dá)新節(jié)點(diǎn)Qnew，同時(shí)將新節(jié)點(diǎn)Qnew添加到擴(kuò)展樹T 中。重復(fù)上述步驟，當(dāng)機(jī)器人到達(dá)目標(biāo)節(jié)點(diǎn)Qgoal或超過(guò)最大迭代次數(shù)時(shí)算法結(jié)束。此時(shí)，由目標(biāo)節(jié)點(diǎn)Qgoal回溯到初始節(jié)點(diǎn)Qstart，即可得到路徑。RRT算法偽代碼如下[17]：

4 改進(jìn)RRT算法

4.1 目標(biāo)偏向策略

由基本RRT算法原理可知，其產(chǎn)生新節(jié)點(diǎn)的方式是在機(jī)器人狀態(tài)空間中隨機(jī)采樣，導(dǎo)致搜索過(guò)于平均，浪費(fèi)了大量計(jì)算時(shí)間和資源，算法效率低。而且算法輸出的路徑質(zhì)量不高，偏離最優(yōu)解。為了克服上述缺陷，高效地規(guī)劃出移動(dòng)機(jī)器人在復(fù)雜環(huán)境下的最優(yōu)路徑，本文在基本RRT 算法的框架中引入一種目標(biāo)偏向策略。即提前設(shè)定一個(gè)偏向概率閾值pbias，在隨機(jī)選取擴(kuò)展節(jié)點(diǎn)時(shí)利用均勻概率分布產(chǎn)生一個(gè)隨機(jī)概率p，若p ＞pbias，則Qrand在機(jī)器人狀態(tài)空間中隨機(jī)產(chǎn)生，否則Qrand=Qgoal。通過(guò)這種方式既保留了原算法的隨機(jī)特性，又使得算法收斂速度加快，從而提升搜索效率。

在基本RRT算法中，規(guī)劃問(wèn)題的起始狀態(tài)點(diǎn)是樹的根節(jié)點(diǎn)，但目標(biāo)狀態(tài)點(diǎn)在搜索過(guò)程中并沒(méi)有被考慮在內(nèi)，只是被動(dòng)地等待隨機(jī)樹擴(kuò)展到目標(biāo)區(qū)域。目標(biāo)偏向策略將目標(biāo)點(diǎn)的作用考慮在內(nèi)，使得隨機(jī)樹以一定概率朝向目標(biāo)點(diǎn)生長(zhǎng)，改變了節(jié)點(diǎn)擴(kuò)展方式，避免了原算法的無(wú)方向生長(zhǎng)。當(dāng)隨機(jī)概率p ＜pbias時(shí)，探索樹朝目標(biāo)點(diǎn)生長(zhǎng)；當(dāng)p ＞pbias時(shí)，探索樹在狀態(tài)空間中隨機(jī)選擇采樣點(diǎn)進(jìn)行生長(zhǎng)。探索樹的目標(biāo)偏向性和隨機(jī)延展性受隨機(jī)概率p 的影響，p 越大隨機(jī)樹朝目標(biāo)點(diǎn)生長(zhǎng)的趨勢(shì)越強(qiáng)，隨機(jī)性越弱。反之，p 越小隨機(jī)樹在整個(gè)狀態(tài)空間中選擇擴(kuò)展點(diǎn)的機(jī)會(huì)越大，隨機(jī)樹的發(fā)散性也就越強(qiáng)，適用于復(fù)雜環(huán)境下的路徑規(guī)劃[18]。不論隨機(jī)樹是否朝目標(biāo)點(diǎn)生長(zhǎng)，當(dāng)路徑與障礙物發(fā)生碰撞時(shí)都會(huì)放棄當(dāng)前節(jié)點(diǎn)重新產(chǎn)生采樣點(diǎn)，這種節(jié)點(diǎn)擴(kuò)展方式使得隨機(jī)樹在避開(kāi)障礙物的同時(shí)朝向目標(biāo)點(diǎn)生長(zhǎng)，保證了生成路徑的準(zhǔn)確性。目標(biāo)偏向RRT算法仍具有概率完備性，在理論上總能在地圖中搜索到一條可行路徑，且規(guī)劃路徑優(yōu)于RRT 算法生成路徑。RRT 算法與目標(biāo)偏向RRT 路徑生成對(duì)比如圖2 所示，紅線表示規(guī)劃路徑，藍(lán)點(diǎn)表示采樣節(jié)點(diǎn)。由圖2 可知，基于目標(biāo)偏向策略的RRT 算法采樣點(diǎn)減少，可大大縮短路徑長(zhǎng)度和規(guī)劃時(shí)間，生成路徑也較為平整光滑，實(shí)驗(yàn)結(jié)果證明了目標(biāo)偏向策略的有效性和準(zhǔn)確性。

在目標(biāo)偏向RRT中，概率閾值pbias的選取會(huì)影響算法規(guī)劃性能。通過(guò)實(shí)驗(yàn)分析對(duì)概率閾值pbias的取值進(jìn)行研究。環(huán)境地圖如圖2（a）所示，起點(diǎn)位置坐標(biāo)為S(1,1)，終點(diǎn)位置坐標(biāo)為G(90,90)。選取不同的概率閾值，在每一個(gè)概率閾值pbias下分別進(jìn)行10 次路徑規(guī)劃。將pbias=0.05 作為第一組實(shí)驗(yàn)數(shù)據(jù)，pbias=0.1 作為第二組實(shí)驗(yàn)數(shù)據(jù)，之后以0.1為間隔進(jìn)行實(shí)驗(yàn)。以路徑長(zhǎng)度、搜索時(shí)間作為評(píng)價(jià)指標(biāo)，記錄10 次實(shí)驗(yàn)結(jié)果的平均值。仿真實(shí)驗(yàn)結(jié)果如圖3所示。

圖2 RRT與目標(biāo)偏向RRT對(duì)比

從圖3（a）可知，隨著概率閾值pbias逐漸增大，平均路徑長(zhǎng)度整體上呈下降趨勢(shì)。表明pbias越大，隨機(jī)樹向終點(diǎn)生長(zhǎng)的趨勢(shì)愈加明顯，使得路徑更為直接，減少了無(wú)效搜索，縮短了路徑長(zhǎng)度。從圖3（b）可知，路徑平均搜索時(shí)間整體上呈現(xiàn)出先降低再上升的趨勢(shì)，pbias取0.2用時(shí)最短。表明pbias較小時(shí)，RRT 算法偏向終點(diǎn)擴(kuò)展的趨勢(shì)小，隨機(jī)樹會(huì)向其他空白區(qū)域作無(wú)效搜索，導(dǎo)致搜索時(shí)間變長(zhǎng)。而當(dāng)pbias較大時(shí)，隨機(jī)樹會(huì)以較大概率朝終點(diǎn)生長(zhǎng)，使得擴(kuò)展節(jié)點(diǎn)選擇單一。所以算法會(huì)花費(fèi)較多時(shí)間避開(kāi)障礙物，增加了路徑搜索時(shí)間。

圖3 概率閾值pbias 對(duì)算法性能指標(biāo)影響

由于本文采用目標(biāo)偏向策略主要目的是減少搜索時(shí)間，因而在后續(xù)實(shí)驗(yàn)中選取概率閾值pbias=0.2。

4.2 基于氣味擴(kuò)散法的路徑搜索

氣味擴(kuò)散法靈感來(lái)源于機(jī)器人主動(dòng)嗅覺(jué)目標(biāo)搜索[19]，也稱為氣味源搜索。機(jī)器人利用傳感器檢測(cè)環(huán)境中的氣味濃度，朝濃度增大方向行進(jìn)并搜索出氣味源的位置。假設(shè)機(jī)器人的終點(diǎn)為氣味源，機(jī)器人所處環(huán)境為二維地圖，且地圖中的障礙物區(qū)域阻斷了氣味擴(kuò)散，即氣味不能以障礙物為介質(zhì)傳播。在地圖中，氣味源會(huì)呈輻射狀地向周圍擴(kuò)散，離終點(diǎn)越遠(yuǎn)氣味濃度越低，反之，氣味濃度越高。由于障礙物阻斷了擴(kuò)散，氣味只能在地圖中的自由區(qū)域內(nèi)完成擴(kuò)散，而自由區(qū)域內(nèi)與終點(diǎn)距離相同位置上的氣味濃度應(yīng)大致相同。由此可在二維地圖上構(gòu)建氣味素濃度場(chǎng)，終點(diǎn)氣味濃度最高且氣味濃度只與終點(diǎn)位置和環(huán)境狀況有關(guān)。無(wú)論機(jī)器人起點(diǎn)處于地圖中的什么位置，只要沿著濃度增大的方向移動(dòng)一定能到達(dá)終點(diǎn)，從而完成路徑規(guī)劃任務(wù)。

先將二維地圖柵格化，排除障礙柵格，自由空間內(nèi)每一個(gè)柵格可作為一個(gè)氣味擴(kuò)散單元。氣味擴(kuò)散采用四鄰域模型，如圖4 所示。在四鄰域模型中，氣味從當(dāng)前柵格朝上下左右四個(gè)方向作擴(kuò)散[20]，直至氣味素?cái)U(kuò)散到自由空間中每一個(gè)柵格。將路徑規(guī)劃的終點(diǎn)作為氣味源，即終點(diǎn)位置所處柵格為最高濃度，按照四鄰域模型逐漸向外擴(kuò)散，每擴(kuò)散一次，氣味濃度值減小一個(gè)等級(jí)，然后記錄每個(gè)柵格的氣味素濃度，當(dāng)擴(kuò)散完成后地圖中的每個(gè)柵格都會(huì)分配到相應(yīng)氣味濃度值。氣味擴(kuò)散法可用下面的表達(dá)式表示：

式（3）中，i 為擴(kuò)散批次，{O(i)}表示第i 批擴(kuò)散柵格的氣味濃度集合，{O(i+1)}為i+1批擴(kuò)散柵格的氣味濃度集合。O(1)表示終點(diǎn)所在柵格的氣味濃度，將其值設(shè)為M。因?yàn)閿U(kuò)散中同一批次柵格的氣味濃度相等，所以將它們稱之為“等濃度集合”，氣味的擴(kuò)散是按照等濃度集合進(jìn)行的[21]，每擴(kuò)散一次，氣味濃度值減1，如果此柵格已經(jīng)標(biāo)記有氣味濃度值則放棄擴(kuò)散。當(dāng)每個(gè)自由柵格都標(biāo)記好氣味濃度值時(shí)擴(kuò)散完成。

圖4 機(jī)器人運(yùn)動(dòng)方向

由氣味擴(kuò)散法的原理可知，終點(diǎn)氣味濃度最高，其值為M，所有柵格的濃度都由終點(diǎn)擴(kuò)散而來(lái)。柵格氣味濃度大小只與終點(diǎn)位置和環(huán)境狀況有關(guān)，而與起始位置無(wú)關(guān)。每一個(gè)柵格都只能向上下左右四個(gè)柵格做擴(kuò)散，而且任一柵格的氣味濃度值都從比它濃度大1 的柵格擴(kuò)散而來(lái)。所以任取一個(gè)濃度為X(X ＜M)的柵格，周圍一定至少有一個(gè)柵格的濃度為X+1，依此類推：氣味濃度為X+1的柵格周圍一定至少有一個(gè)濃度為X+2的柵格，追溯到最后一定能找出濃度為M 的柵格，即是到達(dá)終點(diǎn)。將每個(gè)柵格的氣味濃度值作為此柵格的代價(jià)，由此把二維地圖轉(zhuǎn)化為基于濃度的代價(jià)地圖。因此，從起始柵格出發(fā)，每次向比當(dāng)前柵格代價(jià)高的柵格移動(dòng)，必然能找到一條通往終點(diǎn)位置的可行路徑。這是將氣味擴(kuò)散法應(yīng)用到RRT 算法中的重要基礎(chǔ)。由氣味擴(kuò)散法生成的代價(jià)地圖如圖5 所示。機(jī)器人所處環(huán)境為8×10的柵格圖，圖中橫向?yàn)閤軸，縱向?yàn)閥 軸，終點(diǎn)坐標(biāo)為（1，1），起點(diǎn)坐標(biāo)為（8，10），黑色區(qū)域?yàn)檎系K物，白色區(qū)域?yàn)樽杂煽臻g。設(shè)終點(diǎn)濃度值為30，可從圖中看出經(jīng)過(guò)氣味擴(kuò)散法處理后自由空間中每個(gè)柵格對(duì)應(yīng)的代價(jià)值。機(jī)器人的一個(gè)可行路徑為（8，10）—（8，9）—（7，9）—（7，8）—（6，8）—（6，7）—（6，6）—（5，6）—（5，5）—（4，5）—（3，5）—（2，5）—（1，5）—（1，4）—（1，3）—（1，2）—（1，1）。

圖5 氣味擴(kuò)散法示意圖

本文提出的改進(jìn)RRT 算法在擴(kuò)展節(jié)點(diǎn)過(guò)程中引入氣味擴(kuò)散法，可準(zhǔn)確有效地指導(dǎo)隨機(jī)樹的生長(zhǎng)：在RRT算法生成新節(jié)點(diǎn)時(shí)根據(jù)柵格代價(jià)來(lái)確定接受或是舍棄，使得隨機(jī)樹朝柵格代價(jià)增大的方向生長(zhǎng)，從而到達(dá)終點(diǎn)，克服了原算法偏離最優(yōu)解的缺陷。由于氣味擴(kuò)散法生成的代價(jià)地圖中每個(gè)柵格周圍都存在比當(dāng)前柵格代價(jià)高的柵格，使得隨機(jī)樹趨向于目標(biāo)點(diǎn)生長(zhǎng)，提高了路徑質(zhì)量。改進(jìn)算法不但采用氣味擴(kuò)散法改善擴(kuò)展節(jié)點(diǎn)的選取，而且保留了原算法的隨機(jī)特性，從而避免了陷入局部最優(yōu)解。

4.3 改進(jìn)RRT算法流程

改進(jìn)RRT 算法采用目標(biāo)偏向策略和氣味擴(kuò)散法來(lái)改善擴(kuò)展節(jié)點(diǎn)的選取，加快了算法收斂速度，從而解決了隨機(jī)點(diǎn)分布過(guò)于平均的缺點(diǎn)。算法流程如下：

步驟1 環(huán)境建模：對(duì)地圖進(jìn)行柵格化，利用氣味擴(kuò)散法得到與柵格地圖相對(duì)應(yīng)的代價(jià)地圖。

步驟2 初始化探索樹T。

步驟3 生成隨機(jī)點(diǎn)Qrand，設(shè)定一個(gè)偏向概率閾值pbias，在隨機(jī)選取擴(kuò)展節(jié)點(diǎn)時(shí)利用均勻概率分布產(chǎn)生一個(gè)隨機(jī)概率p，若p ＞pbias，則Qrand在機(jī)器人狀態(tài)空間中隨機(jī)產(chǎn)生，否則Qrand=Qgoal。

步驟4 選擇樹的生長(zhǎng)點(diǎn)：

步驟5 求探索樹的新節(jié)點(diǎn)Qnew：

步驟6 判斷Qnew所在柵格代價(jià)是否比Qnear所在柵格代價(jià)高，若是則轉(zhuǎn)步驟7，否則返回步驟3。

步驟7 更新探索樹T，判定Qnew是否為探索失敗節(jié)點(diǎn)，若不是，則Qnew加入到T 中。否則，放棄Qnew，返回步驟3。

步驟8 判斷是否到達(dá)目標(biāo)點(diǎn)。若‖ Qgoal-Qnew‖＜s，則到達(dá)目標(biāo)，否則，返回步驟3，其中s 為機(jī)器人最短行走距離。

步驟9 從目標(biāo)節(jié)點(diǎn)逆向回溯至探索樹的根節(jié)點(diǎn)，并返回規(guī)劃路徑。

4.4 基于三次B樣條曲線的路徑平滑

通過(guò)改進(jìn)RRT算法輸出的路徑由折線段構(gòu)成，且路徑長(zhǎng)度不是最短，需要作進(jìn)一步平滑處理。常用的路徑平滑方法有多項(xiàng)式插值、曲線擬合、B樣條插值等。高次插值易在區(qū)間兩端產(chǎn)生震蕩，造成較大的逼近誤差，稱為龍格現(xiàn)象。為了使生成路徑逼近原路徑，平滑過(guò)程中應(yīng)避免龍格現(xiàn)象。采用龍格函數(shù)測(cè)試各種路徑平滑方法的逼近性能，如圖6所示。從圖中可看出，三次曲線擬合的效果最差，所得圖形與原曲線相差甚遠(yuǎn)；多項(xiàng)式插值在中間部分插值效果較好，對(duì)于兩端的插值結(jié)果非常不理想；而B 樣條曲線給出了光滑的插值曲線，逼近效果理想，穩(wěn)定性好，收斂性強(qiáng)，能很好地回避龍格現(xiàn)象。

由于B 樣條曲線能避免龍格現(xiàn)象，具有連續(xù)性和局部性[2]等優(yōu)點(diǎn)，且能生成曲率連續(xù)的平滑路徑，本文采用B 樣條曲線對(duì)路徑進(jìn)行平滑處理。由于B 樣條曲線局部平滑性好，不能在縮短路徑方面取得理想效果，所以在采用B 樣條曲線對(duì)路徑平滑處理前先對(duì)路徑作預(yù)處理。預(yù)處理的目的是縮短路徑，以便后續(xù)平滑處理。

假設(shè)利用改進(jìn)RRT 算法進(jìn)行路徑規(guī)劃的輸出結(jié)果由點(diǎn)序列S={( x1,y1),(x2,y2),… ,(xn,yn)}構(gòu)成，對(duì)點(diǎn)序列S進(jìn)行預(yù)處理，預(yù)處理后路徑規(guī)劃的點(diǎn)序列為要使路徑長(zhǎng)度縮短，必須滿足以下兩個(gè)條件：

圖6 三種方法逼近性能比較

條件（1）的作用是使得平滑后的所有路徑點(diǎn)與對(duì)應(yīng)原始路徑點(diǎn)的距離和最??；條件（2）保證了平滑后的點(diǎn)序列中前一個(gè)路徑點(diǎn)與后一個(gè)路徑點(diǎn)的距離和最小。所以路徑最短的目標(biāo)函數(shù)為：

式（6）中α、β 為權(quán)重因子，且滿足α+β=1。路徑平滑的實(shí)質(zhì)即是求上式的最優(yōu)解，求解最優(yōu)解的方法采用梯度下降法（gradient descent），即通過(guò)多次迭代，調(diào)整P(i)使M 取得最小值，使得路徑變短。通過(guò)式（6）得到的路徑P 仍不夠平滑，需對(duì)P 作進(jìn)一步處理。

為了使路徑變?yōu)槔硐氲钠交€，采用三次B 樣條曲線對(duì)P 作進(jìn)一步優(yōu)化處理。三次B 樣條曲線和控制點(diǎn)間的關(guān)系如公式（7）所示：

式（7）中t ∈[0,1]，Pi、Pi+1、Pi+2、Pi+3為控制點(diǎn)。最后依次連接全部三階B 樣條曲線段即可生成最終路徑。

5 實(shí)驗(yàn)結(jié)果及分析

分別在仿真環(huán)境和真實(shí)環(huán)境下進(jìn)行實(shí)驗(yàn)。將本文改進(jìn)RRT 算法與基本RRT、SARRT 等算法進(jìn)行對(duì)比，驗(yàn)證提出算法的優(yōu)越性和正確性。仿真實(shí)驗(yàn)硬件平臺(tái)為筆記本電腦，真實(shí)環(huán)境實(shí)驗(yàn)平臺(tái)為配備筆記本電腦的Turtlebot2。計(jì)算機(jī)配置為Ubuntu 操作系統(tǒng)，處理器為i3-3110M，主頻2.4 GHz，運(yùn)行內(nèi)存為4 GB。

5.1 仿真實(shí)驗(yàn)

在ROS 的Stage 仿真平臺(tái)中選取三種不同場(chǎng)景進(jìn)行實(shí)驗(yàn)，圖7（a）～（c）分別為地圖1、地圖2、地圖3。三種地圖均為10 m×10 m 的迷宮地圖，地圖大小為200×200像素。

圖7 三種地圖

為了方便處理、提高運(yùn)行效率，把地圖劃分為40×40的柵格。改進(jìn)RRT與SARRT[13]算法的相同點(diǎn)是都會(huì)在隨機(jī)樹生長(zhǎng)前對(duì)地圖進(jìn)行預(yù)處理，利用擴(kuò)散方法構(gòu)造代價(jià)地圖，把柵格代價(jià)大小作為產(chǎn)生新節(jié)點(diǎn)的依據(jù)。以地圖2 為例，對(duì)兩種算法的性能進(jìn)行比較。圖8（a）為改進(jìn)RRT 算法得到的代價(jià)地圖，圖8（b）為SARRT 算法得到的代價(jià)地圖。在圖8 中，目標(biāo)點(diǎn)用三角形標(biāo)記，起點(diǎn)用圓形標(biāo)記；紅色區(qū)域表示高代價(jià)柵格，藍(lán)色區(qū)域表示低代價(jià)柵格。觀察發(fā)現(xiàn)，圖8（b）左中右三個(gè)區(qū)域每個(gè)區(qū)域的顏色大致一樣，表明SARRT 的擴(kuò)散結(jié)果過(guò)于平均，因而局部指導(dǎo)能力較差。與圖8（b）相比，圖8（a）顏色層次分明，更能準(zhǔn)確反映地圖中自由空間的結(jié)構(gòu)。改進(jìn)RRT 的擴(kuò)散過(guò)程采用氣味擴(kuò)散法，該方法局部性能優(yōu)越，在復(fù)雜環(huán)境下的指向性更好，能對(duì)隨機(jī)樹的生長(zhǎng)作精確指導(dǎo)。SARRT 的擴(kuò)散過(guò)程采用菲克第二定律，計(jì)算復(fù)雜，且需要多次迭代才能在環(huán)境中完成擴(kuò)散，因此耗費(fèi)更多時(shí)間。由于氣味擴(kuò)散法計(jì)算簡(jiǎn)單，使得改進(jìn)算法在效率和速度上得到極大的提升。氣味擴(kuò)散完成后開(kāi)始執(zhí)行路徑規(guī)劃任務(wù)，分別在地圖1、地圖2、地圖3下進(jìn)行實(shí)驗(yàn)。設(shè)定搜索步長(zhǎng)s=0.30 m，偏向目標(biāo)概率pbias=0.2，路徑最短目標(biāo)函數(shù)式（6）中的權(quán)重因子α=0.5，β=0.5。

圖8 改進(jìn)RRT與SARRT代價(jià)地圖對(duì)比

地圖1 的環(huán)境比較簡(jiǎn)單，設(shè)定機(jī)器人起點(diǎn)坐標(biāo)為（2，2），終點(diǎn)坐標(biāo)為（2，9）。圖9（a）～（c）分別為基本RRT、SARRT 和本文提出的改進(jìn)RRT 算法在地圖1 中的輸出結(jié)果。

圖9 地圖1中三種算法路徑規(guī)劃結(jié)果比較

地圖2的場(chǎng)景比地圖1復(fù)雜，設(shè)定機(jī)器人起點(diǎn)為（2，2），終點(diǎn)為（9，2）。圖10（a）～（c）分別為基本RRT、SARRT和本文提出的改進(jìn)RRT算法在地圖2中的輸出結(jié)果。

圖10 地圖2中三種算法路徑規(guī)劃結(jié)果比較

地圖3 的場(chǎng)景最為復(fù)雜，設(shè)定機(jī)器人起點(diǎn)坐標(biāo)為（1，1），終點(diǎn)坐標(biāo)為（9，2）。在地圖3 中，機(jī)器人可以選擇不同的通道到達(dá)終點(diǎn)，路徑長(zhǎng)度與機(jī)器人選擇的通道密切相關(guān)。圖11（a）～（c）分別為基本RRT、SARRT 和本文提出的改進(jìn)RRT算法在地圖3中的輸出結(jié)果。

圖11 地圖3中三種算法路徑規(guī)劃結(jié)果比較

由圖9 和圖10 知，地圖1 和地圖2 的共同特點(diǎn)是機(jī)器人只有一個(gè)通道到達(dá)終點(diǎn)，在此種情況下改進(jìn)RRT算法表現(xiàn)最優(yōu)。由圖11 知，基本RRT 的路徑?jīng)]有方向性，機(jī)器人可能會(huì)選擇距離最長(zhǎng)的通道，導(dǎo)致路徑長(zhǎng)度增大。而SARRT 和改進(jìn)RRT 的路徑具有方向性，機(jī)器人會(huì)選擇距離最短的通道到達(dá)終點(diǎn)，因而路徑長(zhǎng)度縮短，路徑質(zhì)量更高。比較圖11（b）和（c）可知，與SARRT 相比，改進(jìn)RRT的路徑長(zhǎng)度更短，路徑平滑度得到提升，所以改進(jìn)RRT算法的性能優(yōu)于SARRT。

由于RRT算法具有隨機(jī)性，為了客觀評(píng)價(jià)算法的優(yōu)劣，在三種地圖場(chǎng)景中分別對(duì)三種算法進(jìn)行20 次路徑規(guī)劃。設(shè)定三種算法的搜索步長(zhǎng)s=0.30 m，改進(jìn)RRT 算法的偏向目標(biāo)概率pbias=0.2，路徑最短目標(biāo)函數(shù)式（6）中的權(quán)重因子α=0.5，β=0.5。將路徑長(zhǎng)度、搜索時(shí)間和采樣節(jié)點(diǎn)數(shù)作為三種算法的評(píng)價(jià)指標(biāo)，記錄20 次實(shí)驗(yàn)的平均值。地圖1、地圖2、地圖3 仿真實(shí)驗(yàn)數(shù)據(jù)如表1、表2、表3所示。

表1 地圖1仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比

表2 地圖2仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比

表3 地圖3仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比

通過(guò)圖9～11和表1～3中的仿真實(shí)驗(yàn)數(shù)據(jù)可知，與基本RRT 和SARRT 相比，改進(jìn)RRT 算法的搜索時(shí)間更短，在效率和速度上得到極大提升，而且路徑長(zhǎng)度縮短、路徑更為平滑，使得路徑質(zhì)量得到顯著改善，證明改進(jìn)RRT算法在復(fù)雜環(huán)境下進(jìn)行路徑規(guī)劃具有優(yōu)良的性能。

5.2 Turtlebot2實(shí)驗(yàn)

圖12 改進(jìn)RRT算法實(shí)驗(yàn)場(chǎng)景

為了測(cè)試實(shí)際場(chǎng)景下改進(jìn)RRT算法的性能，將算法應(yīng)用到基于ROS的移動(dòng)機(jī)器人Turtlebot2上。本文的實(shí)驗(yàn)場(chǎng)景為實(shí)驗(yàn)室，如圖12（a）所示。圖12（b）為實(shí)驗(yàn)室的整體環(huán)境圖。首先用無(wú)線控制器控制Turtlebot2掃描室內(nèi)環(huán)境，利用gmapping 算法構(gòu)建2 維地圖，地圖尺寸為576×512 像素。然后在建好的地圖上進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，并將實(shí)驗(yàn)結(jié)果與基本RRT和SARRT進(jìn)行對(duì)比。

在ROS 的Rviz 可視化工具中顯示二維地圖和路徑規(guī)劃結(jié)果，基本RRT 算法的輸出結(jié)果如圖13（a）所示，SARRT算法的輸出結(jié)果如圖13（b）所示，改進(jìn)RRT算法的輸出結(jié)果如圖13（c）所示。圖中的黑色區(qū)域表示障礙物，白色區(qū)域?yàn)闄C(jī)器人運(yùn)動(dòng)的自由空間。

圖13 實(shí)驗(yàn)室環(huán)境下三種算法輸出結(jié)果

表4 列出了三種算法在相同起始點(diǎn)下的路徑長(zhǎng)度、搜索時(shí)間和采樣節(jié)點(diǎn)數(shù)。由實(shí)驗(yàn)數(shù)據(jù)可知，改進(jìn)RRT算法優(yōu)于SARRT 和基本RRT。與基本RRT 相比，改進(jìn)RRT的路徑長(zhǎng)度大大縮減；與SARRT相比，改進(jìn)RRT的搜索速度更快。考慮到算法各方面的性能，認(rèn)為改進(jìn)RRT算法能在真實(shí)環(huán)境下生成最優(yōu)路徑。

表4 實(shí)驗(yàn)室環(huán)境下三種算法實(shí)驗(yàn)數(shù)據(jù)對(duì)比

實(shí)驗(yàn)結(jié)果證明，結(jié)合了目標(biāo)偏向策略和氣味擴(kuò)散法的改進(jìn)RRT 算法能高效完成復(fù)雜環(huán)境下的移動(dòng)機(jī)器人路徑規(guī)劃。相比于基本RRT 和SARRT 算法，改進(jìn)RRT算法優(yōu)化了節(jié)點(diǎn)擴(kuò)展方式，因此搜索速度更快，路徑質(zhì)量更優(yōu)。

6 結(jié)束語(yǔ)

基本RRT 算法不能滿足真實(shí)環(huán)境下移動(dòng)機(jī)器人路徑規(guī)劃的實(shí)際需求。為了提高路徑規(guī)劃的效率和質(zhì)量，對(duì)RRT算法進(jìn)行改進(jìn)，在原算法的基礎(chǔ)上引入目標(biāo)偏向策略和氣味擴(kuò)散法，利用基于濃度場(chǎng)的成本地圖精確指導(dǎo)隨機(jī)樹的生長(zhǎng)，從而選擇最優(yōu)采樣節(jié)點(diǎn)，提高搜索效率。為了使生成路徑是理想平滑曲線，采用基于三次B樣條曲線的路徑平滑方法對(duì)生成的路徑進(jìn)行優(yōu)化，進(jìn)一步改善了路徑質(zhì)量。不同環(huán)境下的仿真實(shí)驗(yàn)表明，改進(jìn)RRT算法能顯著提高搜索效率和避障能力，而且路徑長(zhǎng)度更短、路徑質(zhì)量更優(yōu)，尤其在復(fù)雜環(huán)境下表現(xiàn)出優(yōu)良的性能。將改進(jìn)RRT 算法應(yīng)用到Turtlebot2 上并在真實(shí)環(huán)境下進(jìn)行實(shí)驗(yàn)，結(jié)果表明改進(jìn)RRT 算法可滿足實(shí)際需求，能高效解決移動(dòng)機(jī)器人在復(fù)雜環(huán)境下的路徑規(guī)劃問(wèn)題。