基于單目相機與里程計的室內機器人定位研究

溫倩雯，蘇毅輝，李 庚

（福州大學 物理與信息工程學院，福建 福州 350108）

0 引 言

隨著機器人技術的發展，越來越多需要人力完成的重復煩瑣工作可利用機器人完成。對于自主移動機器人，尤其是室內動態環境中的服務型移動機器人，準確的自我定位技術尤為重要。傳統方法常采用在室內放置如紅外和超聲波發射裝置等主動信號源［1］，或布置一些已知信息的路標實現定位任務［2］。但是，這些方法需要安裝額外的裝置或對環境進行特殊布置，無法做到普遍適用。

Smith等提出的同時定位與建圖（Simultaneous Localization And Mapping，SLAM）技術在不依賴外部設備或特殊標志的情況下，利用傳感器信息計算機器人位姿，并且構建環境地圖，逐漸成為解決室內服務機器人定位問題的熱門技術之一［3］。在室內動態環境、天花板信息基本穩定不變的情況下，選擇其作為視覺信息將減少動態因素改變所帶來的誤差。此外，學術研究［4-5］和市場應用方面也逐漸出現了采用攝像頭朝向天花板方向的SLAM定位方案實例，如科沃斯和石頭等掃地機器人。

針對室內場景下的機器人應用，本文提出兩點改進。一是選擇體積小、成本低且適合在嵌入式系統上實現的單目相機作為視覺傳感器，通過固定攝像頭方向使其朝向天花板，以克服室內動態場景下視覺信息變化多端不可靠的問題，同時優化特征匹配算法，以應對紋理重復度高或紋理稀疏的天花板場景。二是融合輪式里程計，結合里程計預積分信息，為視覺里程計提供初值，加速非線性優化收斂速度，以保證視覺里程計在稀疏紋理場景中不丟失。

1 系統框架

該定位系統主要分為視覺跟蹤、局部建圖及全局優化3個模塊，如圖1所示。視覺跟蹤模塊通過追蹤特征點建立幾何關系以篩選出關鍵幀。局部建圖模塊求解機器人位姿。全局優化模塊通過回環檢測消除累積誤差，提高機器人定位的精度。此外，重定位功能可以在定位丟失或機器人被人為移動后重新進行定位，極大地提升了機器人定位的魯棒性。

圖1 定位系統流程圖

2 系統實現

系統參考ORB-SLAM3的結構實現。

2.1 視覺里程計

對于空間中一個特征點WP=W(X,Y,Z)，在相機坐標系下記為CP=C(X,Y,Z)，通過針孔模型投影到相機平面為p(u,v)。將這樣的投影關系描述為：

式中，K為相機的內參數矩陣。這里先不考慮相機的畸變，于是有：

式中，WφC表示世界坐標到相機坐標的旋轉，是一個3×3的正交矩陣；WΔC表示世界坐標到相機坐標的1×3平移向量；符號d(·)表示齊次坐標變換。

在圖像上提取特征點時，需要特別關注特征點旋轉和光照不變性。室內環境存在大量人工光源，光源距離不同會導致較大的明暗差異。對同一場景下的天花板圖片分別用 ORB［6］、SURF［7］、GFTT［8］、FAST［9］等常用特征點提取特征，結果如圖2所示，從左上至右下的特征點分別為SIFT、ORB、BRISK、KAZE、AKAZE、FAST、AGAST、GFTT和SURF。由實驗結果可知，SURF特征點在不同光照條件下均有較好的表現。相比于其他特征點，無論場景中是否存在人造光源，其數量及分布都表現得更為魯棒。

圖2 不同特征點提取算法的效果對比圖

對于視覺里程計而言，準確的圖像配準結果是后續所有工作的基礎，需先保證配準結果的準確度。在特征匹配方面，考慮到掃地機器人移動速度較低，前后幀相機位姿變換量較小，故采用基于鄰域的特征點搜索算法替代傳統的全局搜索，以顯著降低計算量。由于天花板視圖具有紋理重復的特性，在圖像配準過程中更容易得到誤匹配，如圖3（a）所示，其中綠色直線為正確匹配點對，藍色直線為誤匹配點對。為了改善特征點匹配的精度，依據里程計變化值預估出圖像的仿射變換矩陣，并對參考幀的特征點做仿射投影，以限制鄰域搜索范圍。仿射變換是線性變換與平移變換的疊加，具有共線性、平行性及共線比例不變性的性質。進行變換后，圖像結構信息不會發生變化。

通過圖3的匹配結果和圖4的統計數據可知，在一定范圍內，兩幀間隔越大，本方法的改進效果越明顯。特別地，在重復紋理下，當兩幀視圖存在較大旋轉時，普通方法下的錯誤匹配極高，內點率僅達30%，而本文所改進方法在該情況下內點率仍可達到85%，如圖3（b）所示。基于里程計信息的特征點鄰域搜索算法可顯著降低誤匹配率，提升匹配精度，同時降低計算量。

圖3 紋理重復的場景下傳統特征點匹配算法與本文改進算法匹配結果對比圖

對于單目相機，僅憑單張圖像無法獲得所求點的深度信息，需通過三角測量的方法估計地圖點的深度。三角測量由相機平移得到，測量結果存在不確定性，如圖5所示。紅色部分為測量的不確定度，當平移較小時，深度估計上的不確定性較大，三角化精度過低。但是，增大平移可能導致圖像觀測發生過大變化而無法保證足夠多的匹配數量，因此存在三角測量的矛盾。本文采用深度濾波［10］思想，通過融合多幀多次三角化測量結果以克服偶然誤差。

圖4 傳統特征點匹配算法及本文改進算法匹配結果統計數據對比

圖5 三角化誤差示意圖

2.2 數據融合

輪式里程計通過航跡推算的方式獲得機器人軌跡絕對值，可以幫助單目視覺確定絕對尺度。但是，由于輪胎打滑等因素的影響，里程計信息會造成累積漂移誤差。將視覺信息與輪式里程計信息相結合可優劣互補，但在實際應用中，即使按照固定頻率采集，圖像與輪式里程計數據仍會在硬件和軟件等相關因素的影響下無法得到時間戳嚴格對齊的數據。因此，在兩種數據結合使用前應先對其進行時間戳對齊。一般情況下，里程計的采樣頻率高于圖像采樣頻率，且更容易實現插值，故以相機作為核心傳感器，以采集圖像數據的時刻作為要插入里程計數據的時間點，通過插值的方法獲取這一時刻的里程計數據，且需要索引這一時間點前后的兩個數據，并利用前后數據插值計算得到對應時刻的里程計信息。

將對齊后的里程計信息通過預積分計算出相鄰兩幀圖像時刻之間的變換信息作為視覺里程計的一個初值估計，以提高非線性優化的迭代收斂速度和精度。k和k+1時刻之間里程計的測量為：

式中，ηok為噪聲項，包含平移分量ηvk以及旋轉分量ηφk。在室內環境下時，機器人的運動可以近似在一個平面上，忽略Z方向的運動，只考慮X、Y分方向上的運動。所以，V=［vx,vy］T表示平移的X、Y分量，φ為沿軸的旋轉。在k和k+1時刻間的狀態傳播為：

式中，Vk+1依賴φk。對式（5）左右各乘一個Φ(-φi)，得到：

于是，關鍵幀i到關鍵幀j之間的旋轉預積分測量表示為：

噪聲項為：

積分的噪聲項傳播迭代過程為：

通過預積分得到兩圖像之間的相對位姿作為視覺里程計的一個初值估計，以提高非線性優化的迭代收斂速度和精度。

3 實驗結果

為了驗證該系統的有效性，在常見的室內辦公環境進行實驗。實驗設備如圖6（a）所示。設備搭載的相機型號為樂視深度相機LeTMC-520，采集圖像分辨率為640×480，圖像采集頻率為10 Hz，輪式里程計輸出頻率為20 Hz。需要特別說明的是，實驗設備搭載的相機雖然為深度相機，但在實驗中僅使用其單目圖像信息，未使用其深度計算功能。

實驗測試了ORB-SLAM3以及改進方法在室內動態環境的建圖與定位情況。實驗環境如圖6（b）所示，行走軌跡大致如圖6（b）中紅色線條所示。該場景為典型的室內辦公場景，有行走的人、易移動的椅子等。由于紋理重復、信息單一且存在動態障礙物，ORB-SLAM3在實驗中存在初始化困難、多次丟失位姿、無法成功重定位以及建圖結果混亂無法識別輪廓等問題，而本文所提方法基本不存在上述問題。

圖6 實驗設備及環境

該實驗兩種方法的建圖效果對比如圖7所示，圖中矩形框為關鍵幀的位姿，紅色矩形框為起始幀位姿，綠色矩形框為當前幀位姿，藍色矩形框為一般相機位姿。此外，若存在不同顏色矩形框，則說明機器人在運動過程中曾丟失位姿，構建了一個新的子地圖，其中黑點為全局地圖點，紅點為局部地圖點。從圖7（a）與圖7（b）可以看出，所提方法構建的地圖輪廓清晰，關鍵幀間隔合理。從圖7（c）與圖7（d）可以看出，ORB-SLAM3多次丟失位姿，計算出多條軌跡，如圖7（c）中多種顏色關鍵幀所示，無法與實際情況對應。本文所提方法可從地圖點分辨出天花板紋理，絕大多數地圖點的深度能保持在同一平面，且關鍵幀分布可顯示出機器人的實際行動軌跡。

圖7 建圖結果對比圖

實驗場景2如圖8所示，圖中紅色線條為行走軌跡。機器人在行走過程中有直行和旋轉等運動狀態。圖9為兩種方法的軌跡對比圖，其中Trajectory_OURS為所提方法軌跡，Trajectory_GT為軌跡真值，Trajectory_ORB為ORB-SLAM3軌跡。實驗時，ORB-SLAM3成功初始化耗時久，且短暫運行后即丟失位置，故其軌跡不全。所提方法的軌跡與軌跡真值的平均誤差為0.155 m。

圖8 實驗場景2

圖9 實驗場景2下的軌跡結果對比圖

4 結 語

基于單目相機和輪式里程計，本文提出了一種室內動態環境下的機器人定位方法。該方法固定單目相機朝向天花板，以減少室內動態環境下信息多變帶來的誤差。為適應天花板紋理弱且重復性較高的特點，通過改變特征匹配策略提升紋理重復度高場景下的匹配精度，以獲得更加準確的位姿求解；結合里程計預積分為視覺里程計提供初值，并為單目相機固定尺度信息。實驗證明，在室內動態場景和重復紋理場景下，所提方法能有效避免動態信息的影響，能在常見的室內場景得到可靠的機器人行進軌跡并保證其定位精度。該方法在室內服務機器人的自主運動方面意義重大，具有較好的應用前景。