基于RGBD相機(jī)的3維ICP視覺(jué)里程計(jì)研究

徐碘

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

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

中圖分類(lèi)號(hào)：TP242? ? ? ? ? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識(shí)碼：A? ? ? ? ? ? ? ? ? ? 文章編號(hào)：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? 視覺(jué)里程計(jì)軌跡估計(jì)

1.1 算法框架

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

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

1.2 本文的ICP位姿估計(jì)算法

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

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

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

2? 實(shí)驗(yàn)及分析

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

2.1 幀對(duì)模型與幀間匹配的切換策略對(duì)比實(shí)驗(yàn)

上文提到的相機(jī)大運(yùn)動(dòng)，本文對(duì)比了有無(wú)切換策略對(duì)視覺(jué)里程計(jì)的影響，實(shí)驗(yàn)表明切換策略對(duì)相機(jī)偶然大運(yùn)動(dòng)具有一定的魯棒性。具體實(shí)驗(yàn)數(shù)據(jù)如表1所示。

2.2 視覺(jué)里程計(jì)的綜合表現(xiàn)能力實(shí)驗(yàn)

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

3? 結(jié)語(yǔ)

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

參考文獻(xiàn)

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

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

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

[4] 王消為，賀利樂(lè)，趙濤.基于激光雷達(dá)與雙目視覺(jué)的移動(dòng)機(jī)器人SLAM研究[J].傳感技術(shù)學(xué)報(bào)，2018，31（3）：394-399.

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

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