蔣 林，郭 晨，朱志超，程文凱

1 引言

隨著科學(xué)技術(shù)的發(fā)展，二維圖像包含的信息已不能滿足人們的需要，因此對真實(shí)場景的三維重建越來越受到廣大學(xué)者的關(guān)注。在三維重建中機(jī)器人位姿的實(shí)時(shí)精確估計(jì)主要依靠輪式里程計(jì)、慣性測量單元、全球定位系統(tǒng)和視覺里程計(jì)等[1]。視覺里程計(jì)是其中重要的一種位姿估計(jì)方法。文獻(xiàn)[2]提出了一種基于光一致性假設(shè)的視覺里程計(jì)方法（DVO，densevisual odometry）提高了位姿估計(jì)的精度和魯棒性。文獻(xiàn)[3]借鑒立體視覺里程計(jì)流程設(shè)計(jì)了一套基于Kinect的視覺里程計(jì)系統(tǒng)（FOVIS，fast odometry from vision）僅利用深度信息對相機(jī)位姿進(jìn)行快速估計(jì)。文獻(xiàn)[4]提出了一種幀到模型的視覺里程計(jì)方法（FOV，fast visual odometry），實(shí)現(xiàn)了數(shù)據(jù)集和模型集的快速配準(zhǔn)。

然而，現(xiàn)有的三維重建中視覺里程計(jì)的算法主要在PC機(jī)上運(yùn)行。雖然在PC機(jī)上已經(jīng)獲得較好的結(jié)果[5]，但是當(dāng)這些算法移植到嵌入式平臺后，效果就不太理想或者無法實(shí)現(xiàn)功能。隨著機(jī)器人技術(shù)的發(fā)展，越來越多的三維重建需要在嵌入式平臺進(jìn)行實(shí)現(xiàn)。

利用移動(dòng)機(jī)器人搭載的視覺傳感器獲取序列圖像并進(jìn)行提取特征點(diǎn)，然后對不同運(yùn)動(dòng)方式下獲取的特征點(diǎn)分別采用不同的算法進(jìn)行處理，簡化移動(dòng)機(jī)器人平臺的位姿算法，可以降低計(jì)算量并同時(shí)滿足精度要求。通過離線數(shù)據(jù)集進(jìn)行離線實(shí)驗(yàn)和真實(shí)環(huán)境的機(jī)器人在線實(shí)驗(yàn)驗(yàn)證了提出的位姿估計(jì)方法的有效性。

2 移動(dòng)機(jī)器人的運(yùn)動(dòng)和幾何關(guān)系分析

2.1 室內(nèi)機(jī)器人工作環(huán)境和運(yùn)動(dòng)方式的簡化

基于視覺的三維建模中，相鄰幀之間的位姿估計(jì)傳統(tǒng)方法為：首先計(jì)算基礎(chǔ)矩陣，然后根據(jù)基礎(chǔ)矩陣求解本質(zhì)矩陣，最后（SVD）奇異值分解本質(zhì)矩陣[6]得到位姿轉(zhuǎn)換矩陣，或者通過點(diǎn)云的PnP匹配[7]，得到近似位姿轉(zhuǎn)換矩陣。并且已經(jīng)出現(xiàn)了很多改進(jìn)的算法，雖然在PC機(jī)上能實(shí)現(xiàn)實(shí)時(shí)的三維重建，但是計(jì)算量相對于嵌入式平臺來說依舊較大、計(jì)算時(shí)間較長，嚴(yán)重影響三維建模在嵌入式平臺的的實(shí)時(shí)性和精度。為了提滿足在嵌入式平臺的實(shí)時(shí)性和建模精度，通過分析室內(nèi)環(huán)境建模的機(jī)器人運(yùn)動(dòng)方式的獨(dú)特性，進(jìn)而在不同的運(yùn)動(dòng)方式下采用不同算法，降低計(jì)算的復(fù)雜度提高建模的實(shí)時(shí)性。

日常生活中的運(yùn)動(dòng)方式有多種，例如三個(gè)維度上的平移、旋轉(zhuǎn)、平移加旋轉(zhuǎn)，運(yùn)動(dòng)分析較為復(fù)雜。但是通過對室內(nèi)工作的機(jī)器人工作環(huán)境以及機(jī)器人的驅(qū)動(dòng)方式進(jìn)行分析后，發(fā)現(xiàn)室內(nèi)移動(dòng)機(jī)器人工作方式可以大大簡化，平移只能在XOZ平面，旋轉(zhuǎn)也無法在三個(gè)維度進(jìn)行，僅僅只能繞Y軸進(jìn)行，因此機(jī)器人運(yùn)動(dòng)時(shí)位姿轉(zhuǎn)換矩陣也可以相應(yīng)簡化。

2.2 機(jī)器人與目標(biāo)物體數(shù)學(xué)模型及幾何關(guān)系

基礎(chǔ)矩陣F是攝像機(jī)對極幾何關(guān)系[8]的代數(shù)表達(dá)。對極幾何示意圖，如圖1所示。攝像機(jī)在兩個(gè)時(shí)刻（t1，t2兩時(shí)刻成像中心不同）攝取具有一定相關(guān)性的目標(biāo)圖像，根據(jù)t1，t2時(shí)刻攝像機(jī)對應(yīng)點(diǎn)關(guān)系，基礎(chǔ)矩陣給出t1、t2時(shí)刻攝像機(jī)對的幾何關(guān)系。

圖1 對極幾何示意圖Fig.1 The Epipolar Geometry Diagram

本質(zhì)矩陣是歸一化的基礎(chǔ)矩陣，由對極幾何關(guān)系可知，基礎(chǔ)矩陣與本質(zhì)矩陣滿足，如式[9]（1）～式（3）所示。

式中：F、E—基礎(chǔ)矩陣和本質(zhì)矩陣；Mr—攝像頭標(biāo)定后的內(nèi)參數(shù)矩陣；［T］x—平移矢量T的反對稱矩陣；R—旋轉(zhuǎn)矩陣。

由式（3）可知，本質(zhì)矩陣包含了攝像機(jī)的外參數(shù)R和T。本質(zhì)矩陣可以通過式（2）求得，在對攝像頭進(jìn)行標(biāo)定后內(nèi)參矩陣即為已知。

3 機(jī)器人位姿估計(jì)

3.1 直線行走時(shí)位姿估計(jì)

通常在對運(yùn)動(dòng)物體進(jìn)行位姿分析時(shí)要同時(shí)考慮旋轉(zhuǎn)和平移，但是經(jīng)過上文的對機(jī)器人平臺的驅(qū)動(dòng)進(jìn)行分析后可知，輪式差動(dòng)驅(qū)動(dòng)在運(yùn)動(dòng)時(shí)不會(huì)同時(shí)發(fā)生旋轉(zhuǎn)和平移，因此在位姿估計(jì)計(jì)算時(shí)就可以分別對旋轉(zhuǎn)和平移進(jìn)行分析，降低了分析和計(jì)算難度。

當(dāng)兩輪直線行走時(shí)，由室內(nèi)機(jī)器人的工作環(huán)境和兩輪的工作方式可知此時(shí)，此時(shí)只用求出Z方向上的位移參數(shù)tz就可以獲得位姿轉(zhuǎn)換矩陣。提出一種簡化的計(jì)算參數(shù)tz的方法，可以減少計(jì)算量和獲得滿意的精度。

tz的計(jì)算流程：

（1）根據(jù)加速版的具有魯棒性的特征算法SURF[10]（Speeded Up Robust Features）提取特征點(diǎn)。

（2）對特征點(diǎn)進(jìn)行匹配，并對匹配后的距離進(jìn)行閾值判斷，對距離小于一定閾值的特征點(diǎn)對進(jìn)行保留。

（3）根據(jù)保留下來的特征點(diǎn)找到點(diǎn)云中相對應(yīng)點(diǎn)，并對點(diǎn)云中的該點(diǎn)做可信度判別。利用拉普拉斯四鄰域算子對點(diǎn)云中對應(yīng)的點(diǎn)的深度值做卷積，保留卷積結(jié)果為0的點(diǎn)，這些點(diǎn)就是點(diǎn)云中更加可信的點(diǎn)。然后獲取可信點(diǎn)的深度值Zi，求所有可信點(diǎn)深度值的平均值

（4）令 tz=Zmean，求得 tz。

如果鄰域系統(tǒng)是4鄰域，Laplacian算子的模板為：

表1 4鄰域模板Tab.1 Template of 4 Neighbourhood

上述的計(jì)算流程第二步是為了獲得更高的匹配準(zhǔn)確度，第三步使用拉普拉斯4鄰域算子對特征點(diǎn)相應(yīng)的點(diǎn)云中的點(diǎn)進(jìn)行檢測，檢查該點(diǎn)是否處在一個(gè)平坦的平面上。由于有些特征點(diǎn)在第二步判斷中雖然有一個(gè)很小的距離值，但是該點(diǎn)在三維環(huán)境中可能處在一個(gè)尖銳物體的角點(diǎn)，這時(shí)因?yàn)榇嬖跍y量誤差，并且無法判斷該點(diǎn)誤差值。因此為了避免選取這樣的點(diǎn)，使用Laplacian算子模板做卷積，并且留下做卷積后結(jié)果為0的點(diǎn)，這樣可以使處在平坦物體平面上的特征點(diǎn)被保留，tz的結(jié)果也更加精確。因?yàn)橹本€運(yùn)動(dòng)時(shí)位姿轉(zhuǎn)換矩陣中只有一個(gè)未知參數(shù)tz，當(dāng)tz通過上文計(jì)算得到后，位姿轉(zhuǎn)換矩陣就確定了。

3.2 轉(zhuǎn)彎時(shí)位姿估計(jì)

轉(zhuǎn)向時(shí)的位姿估計(jì)精度要求更高，為了提高位姿估計(jì)的精度，利用SURF進(jìn)行特征點(diǎn)提取和匹配，并且對匹配后的特征點(diǎn)進(jìn)行一系列的處理。通過對描述符的相似度閾值和對特征點(diǎn)Y軸坐標(biāo)進(jìn)行限制，可以選擇出更好的匹配點(diǎn)。

差動(dòng)式驅(qū)動(dòng)的移動(dòng)機(jī)器人兩輪的運(yùn)動(dòng)可知，理論上機(jī)器人平臺在轉(zhuǎn)彎時(shí)不存在平移，只繞著Y軸旋轉(zhuǎn)，旋轉(zhuǎn)矩陣R=，但是由于運(yùn)動(dòng)誤差依舊會(huì)產(chǎn)生微量平移，平移矢量此時(shí)E可表示為：

式中：［T］x—t時(shí)刻機(jī)器人坐標(biāo)系到t′時(shí)刻機(jī)器人坐標(biāo)系的平移矢量T=［tx0 tz］T的反對稱矩陣；R—對應(yīng)的旋轉(zhuǎn)矩陣。

通過將式（4）展開并且整理可得式（5）～式（7）：

將式（7）中的參數(shù)代入R中，旋轉(zhuǎn)向量R和平移矢量T可直接獲得結(jié)果，此時(shí)可以得到旋轉(zhuǎn)是的位姿轉(zhuǎn)換矩陣。與其它方法相比，本小節(jié)的算法大大減少了計(jì)算的復(fù)雜度，并且R和T唯一，保證了對機(jī)器人姿態(tài)控制。

4 實(shí)驗(yàn)研究

為了驗(yàn)證方法的有效性，在室內(nèi)環(huán)境下進(jìn)行實(shí)時(shí)三維重建實(shí)驗(yàn)。實(shí)驗(yàn)平臺為ARM9主控器嵌入式平臺，攝像頭為Kinect 1.0。不同方法下的特征點(diǎn)匹配結(jié)果，如圖2、圖3所示。多種方法在三維重建時(shí)計(jì)算位姿轉(zhuǎn)換矩陣的時(shí)間對比，如圖4所示。在線實(shí)現(xiàn)效果圖，如圖5所示。通過kinect采集轉(zhuǎn)彎時(shí)的兩幀圖片利用SURF特征點(diǎn)匹配后的結(jié)果，如圖2所示。通過上圖可知，由于環(huán)境復(fù)雜存在大量誤匹配點(diǎn)。對圖2顯示的匹配特征點(diǎn)進(jìn)行篩選后的結(jié)果的匹配結(jié)果，如圖3所示。篩選條件為：當(dāng)特征點(diǎn)的特征向量的歐式距離小于閾值和兩幀圖像特征點(diǎn)的Y軸坐標(biāo)應(yīng)相同。這兩個(gè)條件同時(shí)滿足時(shí)該特征點(diǎn)才能夠滿足我們的需求。這里的算法、PnP算法和ICP算法的計(jì)算時(shí)間對比圖，如圖4所示。從圖4中明顯可看出相比傳統(tǒng)的算法這里的算法時(shí)間更少。

圖2 原始SURF特征點(diǎn)提取匹配結(jié)果Fig.2 Matching Results of Original SURF Feature Point

圖3 兩幀圖像的匹配結(jié)果Fig.3 Matching Results of Two Frames of Images

圖5 在線實(shí)驗(yàn)結(jié)果Fig5 The Online Experimental Result

真實(shí)實(shí)驗(yàn)環(huán)境下進(jìn)行的實(shí)時(shí)三維重建，如圖5所示。從圖5中可看出轉(zhuǎn)角部分沒有重影，基本滿足重建模型精度需求。實(shí)際測量值的絕對誤差圖，如圖6所示。絕對誤差的大小與測量距離成正比關(guān)系，但是關(guān)鍵幀的選取將對測量值進(jìn)行閾值限制，保證絕對誤差在較小的范圍內(nèi)。當(dāng)移動(dòng)機(jī)器人平臺直線運(yùn)動(dòng)時(shí)，平移矢量T的參數(shù)tz的估計(jì)值與實(shí)際值依舊存在誤差.圖中橫坐標(biāo)表示實(shí)際移動(dòng)距離，縱坐標(biāo)表示絕對誤差。此處要注意的是“實(shí)際值”其實(shí)并非真正的“真實(shí)值”，因?yàn)檫@個(gè)“實(shí)際值”也是通過測量得到的，這個(gè)測量精度要比Kinect的測量精度高很多，所以可以將此“實(shí)際值”當(dāng)做真實(shí)值。

圖6 實(shí)際測量值的絕對誤差圖Fig.6 The Absolute Error of the Actual Measured Value

5 結(jié)論

提出的將復(fù)雜運(yùn)動(dòng)簡化為同一時(shí)刻僅存在平移或者旋轉(zhuǎn)，然后分別對這兩種情況用不同的方法求解轉(zhuǎn)換矩陣，簡化了轉(zhuǎn)換矩陣的求解過程，降低了計(jì)算量，使基于視覺的實(shí)時(shí)三維重建在嵌入式平臺上得到實(shí)現(xiàn)。基于真實(shí)環(huán)境下的機(jī)器人在線實(shí)驗(yàn)驗(yàn)證了所提出的轉(zhuǎn)換矩陣求解方法的有效性和優(yōu)越性。但是依舊存在一些問題，隨著機(jī)器人的移動(dòng)，轉(zhuǎn)換矩陣的誤差會(huì)逐漸累積，最后會(huì)影響重建結(jié)果，因此后續(xù)需要做回環(huán)檢測和圖優(yōu)化，以獲得更加精準(zhǔn)的重建效果。