烏 萌，郝金明，付 浩，高 揚(yáng)，張 輝

1. 信息工程大學(xué)地理空間信息學(xué)院，河南 鄭州 450052； 2. 地理信息工程國(guó)家重點(diǎn)實(shí)驗(yàn)室,陜西 西安 710054； 3. 西安測(cè)繪研究所，陜西 西安 710054； 4. 國(guó)防科技大學(xué)智能科學(xué)學(xué)院，湖南 長(zhǎng)沙 410073； 5. 國(guó)防大學(xué)聯(lián)合作戰(zhàn)學(xué)院，河北 石家莊 050001

近年來(lái)，伴隨著傳感器與硬件平臺(tái)性能的提升、計(jì)算機(jī)視覺(jué)與機(jī)器學(xué)習(xí)理論的發(fā)展、同時(shí)定位與建圖(simultaneously localization and mapping，SLAM)與運(yùn)動(dòng)恢復(fù)結(jié)構(gòu)(structure from motion，SFM)技術(shù)的迅速普及，地面移動(dòng)測(cè)量系統(tǒng)(mobile mapping system，MMS)與無(wú)人駕駛車(autonomous vehicle，AV，或稱自主駕駛車)兩個(gè)行業(yè)已然走向了深層次的相互借鑒、融合與進(jìn)步[1-6]。目前的兩個(gè)平臺(tái)的主要區(qū)別僅在于駕駛控制方式與應(yīng)用目的的不同，平臺(tái)安裝的傳感器件、采用的載體定位定姿、高精度地圖建立等核心關(guān)鍵技術(shù)幾乎是一致的[7-10]。從長(zhǎng)期發(fā)展來(lái)看，MMS的未來(lái)必將是無(wú)人化、互聯(lián)化甚至實(shí)時(shí)化的地面移動(dòng)測(cè)量，而AV的發(fā)展也將走向?qū)崟r(shí)高精度地圖建立、自主決策控制、車聯(lián)網(wǎng)云端導(dǎo)航等方向。在不遠(yuǎn)的將來(lái)，MMS與AV平臺(tái)將可能融合成為一個(gè)集實(shí)時(shí)測(cè)圖、上傳云端實(shí)時(shí)建圖、更新互聯(lián)發(fā)布、實(shí)時(shí)導(dǎo)航的有機(jī)平臺(tái)。

當(dāng)前，多傳感器融合獲取載體精確的位置姿態(tài)仍是MMS與AV兩個(gè)系統(tǒng)共同的基礎(chǔ)核心關(guān)鍵問(wèn)題[7-8,10]，其中視覺(jué)傳感器的位置姿態(tài)確定離不開視覺(jué)里程計(jì)(visual odometry，VO)技術(shù)。VO技術(shù)作為視覺(jué)SLAM(visual-SLAM，VSLAM)的前端技術(shù)內(nèi)容，近年來(lái)發(fā)展迅速。VO主要分為單目(monocular VO)與雙目(stereo VO)兩類，分別是指通過(guò)單目或雙目相機(jī)作為輸入估計(jì)載體自身運(yùn)動(dòng)位置姿態(tài)變化的過(guò)程[11]。單目VO由于只能恢復(fù)運(yùn)動(dòng)到一個(gè)尺度因子，而絕對(duì)尺度需要利用場(chǎng)景中目標(biāo)大小、運(yùn)動(dòng)約束或集成其他傳感器來(lái)確定，因此其序列圖像定位定姿跟蹤的穩(wěn)定性較差。雙目VO能夠直接利用基線獲得尺度、深度，因此其序列圖像位姿估計(jì)精度和穩(wěn)定性都顯著優(yōu)于單目[12]。但是，目前VO、VSLAM算法精度和穩(wěn)健性與視覺(jué)、POS、LiDAR多傳感器融合后的組合導(dǎo)航結(jié)果相比，精度約低一個(gè)數(shù)量級(jí)，穩(wěn)健性也顯著低于組合系統(tǒng)，主要用于低成本輕量化平臺(tái)的位姿估計(jì)、組合系統(tǒng)的位姿初值獲取以及與LiDAR融合實(shí)現(xiàn)環(huán)境地圖生成。

目前VO的典型方法有基于特征法的ORB-SLAM[13]、基于半直接法的SVO[14]，以及基于直接法(或稱光流法)的多種方法。直接法依利用光流數(shù)量的多少還可分為稠密法(如REMODE[15]、DTAM[16])、半稠密法(如LSD-SLAM[17])、稀疏法(如DSO[18])等,適用于不同計(jì)算平臺(tái)和建圖應(yīng)用需求。其中稀疏法一般用于需要實(shí)時(shí)高效位姿估計(jì)的場(chǎng)合，大多數(shù)稀疏算法是將六自由度的位置姿態(tài)在同一重投影誤差最小化過(guò)程中整體估計(jì)出來(lái)[19-20]，或是以一定合理假設(shè)為前提限定載體部分姿態(tài)變化而后估計(jì)其他位姿參數(shù)，相比六自由度整體估計(jì)的方法計(jì)算效率更高但因載體姿態(tài)的限定而精度略有損失[21-22]。RotROCC法[23]提出首先抵消旋轉(zhuǎn)分量，僅保留平移分量，實(shí)現(xiàn)平移分量解耦，然后利用解耦歸一化重投影殘差，實(shí)現(xiàn)僅利用平移分量進(jìn)行位置變化估計(jì)中的外點(diǎn)剔除，從而實(shí)現(xiàn)載體位姿估計(jì)的優(yōu)化，但其利用的模型存在近似，因此無(wú)法分離平移和深度誤差，也就無(wú)法利用旋轉(zhuǎn)分量進(jìn)行姿態(tài)變化估計(jì)中的外點(diǎn)剔除。本文針對(duì)MMS和AV車輛平臺(tái)下雙目相機(jī)拍攝的圖像序列中視覺(jué)里程計(jì)位姿估計(jì)優(yōu)化問(wèn)題，在RotROCC法的基礎(chǔ)上，利用光流運(yùn)動(dòng)場(chǎng)模型中載體位姿與圖像光流矢量間關(guān)系，不僅將光流矢量解耦為平移、旋轉(zhuǎn)分量，并進(jìn)一步解耦為3個(gè)平移分量、3個(gè)旋轉(zhuǎn)分量和深度分量，充分推導(dǎo)分析了解耦后單個(gè)分量、組合分量誤差對(duì)位姿估計(jì)的影響，分別利用仿真和真實(shí)數(shù)據(jù)試驗(yàn)，驗(yàn)證了不同模型下的誤差分離方法。最后，結(jié)合組合分量的誤差分離模型，針對(duì)雙目VO序列位姿估計(jì)提出了解耦光流運(yùn)動(dòng)場(chǎng)的位姿優(yōu)化算法，并在公開的KITTI數(shù)據(jù)集[24]中進(jìn)行了多個(gè)序列真實(shí)數(shù)據(jù)驗(yàn)證。

1 光流運(yùn)動(dòng)場(chǎng)模型及解耦

1.1 光流運(yùn)動(dòng)場(chǎng)

當(dāng)相機(jī)與場(chǎng)景間有相對(duì)運(yùn)動(dòng)時(shí)，圖像上亮度的運(yùn)動(dòng)變化就稱為光流[25]，因此，圖像中光流的運(yùn)動(dòng)規(guī)律直接反映了與相機(jī)固聯(lián)的載體的位置姿態(tài)變化，這一模型可以通過(guò)式(1)描述[26-27]，下稱光流運(yùn)動(dòng)場(chǎng)模型

(1)

圖1 光流運(yùn)動(dòng)場(chǎng)投影模型圖像平面Fig.1 Projection model of flow motion field

1.2 解耦光流分量

圖像中的光流運(yùn)動(dòng)場(chǎng)展現(xiàn)的是前后幀間載體六自由度的位置姿態(tài)變化速度。為了考察光流及解耦分量在不同相對(duì)運(yùn)動(dòng)場(chǎng)景下的變化規(guī)律，本文選取了KITTI數(shù)據(jù)集的兩個(gè)典型場(chǎng)景給出不同載體運(yùn)動(dòng)狀態(tài)下的光流場(chǎng)。如圖2左側(cè)，是KITTI序列2第3、4幀間運(yùn)動(dòng)下的光流場(chǎng)，該場(chǎng)景以載體快速前向運(yùn)動(dòng)為主，旋轉(zhuǎn)運(yùn)動(dòng)較小。與之對(duì)比，圖2右側(cè)，是KITTI序列2第45、46幀間運(yùn)動(dòng)下光流場(chǎng)，該場(chǎng)景以載體低速轉(zhuǎn)向?yàn)橹鳎揭七\(yùn)動(dòng)較小。圖2(a)左右光流場(chǎng)中的每個(gè)光流矢量均以藍(lán)、綠點(diǎn)為起、終點(diǎn)，箭頭指向光流場(chǎng)方向，鄰幀間的變化量即為位姿變化速度。

將光流矢量解耦為旋轉(zhuǎn)(rotational)角速度分量ω(即旋轉(zhuǎn)分量)和平移(translational)速度分量t(即平移分量)，如圖2(b)、(c)，在左側(cè)前向運(yùn)動(dòng)場(chǎng)景中，旋轉(zhuǎn)分量光流較小且大小均一，平移分量光流大小不一隨深度變化；在右側(cè)旋轉(zhuǎn)運(yùn)動(dòng)場(chǎng)景中，旋轉(zhuǎn)分量光流較大且大小均一，平移光流大小不一隨深度變化?？梢?jiàn)，在不同相對(duì)運(yùn)動(dòng)狀態(tài)下，光流場(chǎng)矢量受不同解耦分量影響差別顯著。

將平移分量進(jìn)一步解耦為載體沿世界坐標(biāo)系三軸的3個(gè)位移分量，即t=[tx,ty,tz]T；將旋轉(zhuǎn)分量進(jìn)一步解耦為載體繞世界坐標(biāo)系三軸的3個(gè)旋轉(zhuǎn)角分量，即ω=[ωx,ωy,ωz]T，類似，在圖中繪制出KITTI序列2第3、4幀間前向運(yùn)動(dòng)場(chǎng)景和序列2第45、46幀間旋轉(zhuǎn)運(yùn)動(dòng)場(chǎng)景下解耦的六自由度分量光流，如圖2(d)—(i)所示。

從圖2(d)—(i)左右場(chǎng)景對(duì)比可見(jiàn)，左側(cè)前向運(yùn)動(dòng)場(chǎng)景的3個(gè)旋轉(zhuǎn)分量光流較小，右側(cè)旋轉(zhuǎn)運(yùn)動(dòng)場(chǎng)景的ωx,ωz分量光流較小，ωy分量光流較大；左右兩場(chǎng)景中3個(gè)平移分量光流中tx、ty兩個(gè)分量的光流均較小，tz分量光流大小隨深度變化?？梢?jiàn)，光流矢量受6個(gè)不同分量大小的影響也是顯著不同的。

2 解耦光流運(yùn)動(dòng)場(chǎng)誤差模型與仿真

2.1 解耦光流運(yùn)動(dòng)場(chǎng)誤差模型

1.2節(jié)中定性分析了不同光流分量的特點(diǎn)，本節(jié)通過(guò)模型推導(dǎo)分析各分量誤差對(duì)模型估計(jì)的影響。由式(1)可知，不失一般性，可以令其中f=1，記為式(2)

(2)

令ρ=1/Z，即逆深度，將式(2)右側(cè)等號(hào)兩邊簡(jiǎn)寫為

(3)

圖2 兩個(gè)典型場(chǎng)景的光流場(chǎng)及解耦光流分量Fig.2 Flow fields and decoupled flow components in 2 typical scenes

(4)

式中

(5)

同時(shí)，令

(6)

2.2 單變量誤差模型與仿真

2.2.1 單變量誤差模型

下面考察單個(gè)位姿參數(shù)LSE及NLSE的數(shù)學(xué)形式，并作仿真比對(duì)：

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

(21)

(22)

(23)

(24)

(25)

(26)

(27)

2.2.2 單變量誤差模型仿真

將2.2.1節(jié)中步驟(1)—(7)的單個(gè)位姿參數(shù)LSE及NLSE進(jìn)行計(jì)算仿真，如圖3所示。仿真圖像特征點(diǎn)對(duì)應(yīng)光流個(gè)數(shù)為300個(gè)，仿真3個(gè)旋轉(zhuǎn)角(速度)分量為3(°)/s，誤差為10%，即0.3(°)/s，即1-Δ=0.1；3個(gè)平移(速度)分量為1 m/s，誤差10%，即0.1 m/s，即1-Δ=0.1；深度分量仿真中，前向tz方向行駛速度為50 km/h,幀率為10 Hz，另兩方向的行駛速度為tx=0.1tz，ty=0.01tz。深度分量?jī)H在特征光流為第50、100、150、200、250時(shí)存在，且誤差為5%，即(1-Δ)/Δ=0.05。

圖3中分別給出了7個(gè)變量包含誤差時(shí)，一幀中300個(gè)特征的LSE與NLSE。

圖3 單變量誤差仿真Fig.3 Error simulation of single variable

從圖3可見(jiàn)，單個(gè)變量的LSE會(huì)隨著特征光流矢量所在圖像坐標(biāo)的不同而出現(xiàn)隨機(jī)殘差，將LSE歸一化后，計(jì)算出單個(gè)變量的NLSE不再隨特征光流所在坐標(biāo)變化，而僅余下單個(gè)變量NLSE的殘差，大小為Δ-1，即變量本身包含的誤差，也就是說(shuō)，最小二乘估計(jì)殘差LSE中隨特征點(diǎn)所在坐標(biāo)產(chǎn)生的隨機(jī)誤差被很好地分離了。

2.3 組合誤差模型與仿真

2.3.1 組合誤差模型

2.2節(jié)是單個(gè)位姿變量包含誤差情況下，將其LSE中隨機(jī)殘差進(jìn)行歸一化分離的數(shù)學(xué)推導(dǎo)和仿真驗(yàn)證。在實(shí)際計(jì)算中，7個(gè)變量一般不會(huì)僅單個(gè)存在誤差，需要分析推導(dǎo)多變量存在誤差的情況下，隨機(jī)殘差的分離方法?？紤]采用去耦合的方法分離位置或姿態(tài)分量部分隨圖像坐標(biāo)產(chǎn)生的隨機(jī)殘差?？紤]到式(5)、式(6)，令

(28)

為解耦的歸一化最小二乘殘差(decoupled normalized least square error，DNLSE1)，令

(29)

為DNLSE2。考察不同組合位置姿態(tài)參數(shù)包含誤差條件下的DNLSE的數(shù)學(xué)形式：

(30)

(31)

(32)

(33)

2.3.2 組合誤差模型仿真

下面將組合位姿參數(shù)在2.3.1節(jié)中(1)—(4)不同情況下的LSE、NLSE、DNLSE進(jìn)行了計(jì)算仿真，如圖4所示。仿真的圖像特征點(diǎn)對(duì)應(yīng)光流個(gè)數(shù)為300個(gè)，仿真的3個(gè)旋轉(zhuǎn)角(速度)分量為ω=[1°,5°,0.5°]T，誤差為5%，即1-Δω=0.05；3個(gè)平移(速度)分量的仿真中，前向tz方向行駛速度為50 km/h,幀率為10 Hz，3個(gè)平移(速度)分量為t=[0.1tz,0.01tz,tz]T，誤差5%，即1-Δt=0.05；深度分量?jī)H在特征點(diǎn)對(duì)應(yīng)光流為第50、100、150、200、250時(shí)存在，誤差為8%，(1-ΔZ)/ΔZ=0.08。

圖4 組合變量誤差仿真Fig.4 Error simulation of combined variables

從圖4可見(jiàn)，組合變量存在誤差時(shí)，LSE隨著特征光流矢量所在圖像坐標(biāo)的不同而出現(xiàn)隨機(jī)殘差；組合變量的NLSE能夠分離大部分這些隨機(jī)誤差，但并不能完全分離；組合變量的DNLSE計(jì)算結(jié)果不再隨特征光流所在坐標(biāo)變化，而僅余下各組合變量本身的誤差部分，這說(shuō)明組合變量存在誤差情況下，利用DNLSE模型時(shí)，最小二乘估計(jì)殘差中隨特征點(diǎn)所在坐標(biāo)的部分殘差能夠被很好地分離。表1將不同組合變量存在誤差時(shí)可采用的DNLSE模型列出，“√”表示可分離的誤差類型。誤差種類不同時(shí)，DNLSE的計(jì)算式不同。利用DNLSE1模型能夠在無(wú)旋轉(zhuǎn)分量誤差的條件下，較好地分離平移分量和深度誤差中光流在圖像坐標(biāo)引入的隨機(jī)殘差部分；利用DNLSE2模型能夠在無(wú)平移分量和深度誤差的條件下，較好地分離旋轉(zhuǎn)分量光流在圖像坐標(biāo)引入的隨機(jī)殘差部分。

表1 組合變量誤差分離采用的DNLSE模型

2.3.3 組合誤差分離試驗(yàn)

2.3.3.1 仿真數(shù)據(jù)試驗(yàn)

將2.3.2節(jié)中情況(1)“僅平移分量存在誤差，旋轉(zhuǎn)分量與深度無(wú)誤差，利用DNLSE1計(jì)算”的過(guò)程，即圖4第1組仿真，在本次試驗(yàn)中用圖像光流場(chǎng)的誤差條(ErrorBar)形式給出LSE和DNLSE計(jì)算結(jié)果的對(duì)比。如圖5(a)，光流矢量起點(diǎn)的紅色“工”形誤差條的高度代表該光流矢量LSE殘差大小，經(jīng)過(guò)DNLSE1模型的誤差分離，在圖5(b)中，光流矢量起點(diǎn)的綠色“工”形誤差條高度明顯降低，說(shuō)明LSE中的殘差被顯著分離。

2.3.3.2 真實(shí)數(shù)據(jù)試驗(yàn)

將2.3.2節(jié)表1列出的(2)、(4)情況對(duì)真實(shí)數(shù)據(jù)進(jìn)行誤差分離計(jì)算過(guò)程試驗(yàn)，以期用盡量少的計(jì)算次數(shù)覆蓋所有誤差的分離。試驗(yàn)中對(duì)KITTI數(shù)據(jù)集序列2第3、4幀與序列2第45、46幀，分別用DNLSE1、DNLSE2模型進(jìn)行計(jì)算，結(jié)果如圖6。圖6中(a)、(c)、(e)是序列2第3、4幀的高速前向運(yùn)動(dòng)場(chǎng)景，(b)、(d)、(f)是序列2第45、46幀的慢速旋轉(zhuǎn)場(chǎng)景。圖6(a)、(b)給出了計(jì)算出的LSE模型誤差，以紅色誤差條顯示，圖6(c)、(d)是計(jì)算出的DNLSE1模型誤差，以綠色誤差條顯示，圖6(e)、(f)是計(jì)算出的DNLSE2模型誤差，以藍(lán)色誤差條顯示。

仔細(xì)對(duì)比圖6(a)、(c)的前向運(yùn)動(dòng)場(chǎng)景和圖6(b)、(d)的旋轉(zhuǎn)運(yùn)動(dòng)場(chǎng)景，均可見(jiàn)圖6(c)、(d)的部分綠色誤差條分別比圖6(a)、(b)的紅色誤差條反而高(如：圖6(a)、(b)黃色箭頭和圖6(c)、(d)紅色箭頭對(duì)應(yīng)的幾個(gè)光流矢量誤差條)，這是DNLSE1顯著分離LSE中的隨機(jī)誤差之后，暴露出了真實(shí)數(shù)據(jù)中存在著的一定旋轉(zhuǎn)誤差，即兩場(chǎng)景下均有點(diǎn)DNLSE1>LSE，圖6中所有該情況的特征光流編號(hào)統(tǒng)計(jì)結(jié)果見(jiàn)表2。類似，對(duì)比圖6(a)、(e)可以看到圖6(e)的部分藍(lán)色誤差條比圖6(a)的紅色誤差條反而高(如：圖6(a)藍(lán)色箭頭和圖6(e)紅色箭頭對(duì)應(yīng)的光流矢量誤差條)，這是DNLSE2顯著分離LSE中的隨機(jī)誤差之后，暴露出了真實(shí)數(shù)據(jù)中存在著的一定平移和深度誤差，即前向運(yùn)動(dòng)場(chǎng)景下有點(diǎn)的DNLSE2>LSE，圖6中所有該情況的特征光流編號(hào)統(tǒng)計(jì)結(jié)果見(jiàn)表2。但是對(duì)比圖6(b)和圖6(f)，從中觀察和試驗(yàn)統(tǒng)計(jì)結(jié)果都得到藍(lán)色誤差條比紅色誤差條高的數(shù)量為0，即旋轉(zhuǎn)場(chǎng)景中無(wú)點(diǎn)DNLSE2>LSE，這說(shuō)明DNLSE2顯著分離LSE中的隨機(jī)誤差之后，由于場(chǎng)景中的平移分量很小，因此沒(méi)有殘余顯著的平移和深度誤差。

表2 “圖6”試驗(yàn)統(tǒng)計(jì)結(jié)果

3 解耦光流運(yùn)動(dòng)場(chǎng)雙目視覺(jué)里程計(jì)位姿優(yōu)化算法

在完成了組合誤差分離的真實(shí)數(shù)據(jù)試驗(yàn)后，本文針對(duì)真實(shí)車輛運(yùn)行時(shí)利用序列圖像進(jìn)行載體位姿估計(jì)的問(wèn)題設(shè)計(jì)實(shí)現(xiàn)了解耦光流運(yùn)動(dòng)場(chǎng)雙目視覺(jué)里程計(jì)位姿優(yōu)化算法，并利用KITTI集的多個(gè)序列進(jìn)行了解算試驗(yàn)。本文采用了雙目視覺(jué)里程計(jì)先進(jìn)行載體位姿初值的估計(jì)，然后利用解耦光流運(yùn)動(dòng)場(chǎng)誤差模型進(jìn)行圖像位置隨機(jī)誤差的分離，并對(duì)殘留位姿誤差作進(jìn)一步優(yōu)化，以使載體位姿的估計(jì)精確化。

3.1 雙目視覺(jué)里程計(jì)初始化

在雙目視覺(jué)里程計(jì)初始估計(jì)的實(shí)現(xiàn)中，本文采用了文獻(xiàn)[19]算法并對(duì)模板濾波作了改進(jìn)，提升了計(jì)算效率(以下稱Badino算法)。具體的初始化計(jì)算過(guò)程如下：對(duì)于第1幀，計(jì)算左圖的Harris角點(diǎn)以固定特征點(diǎn)數(shù)(1024個(gè))，然后計(jì)算這1024個(gè)點(diǎn)的偏差量，將有效的偏差結(jié)果保存到特征矢量；對(duì)于第2幀，利用光流跟蹤這1024個(gè)特征點(diǎn)，去掉跟蹤失敗的點(diǎn)后得到這些點(diǎn)新的像素坐標(biāo)，同時(shí)再次計(jì)算Harris角點(diǎn)以補(bǔ)足至1024個(gè)點(diǎn)，同樣計(jì)算這1024個(gè)點(diǎn)的偏差量，結(jié)果存入特征矢量。接下來(lái)，利用前后兩幀的特征矢量計(jì)算位姿變化。首先選擇在兩幀中均計(jì)算出有效偏差的對(duì)應(yīng)點(diǎn)對(duì)，將上一幀的三維點(diǎn)坐標(biāo)進(jìn)行幀間轉(zhuǎn)移矩陣變換，得到當(dāng)前幀的局部坐標(biāo)，然后投影到當(dāng)前圖像中，與當(dāng)前圖像中的特征點(diǎn)位置進(jìn)行比較，結(jié)果越小的越好。這一過(guò)程不斷迭代，每次迭代后僅保留重投影誤差較小的點(diǎn)對(duì)，拋棄誤差較大的點(diǎn)對(duì)，并且每個(gè)點(diǎn)對(duì)的權(quán)重反比于投影誤差。最后對(duì)結(jié)果進(jìn)行卡爾曼濾波，濾波后的結(jié)果作為雙目VO的估計(jì)初值。

3.2 解耦光流運(yùn)動(dòng)場(chǎng)的位姿優(yōu)化算法

在具備了雙目VO估計(jì)的位姿初值和前后幀對(duì)應(yīng)的初始特征內(nèi)點(diǎn)集后，利用當(dāng)前內(nèi)點(diǎn)集的重投影誤差最小化迭代估計(jì)旋轉(zhuǎn)位置變化量[19-20]，直至重投影誤差小于一定閾值或迭代次數(shù)超限，接著對(duì)式(5)利用DNLSE1進(jìn)行圖像中光流位置引入的隨機(jī)誤差分離和顯著旋轉(zhuǎn)誤差的外點(diǎn)去除。

接著，固定前一輪估計(jì)出的旋轉(zhuǎn)變化量，利用當(dāng)前特征內(nèi)點(diǎn)集的重投影誤差最小化迭代估計(jì)位置變化量，直至重投影誤差小于一定閾值或迭代次數(shù)超限，接著對(duì)式(5)利用DNLSE2進(jìn)行圖像中光流位置引入的隨機(jī)誤差分離和顯著平移深度誤差的外點(diǎn)去除。算法流程見(jiàn)圖7。

4 試驗(yàn)與分析

為驗(yàn)證本文算法的有效性和計(jì)算精度，利用KITTI數(shù)據(jù)集序列00、02分別進(jìn)行了算法解算試驗(yàn)。試驗(yàn)平臺(tái)為L(zhǎng)enovoX240s(CPU酷睿i7-4510U，2.00 GHz×4核，內(nèi)存8 GB)筆記本電腦，本文所有試驗(yàn)都在Ubuntu14.04系統(tǒng)下，利用Matlab2014b、MexOpenCV2.4、C++11語(yǔ)言混合編程實(shí)現(xiàn)。

利用KITTI數(shù)據(jù)集的00序列和02序列的第0幀至第1200幀進(jìn)行算法試驗(yàn)，繪制的二維軌跡如圖8。圖8給出的是位置分量t=[tx,ty,tz]T中的tx和tz分量，其中tz沿世界系Z軸指向載體前向運(yùn)動(dòng)方向，tx沿世界系X軸指向載體右側(cè)橫向運(yùn)動(dòng)方向，ty指向載體重力方向。圖中紅色軌跡為KITTI數(shù)據(jù)集給出的軌跡真值，藍(lán)色軌跡為Badino算法的計(jì)算結(jié)果，綠色為本文提出的位姿優(yōu)化算法進(jìn)行計(jì)算的軌跡結(jié)果。

圖9為兩種算法解算tx、tz方向的誤差對(duì)比，具體統(tǒng)計(jì)結(jié)果見(jiàn)表3。從統(tǒng)計(jì)結(jié)果可見(jiàn)，本文算法較好地分離了初始化計(jì)算結(jié)果中仍然存在的特征光流位置隨機(jī)誤差，以及平移、旋轉(zhuǎn)、深度分量的殘差，明顯減少了Badino算法的累積誤差。

表3 Badino算法與本文算法誤差統(tǒng)計(jì)

圖5 2.3.2節(jié)(1)計(jì)算結(jié)果對(duì)比Fig.5 Computation results comparison of (1) in section 2.3.2

圖6 兩類場(chǎng)景真實(shí)數(shù)據(jù)組合誤差分離試驗(yàn)Fig.6 Combined error separation experiments using data of two real scenes

圖7 解耦光流運(yùn)動(dòng)場(chǎng)的位姿優(yōu)化算法流程Fig.7 Pose estimation algorithm for optical flow-decoupled motion field model

圖8 解算軌跡對(duì)比((a) 序列00，(b) 序列02)Fig.8 Comparisons of calculated trajectories ((a) sequence 00, (b) sequence 02)

圖9 解算誤差對(duì)比((a) 序列00，(b) 序列02)Fig.9 Comparisons of calculation errors ((a) sequence 00, (b) sequence 02)

圖10統(tǒng)計(jì)對(duì)比了序列00與序列02每幀計(jì)算中利用Badino算法完成初始位姿估計(jì)部分所耗費(fèi)時(shí)間和完成本文算法精度優(yōu)化部分所耗時(shí)間。統(tǒng)計(jì)結(jié)果可見(jiàn)，本文提出算法部分的耗時(shí)相對(duì)初始化位姿估計(jì)部分耗時(shí)增加不多或幾乎沒(méi)有增加，計(jì)算效率與原算法基本相當(dāng)，能夠用于多種低功耗嵌入式計(jì)算場(chǎng)合。

5 結(jié) 論

本文利用圖像中光流矢量與載體位置姿態(tài)變化速率之間的模型關(guān)系——光流運(yùn)動(dòng)場(chǎng)模型，將光流矢量解耦到對(duì)應(yīng)的六自由度載體位姿變量，考察解耦后的光流矢量與單個(gè)、組合載體位姿變量誤差之間的關(guān)系，推導(dǎo)出利用NLSE分離LSE中的光流矢量位置隨機(jī)誤差，利用DNLSE進(jìn)一步分離組合變量下NLSE中的光流矢量位置隨機(jī)誤差，并利用仿真和真實(shí)數(shù)據(jù)進(jìn)行了試驗(yàn)驗(yàn)證。在雙目視覺(jué)里程計(jì)算中，利用本文提出的解耦光流運(yùn)動(dòng)場(chǎng)位姿優(yōu)化算法，既能夠分離光流矢量位置的隨機(jī)誤差，同時(shí)能夠通過(guò)外點(diǎn)剔除減少DNLSE1和DNLSE2模型計(jì)算后殘留的平移、深度、旋轉(zhuǎn)角度殘差。試驗(yàn)表明，算法將載體橫向平移平均誤差由4.75%降低至2.2%，誤差平均降低了53.6%；將載體前向平移平均誤差由2.2%降低至1.9%，誤差平均降低了15.4%。算法在明顯減少雙目視覺(jué)里程計(jì)位置姿態(tài)估計(jì)中的累積誤差的同時(shí)計(jì)算效率與原算法相當(dāng)，適用于嵌入式低功耗實(shí)時(shí)位置姿態(tài)估計(jì)和組合導(dǎo)航系統(tǒng)的位姿初值獲取。在計(jì)算量允許的條件下，還可接續(xù)進(jìn)行SLAM圖優(yōu)化算法，預(yù)計(jì)能夠進(jìn)一步降低整體位置姿態(tài)估計(jì)誤差。