基于改進(jìn)RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃

金丹

（上海海事大學(xué)信息工程學(xué)院，上海 201306）

0 引言

機(jī)器人學(xué)一直是科學(xué)技術(shù)研究領(lǐng)域的熱點(diǎn)問題，它集中了多種尖端學(xué)科最先進(jìn)的研究成果。隨著社會(huì)的發(fā)展與科技的進(jìn)步，機(jī)器人的應(yīng)用已經(jīng)越來越普遍了，其中移動(dòng)機(jī)器人的使用更是受到人們的青睞。路徑規(guī)劃問題是移動(dòng)機(jī)器人研究中的一個(gè)重要環(huán)節(jié)，它的目的是使移動(dòng)機(jī)器人自主并且安全地從初始位姿移動(dòng)到目標(biāo)位姿。前人在路徑規(guī)劃方面的研究已經(jīng)有了顯著的成效，但仍有一些算法上的不足。本文所研究的快速擴(kuò)展隨機(jī)樹(Rapidly-exploring Random Tree,RRT）是一種數(shù)據(jù)結(jié)構(gòu)和算法[1]，通過隨機(jī)地并且增量式地在狀態(tài)空間中生成采樣點(diǎn)，快速有效地搜索高維空間，把搜索方向?qū)蚩瞻讌^(qū)域[2-3]，這些特點(diǎn)使得它在路徑規(guī)劃方面有良好的應(yīng)用前景與深入的研究。針對(duì)其搜索過于平均，規(guī)劃路徑非最優(yōu)等缺陷，對(duì)基本快速擴(kuò)展隨機(jī)樹算法加以改進(jìn)，引入Dijkstra算法，實(shí)現(xiàn)了移動(dòng)機(jī)器人最優(yōu)路徑的規(guī)劃。

1 基本RRT算法及其改進(jìn)原理

機(jī)器人的路徑規(guī)劃[4-5]問題被定義為：給定機(jī)器人在運(yùn)動(dòng)區(qū)域的初始位姿qinit和終點(diǎn)位姿qgoal找到一條路徑，即一個(gè)位姿的連續(xù)序列，使得機(jī)器人沿著該路徑能夠從初始位姿運(yùn)動(dòng)到終點(diǎn)位姿，且在移動(dòng)的過程中不與環(huán)境中的任何障礙物發(fā)生碰撞。路徑規(guī)劃示意圖如圖1所示。圖中灰色的部分都是障礙物區(qū)域，白色的部分是機(jī)器人可以自由移動(dòng)的無障礙區(qū)域，帶有箭頭的曲線便是一條從初始節(jié)點(diǎn)qinit到目標(biāo)節(jié)點(diǎn)qgoal的路徑。

圖1 路徑規(guī)劃示意圖

1.1 基本RRT算法原理

快速擴(kuò)展隨機(jī)樹是一種常見的用于機(jī)器人路徑規(guī)劃的方法，本質(zhì)上是一種隨機(jī)生成的數(shù)據(jù)結(jié)構(gòu)——樹。它遵循控制理論的系統(tǒng)狀態(tài)方程，以初始點(diǎn)為樹的根節(jié)點(diǎn)，在控制量的作用下增量式地產(chǎn)生一條從初始狀態(tài)到新狀態(tài)的隨機(jī)樹，當(dāng)隨機(jī)樹中的葉節(jié)點(diǎn)中包含了目標(biāo)點(diǎn)或者目標(biāo)區(qū)域時(shí)，說明在隨機(jī)樹中找到了一條從根節(jié)點(diǎn)到目標(biāo)點(diǎn)的路徑。RRT的擴(kuò)展方式如圖2所示。

圖2 RRT擴(kuò)展過程

在快速擴(kuò)展隨機(jī)樹中，qinit表示根節(jié)點(diǎn)（初始節(jié)點(diǎn)），qrand表示隨機(jī)節(jié)點(diǎn)，qnear表示隨機(jī)樹中離qrand最近的節(jié)點(diǎn)，在qnear和qrand的連線上以一定的步長λ為單位長度獲取一個(gè)新節(jié)點(diǎn)qnew，然后通過碰撞檢測(cè)，以確保從qnear到qnew這一小段都不與任何障礙物發(fā)生碰撞，則qnew就是擴(kuò)展的新節(jié)點(diǎn)加入到隨機(jī)樹中，反之則需要重新尋找一個(gè)適合的新節(jié)點(diǎn)繼續(xù)擴(kuò)展隨機(jī)樹。重復(fù)以上步驟，直至qnew到達(dá)目標(biāo)點(diǎn)或者目標(biāo)區(qū)域算法結(jié)束，此時(shí)便在樹中找到一條從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑。

1.2 改進(jìn)RRT算法原理

RRT算法相對(duì)于其他算法而言，在搜索效率方面已有很大優(yōu)勢(shì)，在理論上總能找到一條可行路徑，但是仍有需要改進(jìn)的地方。由于其擴(kuò)展的新節(jié)點(diǎn)是在運(yùn)動(dòng)區(qū)域中隨機(jī)產(chǎn)生的，覆蓋的區(qū)域過于平均，在產(chǎn)生的冗余點(diǎn)上有一定的耗時(shí)，同時(shí)規(guī)劃出來的路徑質(zhì)量不高，往往與最短路徑的差距較大。

為了提高路徑規(guī)劃的質(zhì)量，減少移動(dòng)機(jī)器人移動(dòng)過程中消耗過多的時(shí)間，更安全高效地避開障礙物，此處借助于Dijkstra算法。

Dijkstra算法是最早用于機(jī)器人路徑規(guī)劃的一種算法，它致力于規(guī)劃出一條移動(dòng)代價(jià)最小的路徑。它應(yīng)用了貪心算法的模式，是目前公認(rèn)的最好的求解最短路徑的方法。此算法具有設(shè)計(jì)思路簡單、分析速度快、工程實(shí)現(xiàn)能力強(qiáng)、通用性強(qiáng)等優(yōu)點(diǎn)，其主要特點(diǎn)是以起始點(diǎn)為中心向外層層擴(kuò)展，直到擴(kuò)展到終點(diǎn)為止。針對(duì)上述RRT算法的缺陷，引入Dijkstra算法[6-8]，用于處理移動(dòng)機(jī)器人路徑規(guī)劃問題的RRT樹形結(jié)構(gòu)生成后的軌跡尋優(yōu)問題。RRT算法與Dijkstra算法的結(jié)合，在程序搜索之前判斷路徑規(guī)劃的約束條件，不符合約束條件的節(jié)點(diǎn)和邊都去掉，大大的降低了遍歷的節(jié)點(diǎn)和邊的個(gè)數(shù)，這樣Dijkstra算法的搜索效率在RRT的基礎(chǔ)上有了明顯提高，從而更快地找到一條最短路徑的最優(yōu)解[9]。

2 RRT算法改進(jìn)前后對(duì)移動(dòng)機(jī)器人路徑規(guī)劃的研究

移動(dòng)機(jī)器人的路徑規(guī)劃要滿足路徑上的權(quán)重系數(shù)盡量小，根據(jù)不同的環(huán)境情況需要選擇其合適的拓展步長來滿足最優(yōu)路徑規(guī)劃的要求，使其在移動(dòng)的過程中盡量符合約束條件的要求。RRT算法在狀態(tài)空間中完成樹形結(jié)構(gòu)圖擴(kuò)張后得到的可行路徑，僅能作為移動(dòng)機(jī)器人路徑規(guī)劃中的一條通路，但仍未達(dá)到最優(yōu)的要求。在移動(dòng)機(jī)器人的作業(yè)過程中，達(dá)到最優(yōu)規(guī)劃是基本的需求，可以大大地減少機(jī)器人能量的消耗，提高機(jī)器人的工作效率。

下面歸納RRT算法的構(gòu)建過程：

第一步：初始化起始節(jié)點(diǎn)qinit和目標(biāo)節(jié)點(diǎn)qgoal,并將起始節(jié)點(diǎn)qinit作為快速擴(kuò)展隨機(jī)樹T的根節(jié)點(diǎn)qroot，初始化快速擴(kuò)展隨機(jī)樹T；

第二步：判斷|qinit-qgoal|≤λ，且用碰撞檢測(cè)函數(shù)collision(qinit,qgoal)判斷這一段(qinit,qgoal)路徑是否不與障礙物發(fā)生碰撞，若是，轉(zhuǎn)到第八步，若不是，轉(zhuǎn)到第三步；

第三步：生成隨機(jī)點(diǎn)qrand；

第四步：在隨機(jī)樹T中找到離qrand最近的點(diǎn)qnearest；

第五步：在qnearest到qrand的連線上取一個(gè)新的節(jié)點(diǎn)qnew作為T擴(kuò)展的新節(jié)點(diǎn)，使dis( )qnearest,qnew=λ,并且(qnearest,qnew)這段路徑不與障礙物發(fā)生碰撞，若存在這樣的qnew，轉(zhuǎn)到第六步，若不存在，轉(zhuǎn)到第三步；

第六步：將新節(jié)點(diǎn)qnew添加到隨機(jī)樹T中；

第七步：判斷|qnew-qgoal|≤λ，若是轉(zhuǎn)到第八步，不是，轉(zhuǎn)到第三步；

第八步：返回獲得的從起始點(diǎn)qinit到目標(biāo)點(diǎn)qgoal的路徑；

第九步：結(jié)束。

在RRT算法上引入Dijkstra算法對(duì)其加以改進(jìn)，解決最短路徑的尋優(yōu)問題，其核心思想是通過計(jì)算RRT算法提供的節(jié)點(diǎn)來規(guī)劃可通的路徑，并且通過搜索找到最短的路徑。可以解釋為是在RRT算法已經(jīng)找到可通路徑的基礎(chǔ)上，利用Dijkstra算法對(duì)擴(kuò)展的隨機(jī)樹進(jìn)行二次重采樣，將可通路徑調(diào)整為最短路徑。但是增加Dijkstra算法后會(huì)增加路徑規(guī)劃時(shí)間，所以需要在多次仿真實(shí)驗(yàn)中獲得合適的參量條件，減少Dijks?tra算法遍歷的次數(shù)，達(dá)到路徑最優(yōu)與時(shí)間消耗相協(xié)調(diào)。改進(jìn)后的算法是在RRT算法的第七步后調(diào)用Di?jkstra算法，所得路徑的總代價(jià)值costdijk＜costrrt，則更新路徑，將新路徑作為最優(yōu)解。

3 RRT算法改進(jìn)前后在MATLAB下仿真結(jié)果

按照以上原始的RRT算法及其改進(jìn)后的算法步驟，在MATLAB的仿真環(huán)境用代碼實(shí)現(xiàn)以上的理論結(jié)果，為了使實(shí)現(xiàn)的結(jié)果更具有說服性和可靠性，本次程序考慮在多種環(huán)境下執(zhí)行仿真實(shí)驗(yàn)。所有圖的尺寸設(shè)為500×500，圖中的起始節(jié)點(diǎn)坐標(biāo)設(shè)為(10,10）,目標(biāo)節(jié)點(diǎn)坐標(biāo)設(shè)為(490,490）,起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)的選擇不是唯一的，視仿真情況而定。隨機(jī)樹的擴(kuò)展步長設(shè)為20，此步長是在不斷地實(shí)驗(yàn)過程中選擇的較為合適的步長，從而不會(huì)因?yàn)椴介L的選擇不當(dāng)影響程序的效果，好的步長的選擇可以起到很大的作用。圖中黑色的區(qū)域設(shè)為障礙物，作為移動(dòng)機(jī)器人移動(dòng)過程中的威脅源，通過碰撞檢測(cè)函數(shù)使移動(dòng)機(jī)器人在移動(dòng)的過程中避開威脅源，成功的從起始節(jié)點(diǎn)到達(dá)目標(biāo)節(jié)點(diǎn)。仿真結(jié)果的前后對(duì)比圖如圖3-5所示。圖中藍(lán)色的路徑為原始RRT算法實(shí)現(xiàn)的結(jié)果，紅色的路徑為改進(jìn)后RRT算法實(shí)現(xiàn)的結(jié)果。

圖3 簡單障礙地圖環(huán)境

從圖3中可以看出在簡單障礙地圖環(huán)境中，相對(duì)于原始RRT算法規(guī)劃出來的路徑而言，改進(jìn)后的RRT算法規(guī)劃出來的路徑達(dá)到最優(yōu)。仿真數(shù)據(jù)顯示，原始RRT規(guī)劃的路徑長度是7.528023e+02，執(zhí)行時(shí)間是1.479903e+01s，改進(jìn)后的RRT算法規(guī)劃的路徑長度是7.194621e+02，執(zhí)行時(shí)間為2.189992e+01s，可見在時(shí)間消耗上并沒有很大差距，也達(dá)到了路徑的最優(yōu)化。

圖4 狹窄通道地圖環(huán)境

從圖4中可以看出在狹窄通道地圖環(huán)境中，相對(duì)于原始RRT算法規(guī)劃出來的路徑而言，改進(jìn)后的RRT算法規(guī)劃出來的路徑達(dá)到最優(yōu)。仿真數(shù)據(jù)顯示，原始RRT規(guī)劃的路徑長度是1.264349e+03，執(zhí)行時(shí)間是5.700553e+02s，改進(jìn)后的RRT算法規(guī)劃的路徑長度是1.012422e+03，執(zhí)行時(shí)間為5.736099e+02s。相對(duì)于圖4的環(huán)境，此處地圖相對(duì)復(fù)雜有難度，RRT算法的搜索節(jié)點(diǎn)較多，搜索過于平均，但在時(shí)間的消耗上依然得到了控制，同時(shí)達(dá)到了路徑的優(yōu)化。

圖5 復(fù)雜障礙地圖環(huán)境

從圖5中可以看出在復(fù)雜障礙地圖環(huán)境中，相對(duì)于原始RRT算法規(guī)劃出來的路徑而言，改進(jìn)后的RRT算法規(guī)劃出來的路徑達(dá)到最優(yōu)。仿真數(shù)據(jù)顯示，原始RRT規(guī)劃的路徑長度是1.144686e+03，執(zhí)行時(shí)間是6.751927e+01s，改進(jìn)后的RRT算法規(guī)劃的路徑長度是9.200832e+02，執(zhí)行時(shí)間為1.089810e+02s，這里的環(huán)境近似于一個(gè)迷宮，地形復(fù)雜，可通的路徑有待搜索，但在原始RRT算法的基礎(chǔ)上，改進(jìn)后的算法依然達(dá)到了預(yù)期的效果，實(shí)現(xiàn)了在路徑和時(shí)間上平衡的要求?？梢娫趶?fù)雜的地圖環(huán)境下依然可以很好地實(shí)現(xiàn)。

4 結(jié)語

本文主要的目的是為了實(shí)現(xiàn)移動(dòng)機(jī)器人的最優(yōu)路徑的規(guī)劃，使其在移動(dòng)的過程中成功地避開障礙物的威脅，通過最短的路徑，并且實(shí)現(xiàn)消耗的時(shí)間最少的效果。以目前熱門的高效搜索RRT算法為基礎(chǔ)，選擇一種最短路徑搜索Dijkstra算法加以改進(jìn)，充分利用兩種算法的優(yōu)點(diǎn)。在MATLAB仿真環(huán)境下，將改進(jìn)前后的RRT算法進(jìn)行對(duì)比，比較改進(jìn)前后實(shí)現(xiàn)的效果。仿真結(jié)果顯示，改進(jìn)后的算法比原始算法在性能上有了明顯的優(yōu)化，與此同時(shí)，也使用各種不同的地圖環(huán)境驗(yàn)證了算法高效性和可靠性，在時(shí)間和路徑兩方面達(dá)到了很好的均衡，即時(shí)間消耗未增加過多，但路徑達(dá)到最優(yōu)。