計算機視覺技術(shù)的位姿估計處理方法

孟建軍,陳曉彤,李德倉,祁文哲

(1. 蘭州交通大學機電技術(shù)研究所,甘肅 蘭州,730070;2. 甘肅省物流及運輸裝備信息化工程技術(shù)研究中心,甘肅 蘭州,730070;3. 甘肅省物流與運輸裝備行業(yè)技術(shù)中心,甘肅 蘭州,730070)

1 引言

智能化和信息化技術(shù)的應(yīng)用在很大程度上解放了人們的雙手,減少勞動力的投入,節(jié)省了企業(yè)的人工成本且保障了人身安全。因此,工程領(lǐng)域智能化與信息化的發(fā)展有助于自動化模式的形成,加快社會經(jīng)濟發(fā)展進程[1]。

采用視覺技術(shù)從周圍環(huán)境圖像中提取有用的信息,并恢復(fù)場景的三維圖像,從而確定目標、識別路徑、判斷沿途是否存在障礙物,最終得到可行駛范圍和障礙規(guī)避[2.3]。在陌生環(huán)境或者是GPS(Global Positioning System)定位無法發(fā)揮功效的情況下,便需要實現(xiàn)自身的實時準確定位和當下環(huán)境的建圖。目前,較為熱門的同步定位與建圖(Simultaneous Localization and Mapping,SLAM)主要依賴激光雷達傳感器和視覺傳感器來幫助其完成一系列動作指令。SLAM算法是一種實現(xiàn)定位并進行地圖構(gòu)建的系統(tǒng)。而ROS機器人系統(tǒng)(Robot Operating System)則可以提供環(huán)境感知、運動控制、路徑規(guī)劃等研究領(lǐng)域經(jīng)典并且前沿的算法代碼庫,可以加強機器人代碼的復(fù)用率的模塊化[4]。

為了實現(xiàn)機器人自主導(dǎo)航作業(yè),許多智能算法被大量應(yīng)用。為提高作業(yè)的準確性,這就要求準確掌握環(huán)境中的物體姿態(tài),從場景圖像中確定目標圖像位姿的問題核心就是位姿估計。近年來,位姿估計逐步成為計算機視覺研究的研究熱點。Sarlin[5]等人提出了層次定位方法,上述方法利用卷積神經(jīng)網(wǎng)絡(luò)在場景圖像中找到候選幀,候選幀聚類后與查詢圖像進行特征點匹配,建立特征點的2D-3D對應(yīng)關(guān)系,使用PnP算法估計相機位姿;Chen等[6]使用熱力圖表示方法回歸目標稀疏關(guān)鍵點,使用模擬退火算法篩選外點,并設(shè)計非線性優(yōu)化方法求解PnP問題,上述方法均利用PnP算法處理位姿問題,但存在一缺陷,若噪聲影響3D點或2D點,易發(fā)生錯誤匹配問題。EPEL＿cvlab[7]將圖像均勻分割成若干網(wǎng)格,借助圖像分割的方式預(yù)測衛(wèi)星的8個三維包圍框角點,使用EPnP求解位姿,EPnP算法在深度范圍變化大的環(huán)境下求解位姿精度明顯降低。這些算法雖然在性能上取得了一定的優(yōu)越性,但仍然存在不足,例如存在歸納偏置問題,從而導(dǎo)致訓練所得到的模型泛化能力不足;存在缺少絕對位置信息、建模能力不足等;其中傳統(tǒng)ICP算法(Iterative Closest Point迭代最近點),首先要將邊界點以及深度過大的點剔除,然后進行點云之間的匹配,并未包含局部坐標信息,最后通過多次迭代搜索最近點,這會造成高額的計算代價,且對深度未知的點無法計算;而EPnP(Efficient Perspective-n-Point)算法精度較高、能很好地解決圖像深度和抗干擾能力差的問題,但是存在模型泛化能力不足等缺陷,分析ICP算法和EPnP算法缺點,結(jié)合二者的優(yōu)點。因此,提出一種將ICP和EPnP結(jié)合的算法,用于計算機視覺圖像處理的位姿估計,在實時性和準確性上有較大提高。

2 算法框架設(shè)計

對環(huán)境物體定位,主要是通過設(shè)定一個形狀特別又易辨識的標桿,利用相機對環(huán)境中的物體進行獲取,提取特征點,并對圖像中的特征點坐標處理,之后再提取其像素坐標,進行比對篩選剔除;接著求解相機的位姿狀態(tài),提高定位精度;最后對圖像中物體姿態(tài)進行位姿估計,算法框架如圖1所示。

圖1 算法框架

3 位姿估計算法

3.1 ICP算法

ICP配準法主要用于解決基于自由形態(tài)曲面的匹配問題,其特點在于在進行算法匹配的過程中不需要進行環(huán)境特征的提取,并且收斂速度和匹配精度完全依賴于相匹配的兩個點云數(shù)據(jù)中最鄰近點對的搜索結(jié)果。目前,最鄰近點對的搜索方式主要有:點到點最鄰近搜索、點到平面最鄰近搜索算法、點到投影、收縮投影點最鄰近搜索[8]。

傳統(tǒng)的ICP算法是一種點集之間的配準方法。通過相機坐標系獲得圖像特征點的坐標值,然后利用奇異值分解或最小二乘法進行位姿的求解,最終得到旋轉(zhuǎn)矩陣和平移矩陣,如式(1)和式(2)所示。

(1)

(2)

(3)

3.2 EPnP算法

PnP(Perspective-n-Point)是在世界坐標系下給定一組由n個3D點組成的點集,將點集在圖像中投影出相應(yīng)的2D圖像,以此解決相機的位姿問題。通過6個自由度組成的相機位姿包括旋轉(zhuǎn)和相機對于世界的平移,其中旋轉(zhuǎn)包括滾動、俯仰以及偏航。位姿估計問題的關(guān)鍵在于相機校準,而且廣泛應(yīng)用于計算機視覺和其他領(lǐng)域中,如機器人等[9]。

(4)

(5)

(6)

(7)

當四個控制點和圖像點對應(yīng)時,將n個參考點帶入式(7),就會產(chǎn)生一個線性方程組,即為:Mx=0,式中x是待求的12個未知數(shù),M為2n*12的矩陣,如式(8)所示。

(8)

根據(jù)歐氏變換,將已知的四個控制點間距轉(zhuǎn)換為四個點在相機坐標系下的坐標,從而將圖像從三維變換為二維進行求解。

3.3 ICP和EPnP混合算法

EPnP算法將參考點的坐標表示為控制點坐標的加權(quán)和,如式(9)所示。

(9)

根據(jù)ICP的代數(shù)求解方法,找出第i個點的誤差項,表示如式(10)所示。

(10)

然后建立最小二乘問題,利用奇異值分解(SVD)的方法求解最小的誤差平方和。鑒于深度圖像有噪聲影響,可能存在數(shù)據(jù)丟失的問題,因此采用混合使用ICP和EPnP的算法,對于已知特征點,采用ICP算法的運動恢復(fù)方法,對于未知特征點使用EPnP算法的運動恢復(fù)方法。并利用三維線性空間內(nèi)4個控制點組成的基來重新表示三維空間,然后求解將n個空間點轉(zhuǎn)化為相機坐標系下的坐標問題,最終得到一個線性方程。上述方程的解位于矩陣M的零空間,則有式(11)所示。

(11)

式中:vi(i=1,2,…,N)表示矩陣MTM所得特征值對應(yīng)的特征向量,βi是第i項化簡后的系數(shù)。由此得到,在相機坐標系下的4個世界坐標系控制點坐標,進而得到n個世界坐標系下參考點在相機坐標系下的坐標。最后,將轉(zhuǎn)化為已知世界坐標系下的3D點到相機坐標系下3D點的對應(yīng)關(guān)系來求解相機位姿的問題,同樣由SVD來進行求解[11]。

4 結(jié)果與分析

4.1 實驗平臺搭建

所有算法的實現(xiàn)均采用的是MATLAB 軟件進行仿真實驗。其中所涉及的數(shù)據(jù)為隨機數(shù)據(jù),避免了仿真數(shù)據(jù)結(jié)果的偶然性。

4.2 ICP算法結(jié)果

對插入的數(shù)據(jù)模擬一個三維圖像,分別從x、y、z三個方向拆分圖像,如圖2所示,其中圖2(a)是x軸方向圖像,圖2(b)是y軸方向圖像,圖2(c)是z軸方向圖像?？砂l(fā)現(xiàn)拆分圖像中存在或多或少的噪音且為未知特征點。

圖2 原始數(shù)據(jù)三個方向拆分圖像

圖3是ICP算法處理結(jié)果圖比對圖,其中圖3(a)是原始圖像,圖3(b)是處理結(jié)果。圖3(a)與圖3(b)對比發(fā)現(xiàn)存在一定的誤差,且在配準精度和效率上存在差異,在處理過程中就近搜索相應(yīng)的就近點,計算點集的重心位置,然后生成新的點集,提供正定矩陣及其特征值和最大特征向量,在旋轉(zhuǎn)矩陣的確定下發(fā)生平移。在原始數(shù)據(jù)深度過大或者噪聲影響過大的情況下會產(chǎn)生誤差,從圖3(b)中可以清楚看出數(shù)據(jù)點發(fā)生的偏移。

圖3 ICP算法處理結(jié)果比對圖

4.3 EPnP算法結(jié)果

EPnP算法主要是處理空間中點對于深度的影響的擬合關(guān)系,因此,本節(jié)利用隨機產(chǎn)生100個數(shù)據(jù)點和噪聲設(shè)置為20,進行圖像仿真,圖4是原始數(shù)據(jù)與噪音圖,采用MATLAB進行仿真實驗,圖中的圈代表了噪音,點代表數(shù)據(jù)點位置。

圖4 原始數(shù)據(jù)與噪音圖

圖5是EPnP算法處理結(jié)果圖,通過圖像中的紅點和藍圈的位置關(guān)系和圖4中的紅點和綠圈的位置關(guān)系比對,可以發(fā)現(xiàn)在很大程度上二者發(fā)生重合。由此可見,EPnP算法可以有效地減少噪音對位置點的影響。

圖5 EPnP算法處理結(jié)果圖

4.4 ICP和EPnP混合算法結(jié)果

基于EPnP算法處理后的效果和ICP算法處理后存在的問題,結(jié)合二者優(yōu)點,采用 ICP和EPnP混合算法。因此,在融入EPnP算法后,選擇4個控制點,同時對這4個控制點進行優(yōu)化,所以加快了運算速度;在式(8)中,只需考慮4個奇異向量,提高了識別精度。

對于已知特征點,采用ICP算法的運動恢復(fù)方法,對于未知特征點使用EPnP算法的運動恢復(fù)方法。利用三維線性空間中的基底來重新標定三維空間的點,然后求解圖像中n個特征點在相機坐標系下的坐標,最后求解相機位姿問題。通過融合EPnP算法便彌補了ICP算法處理過程由于深度和噪聲而產(chǎn)生的誤差影響。理論上所處理的效果圖為如圖6。相對比下會發(fā)現(xiàn),改進后的算法所受到的深度影響較小,與原圖的相似度更高。

圖6 ICP和EPnP混合算法處理結(jié)果圖

4.5 結(jié)果對比及分析

ICP算法是傳統(tǒng)的位姿估計算法,通過對比圖3中a、b兩圖,可以直觀地發(fā)現(xiàn)圖像在上方存在一定的誤差,而其余部分圖像相似度很高,由此可知在特征點已知的情況下,ICP算法可以很好地進行仿真估計,但在點云特征點未知的情況下,就存在數(shù)據(jù)的偏移而引起圖像的失真問題。EPnP算法側(cè)重點在于點云特征點未知的問題上有很好的解決方法,針對圖4和圖5中的噪音和特征點重合程度的對比,可看出EPnP算法有效地解決了噪聲影響問題,更好地進行點云與點云之間的擬合。因此,ICP和EPnP混合算法是綜合了二者各自的優(yōu)點,通過圖6與圖3(b)對比,可以發(fā)現(xiàn)圖像上側(cè)在圖3(b)中的失真問題在圖6中有所減少,有效解決了ICP算法中存在的較為嚴重的噪音問題,可以提高圖像位姿的準確度。

5 結(jié)論

針對傳統(tǒng)ICP算法在初始位置相差較大以及具有部分重疊的點云時無法實現(xiàn)精確配準問題;EPnP算法可以在深度計算上存在一定優(yōu)勢,可以彌補ICP算法在未知處點云的匹配。因此,提出一種使用ICP和EPnP融合算法來進行點云配準。

1)通過設(shè)置控制點對點云進行初始配準,以彌補ICP算法無法對深度相差較大的點云進行配準的缺陷。

2)在ICP算法處理過程中,可發(fā)現(xiàn)控制點數(shù)目較少而導(dǎo)致的圖像擬合問題,若增加控制點數(shù)目,將導(dǎo)致迭代過程出現(xiàn)較大誤差,而使用ICP和EPnP混合算法來進行運動估計以及引入改進的半隨機檢測方法進行閉環(huán)檢測,解決了準確性和實時性不高等問題。

因此,ICP和EPnP融合算法是一種更為便利位姿估計方法,為計算機視覺技術(shù)在人工智能產(chǎn)業(yè)中的應(yīng)用提供參考。