包加桐，楊圣奧，朱潤辰，唐鴻儒，宋愛國

（1.揚州大學電氣與能源動力工程學院，江蘇 揚州 225300; 2.東南大學儀器科學與工程學院，江蘇 南京 210096）

0 引 言

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

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

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

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

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

1 重定位方法框架

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

圖1 重定位方法框架

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

2 基于視覺特征點匹配的粗定位

2.1 圖像特征點提取與幀間匹配

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

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

2.2 重定位模式下相機的位姿估計

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

其中K為相機內(nèi)參矩陣?；谑剑?）可以得到兩個線性方程：

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

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

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

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

3 基于AMCL的精定位

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

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

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

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

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

圖3 相機與機器人運動軌跡圖

4.1 基于AMCL全局分布粒子的重定位實驗

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

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

4.2 基于Cartographer算法的重定位實驗

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

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

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

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

再分別 10 s、15 s和20 s為時間限制，計算重定位成功率如表1所示?？梢钥闯鲈摲椒ㄔ?0 s內(nèi)完成重定位的成功率最高為80%。

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

4.3 視覺與激光結(jié)合的重定位方法實驗

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

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

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

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

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

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

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

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

表3 與兩種重定位方法實驗結(jié)果對比

5 結(jié)束語

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