孟建軍,陳曉彤,李德倉(cāng),祁文哲

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

1 引言

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

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

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

2 算法框架設(shè)計(jì)

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

圖1 算法框架

3 位姿估計(jì)算法

3.1 ICP算法

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

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

(1)

(2)

(3)

3.2 EPnP算法

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

(4)

(5)

(6)

(7)

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

(8)

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

3.3 ICP和EPnP混合算法

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

(9)

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

(10)

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

(11)

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

4 結(jié)果與分析

4.1 實(shí)驗(yàn)平臺(tái)搭建

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

4.2 ICP算法結(jié)果

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

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

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

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

4.3 EPnP算法結(jié)果

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

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

1)通過(guò)設(shè)置控制點(diǎn)對(duì)點(diǎn)云進(jìn)行初始配準(zhǔn),以彌補(bǔ)ICP算法無(wú)法對(duì)深度相差較大的點(diǎn)云進(jìn)行配準(zhǔn)的缺陷。

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

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