趙文愷，李 剛

(廣西大學(xué) 電氣工程學(xué)院，廣西 南寧 530004)

0 引 言

近年來，隨著機(jī)器人行業(yè)的快速發(fā)展，視覺定位技術(shù)成為機(jī)器人導(dǎo)航領(lǐng)域的重要研究方向[1,2]，其中視覺里程計(jì)(visual odometry，VO)是機(jī)器人定位的重要方法之一。若以傳感器選型為基礎(chǔ)，可對視覺里程計(jì)進(jìn)行詳細(xì)地分類，共涉及3種類型，依次是單目、雙目和深度相機(jī)[3]，在這3種相機(jī)類型中，深度相機(jī)與單目相機(jī)均存在一定的缺陷，例如，具有相對較大的噪聲等[4]，故而，在VO領(lǐng)域被普遍運(yùn)用的相機(jī)類型主要是雙目相機(jī)[5-7]。由于采集圖像信息在一定程度上存在差異性，故而，又可細(xì)分為兩種方法，第一，特征點(diǎn)法[8]，此方法具有相對而言比較強(qiáng)的魯棒性，即便噪音十分嘈雜，也可順利地完成工作，因此，此方法也成為一項(xiàng)核心方法；第二，直接法[9]，應(yīng)用頻率相對較低。

現(xiàn)階段，針對VO領(lǐng)域，無論是國內(nèi)，還是國外，均已獲得了許多優(yōu)異的研究成果。根據(jù)文獻(xiàn)[10]可知，其以視覺詞典為核心，通過一系列設(shè)計(jì)得到了視覺里程計(jì)，通過對圖像庫構(gòu)造字典匹配位姿，這種方法體現(xiàn)出了十分明顯的優(yōu)勢，即具有相對較高的匹配精度，然而，由于整個(gè)操作過程具有一定的復(fù)雜性，故而，具體的應(yīng)用依然存在局限性。曹文平[11]對此做出了相應(yīng)的改進(jìn)，運(yùn)用BM立體匹配算法，基于特定的方式計(jì)算了雙目視差，并以像素灰度為基礎(chǔ)，基于有效手段完成了代價(jià)函數(shù)的構(gòu)造操作，盡管具有相對而言比較高的運(yùn)算速度，但依然存在一定的缺陷，這主要體現(xiàn)于視差圖方面，十分容易導(dǎo)致空洞點(diǎn)的出現(xiàn)。此后，文獻(xiàn)[12]提出了一種改進(jìn)后的雙目相機(jī)，此相機(jī)的VO主要以直接法為核心而完成相應(yīng)的設(shè)計(jì)，表現(xiàn)出了一定的優(yōu)勢，主要指對大規(guī)模稠密地圖的恢復(fù)方面，然而，該相機(jī)的應(yīng)用卻需要借助于合適的光照，故限制性因素較強(qiáng)。

現(xiàn)階段，視覺里程計(jì)依然面臨一系列問題，例如，設(shè)備成本相對較高、定位精度有待于進(jìn)一步改進(jìn)等，對此，本設(shè)計(jì)運(yùn)用的視覺里程計(jì)主要有一種，即以雙目相機(jī)為核心的視覺里程計(jì)。提取特征點(diǎn)時(shí)，本文主要運(yùn)用了ORB算法，使特征點(diǎn)范圍在原有基礎(chǔ)上有所增加，從而確保了信息的全面性，同時(shí)，還最大程度確保了ORB的實(shí)時(shí)響應(yīng)性；為了使視差獲取更具精確度，本設(shè)計(jì)構(gòu)建代價(jià)方程計(jì)算視差時(shí)，主要基于BRIEF描述子完成了相應(yīng)的構(gòu)建過程；為了達(dá)到深入改進(jìn)VO精度的目的，以PnP的3D-2D模型為基礎(chǔ)，提出一種特定的迭代算法，即提供初值的算法，并基于g2o對相機(jī)所在位姿進(jìn)行計(jì)算。

1 基于改進(jìn)ORB的特征點(diǎn)檢測與匹配

針對特征點(diǎn)提取，最常見的一些算法有SIFT，SURF，ORB。在這些算法中，SIFT算法與SURF算法均具有一定的缺陷，因而，不能滿足本設(shè)計(jì)的相應(yīng)要求。基于對VO實(shí)時(shí)性的考慮，本文對特征點(diǎn)提取時(shí)，運(yùn)用的算法主要有一種，即基于ORB的特征點(diǎn)提取算法。一般而言，ORB算法對特征匹配時(shí)，主要通過兩個(gè)操作完成相應(yīng)的匹配過程，第一，對FAST關(guān)鍵點(diǎn)進(jìn)行提取；第二，完成BRIEF描述子的構(gòu)造過程，在滿足尺度不變性的基礎(chǔ)上，并盡可能確保旋轉(zhuǎn)不變性的前提下，引入灰度質(zhì)心，并引入圖像金字塔。對于ORB算法，盡管其最大程度確保了特征提取的運(yùn)算速度，然而，在閾值方面，因FAST的設(shè)置相對而言比較單調(diào)，十分容易導(dǎo)致分布不均現(xiàn)象，將在極大程度上影響配準(zhǔn)精度。

1.1 基于改進(jìn)ORB的特征點(diǎn)檢測

基于上述問題，本文運(yùn)用的算法主要是特定的ORB算法，此算法主要以四叉樹圖像分割為基礎(chǔ)[13]，對多閾值FAST提取特征點(diǎn)進(jìn)行了相應(yīng)的設(shè)置。首先采取一系列有效措施完成了高斯圖像金字塔的構(gòu)建操作，以各個(gè)尺度為基礎(chǔ)，對相應(yīng)的單元格進(jìn)行劃分，而其又在一定程度上有關(guān)于待檢測關(guān)鍵點(diǎn)，以各個(gè)單元格為基準(zhǔn)，采取有效措施完成相應(yīng)地FAST檢測，對于關(guān)鍵點(diǎn)數(shù)目，若基于一系列假設(shè)檢測即可獲得其數(shù)值，則隨后展開后續(xù)單元格的檢測，否則將進(jìn)入二次檢測過程。檢測后，對于獲得的特征點(diǎn)，再次基于特定的方式完成相應(yīng)的劃分操作，主要基于四叉樹方式，為了使劃分結(jié)果在一定程度上具有平衡性，應(yīng)以特征點(diǎn)數(shù)目為核心，完成相應(yīng)的順序排列操作，然后展開相應(yīng)的節(jié)點(diǎn)管理工作。最后對其特征方向展開具體的計(jì)算操作，并完成描述子的計(jì)算過程。

存儲特征點(diǎn)時(shí)，采取的存儲方式相對而言比較特殊，主要為四叉樹方式，不但能夠最大程度地節(jié)約存儲空間，還能夠使特征點(diǎn)的分布更具勻稱性。詳情如圖1所示。

圖1 改進(jìn)ORB算法框架

四叉樹主要運(yùn)用于二位空間分割，并對其做出了詳細(xì)定義，即一個(gè)節(jié)點(diǎn)下，可存在的節(jié)點(diǎn)數(shù)相對而言比較小，最多有4個(gè)節(jié)點(diǎn)，將圖像細(xì)分為多個(gè)區(qū)域，主要有4個(gè)區(qū)域，詳情如圖2所示，整個(gè)空間被細(xì)分為多個(gè)同等的象限，共計(jì)4個(gè)，如圖2所示，分別是UL,UR,BL,BR，n1,n2,n3,n4為展開節(jié)點(diǎn)，圓圈代表的含義主要有一點(diǎn)，主要指檢測到的FAST角點(diǎn)。

圖2 四叉樹索引

1.2 基于改進(jìn)ORB的特征點(diǎn)匹配

視覺里程計(jì)中，最核心的一項(xiàng)環(huán)節(jié)為特征匹配環(huán)節(jié)，若特征匹配具有較好的合理性，則十分有利于后續(xù)的位姿估計(jì)。針對匹配方法，比較常見的方法主要涉及兩種，第一種，暴力匹配(brute-force matcher)，此算法相對而言比較簡便，然而，若特征點(diǎn)數(shù)量在一定程度上有所增加，則總體的運(yùn)行時(shí)間必定會有所延長；第二種，快速近似最近鄰(FLANN)，此算法主要運(yùn)用于對實(shí)時(shí)性要求相對較高的場合。然而，若僅以FLANN為基準(zhǔn)，則必定會在一定程度上導(dǎo)致誤匹配，故而，本設(shè)計(jì)運(yùn)用的算法則相對特殊，主要為FLANN和RANSAC的組合匹配算法。

RANSAC算法具有自身獨(dú)特的優(yōu)勢，主要體現(xiàn)于一點(diǎn)，即可使噪聲得到最大程度地消除。在匹配圖像時(shí)，RANSAC算法的關(guān)鍵步驟為采取有效方式完成單應(yīng)性矩陣的計(jì)算過程，這體現(xiàn)于多視圖幾何中，主要指兩幅圖片特征點(diǎn)處于共同平面上的映射關(guān)系。假定像素點(diǎn)坐標(biāo)是(u,v)，單應(yīng)性矩陣H定義為

(1)

若要計(jì)算H矩陣的8個(gè)參數(shù)，至少需要4對匹配點(diǎn)，而實(shí)際給定點(diǎn)數(shù)遠(yuǎn)遠(yuǎn)大于8對，因此使用隨機(jī)一致性檢驗(yàn)求解。描述如下：

(1)選定輸入?yún)?shù)時(shí)，以多數(shù)的匹配點(diǎn)為核心，采取特定的方式選定這些匹配點(diǎn)的4對;

(2)經(jīng)過相應(yīng)的采集處理后，以所得8個(gè)點(diǎn)為基礎(chǔ)，采取有效方式完成單應(yīng)性矩陣H的估算過程;

(3)以上述H矩陣為基準(zhǔn)，采取特定的計(jì)算方式對剩余匹配成功點(diǎn)對數(shù)n進(jìn)行相應(yīng)的驗(yàn)證;

(4)迭代K次，找到最大n滿足的矩陣H。

2 基于BRIEF描述子的雙目視差

立體匹配共涉及兩種，第一種，局部立體匹配[14]，這種算法又可細(xì)分為兩種，分別為BM和SGBM，具有相對而言比較差的視覺效果；第二種，全局立體匹配，此算法具有相對較高的計(jì)算精度，然而，整體的計(jì)算量相對龐大，在具體運(yùn)用過程中，并無較好的應(yīng)用型。本設(shè)計(jì)運(yùn)用的算法同樣未局部立體匹配算法，但這種算法主要以BRIEF描述子為基礎(chǔ)，故整體性能有所改進(jìn)，對特征點(diǎn)視差的提取具有一定的優(yōu)勢。

此前，基于ORB特征提取能夠獲得BRIEF描述子，BRIEF是一種十分特殊的二進(jìn)制編碼，其組成方式主要為向量組成，即0,1向量，在特征點(diǎn)周圍的圖像塊A處隨意選定像素點(diǎn)p1和p2，A(p1) 代表的含義相對固定，主要為p1點(diǎn)灰度值，A(p2) 同樣具有固定含義，即p2點(diǎn)灰度值，若I(p1) 與I(p2) 相比，后者數(shù)值相對較大，則取0，反之，則取1。定義式(2)為

(2)

以得到的BRIEF向量為特征信息進(jìn)行匹配，算法描述如下：

(1)將待匹配特征點(diǎn)為核心，采取有效方式與手段在參考圖像中構(gòu)建圖像主窗口，中心點(diǎn)的表征方式主要基于一種，即多個(gè)像素的BRIEF表征;

(2)相同行內(nèi)，以目標(biāo)圖像為基準(zhǔn)，用窗口完成相應(yīng)的掃描操作，并分別構(gòu)建等同的子窗口;

(3)構(gòu)造代價(jià)函數(shù)(cost funcation)，對特定的相似度進(jìn)行評估，主要指主窗口與子窗口，并在確定目標(biāo)點(diǎn)時(shí)，以此為基準(zhǔn)，即以具有相對較高相似度的最高窗口的中點(diǎn)確定為目標(biāo)點(diǎn);

(4)以左右圖像為基礎(chǔ)，采取適當(dāng)?shù)挠?jì)算方式確定其像素點(diǎn)視差;

(5)對(1)～(4)步驟進(jìn)行重復(fù)操作，獲得所有有用的像素點(diǎn)視差。

詳情如圖3所示。

圖3 立體匹配

常用代價(jià)函數(shù)構(gòu)造方法有：

SAD(sum of absolute difference)

(3)

SSD(sum of squared difference)

(4)

NCC(normalized cross correlation)

(5)

式中：L、R代表的含義相對而言比較固定，主要指為左右像素特征，i，j同樣具有各自的固定含義，分別為遍歷整個(gè)窗口元素，d值的計(jì)算次數(shù)每增加一次，則應(yīng)相應(yīng)地增加1，當(dāng)S的數(shù)值處于峰值時(shí)，則相應(yīng)的匹配會處于結(jié)束狀態(tài)，而所得點(diǎn)便是匹配點(diǎn)，d代表的含義相對固定，指視差。

3 運(yùn)動估計(jì)

以前后時(shí)刻兩幀圖像為基礎(chǔ)，采取特定的操作方式獲得特征匹配點(diǎn)后，則應(yīng)對其運(yùn)動展開合理的預(yù)估?；陔p目相機(jī)，借助于內(nèi)置的特定運(yùn)行方式能夠得到特征點(diǎn)3D坐標(biāo)，并可由此得到特征點(diǎn)在相應(yīng)圖像的2D坐標(biāo)，故而，對相機(jī)運(yùn)動進(jìn)行估計(jì)時(shí)，可基于PnP(perspective-n-point)予以操作。PnP是3D-2D運(yùn)動恢復(fù)模型，具體的求解方法共有多種，例如，EPnP等。然而，所用的方法均為線性計(jì)算方法，均有一定的缺陷。本設(shè)計(jì)所用的運(yùn)動估計(jì)方法則恰好相反，主要為以非線性優(yōu)化為基礎(chǔ)的方法，先以線性方法為核心，采取有效措施完成相應(yīng)的初值求解過程，再完成最小二乘方程的構(gòu)造操作，從而實(shí)現(xiàn)優(yōu)化位姿的目的[15]。

3.1 PnP投影模型

PnP在求解位姿時(shí)至少需要3對3D-2D點(diǎn)，模型如圖4所示。

圖4 PnP投影模型

此中，相機(jī)光心為P，世界坐標(biāo)系3D點(diǎn)與像素坐標(biāo)2D點(diǎn)相對而言比較固定，主要指A，B，C與a，b，c，字母的大寫與小寫之間具有較好的一致性。由此可知：△Pab～△PAB，△Pbc～△PBC，△Pac～△PAC。由余弦定理可得

PA2+PB2-2PA·PB·cos(a,b)=AB2
PB2+PC2-2PB·PC·cos(b,c)=BC2
PA2+PC2-2PA·PC·cos(a,c)=AC2

(6)

設(shè)u=AB2/PC2,v=BC2/AB2,w=AC2/AB2， 化簡得

(1-u)y2-ux2-ycos(a,b)+2uxycos(a,b)+1=0
(1-w)x2-wy2-xcos(a,b)+2wxycos(a,b)+1=0

(7)

可知該方程組為二元二次方程，利用吳消元法求解PA，PB，PC距離，最后得出相機(jī)運(yùn)動。

3.2 非線性優(yōu)化運(yùn)動估計(jì)

上述求解為線性方法，在3D點(diǎn)或2D點(diǎn)受噪聲干擾，出現(xiàn)誤匹配時(shí)位姿估計(jì)結(jié)果不佳，算法失效，因此可以把PnP構(gòu)造成非線性最小二乘問題，通過最小化重投影誤差(reprojection error)求解最優(yōu)值。

假設(shè)計(jì)算n個(gè)三維空間點(diǎn)Pi(Xi,Yi,Zi) 及其投影點(diǎn)Ui(ui,vi,zi)，為相機(jī)位姿 (R,t) 的李代數(shù)，相機(jī)內(nèi)參為K，關(guān)系式如下

(8)

由于存在噪聲，預(yù)測值與觀測值存在誤差，構(gòu)建最小二乘方程如式(9)

(9)

3.3 圖優(yōu)化的PnP求解

為了獲得相對而言比較低的方程值，應(yīng)采取有效方法與手段計(jì)算最優(yōu)位姿。所謂圖優(yōu)化，主要指以圖像的方式對優(yōu)化問題加以詳細(xì)地描述，而凸優(yōu)化的求解方式則應(yīng)基于一定的軟件，即g2o?；趫D的頂點(diǎn)等方式的詳細(xì)構(gòu)建完成最優(yōu)位姿的求解操作，此中，頂點(diǎn)代表的含義相對而言比較固定，主要指待優(yōu)化變量，相機(jī)的位姿，邊為誤差項(xiàng)。詳情如圖5所示。

圖5 PnP圖優(yōu)化

運(yùn)動位姿求解流程如下：

(1)運(yùn)動位姿初值的獲取方式主要基于一種，即傳統(tǒng)PnP線性求解法;

(2)以位姿初值為基準(zhǔn)，將其設(shè)定為圖優(yōu)化模型頂點(diǎn)，并對邊進(jìn)行了相應(yīng)地定義，即定義為重投影誤差;

(3)以L—M為基準(zhǔn)完成相應(yīng)的迭代操作;

(4)基于g2o，采取一系列有效措施與手段獲得最優(yōu)運(yùn)動位姿。

4 實(shí)驗(yàn)結(jié)果與分析

(1)實(shí)驗(yàn)環(huán)境

實(shí)驗(yàn)平臺采用華碩A55V，配置Intel(R) Core CPU i5-3210M，主頻2.5 GHZ，4 G內(nèi)存，軟件環(huán)境為Clion+Ubuntu14.04操作系統(tǒng)。雙目視覺傳感器的組成主要基于一種方式，即以型號XG200的兩臺工業(yè)相機(jī)為基礎(chǔ)，采取特定的技術(shù)手段對其予以支架組合方式，獲得相應(yīng)的雙目視覺傳感器?？刹扇∈謩诱{(diào)節(jié)方式對焦距進(jìn)行相應(yīng)地調(diào)節(jié)，同時(shí)，還可基于相同的方式調(diào)節(jié)基線距離，圖像分辨率640*480。

(2)特征提取與特征匹配

實(shí)驗(yàn)過程中，對每張圖片進(jìn)行了相應(yīng)的設(shè)定，所提特征點(diǎn)的數(shù)量是固定的，最多不超過500個(gè)點(diǎn)。圖6為OpenCV中ORB與改進(jìn)ORB特征提取結(jié)果對照，由此可知，對于特征部分，若基于改進(jìn)后的ORB算法，則可獲得相對而言比較勻稱的分布。

對相鄰兩幀圖像進(jìn)行匹配，圖7(a)與圖7(b)代表的含義分別是只有FLANN算法的結(jié)果、FLANN和RANSAC組合匹配算法的結(jié)果，據(jù)此可知，后者可以獲得相對而言比較優(yōu)良的VO特征匹配效果。

圖7 消除誤匹配效果

(3)測試數(shù)據(jù)集

根據(jù)表1，以相鄰兩幀圖像為基準(zhǔn)，采取一系列有效措施對兩者的定位精度進(jìn)行相應(yīng)的測量，假定坐標(biāo)系原點(diǎn)(0,0)為最初位置，采樣節(jié)點(diǎn)的數(shù)量共計(jì)4個(gè)，對比兩者的誤差。

據(jù)表1能夠得知，在兩幀圖像間位置進(jìn)行相應(yīng)計(jì)算時(shí)，該視覺里程計(jì)具有相對而言比較高的定位精度，且平均相對誤差比較低，具體誤差百分比為2.65%，基本符合相應(yīng)的要求。

對于優(yōu)化前與優(yōu)化后的位置誤差進(jìn)行詳細(xì)測定，并對于平均每幀耗時(shí)進(jìn)行相應(yīng)地測定，對數(shù)據(jù)進(jìn)行采集時(shí)，以Y軸為基礎(chǔ)，使雙目相機(jī)展開一定距離的直線運(yùn)動，運(yùn)動距離為50 m。因相機(jī)始終處于相對而言比較固定的高度位置，相機(jī)的整個(gè)運(yùn)動也始終保持直線運(yùn)動趨勢，故而，相應(yīng)誤差的來源則共涉及一種途徑，即Y軸移動距離。據(jù)圖8能夠得知，針對視覺里程計(jì)，若采取優(yōu)化處理后，則可使誤差在較大程度上有所減小，然而，位置誤差同樣會伴隨采樣距離的增加而增大，這是由于本文的優(yōu)化處理操作具有一定的局限性，僅針對前端視覺里程計(jì)相鄰兩幀圖像間的位姿，后端并未做出相應(yīng)地優(yōu)化處理，與具體情況具有較好的一致性。

據(jù)表2，對于運(yùn)行時(shí)間，將優(yōu)化與未優(yōu)化時(shí)，均加入其中，同時(shí)，對于誤差范圍，亦是如此，由此可知，若采取優(yōu)化處理后，每幀圖像處理時(shí)間并無顯著增加，然而，卻在一定程度上降低了相對誤差，總體而言，符合相應(yīng)的需求。

表1 采樣與視覺定位位置對比/m

圖8 采樣位置與位置誤差

表2 運(yùn)行時(shí)間和誤差范圍

5 結(jié)束語

本設(shè)計(jì)對視覺里程提出了一定的要求，主要是應(yīng)能滿足對相鄰兩幀相機(jī)運(yùn)動狀態(tài)進(jìn)行實(shí)時(shí)估計(jì)的目的，然后，由于現(xiàn)階段視覺里程計(jì)存在一系列問題，例如，定位精度相對較低等，故，提出了一種改進(jìn)視覺里程計(jì)的方案，此方案主要以雙目相機(jī)為基準(zhǔn)。在現(xiàn)有ORB算法的基礎(chǔ)上進(jìn)行改進(jìn)，提取描述子作為信息獲得視差，恢復(fù)特征點(diǎn)三維坐標(biāo)，采用FLANN和RANSAC組合算法進(jìn)行匹配，對匹配到的特征點(diǎn)構(gòu)建最小二乘問題，求得最佳位姿。根據(jù)實(shí)驗(yàn)結(jié)果可知，對于相鄰兩幀圖像，該方案可以較好地對兩者之間的位姿進(jìn)行估計(jì)，符合了相應(yīng)的要求。同時(shí)，運(yùn)用的整個(gè)過程中，因并未展開相應(yīng)的優(yōu)化處理操作，會導(dǎo)致一定的累積誤差，故應(yīng)實(shí)施一定的人工校準(zhǔn)，后續(xù)工作也將對此進(jìn)行改進(jìn)。