利用解耦光流運動場模型的雙目視覺里程計位姿優(yōu)化方法

烏 萌，郝金明，付 浩，高 揚，張 輝

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

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

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

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

1 光流運動場模型及解耦

1.1 光流運動場

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

(1)

圖1 光流運動場投影模型圖像平面Fig.1 Projection model of flow motion field

1.2 解耦光流分量

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

將光流矢量解耦為旋轉(zhuǎn)(rotational)角速度分量ω(即旋轉(zhuǎn)分量)和平移(translational)速度分量t(即平移分量)，如圖2(b)、(c)，在左側(cè)前向運動場景中，旋轉(zhuǎn)分量光流較小且大小均一，平移分量光流大小不一隨深度變化；在右側(cè)旋轉(zhuǎn)運動場景中，旋轉(zhuǎn)分量光流較大且大小均一，平移光流大小不一隨深度變化?？梢姡诓煌鄬\動狀態(tài)下，光流場矢量受不同解耦分量影響差別顯著。

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

從圖2(d)—(i)左右場景對比可見，左側(cè)前向運動場景的3個旋轉(zhuǎn)分量光流較小，右側(cè)旋轉(zhuǎn)運動場景的ωx,ωz分量光流較小，ωy分量光流較大；左右兩場景中3個平移分量光流中tx、ty兩個分量的光流均較小，tz分量光流大小隨深度變化?？梢姡饬魇噶渴?個不同分量大小的影響也是顯著不同的。

2 解耦光流運動場誤差模型與仿真

2.1 解耦光流運動場誤差模型

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

(2)

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

(3)

圖2 兩個典型場景的光流場及解耦光流分量Fig.2 Flow fields and decoupled flow components in 2 typical scenes

(4)

式中

(5)

同時，令

(6)

2.2 單變量誤差模型與仿真

2.2.1 單變量誤差模型

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

(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)的單個位姿參數(shù)LSE及NLSE進行計算仿真，如圖3所示。仿真圖像特征點對應光流個數(shù)為300個，仿真3個旋轉(zhuǎn)角(速度)分量為3(°)/s，誤差為10%，即0.3(°)/s，即1-Δ=0.1；3個平移(速度)分量為1 m/s，誤差10%，即0.1 m/s，即1-Δ=0.1；深度分量仿真中，前向tz方向行駛速度為50 km/h,幀率為10 Hz，另兩方向的行駛速度為tx=0.1tz，ty=0.01tz。深度分量僅在特征光流為第50、100、150、200、250時存在，且誤差為5%，即(1-Δ)/Δ=0.05。

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

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

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

2.3 組合誤差模型與仿真

2.3.1 組合誤差模型

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

(28)

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

(29)

為DNLSE2?？疾觳煌M合位置姿態(tài)參數(shù)包含誤差條件下的DNLSE的數(shù)學形式：

(30)

(31)

(32)

(33)

2.3.2 組合誤差模型仿真

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

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

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

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

2.3.3 組合誤差分離試驗

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

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

2.3.3.2 真實數(shù)據(jù)試驗

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

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

表2 “圖6”試驗統(tǒng)計結(jié)果

3 解耦光流運動場雙目視覺里程計位姿優(yōu)化算法

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

3.1 雙目視覺里程計初始化

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

3.2 解耦光流運動場的位姿優(yōu)化算法

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

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

4 試驗與分析

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

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

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

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

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

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

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

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

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

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

5 結(jié) 論

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