基于RGB-D SLAM的智能車自主避障與路徑規劃試驗研究*

李兆凱 李龍勇 李澤暉 孔德成 宋緒丁

（1.長安大學，汽車學院，西安 710064；2.長安大學，道路施工技術與裝備教育部重點實驗室，西安 710064；3.一汽解放汽車有限公司，長春 130011；4.吉林大學，長春 130025）

主題詞：RGB-D SLAM 自主移動小車 避障試驗 路徑規劃 無人駕駛

1 前言

智能移動機器人能夠根據外界環境及任務目標自主移動，近年來在工業領域獲得了廣泛應用。受應用場景的限制，機器人的定位與避障問題較難解決[1-2]。無人駕駛車輛作為典型的自主式移動平臺，解決其定位與避障問題具有重要意義。

即時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）技術被廣泛應用于機器人領域進行建圖與定位工作[3-4]。根據數據源的不同，SLAM可以分為基于距離的SLAM 與基于視覺的SLAM。視覺SLAM 通過處理機器人（相機）拍攝的照片構建環境地圖，成本低、場景表達能力強，被認為是未來SLAM的主要發展方向[5]。視覺SLAM 可分為單目SLAM、雙目SLAM與RGB-D（Red-Green-Blue-Depth）SLAM 3種，其中，RGB-D SLAM 可以直接獲取圖像中物體的深度[6]。P.Henry等人在2010年首次提出RGB-D SLAM方法，并成功構建出全局一致的環境地圖[7]；F.Endres 等人在2012 年提出使用Octomap 結構將環境地圖轉化為可用于機器人導航的柵格地圖的RGB-D SLAM 方法[8]，在2014年提出生成三維地圖的建圖方法[9]；悉尼科技大學的Hu等人提出RGB-D SLAM 改進方法，使用光束平差法發展了一種快速建圖的新方法[10]。

路徑規劃作為無人駕駛的關鍵技術之一，是自主移動機器人的一項重要功能，分為全局路徑規劃和局部路徑規劃2 類方法。A*算法作為常用的全局路徑規劃算法，結合了啟發式方法與圖搜索算法，能快速規劃一條避障的最優路徑[11]。動態A*算法考慮實際應用場景下障礙物的動態變化，實時規劃路徑，避免全局重新規劃[12]，但存在著增加不必要轉彎的缺點，而使用平滑的地圖作為路徑規劃可以解決這一問題[13]。使用跳點搜索方法減少環境地圖中的擴展節點，可提高動態A*算法搜索效率[14]。

本文使用RGB-D SLAM 的方法實現環境全局地圖的建立與小車實時定位的功能，在全局地圖的基礎上應用動態A*算法實現避障與路徑規劃，以期實現無人小車的自主移動與避障功能。

2 算法理論基礎

2.1 RGB-D SLAM

RGB-D SLAM的計算流程如圖1所示，可分為前端與后端2個部分。前端進行信號采集與控制，對相機捕獲的圖像進行特征提取與匹配，在估算小車位姿的同時進行地圖構建，前端的視覺里程計通過估計相鄰圖像間的關系來估計運動，并采用特征點法計算出小車的運動。后端用來處理噪聲，并盡可能地估計出當前狀態的不確定性（最大后驗概率），所有被認為在前端可能產生噪聲的數據，均可通過后端進行處理，以得到全局一致的軌跡。

在搭建前端的過程中會不可避免地出現漂移現象，但這種累積漂移可通過算法進行優化，即后端優化與回環檢測。后端對全局狀態進行估計與優化，主要目的是處理傳感器信號所帶來的噪聲問題。回環檢測通過小車重復經過同一位置時地圖及位姿的匹配對相機軌跡和地圖進行全局一致性估計，從而消除漂移量。

使用小孔成像原理對相機進行建模，通過投影變換得到相機的內參矩陣K與外參矩陣H，進而得到世界坐標系、相機坐標系及圖像坐標系之間的轉換關系：

式中，Xw、Yw、Zw為特征點在世界坐標系中的坐標；u、v為該點在圖像坐標系中的投影坐標；Fw(X)為映射函數。

特征點是圖像中能夠反映本質的像素點，包含圖像之間的匹配關系，是SLAM的基礎支撐。本文使用ORB（Oriented FAST and Rotated BRIEF）特征點進行計算，ORB 特征點算子結合了加速分割測試特征（Features from Accelerated Segment Test，FAST）角點檢測算法與二進制魯棒獨立基本特征（Binary Robust Independent Elementary Features，BRIEF）描述子算法，具備旋轉、平移與光照等不變性[15-17]。

FAST 角點檢測算法對每個像素點p周圍各點與該點灰度值之差的絕對值與閾值εd進行比較，并統計前者大于后者的數量N，當N＞12個時，則標記p為角點：

式中，I(i)、I(p)分別為第i點、臨近點的灰度值。

ORB 算子通過灰度質心法為其加入了方向特性，通過對圖像金字塔上每一層圖像進行角點檢測，使其具有尺度不變性。BRIEF 描述子通過比較測試點對的灰度值來描述圖像，其表達式為：

式中，fnd(p)為P點描述子的值，其結果的二進制形式等價于將式（4）得到的值串聯形成的向量；nd為特征點周圍按照選定方法挑選的“點對”的數量；xj、yj為以特征點為中心的鄰域窗口中選取的隨機點。

通過將測試點對與對應的旋轉矩陣相乘，使得特征描述子具有旋轉不變性。本文選擇漢明距離作為特征之間的相似性度量，增加了篩選錯誤匹配的條件，利用次優匹配來進一步剔除錯誤的匹配。經前后幀的特征點匹配后，采用迭代最近點（Iterative Closest Point，ICP）算法進行相機的位姿估計。ICP算法對2幀圖像中匹配的特征點建立歐拉變換公式，對每組圖像建立最小二乘問題：

式中，J為待優化目標；pi、為匹配的特征點；R為2幀圖像的旋轉矩陣；t為平移向量；n為運動中檢測到的特征點數量。

使用奇異值分解（Singular Value Decomposition，SVD）算法求解該問題可得到2幀圖像對應的位姿變化。

本文根據相機位姿的距離及2 幀照片之間的時間差選擇采集的部分圖像作為關鍵幀，減小后端優化計算量，構建局部地圖。圖像間的距離為：

式中，Ra、Rb分別為2個圖像幀的旋轉矩陣；ta、tb分別為2個圖像幀的平移向量；r為權重系數，默認取1，本文中由于旋轉對應的相機位姿變化較大，取1.6。

如果當前幀被判定為關鍵幀，根據位姿估計得到每個關鍵幀對應的相機位姿及運動軌跡，將該關鍵幀對應的三維點云地圖加入到局部地圖中，并對局部地圖中的相機位姿與路標地圖點進行優化。通過對關鍵幀的檢測，識別之前訪問過的場景，增加圖模型的回環約束，消除累積誤差，得到全局一致的地圖。

2.2 動態A*算法

A*算法是一種基于狀態空間的、廣泛應用于機器人路徑規劃的啟發式搜索算法，能夠根據起始點、目標點以及障礙物位置進行啟發式搜索，得到一條避免碰撞的最短路徑。其基本數學模型為：

式中，F(n)為當前節點的代價估計函數，即經過節點n的最優路徑消耗的總能量；H(n)為從n點到達終點的最優路徑消耗的能量；G(n)為從起始點到n點的最優路徑消耗的能量。

其中選擇n點到終點的歐式距離作為最優路徑消耗的能量：

式中，tx、ty為目標點坐標；nx、ny為當前節點n的坐標。

從起始節點出發，依次對當前節點的代價函數進行計算，用子節點中權重最小者對當前節點進行更新，直到遍歷所有節點，將障礙物的代價函數賦值為無窮大。對路網中所有節點賦值后，從目標點反向尋找最優路徑，算法每次在擴展時，均選取F(n)值最小的節點作為最優路徑上的下一個節點。特殊地，如果H(n)=0，則表示無任何可利用的當前節點與終點的信息，此時，A*算法退化為非啟發式的迪杰斯特拉（Dijkstra）算法，其搜索空間隨之變大，待處理節點的數量增多，搜索時間相應變長。例如，運用迪杰斯特拉搜索算法的可視圖法計算每個節點到其他所有節點的最短路徑，主要特點是以起始點為中心向外層層擴展，直到終點為止，由于它遍歷計算的節點很多，因而效率很低[18]。

A*算法在全局網絡中，依據擴展節點，選擇當前“代價”最低的節點進行下一步搜索，直到搜索到終點，從而規劃出“成本”最低的路徑。在整個路徑規劃過程中，由于省略了大量無謂的搜索路徑，因而在計算效率方面具有一定優勢。

此外，不同于基本A*算法，動態A*算法通過調整路徑規劃范圍，針對場景地圖變化需重新規劃路徑時不再進行全局規劃，而是保留已通過路徑，并以當前位置為起始點進行二次規劃，從而減少了計算量。

3 自主移動小車設計

3.1 硬件設計

小車采用四輪式底盤，其中前部為2 個獨立驅動輪，實現驅動與差速轉向功能，后部為2個相互獨立、釋放轉動自由度的萬向輪，避免在車輛移動過程中的滑移量干擾。選用額定功率為300 W的8寸輪轂電機（車輪直徑約200 mm）作為驅動電機，采用無刷直流（Brushless Direct Current，BLDC）驅動器驅動車輪轉動。選擇STM32F103ZET6 作為底層控制器，采用1 000 線AB 兩相編碼器測速。自主移動小車的結構如圖2所示。

圖2 自主移動小車

小車搭載Kinect v2 深度相機作為主傳感器，該相機能夠同時獲取環境的色彩及深度信息，如圖3所示。

圖3 自主移動小車主傳感器

3.2 差速運動數學模型

自主小車的底盤采用雙輪轂電機驅動與轉向控制方案，因此，差速運動數學模型成為小車實現精確控制的基礎。圖4為小車差速運動模型。

圖4 小車差速運動模型

圖4中，A(xl,yl)、B(xr,yr)、M(x,y)分別為左輪、右輪及左右輪軸心連線中點的坐標，ω、vM分別為小車角速度、線速度，vl、vr分別為左、右輪線速度，常量R為車輪半徑，θ1為位置1與位置2左、右輪軸心連線延長線的夾角，θ2為從位置1 運動到位置2 后的左、右輪軸線連線與x軸的夾角，θ3為相鄰時刻航向角增量，l為輪距。

在微小時段Δt內，小車左輪駛過弧長為：

小車右輪駛過弧長為：

小車車體轉角增量為：

若Δt足夠小，可認為：

若Δt非足夠小，則有：

假設初始狀態下，A、B點連線（向量AB）與x軸的夾角為β，則t時刻小車行進方向與x軸夾角θ為：

且有：

小車車體轉動角速度為：

由于M為A、B點連線中點，因而在任意時刻，小車M點運動線速度為：

小車M點運動半徑為：

圖示時刻A、B點連線與x軸平行，假設AB轉過一角度θ后變為A1B1，定義逆時針旋轉為正方向，在任意時刻，由幾何關系可得：

式中，xl、xr與yl、yr分別為小車左、右輪心的橫坐標與縱坐標。

定義BC為B點的速度，定義γ為x軸單位向量旋轉至與BC平行所得夾角，以逆時針旋轉為正方向，由幾何關系分析可得，任意時刻下：

因而，

定義AD為A點的速度，定義φ為x軸單位向量旋轉至與AD平行所得夾角，同理，有：

因而，

根據式（17）將式（21）、式（23）的速度矢量寫為分量形式，有：

假設初始時刻M點坐標為M(x1,y1)，因而M(x,y)運動軌跡參數方程為：

3.3 下位機程序設計

下位機程序面向自主小車的底層控制，在Keil環境下編寫，主要實現小車行進的閉環控制、數據采集以及數據通信功能。主程序流程如圖5所示。

圖5 下位機主程序流程

STM32作為控制器，接收控制信息，控制小車行進，并通過其自帶的編碼器模式，利用定時器獲取傳回的脈沖信號，對脈沖信號進行濾波處理，再對單個脈沖所代表的路程進行分析，盡可能精確地反饋小車的實際運動，單片機將得到的里程計數據進行處理之后，通過串口發送給上位機。底層的程序主要包括CAN總線的通用異步收發傳輸器（Universal Asynchronous Receiver∕Transmitter，UART）的串口通信、模數轉換器（Analogto-Digital Converter，ADC）、數模轉換器（Digital-to-Analog Converter，DAC）及脈沖寬度調制（Pulse Width Modulation，PWM）方波輸出、定時器的輸入捕捉和對編碼器的正交解碼以及對IO口的控制。通過對這些底層資源的操作，實現了小車的閉環控制、傳感器的數據采集和傳感器相互間以及與上位機之間的數據通信功能。

3.4 整車軟件系統設計

小車的上位機搭載Ubuntu 16.04系統，Ubuntu系統內安裝的機器人操作系統（Robot Operating System，ROS）作為小車上位機程序的運行平臺。在ROS中實現用戶界面（User Interface，UI）、人機交互、圖像處理、SLAM、路徑規劃、運動決策等功能并通過控制串口收發實現對小車運動的控制與傳感器信號的獲取。小車的下位機包括電機控制、編碼器數據采集、里程計數據融合等模塊。小車的上、下位機子模塊及其相互關系如圖6所示。

圖6 自主移動小車功能模塊及其關系

圖7 所示為本文無人小車基于特征點法搭建的RGB-D SLAM系統流程。

圖7 基于特征點法的系統流程

其中，Kinect v2傳感器采集周圍環境信息數據并通過USB 3.0 串口傳到上位機，上位機通過驅動軟件Freenect 獲取景深數據。景深攝像傳感器最先獲取點云信息，將其處理后，通過顏色顯示出實際的2D深度信息，作為景深攝像頭的原始數據（見圖8），為后期建圖提供必要的數據支持。

圖8 傳感器數據獲取

在獲取點云數據后，結合之前的里程計信息與數據的后處理工作進行地圖的實時構建。同時，得到原始數據后，在對數據進行后處理的基礎上完成回環檢測，實現小車的實時定位與建圖，如圖9所示。

圖9 建圖過程

獲取地圖后，通過全局與局部的方式進行路徑規劃，獲取小車最佳行進路線，完成避障。

4 試驗研究

4.1 算法可靠性驗證

將無人小車置于開闊的室外環境，驗證RGB-D SLAM 及動態A*算法的可靠性。設定小車的目標地點為前方路口，正前方路徑上的灌木叢為障礙物。試驗中RGB-D SLAM 處理后的圖片如圖10 所示，結果表明，RGB-D SLAM能夠建立未知環境的稀疏點云地圖，為小車提供定位與地圖信息。

圖10 RGB-D SLAM驗證試驗

4.2 自主避障試驗

簡單場景下的自主避障試驗在室外環境進行，選定灌木叢為障礙物，試驗中小車的避障路徑如圖11 所示。小車未觀測到障礙物時，朝著正前方直線運動，見圖11a、圖11b；小車檢測到前方障礙物后，開始向右轉彎，避開障礙物，見圖11c、圖11d；繞過障礙物后小車繼續沿直線行駛，見圖11e、圖11f。

圖11 自主避障試驗

簡單場景下的自主避障試驗表明：RGB-D SLAM與A*算法結合能夠實現未知環境的地圖構建與無人小車的實時定位功能，并能夠根據地圖中出現的障礙物實時改變運動路徑，實現自主避障功能。

4.3 綜合試驗

以上試驗表明小車具有自主行駛與避障能力。綜合試驗在更加復雜的場景下進行，對無人小車的路徑規劃能力進行測試。綜合試驗分為2 個部分：首先，在沒有先驗環境信息的條件下進行路徑規劃試驗，考察小車完成自主建圖、定位及路徑規劃任務的能力；第二，在首次試驗建立的場景柵格地圖基礎上，進行小車在已有路網信息下的定位及全局路徑規劃試驗。

4.3.1 無先驗環境信息

測試環境為十字路口場景，道路的前方與左側存在車輛（視為障礙物）。為考察小車在未知場景下自主捕獲環境信息、進行場景分析及路徑規劃的能力，試驗中，將無人小車置于正對路口處，測試其能否避開障礙物并從右側路口駛離。

試驗過程中小車的運動軌跡如圖12所示。

如圖12a、圖12b所示，小車駛入未知環境后沿著直線行駛，建立的局部地圖中出現障礙物1（直線行進方向的前車）后，小車退回到左側入口處，執行左轉策略，駛入左側出口，避開障礙物1，如圖12c、圖12d所示。小車在左側通道內繼續行駛，發現障礙物2（左側車）后，執行調頭策略，返回到點云地圖上第1個路口的右側出口，通過右側出口駛離試驗場景，如圖12e、圖12f所示。

圖12 未知場景下小車自主行駛試驗

由試驗過程可見，小車在移動中不斷進行建圖與定位，在全局地圖未知的情況下，根據已有的局部地圖規劃出最優路徑，實現了復雜場景下的建圖、定位及路徑規劃工作。在行進過程中建立的增量式點云地圖實現了小車的實時定位；根據所建地圖，規劃出最短的駛離路線。當障礙物1出現后，動態A*算法在保留已有路徑規劃的基礎上，在當前點重新規劃出最短路徑，駛向左側通道；之后，建立的點云地圖上出現障礙物2，小車繼續重新規劃路徑，掉頭后駛向右側通道，最終駛離試驗環境。

試驗結果表明，RGB-D SLAM 能夠實時構建局部地圖以確定小車的位置，并能夠在建立的點云地圖上檢測到障礙物位置，規劃出最短路徑。試驗過程中小車進行了3 次動態路徑規劃，每次基于當前點規劃最短路徑，最終駛離試驗場景，由此可見，動態A*算法能夠應對障礙物動態出現的場景，并能二次規劃出最短路徑。

4.3.2 有先驗環境信息

為測試動態A*算法的全局規劃能力，比較全局規劃與局部規劃的差異，相同場景下，利用第1 次試驗建立的全局地圖進行全局路徑規劃試驗。試驗中，小車的行駛路線如圖13 所示。在開始階段，小車沿著直線運動，見圖13a、圖13b；運動至路口左側附近，直接執行了右轉策略，見圖13c、圖13d；進入右側通道后駛離，見圖13e、圖13f。

圖13 已知場景下小車自主行駛試驗

試驗中小車基于全局地圖只進行1次路徑規劃，在左側路口附近直接右轉駛離。與未知環境下路徑規劃相比，小車不需要進行地圖構建、障礙檢測工作，減少了路徑規劃的次數與運動路徑的長度。

綜合試驗表明，RGB-D SLAM 與動態A*算法能夠滿足未知環境下小車自主運動及動態路徑規劃的要求。2 次試驗的對比表明，與局部路徑規劃相比，有先驗環境信息的全局路徑規劃能夠一次規劃出最優路徑，基于RGB-D SLAM 建立的點云地圖可很好地為全局路徑規劃提供地圖信息。

5 結束語

本文基于RGB-D SLAM與動態A*算法設計搭建了自主式無人小車硬件及軟件系統，并進行了試驗研究。結果表明：采用RGB-D SLAM與A*算法相結合的方法，可實現無人小車在未知環境下的地圖構建與實時定位功能，完成小車運動路徑隨地圖中所現障礙物的實時改變，使得小車具備自主行駛與避障能力，該方法也在一定程度上解決了GPS 信號缺失條件下的無人小車定位與導航問題；采用動態A*算法，無人小車能夠很好地應對動態出現的障礙物，并能二次規劃出最短路徑，實現了復雜場景下的避障與路徑規劃功能；具有先驗環境信息的無人小車可一次規劃出全局最優路徑，減少了規劃次數、縮短了運動長度、提高了執行效率。