徐碘

摘? 要：針對傳統(tǒng)的迭代最近點算法在視覺里程計應用中存在的外點干擾與相機偶然大運動導致的位姿估計不準確或失效問題，提岀基于3維ICP匹配的視覺里程計RGBD相機的切換策略，提髙位姿估計準確度和魯棒性。并在ICP算法起始階段估計當前幀點云與模型點云重疊率，減少外點對位姿估計的影響。實驗在標準測試數(shù)據(jù)集下，與典型視覺里程計對比有效提髙小規(guī)模運動下位姿估計的準確度，同時解決了相機偶然大運動下系統(tǒng)失效問題。

關(guān)鍵詞：3維ICP算法? 視覺里程計? RGBD相機? 位姿估計

中圖分類號：TP242? ? ? ? ? ? ? ? ? ? ? ? ? ?文獻標識碼：A? ? ? ? ? ? ? ? ? ? 文章編號：1674-098X（2020）10（b）-0036-03

Abstract： In order to solve the problem of inaccuracy or failure of pose estimation caused by external point interference and occasional big motion of camera in the traditional iterative nearest point algorithm in the application of visual odometer， we present a switching strategy based on 3D ICP matching for visual odometer RGBD camera to improve the accuracy and robustness of pose estimation. In the initial stage of ICP algorithm， the overlap rate of current frame point cloud and model point cloud is estimated to reduce the influence of external points on pose estimation. Compared with the typical visual odometer， the experiment can effectively improve the accuracy of low pose estimation in small-scale motion， and solve the problem of system failure in accidental large motion.

Key Words： 3D ICP algorithm; Visual odometer; RGBD camera; Pose estimation

1? 視覺里程計軌跡估計

1.1 算法框架

在估計相機位姿時，本文選擇將圖像信息以特征點云的形式進行輸入。為了增強最終模型的精準性，本文選擇首先綜合模型DK（當前幀點云模型）和模型MK—1（世界坐標系下的點云模型）的信息估計位姿，其次使用卡爾曼濾鏡對模型MK—1進行結(jié)合得到MK。

本文采用的當前幀點云位姿計算的初值是實驗中前一幀點云計算得到的位姿TK—1，但實驗中會產(chǎn)生一些不可控因素，較為突出的是實驗中相機的偶然大幅度運動，此時會對計算結(jié)果產(chǎn)生較大的影響。判斷之后，本文選擇跳過當前幀，代之以計算上一幀DK—1與下一幀DK—1的相對位姿，本文利用RANSAC算法剔除外點并在匹配時選用傳統(tǒng)的特征點描述，獲得相應的特征點后再采用ICP算法（原始）進行位姿計算。

1.2 本文的ICP位姿估計算法

首先使用歐式距離度量在點云模型MK-1與DK之中檢索最近點，接著通過確定其配準關(guān)系求解距離和及距離和的最小二乘問題獲得位姿，再使點云模型DK處于與模型MK-1相同的坐標系下，尋找下一次迭代的最近點再進行位姿運算，重復操作直到位姿收斂，這是傳統(tǒng)ICP算法的主要思路。

在ICP算法中必不可缺的一步便是估計當前幀點云與模型點云的最小部分重疊率ξ=[α，β]，這個最小重疊率的值呈現(xiàn)了當前幀點云與模型點云之間的最小重疊百分比，經(jīng)過優(yōu)化運算中相應的目標函數(shù)可以得到：Ψ（ξ）=e（ξ）/ξ1+γ。

在本文中，位姿解算采用不同于傳統(tǒng)ICP算法的各向異性的ICP算法，這是由于在不同特征點中，其確定度有所不同。需做特殊說明的是，本文不采用傳統(tǒng)ICP算法思路中經(jīng)常使用到的歐氏距離尋找點云模型的最近點，而是采用馬氏距離，即點di與mj之間的距離，其距離公式為：dist（di，mj）2=（μdi-μmj）

2? 實驗及分析

本文的實驗步驟具體如下：第一，驗證引入自適應部分重疊率是否能夠增加位姿計算精度;第二，將本文算法與改進前的算法進行對比實驗。一般的，從體現(xiàn)系統(tǒng)隨時間增加產(chǎn)生累積誤差的程度來看，相對軌跡誤差對視覺里程計評價更適宜。具體實驗如圖1所示。

2.1 幀對模型與幀間匹配的切換策略對比實驗

上文提到的相機大運動，本文對比了有無切換策略對視覺里程計的影響，實驗表明切換策略對相機偶然大運動具有一定的魯棒性。具體實驗數(shù)據(jù)如表1所示。

2.2 視覺里程計的綜合表現(xiàn)能力實驗

針對前文提到的整體視覺里程計系統(tǒng)，實驗選擇采用TUM RGB—D標準數(shù)據(jù)集中6個常用數(shù)據(jù)集對其進行整體評估實驗并且選取兩個典型的算法（DSO[6]、DVO[7]）實行對比。實驗驗證精度高于典型算法的3個視頻序列為相機做小規(guī)模運動，而在相機有偶然大運動的視頻序列上可以達到與典型算法同等的估計效果。具體實驗數(shù)據(jù)如表2所示。

3? 結(jié)語

本文對于現(xiàn)有視覺里程計算法中外點影響大、相機偶然大運動魯棒性欠缺的問題設想并驗證了一種改進方法并使用了一種切換策略以提升系統(tǒng)精確度及魯棒性，實驗結(jié)果顯示運用該改進方法的視覺里程計系統(tǒng)使用效果優(yōu)異，能一定程度上緩解前文提到的外點影響大、相機偶然運動魯棒性欠缺的問題。

參考文獻

[1] 邢科新，竺海光，林葉貴，等.基于前后端圖優(yōu)化的RGB-D三維SLAM[J].浙江工業(yè)大學學報，2018，46（6）：616-621.

[2] 劉強，段富海，桑勇，等.復雜環(huán)境下視覺SLAM閉環(huán)檢測方法綜述[J].機器人，2019，41（1）：112-123，136.

[3] 朱永豐，朱述龍，張靜靜，等.基于ORB特征的單目視覺定位算法研究[J].計算機科學，2016，43（S1）：198-202，254.

[4] 王消為，賀利樂，趙濤.基于激光雷達與雙目視覺的移動機器人SLAM研究[J].傳感技術(shù)學報，2018，31（3）：394-399.

[5] 伊星星.基于RGB-D的移動機器人實時定位與建圖研究[D].哈爾濱：哈爾濱工業(yè)大學，2017.

[6] 孫建輝，劉坤.基于序列圖像特征識別實時反饋伺服跟蹤研究[J].浙江工業(yè)大學學報，2015，43（2）：185-189.