視覺與激光結合的室內移動機器人重定位方法

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

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

0 引 言

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

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

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

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

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

1 重定位方法框架

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

圖1 重定位方法框架

重定位時，利用視覺特征匹配尋找關鍵圖像幀及參考相機位姿，基于EPnP算法解算出當前相機在視覺地圖中的全局位姿。基于最近鄰匹配從上述位姿關系映射表中檢索出最佳的柵格地圖中的機器人位姿，完成粗定位。將粗定位結果作為自適應蒙特卡洛定位（AMCL）算法中粒子濾波的先驗信息，經粒子收斂后實現對機器人精確位姿的估計，最終完成整個重定位過程。

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

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

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

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

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

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

其中K為相機內參矩陣。基于式（1）可以得到兩個線性方程：

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

x——12個未知量構成的列向量。

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

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

3 基于AMCL的精定位

4 實驗結果與分析

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

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

圖2 室內環境的柵格地圖和稀疏特征點地圖

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

同樣分別以10 s、15 s和20 s為時間限制計算重定位成功率如表2所示。可以看出該方法在20 s內均能完成重定位，且大部分重定位過程都能在15 s內完成。

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

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

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

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

5 結束語

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