未知環境下移動機器人實時路徑規劃

張捍東，陳 陽，吳玉秀

安徽工業大學 電氣與信息工程學院，安徽 馬鞍山 243002

1 引言

移動機器人自主導航技術是移動機器人研究的重要分支之一，而路徑規劃是導航研究的一個重要環節，所謂路徑規劃是指在具有障礙物的環境中，按照一定的評價標準，尋找一條從起始狀態到目標狀態的無碰撞最優或次優路徑[1]。

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

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

本文提出一種改進的RRT算法，該方法在滾動窗口內實現RRT算法，計算量大大減少；在隨機環境中，機器人應用搭載的激光雷達，采集可調節滾動窗口內的環境信息，針對不同環境自主調整規劃參數，實現隨機環境中實時的路徑規劃。

2 RRT算法的基本原理

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

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

基本的RRT算法描述：

3 改進的RRT算法

3.1 基本原理

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

3.2 啟發式估價函數的引入

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

3.3 滾動窗口的構建

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

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

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

圖1 滾動窗口示意圖

3.4 子目標點的選取

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

圖2 子目標點選取示意圖

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

圖3 復雜環境子目標點選取

3.5 動態監測機制的引入

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

圖4 動態監測機制圖示

圖5 機器人朝向分析

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

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

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

3.6 步長的選取

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

3.7 算法的實現

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

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

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

步驟3在滾動窗口內的Wfree空間中隨機選取一個狀態xrand，并根據最短路徑思想選取xnear，根據步長λ和節點取消復原方法確定xnew，跳轉至步驟5。

步驟4 自適應調整λ、r為λnew、rnew，并跳轉至步驟3。

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

步驟6判斷移動機器人是否到達目標區域xgoal，若到達，結束，否則跳轉至步驟3。

4 實驗

4.1 實驗平臺和實驗環境

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

圖6 實驗平臺

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

圖7 實際的環境

圖8 處理后的環境信息顯示

4.2 隨機點狀環境

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

圖9 隨機點狀實驗環境

圖10 隨機點狀環境下路徑規劃過程對比

圖11 隨機點狀環境下實際路徑對比

4.3 通道環境

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

圖12 通道實驗環境

圖13 通道環境下路徑規劃過程對比

圖14 通道環境下實際路徑對比

4.4 陷阱環境

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

圖15 陷阱實驗環境

圖16 陷阱環境下路徑規劃過程對比

圖17 陷阱環境下實際路徑對比

5 結束語

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