未知環(huán)境下移動機器人實時路徑規(guī)劃

張捍東，陳 陽，吳玉秀

安徽工業(yè)大學(xué) 電氣與信息工程學(xué)院，安徽 馬鞍山 243002

1 引言

移動機器人自主導(dǎo)航技術(shù)是移動機器人研究的重要分支之一，而路徑規(guī)劃是導(dǎo)航研究的一個重要環(huán)節(jié)，所謂路徑規(guī)劃是指在具有障礙物的環(huán)境中，按照一定的評價標準，尋找一條從起始狀態(tài)到目標狀態(tài)的無碰撞最優(yōu)或次優(yōu)路徑[1]。

根據(jù)對環(huán)境信息的掌握程度可以將移動機器人路徑規(guī)劃問題分為全局路徑規(guī)劃和局部路徑規(guī)劃[2-3]。全局路徑規(guī)劃需要掌握所有的環(huán)境信息，根據(jù)所有的環(huán)境信息再進行路徑規(guī)劃，而局部路徑規(guī)劃只需掌握移動機器人當(dāng)前可獲取的環(huán)境信息。

快速擴展隨機樹（Rapidly-exploring Random Tree，RRT）算法[4]是近幾年發(fā)展起來的基于采樣的單查詢路徑規(guī)劃方法，在處理非完整約束的路徑規(guī)劃問題時具有較大的優(yōu)勢。這種基于隨機采樣的運動規(guī)劃方法由于其搜索方向的隨機性，具有概率完備性，在有解的前提下，算法可獲得可行解[5]。但由于該算法通常只能應(yīng)用于已知環(huán)境中，所以環(huán)境信息獲取時的計算量會很大[6]。

本文提出一種改進的RRT算法，該方法在滾動窗口內(nèi)實現(xiàn)RRT算法，計算量大大減少；在隨機環(huán)境中，機器人應(yīng)用搭載的激光雷達，采集可調(diào)節(jié)滾動窗口內(nèi)的環(huán)境信息，針對不同環(huán)境自主調(diào)整規(guī)劃參數(shù)，實現(xiàn)隨機環(huán)境中實時的路徑規(guī)劃。

2 RRT算法的基本原理

RRT算法在路徑規(guī)劃時以狀態(tài)空間中的一個初始點作為根節(jié)點，通過隨機采樣增加葉節(jié)點的方式，生成一個隨機擴展樹。當(dāng)隨機樹的葉節(jié)點包含目標點或目標區(qū)域時，從目標狀態(tài)出發(fā)，找到父節(jié)點，依次進行，直至初始狀態(tài)點，即可得到一條規(guī)劃路徑。

對移動機器人的工作空間（WorkSpace）進行環(huán)境建模，考慮輪式移動機器人的工作環(huán)境是二維工作空間W，且W∈R3，移動機器人的狀態(tài)空間包含機器人的坐標位置和朝向角?；镜腞RT算法描述[7-8]如下所示：首先將起點xinit放入隨機樹T中，作為根節(jié)點；在Wfree中隨機選取一個位姿狀態(tài)xrand；用Nearest_Neighbor()函數(shù)搜索樹上距離xrand最近的擴展節(jié)點xnear；根據(jù)給定的標準選取輸入u使得xnear盡可能接近xnew，反復(fù)調(diào)用函數(shù)EXTEND()，直到檢測到障礙物為止，此時產(chǎn)生的一個新節(jié)點xnear被添加到樹T上；選擇新的隨機狀態(tài)xrand，重復(fù)執(zhí)行上述算法，直至xnew=xgoal或xnew∈xgoal時，程序結(jié)束。

基本的RRT算法描述：

3 改進的RRT算法

3.1 基本原理

移動機器人實際工作環(huán)境是未知甚至復(fù)雜的，往往很難獲取完整全局信息，且為了保證實時性，系統(tǒng)計算量不宜過大。針對上述問題，首先采用僅獲取滾動窗口內(nèi)環(huán)境信息的方法，因為RRT算法具有概率完備性，進而采用將RRT算法與滾動窗口相結(jié)合的方法，該方法通過分析滾動窗口內(nèi)移動機器人上搭載的傳感器探測到的環(huán)境信息結(jié)合RRT算法進行路徑規(guī)劃?？紤]到某些環(huán)境下固定的窗口大小不能滿足要求，本文提出了一種滾動窗口大小根據(jù)環(huán)境自適應(yīng)調(diào)節(jié)的方法，并引入啟發(fā)式估價函數(shù)，規(guī)劃機器人從當(dāng)前位姿到子目標點的路徑，再控制移動機器人運動至子目標點。為了避免移動機器人運動到子目標點后陷入死鎖，添加了一種動態(tài)監(jiān)測機制，保證機器人到達合理的子目標點，重復(fù)上述過程，直至到達目標區(qū)域。

3.2 啟發(fā)式估價函數(shù)的引入

類比A*算法[9-10]，為每個子節(jié)點x定義一個估價函數(shù)：表示從當(dāng)前滾動窗口內(nèi)的起始位姿到節(jié)點x的實際代價即實際路徑長度，h(x)表示從節(jié)點x到目標xgoal的估計代價即啟發(fā)式估價函數(shù)，令Dis(x1,x2)代表隨機樹中兩個位姿節(jié)點間的歐幾里得距離，取h(x)=Dis(x,xgoal)，x的節(jié)點選取滿足使得f(x)最小。這樣可以保證擴展樹始終朝著目標點的方向生長，但是這可能會使得擴展樹陷入局部最優(yōu)，本文采用文獻[11]提出的節(jié)點取消復(fù)原的方法來解決局部最優(yōu)問題。主要思想是探索以前用過的空間是無用的，且易陷入局部最優(yōu)。解決辦法是生成新節(jié)點時，若xnew與父節(jié)點xnear的距離小于其與擴展樹上的其他任何節(jié)點的距離，則將該節(jié)點加入擴展樹。

3.3 滾動窗口的構(gòu)建

移動機器人的實際工作環(huán)境往往是未知且復(fù)雜的，且對于實時性的要求高，所以全局路徑規(guī)劃往往是不可行的。

滾動規(guī)劃算法主要包括：環(huán)境信息預(yù)測、局部滾動優(yōu)化、反饋信息校正[12]。主要思路是將傳感器可探測的環(huán)境作為當(dāng)前窗口，在該窗口內(nèi)進行路徑規(guī)劃，選取最優(yōu)子目標點，找出起始位置到子目標點可行的局部路徑，子目標點的選取方法將在下一小節(jié)說明。機器人運動過程中，每一個運動周期內(nèi)的環(huán)境信息總在更新，從而將完整的規(guī)劃路徑分割成一步步的局部路徑，與全局路徑規(guī)劃相比，計算量大大減小，可用于實時路徑規(guī)劃。

滾動窗口的選取過程如下：如圖1所示，令機器人搭載激光雷達傳感器的掃描半徑為r，掃描區(qū)域為圖1中View代表的區(qū)域即0～180°，直角坐標系表示當(dāng)前時刻機器人的坐標系，朝向目標區(qū)域。

圖1 滾動窗口示意圖

3.4 子目標點的選取

當(dāng)隨機樹在擴展過程中遇到障礙物時，為了使隨機樹快速成功地繞過障礙物，避免在局部復(fù)雜區(qū)域進行無效搜索，消耗迭代次數(shù)，這時就需要確定隨機樹的局部擴展方向[13-14]。如圖2所示，Obs區(qū)域為工作空間中的障礙物區(qū)域，實際操作時將激光雷達掃描到的障礙物進行膨脹處理即可。膨脹處理后的障礙物如圖2中Obs區(qū)域中用純黑色區(qū)域表示，定義移動機器人當(dāng)前位姿為滾動窗口圓心處S，目標區(qū)域中心G，為獲取滾動窗口邊界上的子目標點，定義子目標點為點P。有其中為定值與正相關(guān)，圖2中取B為子目標點xsub。

圖2 子目標點選取示意圖

由于滾動窗口的狀態(tài)空間采樣次數(shù)和窗口大小有關(guān)，在不同的工作環(huán)境，應(yīng)當(dāng)配置不同的參數(shù)[15]，而實際的環(huán)境是未知的甚至是復(fù)雜的；如圖3所示，移動機器人所處環(huán)境為通道環(huán)境且障礙物已經(jīng)過膨脹處理，若采用原始半徑為r的滾動窗口，則機器人前方0～180°滾動窗口邊界無滿足子目標點選取的xsub，從而使移動機器人陷入死鎖。為解決該問題，本文提出一種滾動窗口大小自適應(yīng)的方法，當(dāng)局部環(huán)境中無障礙物，則子目標點為起始位姿和目標位姿連線與滾動窗口的交點，當(dāng)局部環(huán)境類于圖2。滾動窗口上存在xsub∈Wfree，通過計算篩選選取B為子目標點xsub，當(dāng)局部環(huán)境如圖3所示，半徑為r的滾動窗口上不存在xsub，分析激光雷達獲取的環(huán)境信息，取最大障礙距離dmax和最小障礙距離dmin，令新的滾動窗口半徑為rnew=(dmax+dmin)/2，在新的滾動窗口中判斷是否存在可行的子目標點xsub，若存在，xsub的選取類于圖2的局部環(huán)境；否則，更新滾動窗口下dmax、dmin、rnew值，并繼續(xù)上述判斷過程。為了防止移動機器人從復(fù)雜環(huán)境逃離后一直以較小滾動窗口進行路徑規(guī)劃，從而導(dǎo)致系統(tǒng)效率過低，所以當(dāng)移動機器人到達子目標點后需判斷新的窗口環(huán)境，若新的窗口環(huán)境存在滿足要求的子目標點xsub，則將已經(jīng)更新了的rnew還原至初始值r，并繼續(xù)上述判斷過程。

圖3 復(fù)雜環(huán)境子目標點選取

3.5 動態(tài)監(jiān)測機制的引入

當(dāng)環(huán)境信息如圖4所示，障礙物類似于圖中Obs的凹型障礙，移動機器人的起始位姿xinit，起點為O，如圖4所示的實線空心半圓，根據(jù)前文所述的子目標點選取策略，選取B為子目標點，若移動機器人運動至B點，考慮到實際所采用傳感器的最小測量距離，此時機器人距離障礙物太近，導(dǎo)致傳感器無法進行下一步規(guī)劃，無法繼續(xù)進行避障。為了解決該問題，引入了一種動態(tài)監(jiān)測機制，當(dāng)機器人位于起始點O，滾動窗口外的區(qū)域?qū)τ跈C器人而言為未知環(huán)境，此時規(guī)劃路徑時當(dāng)作無障礙路徑處理，B為子目標點，OB為路徑規(guī)劃方向，動態(tài)監(jiān)測機制將在移動機器人運動的過程中監(jiān)測事先規(guī)劃好的路徑，向未知區(qū)域延伸，并實時更新滾動窗口內(nèi)的環(huán)境信息，若原路徑方向出現(xiàn)障礙物，則重新選取子目標點，如圖4所示，當(dāng)機器人移動至點O1，滾動窗口更新為圖中虛線空心圓，此時監(jiān)測到原路徑OB方向出現(xiàn)障礙物，重新選取子目標點，考慮在圖4環(huán)境中啟發(fā)式估價函數(shù)約束下RRT選取的隨機性，假定選取右側(cè)路徑，更新后的環(huán)境信息類似于前文中圖2所示環(huán)境；為了讓機器人逃逸出這種陷阱環(huán)境，采取的方法是約束RRT的擴展范圍，使其朝著臨近障礙物的位置擴展，圖4中線段L的方向為機器人大致的運動方向即當(dāng)前時刻機器人的朝向；通過實驗分析，在該類環(huán)境下還需考慮機器人逃逸出障礙時的朝向問題。在機器人實際運動過程中，對傳感器采集到的環(huán)境中的障礙物進行膨脹處理。當(dāng)機器人運動至點O2，此時滾動窗口邊界與連續(xù)的障礙物存在交點，而當(dāng)機器人運動至點O3，此時滾動窗口內(nèi)的可視障礙物邊界點為點E，點E位于滾動窗口內(nèi)而非邊界上，由于空間物體的連續(xù)性，可知此時障礙物的延伸方向為機器人當(dāng)前坐標系下的O3S的左側(cè)，控制機器人到達子目標點Ssub，滾動窗口環(huán)境如圖5所示，分析機器人逃逸后的朝向問題。

圖4 動態(tài)監(jiān)測機制圖示

圖5 機器人朝向分析

圖5 中，點S為滾動窗口內(nèi)傳感器可探測的最邊界，根據(jù)前文所述的約束，控制子目標點到達膨脹處理后的點Ssub，此時機器人的View范圍沒有探測到障礙物，且考慮到障礙物邊界的種類，大致可以分為如圖5所示的m、n、k三種類型；為了保證機器人成功逃逸出陷阱環(huán)境，當(dāng)機器人成功越過障礙物邊界頂點E、點E1或點E3即可，采取的方法是：當(dāng)機器人位姿從O3變?yōu)镾sub，即從窗口可探測到障礙物進入窗口無障礙物環(huán)境，此時RRT的擴展方向參考上一次規(guī)劃的方向，即在當(dāng)前機器人坐標系下的第二象限結(jié)合啟發(fā)式估價函數(shù)在臨近-x軸方向進行子目標點的選取。當(dāng)目標位于機器人坐標系下前方區(qū)域，且滾動窗口View內(nèi)在目標物方向有可行通道時，解除陷阱障礙逃逸約束。

成功越過障礙物邊界頂點的可行性分析如下：

若障礙物邊界的延伸方向為m、n、k，由于子目標點的選取在當(dāng)前機器人坐標下的第二象限臨近-x軸的方向，所以最多進行兩次規(guī)劃，規(guī)劃的子目標點為Ssub1、Ssub2，此時O3SsubSsub1Ssub2組成的封閉幾何圖形近似為正方形，比值越小越接近正方形，rrobot表示機器人半徑，且，則存在點Ssub2在O3S1上的投影可越過障礙物邊界頂點。

3.6 步長的選取

隨機樹在擴展過程中，設(shè)定步長的不同會導(dǎo)致路徑規(guī)劃結(jié)果的不同，規(guī)劃所消耗的時間也會不同。若步長太小，相應(yīng)的擴展節(jié)點會增多，會導(dǎo)致處理時間增加，不利于系統(tǒng)的實時性要求，若步長太大，當(dāng)機器人處于復(fù)雜環(huán)境下，可能不存在滿足步長要求的隨機點xnew，導(dǎo)致隨機樹重復(fù)搜索甚至陷入死鎖[14]。為了滿足實際需求，本文采用了一種變步長的方法。當(dāng)搭載的傳感器探測到局部環(huán)境中不存在障礙物時，則采用原始步長λ且采用貪婪法使得移動機器人向目標區(qū)域移動，當(dāng)探測到的局部環(huán)境類似于前文圖2所示，則選取步長為原始步長和滾動窗口半徑二者取小，即λnew=min(λ,r)，當(dāng)探測到的局部環(huán)境類似于前文圖3所示，選取步長為原始步長和更新后的滾動窗口半徑二者取小，即λnew=min(λ,rnew)。

3.7 算法的實現(xiàn)

綜合前文所述，所提算法具體的實現(xiàn)步驟如下：

步驟1初始化，包括根據(jù)機器人的尺寸將獲取的障礙物進行膨脹處理、移動機器人識別出目標機器人的位姿并朝向目標、全向激光雷達獲取前方的環(huán)境信息，設(shè)置初始 r、λ(r＞λ)等。

步驟2判斷滾動窗口邊界是否存在可行的子目標點xsub，若存在，跳轉(zhuǎn)至步驟3，否則跳轉(zhuǎn)至步驟4。

步驟3在滾動窗口內(nèi)的Wfree空間中隨機選取一個狀態(tài)xrand，并根據(jù)最短路徑思想選取xnear，根據(jù)步長λ和節(jié)點取消復(fù)原方法確定xnew，跳轉(zhuǎn)至步驟5。

步驟4 自適應(yīng)調(diào)整λ、r為λnew、rnew，并跳轉(zhuǎn)至步驟3。

步驟5判斷擴展樹是否到達子目標點，若到達，控制移動機器人輸入（速度、角度）使得機器人朝向子目標點運動，動態(tài)監(jiān)測規(guī)劃的路徑，若路徑中無障礙物，跳轉(zhuǎn)至步驟6，否則跳轉(zhuǎn)至步驟3。

步驟6判斷移動機器人是否到達目標區(qū)域xgoal，若到達，結(jié)束，否則跳轉(zhuǎn)至步驟3。

4 實驗

4.1 實驗平臺和實驗環(huán)境

實驗平臺為HCR開源移動機器人機械套件和自主搭建的軟硬件平臺，采用的主控制器為嵌入式ARM開發(fā)板，主頻1.6 GHz，如圖6所示，該平臺搭載全景視覺模塊和激光雷達模塊。實驗環(huán)境為室外非強光環(huán)境。該平臺搭載的激光雷達，掃描頻率為6 Hz，精度為1°，可獲取移動機器人前方0～180°區(qū)域的環(huán)境信息，該平臺搭載了全景視覺模塊，具有識別特定特征目標的能力，且可計算出目標相對于機器人當(dāng)前位姿的坐標和角度，實驗中將目標物用另外一臺符合識別特征的機器人代替，期望的結(jié)果是移動機器人從起點出發(fā)，向目標機器人移動，且運動過程中僅通過分析滾動窗口內(nèi)激光雷達獲取的實時環(huán)境信息，結(jié)合算法進行路徑規(guī)劃，躲避環(huán)境中的障礙，并成功抵達目標區(qū)域。

圖6 實驗平臺

將如圖7的實驗環(huán)境在移動機器人平臺上處理后，環(huán)境顯示如圖8所示?？芍诋?dāng)前窗口邊界內(nèi)激光雷達可探測到的環(huán)境是已知的，窗口外以及未探測到的環(huán)境是未知的，環(huán)境中激光雷達可探測到的障礙物（例如障礙物A）已進行了可視化標記處理，無法探測的信息（例如障礙物B）僅顯示。

圖7 實際的環(huán)境

圖8 處理后的環(huán)境信息顯示

4.2 隨機點狀環(huán)境

當(dāng)移動機器人處于如圖9所示的隨機點狀環(huán)境中，針對兩種算法分別進行了20次實驗，圖10給出了兩種算法的路徑規(guī)劃過程，順序為A-B-C-D?？芍谠摥h(huán)境下機器人在滾動窗口內(nèi)根據(jù)約束條件自主實時規(guī)劃可行的局部路徑，控制機器人到達子目標點，并重復(fù)上述過程，最終到達目標區(qū)域；其中，實心圓為目標機器人，空心圓為目標區(qū)域，障礙物已經(jīng)過膨脹處理；圖11給出了二者實際路徑結(jié)果對比，其中滾動RRT路徑的平均路徑長度為3 267 mm，所提算法的平均路徑長度為3 228 mm。實驗表明，所提算法和常規(guī)滾動RRT規(guī)劃算法在隨機點狀環(huán)境中都可以實時規(guī)劃路徑，且規(guī)劃的路徑平均長度接近。

圖9 隨機點狀實驗環(huán)境

圖10 隨機點狀環(huán)境下路徑規(guī)劃過程對比

圖11 隨機點狀環(huán)境下實際路徑對比

4.3 通道環(huán)境

當(dāng)移動機器人處于如圖12所示的通道環(huán)境中，此時環(huán)境類似于圖3所示。圖13給出了兩種算法的路徑規(guī)劃過程對比，順序為A-B-C-D，其中虛線半圓為所提算法當(dāng)前時刻滾動窗口大小，可知，在該環(huán)境下，由于傳統(tǒng)滾動RRT算法滾動窗口半徑固定，當(dāng)機器人運動至A中的子目標點后會導(dǎo)致無法繼續(xù)選取滿足條件的子目標點，從而使移動機器人陷入死鎖，而所提算法由于窗口半徑可自適應(yīng)調(diào)整，當(dāng)機器人到達A中的子目標點后，自主調(diào)整窗口半徑，使得移動機器人在窗口邊界上可以找到滿足條件的子目標點，由B、C可知調(diào)整后的滾動窗口避免機器人陷入死鎖，由D可知機器人成功通過了該環(huán)境。圖14給出了所提算法與滾動RRT算法實際路徑的結(jié)果對比，其中，實心圓為目標機器人，空心圓為目標區(qū)域，障礙物已經(jīng)過膨脹處理。實驗結(jié)果表明所提算法在通道環(huán)境中可實時進行路徑規(guī)劃。

圖12 通道實驗環(huán)境

圖13 通道環(huán)境下路徑規(guī)劃過程對比

圖14 通道環(huán)境下實際路徑對比

4.4 陷阱環(huán)境

當(dāng)移動機器人處于如圖15所示的陷阱環(huán)境中，此時環(huán)境類似于圖4所示。圖16給出了兩種算法的路徑規(guī)劃過程對比，順序為A-B-C-D-E-F-G-H-I，可知，傳統(tǒng)的滾動RRT算法在選取了當(dāng)前環(huán)境下的子目標點后，由于窗口外的環(huán)境為未知環(huán)境，機器人運動至D中的子目標點后，由于此時視野域已被障礙物完全覆蓋，無法進行下一步規(guī)劃，從而使機器人陷入死鎖。而所提算法中加入的動態(tài)監(jiān)測機制，實時監(jiān)測規(guī)劃好的路徑，在機器人向B和C中的子目標點運動的過程中，監(jiān)測到原路徑不可行，重新規(guī)劃路徑，根據(jù)前文所述的約束，機器人在D、E、F中沿著障礙物前進，在G、H中成功越過了障礙，逃逸成功后在i中解除約束。圖17給出了所提算法與滾動RRT算法實際路徑規(guī)劃的結(jié)果對比，其中，實心圓為目標機器人，空心圓為目標區(qū)域，障礙物已經(jīng)過膨脹處理。實驗結(jié)果表明所提算法在陷阱環(huán)境中可實時進行路徑規(guī)劃，且可逃逸出該障礙環(huán)境并成功抵達目標區(qū)域。

圖15 陷阱實驗環(huán)境

圖16 陷阱環(huán)境下路徑規(guī)劃過程對比

圖17 陷阱環(huán)境下實際路徑對比

5 結(jié)束語

本文分析了基本的RRT算法，將基本RRT與自適應(yīng)滾動窗口相結(jié)合，并加入了動態(tài)監(jiān)測機制，用于實時監(jiān)測規(guī)劃好的路徑，將該方法應(yīng)用于未知環(huán)境下的移動機器人實時的路徑規(guī)劃。通過對大量實驗的結(jié)果對比分析，表明該方法可以滿足實時路徑規(guī)劃的需求，在隨機點狀環(huán)境中所提算法和滾動RRT的效果比較接近，在通道環(huán)境下，固定窗口半徑的滾動RRT算法相較與所提算法易陷入死鎖，在類似于凹型陷阱環(huán)境下，所提算法加入的動態(tài)監(jiān)測機制，可以使得移動機器人逃逸出陷阱并成功抵達目標區(qū)域，避免陷入死鎖。由于機器人在運動過程中對規(guī)劃好的路徑是實時監(jiān)測的，當(dāng)監(jiān)測到原規(guī)劃路徑時可行變?yōu)椴豢尚?，則進行重新規(guī)劃，該過程為實時的，因此本文算法對于動態(tài)環(huán)境下的機器人實時路徑規(guī)劃同樣具有一定的實用性和有效性。