任平洋，李文鋒

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

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

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

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

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

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

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

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

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

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

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

1.2 平面參數(shù)的獲取

在室內(nèi)環(huán)境中，難以在圖像色彩不豐富的地面和墻面中提取ORB 特征點。但豐富的平面參數(shù)可以構(gòu)建約束條件，以去除場景中的動態(tài)點，優(yōu)化相機位姿計算。

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

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

具體平面聚類步驟如下：

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

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

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

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

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

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

1.3 帶平面參數(shù)的位姿計算

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

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

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

第k-1 幀平面內(nèi)的點（x，y，z）經(jīng)位姿變換矩陣T 變換為第k 幀的點（x′，y′，z′）：

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

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

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

由式（10）得：

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

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

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

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

1.4 基于靜態(tài)匹配點對的位姿計算

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

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

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

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

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

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

2.1 TUM 數(shù)據(jù)集結(jié)果與分析

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

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

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

表1 對比實驗結(jié)果Tab.1 Comparison of experimental results

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

2.2 室內(nèi)實際場景實驗結(jié)果與分析

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

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

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

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

圖7 動態(tài)特征點去除Fig.7 Dynamic feature point removal

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

圖8 室內(nèi)定位實驗Fig.8 Indoor localization experiment

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

3 結(jié)語

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