基于RGB-D信息的移動機器人SLAM和路徑規(guī)劃方法研究與實現(xiàn)

李元，王石榮，于寧波

（1. 南開大學(xué) 機器人與信息自動化研究所，天津 300353; 2. 南開大學(xué) 天津市智能機器人技術(shù)重點實驗室，天津300353）

近年來，智能移動機器人技術(shù)得到迅速發(fā)展，已廣泛應(yīng)用于工業(yè)、軍事、物流、辦公和家庭服務(wù)等領(lǐng)域[1-2]。智能移動機器人在環(huán)境中自主運行，通常需要搭載激光雷達、GPS、 聲吶、單目或雙目視覺設(shè)備等以獲取環(huán)境信息，輔助移動機器人進行自主定位、建圖、環(huán)境探測等任務(wù)[3-4]。然而，采用大量傳感器雖然可以提高系統(tǒng)的精度和魯棒性，但也顯著提高了機器人硬件和軟件系統(tǒng)的復(fù)雜性以及成本，限制了移動機器人的進一步推廣應(yīng)用[5]。為此，建立一個硬件結(jié)構(gòu)簡單、性能良好、經(jīng)濟性和通用性好的移動機器人系統(tǒng)，成為一個迫切需要解決的問題。

隨著計算機視覺技術(shù)的進步，單目或雙目視覺設(shè)備成為移動機器人的一種選擇[6-7]。RGB-D傳感器能同時獲取色彩與深度信息，易安裝使用，成本低，被廣泛應(yīng)用于目標識別、跟蹤等領(lǐng)域[8-9]，利用RGB-D傳感器進行SLAM的研究迅速發(fā)展。美國華盛頓大學(xué)與德國弗萊堡大學(xué)各自實現(xiàn)了基于RGB-D傳感器的SLAM系統(tǒng)[10-11]，利用圖像特征估計圖像間的變換關(guān)系，并利用ICP (iterative closest point)等算法優(yōu)化?？突仿〈髮W(xué)開發(fā)出利用多邊形重構(gòu)和融合算法，在原始數(shù)據(jù)中提取平面特征，顯著提高了算法的實時性[12]。2007年Klein等[13]首次提出了并行跟蹤與建圖算法PTAM (parallel tracking and mapping)，關(guān)鍵幀以及多線程處理方法被廣泛采用。ORB-SLAM算法由西班牙薩拉戈薩大學(xué)Raul Mur-Artal等[14]提出，由跟蹤、局部建圖和閉環(huán)檢測3個并行線程組成，具備重新定位能力，取得了良好的穩(wěn)定性、魯棒性和實時性[15]。

國內(nèi)基于RGB-D信息的研究也迅猛發(fā)展。清華大學(xué)孫富春教授[8-9]團隊利用Kinect傳感器和RGB-D信息完成了場景建模、機器人自主導(dǎo)航和人機協(xié)作等工作。北京工業(yè)大學(xué)賈松敏教授團隊[16]，通過提取Kinect RGB信息的特征點進行粗匹配，并結(jié)合深度信息，通過迭代最近點算法實時獲取位姿，采用關(guān)鍵幀選取機制及線程并行處理的方法，提高了算法運行效率。上海交通大學(xué)曹其新教授團隊[17]，從兩個方面對Kinect Fusion算法進行改進提高，通過使用環(huán)境中的邊線特征點匹配提高了算法的定位魯棒性，并通過在點云模型中預(yù)設(shè)地面點云提高了算法精度。第二炮兵工程大學(xué)張國良教授課題組[18]提出了一種基于快速視覺里程計及大回環(huán)局部優(yōu)化模型，提高配準精度并實現(xiàn)對累積誤差快速優(yōu)化的同步定位與三維點云地圖創(chuàng)建方法。

本文使用基礎(chǔ)運動底盤，搭載通用RGB-D傳感器，通過開發(fā)整合相應(yīng)算法，建立了一個硬件結(jié)構(gòu)簡單、易開發(fā)維護的自主移動機器人系統(tǒng)，實現(xiàn)了自主定位、障礙規(guī)避、運動規(guī)劃與控制等功能。

1 硬件平臺與系統(tǒng)架構(gòu)

本文建立的移動機器人系統(tǒng)由一個運動底盤、一個RGB-D傳感器和一個移動運算平臺組成，如圖1所示。Yujin Kobuki運動底盤由兩輪差速驅(qū)動，最大線速度可達70 cm/s，最大旋轉(zhuǎn)速度為180 (°)/s，最大運載能力5 kg。采用Kinect V2傳感器，TOF(time of flight)測量，水平可視角度為70.6°，垂直可視角度為60°，最大探測距離為8 m，最小探測距離為0.5 m，探測精度為2 cm。采用惠普ENVY 15-j105tx作為移動運算平臺，配備了core i7 4 702 m中央處理器以及8 GB運行內(nèi)存?；赨buntu 14.04 LTS操作系統(tǒng)，安裝配置了機器人操作系統(tǒng)ROS Indigo、libfreenect2、PCL、OpenCV 以及不萊梅大學(xué)提供的Kinect V2的ROS驅(qū)動等基礎(chǔ)工具包[19]。在此環(huán)境下，可方便地開發(fā)后續(xù)的移動機器人自主定位、障礙物檢測規(guī)避與運動控制等算法。

圖1 移動機器人系統(tǒng)的硬件平臺Fig. 1 Hardware platform of the mobile robot system

本文的移動機器人工作流程如圖2所示，主要包括以下功能單元。

1)實時定位：根據(jù)Kinect V2獲取的環(huán)境RGBD信息，利用ORB-SLAM算法計算當(dāng)前位姿。

2)環(huán)境地圖創(chuàng)建：結(jié)合Kinect V2點云信息和當(dāng)前位姿，利用GMapping建立二維柵格地圖。

3)路徑點生成：利用柵格地圖，結(jié)合定位信息，利用改進人工勢場法規(guī)劃運動路徑點。

4)路徑曲線擬合：根據(jù)規(guī)劃的路徑點，利用二次規(guī)劃進行曲線擬合，獲得可解析的路徑曲線。

5)路徑跟蹤：根據(jù)路徑曲線，考慮運動約束，生成控制量，控制移動機器人運動。

圖2 系統(tǒng)架構(gòu)與工作流程Fig. 2 Architecture and workflow of the system

2 主要研究方法

2.1 基于ORB-SLAM的實時定位

基于采用的移動機器人系統(tǒng)軟硬件平臺，實現(xiàn)了如圖3所示的ORB-SLAM算法。

提取RGB-D數(shù)據(jù)中彩色圖像的ORB圖像特征，與上一幀圖像特征點做初始匹配，得到初始點對，同時利用深度信息進行尺度判斷。若存在足夠的匹配點對，則對包含深度信息的當(dāng)前幀與前一幀匹配點對進行基于RANSAC算法的前后兩幀位姿估計。若沒有足夠的匹配點對，則首先使用DBoW2庫建立關(guān)鍵幀ORB特征的詞袋向量，在關(guān)鍵幀詞袋數(shù)據(jù)庫進行搜索匹配，選取最佳位姿估計。進而利用covisibility graph獲取局部地圖進行當(dāng)前幀位姿優(yōu)化，獲得當(dāng)前幀位姿等信息，然后判斷是否為新的關(guān)鍵幀，用于進行局部建圖，并采用g2o對當(dāng)前正在處理的關(guān)鍵幀與相關(guān)聯(lián)的covisibility graph內(nèi)不同時刻的關(guān)鍵幀進行局部集束調(diào)整(bundle adjustment)，最后進行閉環(huán)檢測。由此，實現(xiàn)了移動機器人的自主實時定位與特征地圖創(chuàng)建。

2.2 基于GMapping的二維柵格地圖創(chuàng)建

柵格地圖用于表示二維平面坐標點存在障礙物的概率值，具有構(gòu)建速度快、存儲空間小、易擴展等優(yōu)勢[20]。為實現(xiàn)路徑規(guī)劃，有必要建立二維柵格地圖，對此GMapping算法具有良好性能[21]。

GMapping算法的輸入為當(dāng)前位姿及二維激光類型觀測數(shù)據(jù)，輸出為二維柵格地圖。當(dāng)前實時位姿通過ORB-SLAM獲取，而二維激光類型觀測數(shù)據(jù)由RGB-D數(shù)據(jù)生成。利用獲取的點云信息，根據(jù)移動機器人高度、RGB-D的實際觀測視角及深度范圍添加采樣約束，取約束范圍內(nèi)離移動機器人最近的點集，降維生成在指定坐標系平面(移動機器人基坐標系平面)的激光類型數(shù)據(jù)。這種方法保留了環(huán)境立體信息，有利于自主移動機器人系統(tǒng)及時根據(jù)環(huán)境變化進行響應(yīng)。

圖3 基于RGB-D信息的ORB-SLAM算法流程Fig. 3 The ORB-SLAM algorithm workflow for RGB-D data

建圖過程中，GMapping把訂閱ORB-SLAM生成的移動機器人實時位姿作為里程計信息，通過RGB-D點云生成的激光類型數(shù)據(jù)，實時建立環(huán)境的二維柵格地圖?；诮▓D環(huán)節(jié)的優(yōu)化和閉環(huán)檢測環(huán)節(jié)的約束，ORB-SLAM提供的實時位姿具有較好的穩(wěn)定性與精確性，且位姿更新頻率能達到20 Hz，滿足需求；同時，根據(jù)移動機器人高度約束，取符合高度范圍內(nèi)的RGB-D點云用于地圖構(gòu)建。由于GMapping柵格地圖構(gòu)建的方式實時性較好，在本平臺上能達到最少5 Hz的更新頻率，為自主移動機器人平臺實時進行路徑規(guī)劃與控制提供了保證。

式中：s 為參數(shù)，假設(shè)路徑點為 m個，進而可以將等式(8)表示為矩陣形式，即

2.3 基于人工勢場法的路徑點生成

對于移動機器人的路徑規(guī)劃，人工勢場法是一種通用算法，傳統(tǒng)人工勢場法需處理勢場中存在的局部最小、目標不可達等問題[22]。為避免上述問題，本文采用改進人工勢場法進行路徑規(guī)劃[23]。

在空間中某個位置 X =[x y]T， 引力勢場函數(shù)為

式中：ε是引力比例因子， ρ (q,qgoal)為 機器人 q與目標qgoal之間的距離。引力可表示為勢場的負梯度函數(shù)，如式(2)所示：

斥力對應(yīng)的勢場函數(shù)為

式中：η為斥力比例因子， ρ ( q,qobs)為 機器人 q與障礙物 qobs之 間的最小距離， ρ0為障礙物的影響范圍。進而斥力函數(shù)可表示為

式中：分別為排斥機器人遠離障礙物的分量和吸引機器人向目標運動的分量。

因此，移動機器人承受的總作用力由引力和斥力的合力構(gòu)成，可表示為

根據(jù)上述方法，結(jié)合定位信息以及柵格地圖，可獲得移動機器人的路徑點。

2.4 基于二次規(guī)劃的運動路徑曲線擬合

前述獲得的路徑點存在曲線不平滑、不可解析的問題。為此，利用二次規(guī)劃方法，將路徑點進行曲線擬合，生成一條平滑可解析的路徑曲線，并最終生成對移動機器人的控制量。

將生成的路徑點表示為

式中：

限定移動機器人路徑曲線必須通過初始點以及終止點，有如下約束：

為了能夠?qū)崿F(xiàn)移動機器人的動態(tài)路徑規(guī)劃，在控制過程中采取分段規(guī)劃的策略。因此，路徑曲線還需滿足起始角度和終止角度約束：

根據(jù)式(8)可得

式中：

設(shè) ta nθt為路徑曲線上某一點角度的正切值，進而可得約束，即

式中：

在上述條件下進行路徑曲線擬合，即在滿足上述約束下求解最優(yōu)參數(shù)使得式(21)取最小值：

式中 BTB為標量。進而可將上述曲線擬合問題轉(zhuǎn)化為二次規(guī)劃問題，目標函數(shù)為

需同時滿足式(13)和式(18)的線性約束。

由此，可將路徑點擬合成可解析的路徑曲線。

2.5 移動機器人運動控制

移動機器人從初始位姿到目標位姿，可用經(jīng)典的控制方法，如基于極坐標的點鎮(zhèn)定控制方法[24]。本文生成了可解析的路徑曲線，能夠采用非線性跟蹤控制器[25]。

式中 為當(dāng)前位姿。位姿開環(huán)誤差為

采用如下的非線性控制器：

式中 k1,k2為參數(shù)，可以保證閉環(huán)系統(tǒng)漸進穩(wěn)定[25]。

3 實驗結(jié)果與分析

利用本文的自主移動機器人系統(tǒng)進行實驗，以驗證本文方法的有效性，如圖4所示。

圖4 自主移動機器人系統(tǒng)Fig. 4 The autonomous mobile robot system

分別將移動機器人置于以下8個初始位姿：[0 ?2.4 0]T,[?2.4 ?2.4 0]T,[?2.4 0 0]T,[?2.4 2.4 0]T，[0 2.4 0]T,[2.4 2.4 0]T,[2.4 0 0]T,[2.4?2.4 0]T，期望位姿設(shè)為 [0 0 π/2]T。x , y 單 位為m，θ 單位為弧度。

圖5所示為實時定位與運動控制實驗結(jié)果。虛線為基于理想模型和控制器的理論運動路徑，實線為實驗得到的實際運行路徑。可以看出，兩種路徑趨勢一致，且路徑曲線平滑，定位平穩(wěn)。

圖5 理論計算路徑(虛線)與實驗運行路徑(實線)Fig. 5 The calculated paths (dashed lines) and experimental paths (full lines)

圖6 為8次實驗中移動機器人到達期望位姿的位置和姿態(tài)誤差，實心圓點代表位置誤差，箭頭方向代表姿態(tài)誤差。移動機器人距離目標點 X軸方向的平均距離偏差為0.43 cm，Y軸方向的平均距離偏差為1.12 cm，總體距離誤差都在3cm以內(nèi)。姿態(tài)角偏差的絕對平均值為2.16°。實驗表明，采用RGBD信息和ORB-SLAM算法進行移動機器人實時定位確實有效，為后續(xù)的運動規(guī)劃與控制打下了良好基礎(chǔ)。

圖6 移動機器人到達目標點的位置和姿態(tài)誤差Fig. 6 Position and orientation errors at the target pose

為驗證本文方法和系統(tǒng)的有效性，在障礙環(huán)境下進行實驗，結(jié)果如圖7所示。圖7(a)為實驗環(huán)境，圖7(b)為移動機器人在此存在障礙物的環(huán)境下進行地圖創(chuàng)建和運動規(guī)劃與控制的實驗結(jié)果?？梢钥闯?，根據(jù)RGB-D點云生成的激光類型數(shù)據(jù)，通過GMapping算法成功建立了二維環(huán)境柵格地圖，利用改進人工勢場法以及二次規(guī)劃和軌跡跟蹤等算法，有效規(guī)避了障礙物，最終成功實現(xiàn)了從初始位置運動到目標位置運動，運動路徑平滑。

圖7 移動機器人室內(nèi)實驗Fig. 7 Mobile robot experiment in indoor environment

4 結(jié)束語

本文利用基礎(chǔ)運動底盤，搭載通用RGB-D傳感器與移動計算平臺，建立了一個架構(gòu)簡單、經(jīng)濟性好、易開發(fā)維護、性能強大、擴展性好的自主移動機器人系統(tǒng)。采用ORB-SLAM算法、GMapping算法、改進人工勢場法、二次規(guī)劃算法以及運動控制算法，實現(xiàn)了移動機器人的自主定位、路徑規(guī)劃、運動控制、障礙物規(guī)避等功能，并實時建立了環(huán)境二維柵格地圖。由此，為移動機器人在家庭等各種場景下的推廣應(yīng)用提供了一個解決方案。

將來可以基于該系統(tǒng)的硬件和軟件架構(gòu)，面向具體應(yīng)用有針對性地開發(fā)整合相關(guān)算法，提高系統(tǒng)的智能化程度與控制性能，進一步拓展移動機器人的功能和應(yīng)用范圍。