視覺(jué)與激光結(jié)合的室內(nèi)移動(dòng)機(jī)器人重定位方法

包加桐，楊圣奧，朱潤(rùn)辰，唐鴻儒，宋愛(ài)國(guó)

（1.揚(yáng)州大學(xué)電氣與能源動(dòng)力工程學(xué)院，江蘇 揚(yáng)州 225300; 2.東南大學(xué)儀器科學(xué)與工程學(xué)院，江蘇 南京 210096）

0 引 言

移動(dòng)機(jī)器人重定位[1]一般指在沒(méi)有先驗(yàn)信息的前提下，機(jī)器人僅依靠自身傳感器估計(jì)其在已知全局地圖中的位姿。重定位發(fā)生于同步定位與建圖（SLAM）結(jié)束后的自主導(dǎo)航過(guò)程中，是自主導(dǎo)航的重要前提[2]。在初始位姿未知或發(fā)生“綁架”時(shí)，機(jī)器人均需要進(jìn)行重定位。其中在機(jī)器人初始上電工作時(shí)或突發(fā)狀況下被強(qiáng)制重啟后，需要估計(jì)機(jī)器人的初始位姿；機(jī)器人“綁架”指在導(dǎo)航時(shí)由于某種外部因素（如人為搬離、外部碰撞等）機(jī)器人位姿發(fā)生突變，從而導(dǎo)致原有依賴位姿連續(xù)變化的定位算法[3]失效。

由于室內(nèi)環(huán)境下激光雷達(dá)測(cè)距較精確且不易受環(huán)境光照影響，同時(shí)在考慮激光雷達(dá)成本情況下，許多研究工作致力于利用單線激光雷達(dá)實(shí)現(xiàn)機(jī)器人重定位。其中主要涉及兩個(gè)關(guān)鍵問(wèn)題：如何表征和匹配局部感知信息與全局地圖信息以及如何確定全局地圖中待搜索區(qū)域。傳統(tǒng)的方法多采用占用柵格來(lái)描述激光雷達(dá)測(cè)得的局部環(huán)境信息以及全局地圖信息，再基于概率運(yùn)算完成候選搜索區(qū)域內(nèi)的重定位。例如在經(jīng)典的自適應(yīng)蒙特卡羅定位（AMCL）算法[4]中，在重定位時(shí)隨機(jī)分布粒子表示機(jī)器人所有可能位姿，根據(jù)柵格占用情況基于似然域模型計(jì)算每個(gè)粒子的權(quán)重，利用重要性采樣更新粒子以及KL散度采樣調(diào)整粒子數(shù)等，最終加權(quán)平均各粒子得到估計(jì)位姿。然而該方法由于在全局地圖范圍分布粒子，粒子收斂時(shí)間較長(zhǎng)且在環(huán)境特征相似的場(chǎng)景中易出現(xiàn)錯(cuò)誤定位。在Google的Cartographer算法[5]中同樣使用占用柵格來(lái)表示環(huán)境信息，通過(guò)匹配柵格化后的激光雷達(dá)數(shù)據(jù)與地圖數(shù)據(jù)并基于極大似然估計(jì)來(lái)求解最有可能的位姿。由于重定位時(shí)該算法也需在全局地圖上搜索，定位時(shí)間較長(zhǎng)且在單一結(jié)構(gòu)環(huán)境中易定位失敗。

為了簡(jiǎn)化激光雷達(dá)與地圖數(shù)據(jù)匹配及全局搜索過(guò)程，許多研究嘗試將深度學(xué)習(xí)應(yīng)用于機(jī)器人重定位。例如文獻(xiàn)[6]將連續(xù)多幀的激光雷達(dá)數(shù)據(jù)形成多通道圖像，送入卷積神經(jīng)網(wǎng)絡(luò)提取圖像特征，經(jīng)全連接網(wǎng)絡(luò)預(yù)測(cè)機(jī)器人位姿。文獻(xiàn)[7]采用了堆棧自動(dòng)編碼器自動(dòng)學(xué)習(xí)激光雷達(dá)數(shù)據(jù)特征，根據(jù)相似性度量對(duì)SLAM過(guò)程中的激光雷達(dá)數(shù)據(jù)幀進(jìn)行分割并生成訓(xùn)練數(shù)據(jù)集。在堆棧自動(dòng)編碼器網(wǎng)絡(luò)后加上Softmax層，使用數(shù)據(jù)集訓(xùn)練得到分類器，在重定位時(shí)用于預(yù)測(cè)激光雷達(dá)數(shù)據(jù)幀的類別并關(guān)聯(lián)到SLAM時(shí)的地圖區(qū)域，從而縮小全局地圖搜索區(qū)域。

此外由于視覺(jué)感知可以獲取豐富的場(chǎng)景信息，隨著無(wú)人駕駛技術(shù)的興起[8]，視覺(jué)SLAM也一直是研究熱點(diǎn)，出現(xiàn)了許多基于單目、雙目、RGB-D相機(jī)的視覺(jué)SLAM算法。例如ORB-SLAM2算法[9]提取和匹配當(dāng)前圖像幀與關(guān)鍵幀的ORB特征，通過(guò)估計(jì)相機(jī)運(yùn)動(dòng)實(shí)現(xiàn)局部位姿跟蹤，其憑借完善易用等優(yōu)勢(shì)成為主流的特征點(diǎn)SLAM算法。然而視覺(jué)SLAM主要關(guān)注的是如何從連續(xù)圖像幀中估計(jì)出相機(jī)位姿變化并通過(guò)后端優(yōu)化生成視覺(jué)地圖，視覺(jué)地圖一般無(wú)法直接用于機(jī)器人導(dǎo)航和避障。文獻(xiàn)[10]則在視覺(jué)物體識(shí)別的基礎(chǔ)上構(gòu)建語(yǔ)義地圖，實(shí)現(xiàn)了基于語(yǔ)義似然估計(jì)的粒子濾波重定位，是將視覺(jué)信息與激光信息結(jié)合的一種有效嘗試。

可以看出，如何縮小候選地圖搜索區(qū)域是解決重定位問(wèn)題的關(guān)鍵。本文提出了一種視覺(jué)與激光結(jié)合的重定位方法。在SLAM階段，同時(shí)進(jìn)行視覺(jué)SLAM與激光SLAM，記錄視覺(jué)系統(tǒng)中相機(jī)位姿和激光系統(tǒng)中機(jī)器人位姿并生成位姿映射表。在重定位時(shí)利用當(dāng)前圖像與視覺(jué)SLAM系統(tǒng)中的關(guān)鍵圖像幀進(jìn)行特征匹配，由于關(guān)鍵圖像幀對(duì)應(yīng)的相機(jī)位姿已知，可以根據(jù)EPnP[9]算法解算出當(dāng)前的相機(jī)位姿。根據(jù)最近鄰匹配從位姿映射表檢索出激光SLAM系統(tǒng)中的最佳的可能位姿，從而縮小了候選地圖搜索區(qū)域。該位姿進(jìn)一步作為AMCL算法的先驗(yàn)信息，在該位姿周圍分布粒子，更新粒子直至收斂，完成全局重定位。

1 重定位方法框架

視覺(jué)與激光結(jié)合的重定位方法框架如圖1所示。由于重定位發(fā)生在已知全局地圖的導(dǎo)航過(guò)程中，因此預(yù)先開(kāi)展SLAM過(guò)程，生成全局地圖。在使用單線激光雷達(dá)測(cè)量環(huán)境的基礎(chǔ)上，增加RGB-D相機(jī)通過(guò)視覺(jué)感知來(lái)記錄和區(qū)分機(jī)器人所在環(huán)境，同時(shí)開(kāi)展激光SLAM和視覺(jué)SLAM。在SLAM過(guò)程中，當(dāng)機(jī)器人位姿發(fā)生顯著變化時(shí)（根據(jù)設(shè)定閾值來(lái)判定），同步記錄激光系統(tǒng)中的機(jī)器人位姿和視覺(jué)系統(tǒng)中的相機(jī)位姿，建立位姿關(guān)系映射表并存儲(chǔ)。激光SLAM結(jié)束后，生成二維占用柵格地圖。視覺(jué)SLAM結(jié)束后，生成稀疏特征點(diǎn)地圖，記錄關(guān)鍵圖像幀序列以及對(duì)應(yīng)相機(jī)位姿等。

圖1 重定位方法框架

重定位時(shí)，利用視覺(jué)特征匹配尋找關(guān)鍵圖像幀及參考相機(jī)位姿，基于EPnP算法解算出當(dāng)前相機(jī)在視覺(jué)地圖中的全局位姿?；谧罱徠ヅ鋸纳鲜鑫蛔岁P(guān)系映射表中檢索出最佳的柵格地圖中的機(jī)器人位姿，完成粗定位。將粗定位結(jié)果作為自適應(yīng)蒙特卡洛定位（AMCL）算法中粒子濾波的先驗(yàn)信息，經(jīng)粒子收斂后實(shí)現(xiàn)對(duì)機(jī)器人精確位姿的估計(jì)，最終完成整個(gè)重定位過(guò)程。

2 基于視覺(jué)特征點(diǎn)匹配的粗定位

2.1 圖像特征點(diǎn)提取與幀間匹配

圖像特征點(diǎn)一般用關(guān)鍵點(diǎn)和描述子來(lái)表示，用于表征圖像的本質(zhì)特征，具有不變性和可區(qū)分性。本文采用ORB[11]特征點(diǎn)來(lái)表示圖像，其利用具備尺度不變性和旋轉(zhuǎn)不變性的改進(jìn)FAST算法來(lái)檢測(cè)各個(gè)角點(diǎn)，再使用速度極快的二進(jìn)制描述子BRIEF來(lái)表示角點(diǎn)特征。

在視覺(jué)SLAM中圖像特征點(diǎn)也被視為路標(biāo)，由于相機(jī)運(yùn)動(dòng)的連續(xù)性或出現(xiàn)回環(huán)時(shí)，路標(biāo)會(huì)重復(fù)出現(xiàn)在多個(gè)圖像幀中。為了從多圖像幀中估計(jì)相機(jī)位姿變換，需要獲得多圖像幀上匹配的特征點(diǎn)對(duì)。由于圖像特征點(diǎn)采用了二進(jìn)制描述子，因此采用漢明距離來(lái)度量不同角點(diǎn)特征之間的距離，當(dāng)漢明距離小于設(shè)定的閾值即判定匹配成功。

2.2 重定位模式下相機(jī)的位姿估計(jì)

使用RGB-D相機(jī)基于ORB-SLAM2[9]算法完成視覺(jué)SLAM過(guò)程，生成稀疏特征點(diǎn)地圖，記錄關(guān)鍵圖像幀序列以及對(duì)應(yīng)相機(jī)位姿。在重定位模式下，當(dāng)相機(jī)采集的當(dāng)前圖像幀與視覺(jué)SLAM過(guò)程中的關(guān)鍵圖像幀匹配成功后，選取組匹配特征點(diǎn)對(duì)，獲取關(guān)鍵圖像幀中每個(gè)匹配特征點(diǎn)的全局3D坐標(biāo)以及其在當(dāng)前圖像幀中的2D投影，使用EPnP算法求解該3D-2D位姿估計(jì)問(wèn)題。

其中K為相機(jī)內(nèi)參矩陣。基于式（1）可以得到兩個(gè)線性方程：

式中：M——2n×12的矩陣；

x——12個(gè)未知量構(gòu)成的列向量。

2.3 基于最近鄰匹配的粗定位

在視覺(jué)SLAM的過(guò)程中同時(shí)開(kāi)展基于單線激光雷達(dá)的SLAM，本文采用了機(jī)器人操作系統(tǒng)（ROS）中常用的GMapping[12]算法來(lái)定位機(jī)器人和生成柵格地圖。以建圖起始點(diǎn)為地圖坐標(biāo)系（世界坐標(biāo)系）原點(diǎn)，當(dāng)機(jī)器人移動(dòng)距離超過(guò)設(shè)定閾值（實(shí)驗(yàn)中設(shè)定為0.25 m）時(shí)，同時(shí)記錄視覺(jué)系統(tǒng)中的相機(jī)位姿和激光系統(tǒng)中的機(jī)器人位姿。設(shè)生成的相機(jī)位姿與機(jī)器人位姿的變化軌跡表示為：

3 基于AMCL的精定位

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

為了驗(yàn)證本文提出的視覺(jué)與激光結(jié)合的重定位方法的有效性，與基于AMCL全局分布粒子的重定位方法、基于Cartographer算法的重定位方法進(jìn)行了比較實(shí)驗(yàn)。

采用 TurtleBot2機(jī)器人，搭載 Nvidia Jetson TX2微型計(jì)算機(jī)，配備RPLIDAR A3單線激光雷達(dá)和 Microsoft Kinect RGB-D相機(jī)。實(shí)驗(yàn)環(huán)境為長(zhǎng)約14.2 m、寬約6.8 m，面積約97 m2的實(shí)驗(yàn)室，靠墻的四周均放置有辦公桌、書(shū)柜等，中間有兩排工作臺(tái)，整體環(huán)境布局呈“回”字型。在重定位前預(yù)先同時(shí)開(kāi)展激光SLAM和視覺(jué)SLAM，生成的柵格地圖和稀疏特征點(diǎn)地圖如圖2所示。為了評(píng)價(jià)重定位的準(zhǔn)確性、成功率和實(shí)時(shí)性，在室內(nèi)環(huán)境中人工標(biāo)定出機(jī)器人出發(fā)位姿O點(diǎn)，以及A點(diǎn)至E點(diǎn)共5個(gè)觀測(cè)位姿點(diǎn)。重定位實(shí)驗(yàn)中，將移動(dòng)機(jī)器人從O點(diǎn)“綁架”至 A點(diǎn)O→A，接著A→B，B→C，C→D，D→E，每組共5次綁架。將機(jī)器人重定位結(jié)果與人工標(biāo)定結(jié)果進(jìn)行比對(duì)，若在60 s時(shí)間內(nèi)，兩者平面距離小于移動(dòng)機(jī)器人的底盤(pán)半徑0.18 m且航向偏差小于0.25 rad時(shí)，判定該點(diǎn)重定位成功；否則判定重定位失敗。

圖2 室內(nèi)環(huán)境的柵格地圖和稀疏特征點(diǎn)地圖

圖3顯示了SLAM過(guò)程中綁定的相機(jī)位姿與機(jī)器人位姿在同一坐標(biāo)系下的二維軌跡圖。由于視覺(jué)SLAM和激光SLAM系統(tǒng)相互獨(dú)立，雖然運(yùn)動(dòng)軌跡之間存在誤差，但整體運(yùn)動(dòng)軌跡變化趨勢(shì)基本一致。

圖3 相機(jī)與機(jī)器人運(yùn)動(dòng)軌跡圖

4.1 基于AMCL全局分布粒子的重定位實(shí)驗(yàn)

該實(shí)驗(yàn)是在沒(méi)有先驗(yàn)信息的情況下，希望通過(guò)全局分布粒子的方式經(jīng)機(jī)器人的運(yùn)動(dòng)模型和觀測(cè)模型實(shí)現(xiàn)粒子收斂完成重定位過(guò)程。圖4顯示了將機(jī)器人從某一位姿綁架至B點(diǎn)的重定位情況。圖4（a）中綠色的箭頭代表全局分布的帶有權(quán)重的粒子，黑色圓圈為機(jī)器人模型，紅色點(diǎn)為激光雷達(dá)測(cè)距障礙物點(diǎn)。當(dāng)機(jī)器人開(kāi)始移動(dòng)，粒子快速收斂，但最終收斂到了錯(cuò)誤的位姿如圖4（b）所示，重定位失敗。為了定量評(píng)價(jià)該方法的重定位效果，進(jìn)行了4組O→E共計(jì)20次機(jī)器人綁架實(shí)驗(yàn)，其中只有第2組D點(diǎn)處重定位成功，耗時(shí)約5.4 s，其余重定位實(shí)驗(yàn)均以失敗告終。據(jù)此斷定在全局進(jìn)行撒粒子一般難以解決重定位問(wèn)題。

圖4 機(jī)器人綁架到B點(diǎn)的重定位結(jié)果

4.2 基于Cartographer算法的重定位實(shí)驗(yàn)

Cartographer算法是基于圖優(yōu)化理論的經(jīng)典SLAM算法，源碼中提供的pure_localization接口可實(shí)現(xiàn)全局重定位。其基本原理是利用激光雷達(dá)生成的局部地圖與SLAM過(guò)程中的子圖依次進(jìn)行匹配完成全局定位。圖5顯示了將機(jī)器人從O點(diǎn)綁架到A點(diǎn)的重定位情況。如圖5（a）所示，機(jī)器人被綁架后柵格地圖上機(jī)器人模型的位置未發(fā)生改變，但是激光數(shù)據(jù)已發(fā)生變化。經(jīng)過(guò)一段時(shí)間后，機(jī)器人模型出現(xiàn)在地圖的A點(diǎn)處，重定位完成，如圖5（b）所示。

圖5 機(jī)器人綁架到A點(diǎn)的重定位結(jié)果

為了定量評(píng)價(jià)該方法的重定位效果，進(jìn)行了10組O→E共計(jì)50次機(jī)器人綁架實(shí)驗(yàn)，記錄各位姿點(diǎn)處的重定位結(jié)果，重定位位置與航向誤差如圖6所示?？梢钥闯?，該方法在各位置點(diǎn)處x軸方向和y軸方向的位置偏差均在18 cm以內(nèi)，有一定的誤差波動(dòng)，航向角的偏差大部分處于0.1 rad以內(nèi)。

圖6 重定位位置和航向誤差

再分別 10 s、15 s和20 s為時(shí)間限制，計(jì)算重定位成功率如表1所示。可以看出該方法在20 s內(nèi)完成重定位的成功率最高為80%。

表1 基于Cartographer算法的重定位成功率

4.3 視覺(jué)與激光結(jié)合的重定位方法實(shí)驗(yàn)

重定位時(shí)首先進(jìn)行視覺(jué)粗定位，再進(jìn)行基于AMCL的精定位。圖7顯示了機(jī)器人從某一位姿被綁架到A點(diǎn)的重定位情況。機(jī)器人首先通過(guò)視覺(jué)特征的提取與匹配，快速估計(jì)出相機(jī)位姿，如圖7（a）所示，其中藍(lán)色的框圖為關(guān)鍵幀，稀疏的點(diǎn)為特征點(diǎn)，黑色代表已觀測(cè)過(guò)的特征點(diǎn)，紅色代表當(dāng)前圖像幀中的特征點(diǎn)。將粗定位結(jié)果作為AMCL先驗(yàn)信息，在先驗(yàn)位姿周圍分布帶有權(quán)重值的粒子群，如圖7（b）所示，其中綠色箭頭代表不同的粒子。移動(dòng)機(jī)器人經(jīng)旋轉(zhuǎn)、移動(dòng)等可使得粒子群更新直至收斂，如圖7（c）和（d）所示。

圖7 機(jī)器人綁架到A點(diǎn)的重定位結(jié)果

為了定量評(píng)價(jià)該方法的重定位效果，同樣進(jìn)行了10組O→E共計(jì)50次機(jī)器人綁架實(shí)驗(yàn)，記錄各位姿點(diǎn)處的重定位結(jié)果，重定位位置與航向誤差如圖8所示。可以看出，該方法在各位置點(diǎn)處x軸方向和y軸方向的位置偏差均在10 cm以內(nèi)，且誤差波動(dòng)幅度較小，航向角的偏差也基本在0.2 rad以內(nèi)。

圖8 重定位位置和航向誤差

同樣分別以10 s、15 s和20 s為時(shí)間限制計(jì)算重定位成功率如表2所示?？梢钥闯鲈摲椒ㄔ?0 s內(nèi)均能完成重定位，且大部分重定位過(guò)程都能在15 s內(nèi)完成。

表2 本文方法的重定位成功率

進(jìn)一步統(tǒng)計(jì)各點(diǎn)處的重定位誤差、時(shí)間及成功率的平均值，對(duì)本文方法與基于Cartographer算法的重定位方法進(jìn)行對(duì)比，見(jiàn)表3。由于基于AMCL全局分布粒子的重定位方法成功率較低，不參與對(duì)比分析。在本文方法中，同時(shí)還統(tǒng)計(jì)了視覺(jué)粗定位的平均定位誤差與定位時(shí)長(zhǎng)。

從表3中可以看出，本文方法的重定位平均誤差略高于對(duì)比方法，但整體平均位置誤差小于5 cm，平均航向誤差小于0.1 rad，滿足一般系統(tǒng)實(shí)際的定位需求。本文方法的平均重定位時(shí)間為11.4 s，對(duì)比方法需要19.9 s，且以不同時(shí)長(zhǎng)為限制的情況下統(tǒng)計(jì)的成功率明顯優(yōu)于對(duì)比方法。

表3 與兩種重定位方法實(shí)驗(yàn)結(jié)果對(duì)比

5 結(jié)束語(yǔ)

解決初始位姿估計(jì)與機(jī)器人綁架問(wèn)題是機(jī)器人自主導(dǎo)航的前提。為了綜合利用相機(jī)與激光雷達(dá)分別在視覺(jué)感知方面的豐富性以及距離測(cè)量方面的精確性，提出了一種結(jié)合視覺(jué)與激光的重定位方法。將視覺(jué)粗定位結(jié)果作為先驗(yàn)信息，基于自適應(yīng)蒙特卡洛方法再進(jìn)行精定位。與基于AMCL全局分布粒子的重定位方法和基于Cartographer算法的重定位方法進(jìn)行了比較實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明提出的方法定位時(shí)間短、成功率高且定位準(zhǔn)確，能夠滿足一般室內(nèi)環(huán)境下移動(dòng)機(jī)器人自主導(dǎo)航需求。然而本文方法在重定位的實(shí)時(shí)性方面仍有較大的提升空間。