李元，王石榮，于寧波

（1. 南開大學(xué) 機(jī)器人與信息自動(dòng)化研究所，天津 300353; 2. 南開大學(xué) 天津市智能機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，天津300353）

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

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

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

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

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

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

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

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

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

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

3)路徑點(diǎn)生成：利用柵格地圖，結(jié)合定位信息，利用改進(jìn)人工勢(shì)場(chǎng)法規(guī)劃運(yùn)動(dòng)路徑點(diǎn)。

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

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

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

2 主要研究方法

2.1 基于ORB-SLAM的實(shí)時(shí)定位

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

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

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

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

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

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

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

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

2.3 基于人工勢(shì)場(chǎng)法的路徑點(diǎn)生成

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

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

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

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

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

式中：分別為排斥機(jī)器人遠(yuǎn)離障礙物的分量和吸引機(jī)器人向目標(biāo)運(yùn)動(dòng)的分量。

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

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

2.4 基于二次規(guī)劃的運(yùn)動(dòng)路徑曲線擬合

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

將生成的路徑點(diǎn)表示為

式中：

限定移動(dòng)機(jī)器人路徑曲線必須通過初始點(diǎn)以及終止點(diǎn)，有如下約束：

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

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

式中：

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

式中：

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

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

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

由此，可將路徑點(diǎn)擬合成可解析的路徑曲線。

2.5 移動(dòng)機(jī)器人運(yùn)動(dòng)控制

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

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

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

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

3 實(shí)驗(yàn)結(jié)果與分析

利用本文的自主移動(dòng)機(jī)器人系統(tǒng)進(jìn)行實(shí)驗(yàn)，以驗(yàn)證本文方法的有效性，如圖4所示。

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

分別將移動(dòng)機(jī)器人置于以下8個(gè)初始位姿：[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所示為實(shí)時(shí)定位與運(yùn)動(dòng)控制實(shí)驗(yàn)結(jié)果。虛線為基于理想模型和控制器的理論運(yùn)動(dòng)路徑，實(shí)線為實(shí)驗(yàn)得到的實(shí)際運(yùn)行路徑?？梢钥闯?，兩種路徑趨勢(shì)一致，且路徑曲線平滑，定位平穩(wěn)。

圖5 理論計(jì)算路徑(虛線)與實(shí)驗(yàn)運(yùn)行路徑(實(shí)線)Fig. 5 The calculated paths (dashed lines) and experimental paths (full lines)

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

圖6 移動(dòng)機(jī)器人到達(dá)目標(biāo)點(diǎn)的位置和姿態(tài)誤差Fig. 6 Position and orientation errors at the target pose

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

圖7 移動(dòng)機(jī)器人室內(nèi)實(shí)驗(yàn)Fig. 7 Mobile robot experiment in indoor environment

4 結(jié)束語

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

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