嵌入式平臺上的三維重建算法研究

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

1 引言

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

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

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

2 移動機器人的運動和幾何關(guān)系分析

2.1 室內(nèi)機器人工作環(huán)境和運動方式的簡化

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

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

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

基礎(chǔ)矩陣F是攝像機對極幾何關(guān)系[8]的代數(shù)表達(dá)。對極幾何示意圖，如圖1所示。攝像機在兩個時刻（t1，t2兩時刻成像中心不同）攝取具有一定相關(guān)性的目標(biāo)圖像，根據(jù)t1，t2時刻攝像機對應(yīng)點關(guān)系，基礎(chǔ)矩陣給出t1、t2時刻攝像機對的幾何關(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ì)矩陣包含了攝像機的外參數(shù)R和T。本質(zhì)矩陣可以通過式（2）求得，在對攝像頭進(jìn)行標(biāo)定后內(nèi)參矩陣即為已知。

3 機器人位姿估計

3.1 直線行走時位姿估計

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

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

tz的計算流程：

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

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

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

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

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

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

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

3.2 轉(zhuǎn)彎時位姿估計

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

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

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

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

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

4 實驗研究

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

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

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

圖5 在線實驗結(jié)果Fig5 The Online Experimental Result

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

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

5 結(jié)論

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