基于改進視覺里程計算法的移動機器人定位

任平洋，李文鋒

（武漢理工大學 物流工程學院，武漢430063）

自主移動機器人要實現導航[1]，首先需要確定自身的位置和姿態，即實現定位。常用的定位方法有激光雷達定位、基于慣性導航系統的定位、GPS、基于人工標志[2-3]的定位、視覺里程計定位[4]等。其中，視覺里程計定位技術以其結構簡單、獲取環境信息豐富、對累計誤差的抑制能力強等優勢，在移動機器人定位導航、深空探測及增強現實等領域受到廣泛關注[5-7]。視覺里程計利用連續圖像幀的信息，估計相機初始位姿。傳統的視覺里程計算法假定環境中的特征點為靜態的，計算相機相對于靜態特征點運動狀況從而得到相機位姿。然而，在實際應用場景中，可能存在人、動物、車輛等動態物體，若未能有效剔除動態物體產生的特征點，視覺里程計位姿估計會出現較大誤差。

針對動態環境中視覺里程計面臨的問題，文獻[8]提出了三幀差分法，利用相鄰三幀的圖像進行相互差分判斷動態物體，在相機靜態或運動較緩慢的場景中取得了較好效果。文獻[9]采用光流法計算兩幀間相機變換矩陣，利用求解的變換矩陣對相機進行位姿補償，隨后利用幀間差分法檢測運動物體，剔除在運動物體上的特征點，但所提算法對于垂直于相機平面的運動物體檢測較差，且差分法受圖像閾值影響較大。文獻[10]中通過在ORB-SLAM2 系統中添加前端實現檢測，使用CNN 對幀中的先驗動態對象進行像素級分割，后續算法不會在這些對象上提取特征，但此方法僅對先驗對象有作用，且運動估計的結果依賴于訓練樣本質量。

上述算法在相機運動緩慢、動態特征少、靜態特征豐富的場景中取得了較好的效果。然而，在紋理較少或弱紋理場景下，難以提取到豐富的特征來生成準確的描述子，導致算法性能大幅降低。而在結構化的環境存在許多平面特征，這些高維特征能為傳統視覺里程計提供更加穩定和豐富的幾何信息，從而改善VO 算法性能。此外，面特征在低紋理、光照變化的場景中相較于點特征與像素光度信息更加魯棒。因此，本文提出一種動態環境下基于點面特征的視覺里程計算法，能夠在結構化、多紋理以及低紋理環境中實現高精度機器人定位。

1 基于點面特征的視覺里程計

圖1 為動態環境下基于平面特征的視覺里程計算法框架。算法分為以下3 個部分：①利用ORB提取場景中的特征點，基于兩點相對距離不變原則對特征點分類；②恢復場景點云，對點云進行降采樣。計算局部法向量，利用DBSCAN+KDTREE 對法向量進行聚類擬合，得到平面點云參數；③根據將得到的相互垂直的平面參數不同分類計算相機位姿變換矩陣。

圖1 基于點面特征的視覺里程計算法Fig.1 Visual odometry based on the features of points and planes

1.1 基于相對距離不變原則的特征點分類

相機在空間中的運動是一種歐式幾何變換，因此相機觀測到的兩特征點之間歐式距離不會隨著相機運動而產生影響，只和特征點自身的運動狀態相關。移動機器人通過相機采集連續幀的圖片，利用ORB 算法提取圖像特征點。如果兩特征點之間無相對運動，則兩特征點之間的相對空間距離不變[11]。因此，計算兩個特征點在前后兩幀的相對距離是否發生變化，即可判斷出兩特征點的運動狀態是否一致。

利用ORB 提取圖像幀的特征點集。對點集進行輪詢，找到兩點之間相互距離不變的兩點放入運動狀態點集D 中。循環特征點集其余點，判斷特征點和運動狀態點集D 中隨機選取的一個特征點之間的距離是否發生變化，如果未發生變化則將該點放入此運動狀態點集D 中。放入點集中的點都會標記已被分類，后續循環到此點時不會被再次分類。該類聚類完成后找到下一個兩點之間相對距離不變的特征點放入新的運動狀態點集，重復上述聚類過程直到所有運動狀態點集聚類完成。算法流程如圖2 所示。

圖2 特征點分類算法Fig.2 Feature point classification

1.2 平面參數的獲取

在室內環境中，難以在圖像色彩不豐富的地面和墻面中提取ORB 特征點。但豐富的平面參數可以構建約束條件，以去除場景中的動態點，優化相機位姿計算。

為此，本文利用RGB-D 相機獲取場景彩色圖片和深度圖片，通過相機內參恢復場景三維點云；對點云降采樣減少運算數據量； 計算各點的局部法向量，利用DBSCAN[12]對法向量聚類以得到平面點云。在聚類過程中，利用KDTREE 對聚類進行搜索加速。

DBSCAN 是一種基于密度的聚類算法，具有抗噪聲、無需指定類別種數、可以在空間數據中發現任意形狀的類等優點。DBSCAN 算法的復雜度取決于鄰域點查找的復雜度。最直接的方法是線性掃描查找，此時算法整體時間復雜度為O（n2）。而在數據量較大時，DBSCAN 算法速度會明顯降低。為加快尋找速度，本文采用KDTREE 對最近鄰搜索進行加速。KDTREE 是以二叉搜索樹為基礎的用于空間檢索的數據結構，能夠實現在隨機分布的空間內以O（logn）的時間復雜度對點的搜索。

具體平面聚類步驟如下：

（1）得到降采樣點云數據中點pi（xpi，ypi，zpi）的局部法向量n=（Api，Bpi，Cpi），帶入平面方程可得：

由點的坐標和局部法向量求出平面參數Dpi。

（2）以（Api，Bpi，Cpi，Dpi）為聚類點進行DBSCAN+KDTREE 聚類。法向量3 個坐標值的取值范圍均為（-1，1），而平面參數Dpi的取值范圍為（0，10）。平面參數Dpi在距離度量中占比較大，因此在聚類時需對Dpi乘一個比例系數k=0.1。

3）利用SVD 分解求出每個聚類點集的平面參數。

上述聚類方法需求場景中點的局部法向量，得到平面參數較耗時間，為充分利用上一幀平面檢測的結果，本文采用構造虛擬平面點的方法得到平面參數。瞬時內，相機運動可近似為勻速運動，由上一幀得到的平面點云經過上一幀的位姿變換后得到當前幀的虛擬平面點云。找到虛擬平面點云在當前幀近點，對近點擬合平面得到平面參數。記上一幀為k-1 幀、當前幀為k 幀、下一幀為k+1 幀。

式中：pk-1為k-1 時刻點p 的空間位置；pk為k 時刻點p 的空間位置；為相機k-1 時刻到k 時刻匹配點的旋轉矩陣為兩時刻之間平移向量。由于相鄰兩幀時間間隔小，可近似認為相機是勻速運動。用k-1 時刻到k 時刻的位姿變換近似表示k 時刻到k+1 時刻的位姿變換，則有：

1.3 帶平面參數的位姿計算

設第k-1 幀到第k 幀之間相機位姿變換矩陣T為

式中：R 為第k-1 幀到第k 幀的旋轉矩陣；t 為兩幀間的平移矩陣。旋轉矩陣和平移矩陣都為3 個自由度。

假定第k-1 幀的平面參數為（Ak-1，Bk-1，Ck-1，Dk-1），其中平面法向量為nk-1=（Ak-1，Bk-1，Ck-1）；與其匹配的第k 幀的平面參數為（Ak，Bk，Ck，Dk），平面法向量為nk=（Ak，Bk，Ck）。將平面方程寫成矩陣形式為

第k-1 幀平面內的點（x，y，z）經位姿變換矩陣T 變換為第k 幀的點（x′，y′，z′）：

由式（5）～式（7）進行矩陣變換可得：

對比式（5）和式（8）可得匹配平面間滿足：

獲得相鄰兩幀的平面參數后，計算平面法向量間是否相互垂直。如果存在3 個相互垂直的法向量，則可由平面參數直接計算出相機位姿變換矩陣。如果存在3 對匹配的相互垂直平面的平面參數與由（9）式得：

由式（10）得：

如果場景中提取的相互垂直平面沒有3 對，僅由平面參數無法獲得準確的位姿變換。此時根據平面參數區分動、靜態特征點集。基于兩點相對距離不變原則對特征點聚類，對每個特征點集計算位姿變換矩陣。因為平面在場景中通常為靜態物體，靜態特征點集的位姿變換矩陣應使當前幀場景中的平面進行重投影得到的平面參數，與上一幀平面參數相比得到的誤差最小。由平面參數轉換公式對每一個位姿變換求誤差err，取err 的最小值為靜態特征點集。err 由式（14）可得：

式中：n 為場景中匹配的平面數量；［nkDk］為第k幀的平面參數；［nk-1Dk-1］為第k-1 幀的平面參數。如圖3 所示，虛線平面為第k 幀平面投影至第k-1幀的結果。

圖3 平面位姿投影Fig.3 Planar pose projection

考慮系統的穩定性，如果當前幀場景中未提取出足夠的平面區分靜態特征點集與動態特征點集，即動態點集與靜態點集計算出的err 誤差相近。此時算法退化為默認靜態物體在空間場景中占主要成分。計算各特征點集凸包，取凸包面積最大的點集為靜態特征點集，利用靜態特征點集求解出R 和t。

1.4 基于靜態匹配點對的位姿計算

當平面參數不足時則可通過平面參數和空間參數對動態特征點去除。獲得靜態匹配點后，采用EPNP 位姿估計的方法求得相機位姿變換矩陣T。但此時求得的位姿變換矩陣R 和t 不是最優解，需通過非線性的PnP[13]方法進行相機運動位姿優化計算。將上一幀圖像的三維特征點重投影到下一幀圖像上，最小化重投影誤差以獲取相機位姿。如圖4所示，通過特征匹配可知和p1是p2同一個空間點P的投影；p1′為通過重投影計算出的像素坐標；err 為重投影計算出來的誤差值。

圖4 重投影誤差Fig.4 Reprojection error

式中：K 為相機內參矩陣；T 為位姿變換矩陣。通過最小化如下誤差函數可以獲取準確的相機的位姿估計。

式中：ui為匹配點的真實像素坐標；Pi為第i 個特征點的三維世界坐標。

2 實驗結果與分析

為驗證本文算法對視覺里程計定位精度的提升，分別在TUM 數據集及真實室內場景進行對比實驗。算法運行環境： 操作系統Ubuntu16.04，CPU型號I7-7700HQ，CPU 主頻2.8 GHz，內存8 G。實驗為CPU 實時運行，未經GPU 加速。

2.1 TUM 數據集結果與分析

TUM 數據集是慕尼黑工業大學制作的實驗數據集，其中fr3 系列為動態場景數據集，其包括sitting_halfphere、sitting_rpy、walking_rpy、walking_static、walking_xyz 等子數據集，其中sitting 子類環境中人與相機運動程度較低，walking 子類環境中人與相機運動劇烈。在walking 高動態數據集中，分別應用本文算法與ORB_SLAM2 算法估計相機位姿，并與真實的相機軌跡進行比較。

圖5 為walking_xyz 數據集對比實驗，虛線代表相機真實軌跡，實線代表算法計算的相機軌跡。由圖可知，ORB-SLAM2 算法在處理高動態環境時，得到的相機軌跡誤差較大。該算法會在動態物體上提取特征點，視為靜態點并用于相機位姿計算。本文提出的算法基于運動狀態對特征點分類，檢測出人身上的動態特征點，從而在相機位姿估計時使用較為準確的靜態匹配點對。

圖5 動態場景下算法估計相機軌跡與真實軌跡比較Fig.5 Comparison of camera trajectory and groudtruth in dynamic scene

表1 對比實驗結果Tab.1 Comparison of experimental results

本文用RMSE 來衡量真實軌跡和算法得到的相機軌跡之間的誤差。實驗分別對比了ORBSLAM2、文獻[9]和文獻[8]算法的定位精度。與其他三種算法相比，本文定位精度平均提升67%。本文所提算法在低動態和高動態環境中相較于其它算法都提升了定位精度，尤其在高動態場景中定位精度提升幅度較大。對比實驗充分說明本文算法在動態場景中具有的良好魯棒性和精確性。

2.2 室內實際場景實驗結果與分析

為進一步驗證基于點面特征的視覺里程計算法的有效性，本文設計如下的在線實驗場景：相機搭載在turtlebot3 移動機器人上部，基于ROS 平臺，通過鍵盤控制機器人走出一個直角路徑。實驗檢驗本文算法在直線運動以及旋轉運動中的穩定性。場景中有人在行走形成一個運動場景，從而驗證算法在動態場景下的抗干擾能力。本次實驗軌跡為一個長寬都為3 m 的直角路徑，具體運動為先直行、再左轉、再直行。

圖6（a）是由Kinect_v2 拍攝的場景RGB 圖像，場景中有地面、墻面、椅子、人體等物體；圖6（b）是由RGB 圖像和深度圖像恢復而出的場景點云圖；圖6（c）是本文提出的基于局部法向量聚類提取平面算法的結果。由圖可知在面對復雜環境時，可以較為快速地、完整地、準確地提取場景中的平面點云，為位姿計算、動態特征點的判別提供精準的平面參數。同時實驗得出計算一幀平面的時間為22.457 ms，雖然耗時增加，但在移動機器人行走時能實時運行，且對動態環境下定位精準度有較大的提升。

圖6 平面點云提取Fig.6 Planar point cloud extraction

動態特征點點去除情況如圖7 所示，圖7（a）為原始提取ORB 特征點，兩幀圖像間特征點匹配的結果；圖7（b）是去除動態特征點后特征點匹配的結果。從圖中可以看出，人體運動產生的動態特征點得到了去除，從而有利于視覺里程計定位精度的提升。

圖7 動態特征點去除Fig.7 Dynamic feature point removal

由于在地面上運動，垂直于地面的y 軸上沒有位移，因此僅考慮xz 平面上的定位效果。本次實驗取到終點（-3，3）的誤差。分別對Orb-slam2 算法、僅考慮利用空間參數去除動態點集的點特征算法與綜合考慮點面特征結合算法進行對比實驗。實驗結果軌跡圖如圖8 所示。

圖8 室內定位實驗Fig.8 Indoor localization experiment

圖中虛線為ORB-SLAM2 算法的運行結果，由圖可見由于人在場景中的影響，算法計算出來的視覺里程計產生了偏差。圖中點線為僅考慮空間參數去除動態點時的軌跡曲線，相較于不去除動態特征點，定位精度有的較大提升。圖中實線為綜合考慮點面特征時的軌跡曲線。有了平面參數的補充，定位精度進一步提升。ORB-SLAM2 算法得到的終點坐標為（-2.819，2.802），綜合偏差為0.268 m；僅考慮點特征算法得到的終點坐標為（-2.906，3.089），綜合偏差為0.129 m；綜合考慮點面特征算法得到的終點坐標為（-2.978，3.028），綜合偏差為0.034 m。實驗驗證了本文提出算法在真實場景下對視覺里程計定位精度的提升。

3 結語

針對自主移動機器人定位問題，本文提出一種基于點面特征的視覺里程計算法。基于兩點相對距離不變原則對提取的特征點分類，將運動狀態相同的特征點分為一類；計算點云局部法向量，由DBSCAN+KDTREE對局部法向量聚類提取出場景中平面參數；根據提取出相互垂直平面數量給出不同的視覺里程計位姿變換求解辦法。通過TUM 數據集和實驗室環境進行實驗驗證了算法的可行性和有效性。本文所提算法有效的減少了動態物體對相機位姿估計產生的影響。同時提高了視覺里程計在特征點提取較少，平面較多的場合運行時的精準度，為移動機器人使用RGB-D 在室內場景中完成定位建圖提供了有效的技術方案。但此方法在靜態特征點占比較小且提取出的平面參數無法判斷出動態特征點時會失效，后續可以針對此場景加上其它傳感器進一步判斷動態物體，去除動態物體的影響。