基于交互式多模型MSCKF的雙目視覺/慣性里程計算法

王 磊，程向紅，李 進，王 樂

（1. 安徽科技學(xué)院電氣與電子工程學(xué)院，蚌埠 233100； 2. 東南大學(xué) 微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室，南京 210096）

精確的位姿估計是實現(xiàn)移動平臺智能控制和自主作業(yè)的關(guān)鍵。視覺慣性里程計（Visual inertial odometry, VIO）將慣性導(dǎo)航系統(tǒng)測量信息與視覺測量信息結(jié)合起來，實現(xiàn)對移動平臺的位姿估計。隨著微機電系統(tǒng)（Micro-electro-mechanical system, MEMS）慣性測量單元（Inertial measurement unit, IMU）的發(fā)展，VIO具有成本低、小型化、功耗低等優(yōu)點，被廣泛應(yīng)用于移動機器人、無人機和手持設(shè)備[1-4]。由于視覺/慣性組合導(dǎo)航系統(tǒng)具有模型復(fù)雜、非線性強的特點，如何有效地利用慣性測量信息和視覺信息的互補性來實現(xiàn)精確的位姿估計一直是該領(lǐng)域研究的熱點和難點。

根據(jù)是否將視覺特征信息加入到狀態(tài)向量中，可以將實現(xiàn)VIO 的組合方式分為緊耦合和松耦合兩大類，大量的研究證明緊耦合的方式相對于松耦合，精確性更高。緊耦合VIO 憑借其在精度和魯棒性方面的優(yōu)勢，近年來逐漸成為主流方法[5,6]。根據(jù)VIO 系統(tǒng)狀態(tài)估計的實現(xiàn)方式，可以將其分為基于濾波器估計和基于關(guān)鍵幀非線性優(yōu)化的兩類方法[7-9]。對于采用緊耦合組合方式的VIO 系統(tǒng)，經(jīng)典的濾波方法包括多狀態(tài)約束卡爾曼濾波(Multi-state-constraint Kalman filter, MSCKF)算法[10-12]和魯棒視覺里程計算法(Robust visual inertial odometry, ROVIO)[13]。MSCKF 算法采用一種類似于滑動窗口的結(jié)構(gòu)，它關(guān)注特征點在連續(xù)一段時間內(nèi)各個時刻的所有狀態(tài)，并對這些時刻的狀態(tài)建立非線性三角化約束，從而對窗口內(nèi)的狀態(tài)量執(zhí)行消元操作，然后對系統(tǒng)參數(shù)進行濾波更新和優(yōu)化。該方法大大減少了濾波器輸入的狀態(tài)參數(shù)，有效地解決了計算復(fù)雜度隨時間遞增的問題，提高了系統(tǒng)的魯棒性和收斂性。Bloesch 等人提出的ROVIO 算法通過將圖像光度誤差作為擴展卡爾曼濾波器（Extended Kalman filter, EKF）的新息項，將觀測到的特征點分解為方向矢量與逆深度，降低了模型非線性的影響、提高了系統(tǒng)魯棒性。2017 年，Bloesch 等人又采用基于迭代EKF 濾波器的方法對ROVIO 進行了改進，同時將其適用對象擴展到多目相機[14]。2018 年，Schneider 等人則在ROVIO 的基礎(chǔ)上，進一步采用BRISK 和FREAK 特征替換FAST 角點，設(shè)計出一種更為成熟的開源濾波緊耦合視覺慣性SLAM 框架[15]。

上述基于濾波器的VIO 系統(tǒng)實現(xiàn)方法中，系統(tǒng)模型采用的均是單一固定模型。然而，由于移動平臺實際工作環(huán)境中往往存在著相似特征較多、紋理重復(fù)性較強，并且經(jīng)常受到不均勻光照、陰影等噪聲影響，視覺圖像中的像素顏色容易發(fā)生畸變。因此，有必要針對VIO系統(tǒng)中廣泛存在的模型不固定和量測噪聲易發(fā)生變化等問題，研究具有環(huán)境自適應(yīng)能力的視覺與慣性測量信息融合方法。針對上述問題，考慮到交互式多模型（Interacting multiple model, IMM）算法在處理模型不確定性以及可變量測噪聲方面的優(yōu)勢[16-19]，本文將其與MSCKF 算法相結(jié)合提出了一種改進的多狀態(tài)約束卡爾曼濾波算法IMM-MSCKF。該算法建立一組并行的模型匹配子濾波器，并將各子濾波器的輸入、輸出進行交互融合，實現(xiàn)對VIO 系統(tǒng)的狀態(tài)估計。

1 雙目視覺/慣導(dǎo)里程計緊耦合MSCKF 算法

VIO 系統(tǒng)的本質(zhì)是一個位姿狀態(tài)估計問題。基于濾波器的方法以貝葉斯推理為算法架構(gòu)，實現(xiàn)位姿估計需要計算出世界坐標(biāo)系{W}、慣性坐標(biāo)系{I}以及相機坐標(biāo)系{C}之間的位姿變換矩陣。從相機坐標(biāo)系{C}到慣性坐標(biāo)系{I}的變換矩陣可以通過預(yù)先標(biāo)定得到，因此，VIO 系統(tǒng)的主要目的是通過濾波方式估計出各個時刻從慣性坐標(biāo)系{I}到世界坐標(biāo)系{W}的變換矩陣雙目立體視覺采用兩臺相對位置固定的相機獲取特征的三維位置信息，左、右相機坐標(biāo)系之間的位姿變換矩陣通過預(yù)先標(biāo)定可以獲得。

1.1 IMU 狀態(tài)模型

IMU 狀態(tài)矢量構(gòu)成如式(1)所示：

根據(jù)慣性導(dǎo)航系統(tǒng)原理和慣性器件誤差模型，IMU 連續(xù)時間運動學(xué)模型可以表示為：

其中，Wa為加速度；nbw和nba分別為陀螺儀和加速度計的隨機高斯白噪聲；Ω(ω)為角速度矢量ω的四元數(shù)乘法矩陣，可表示為：

根據(jù)陀螺儀量測ωm和加速度計量測am，IMU 實際的角速度ω和線加速度a可以表示為：

結(jié)合式(2)，IMU 連續(xù)時間模型可演化為：

其中，為誤差四元數(shù)δq的角度偏差。

實際測量過程中，陀螺儀和加速度計以一定的時間間隔采樣，IMU 狀態(tài)以離散形式傳播。因此，式(5)中IMU 運動學(xué)模型可進一步表示為：

至此，可得到IMU 狀態(tài)的線性化連續(xù)時間模型：

1.2 狀態(tài)傳播與增廣

不同于傳統(tǒng)EKF-VIO 將特征點加入到狀態(tài)向量與IMU 狀態(tài)一起估計，MSCKF 算法將過去不同時刻的相機位姿加入狀態(tài)向量，特征點與多個相機位姿相對應(yīng)，多個相機位姿狀態(tài)之間形成幾何約束。MSCKF算法中VIO 系統(tǒng)k時刻的狀態(tài)向量可表示為：

其中xIk為IMU 的狀態(tài)向量，為n組相機的歷史姿態(tài)和位置向量。因此，VIO 系統(tǒng)的誤差狀態(tài)向量可以描述為：

當(dāng)檢測新的雙目視覺圖像幀輸入時，相機位姿會被添加到當(dāng)前狀態(tài)滑動窗口中。設(shè)相機與IMU 之間的變換矩陣為相機的位姿可表示為：

假設(shè)VIO 狀態(tài)中已包含了n個相機位姿狀態(tài)，則第n+1 個狀態(tài)的協(xié)方差矩陣Pk|k可通過如下計算：

其中，I6n+15為單位矩陣；根據(jù)式(14)，雅可比矩陣Jk可以由如下進行計算：

1.3 協(xié)方差傳播

設(shè)IMU 采樣周期為Δt，IMU 狀態(tài)與相機姿態(tài)之間的協(xié)方差矩陣可以通過乘以誤差狀態(tài)變換矩陣進行傳播。相機狀態(tài)協(xié)方差以及IMU 相機互協(xié)方差可表示為：

其中，通過歐拉積分對采樣時間Δt進行積分，誤差狀態(tài)變換矩陣Φ（tk+Δt,tk）可簡化為：

進一步，IMU 預(yù)測協(xié)方差矩陣可表示為：

其中，QI為IMU 過程噪聲協(xié)方差。

1.4 狀態(tài)修正

設(shè)雙目視覺對某特征點pj形成第i次觀測，此時左、右相機位姿分別為在VIO 狀態(tài)向量中，只需要包含左相機的位姿即可，右相機位姿可在此基礎(chǔ)上通過預(yù)先標(biāo)定的參數(shù)計算得到。

雙目相機第i次觀測可以表示為：

將對特征pj的多次觀測按行堆疊，得到觀測模型：

其中，Ho可以通過QR 分解計算得到：

其中，Q1、Q2為酉矩陣，TH為上三角矩陣。將結(jié)果代入式(25)，并乘以[Q1,Q2]T，得到：

其中nn是噪聲向量，其協(xié)方差矩陣

至此，可以直接采用EKF 量測更新過程，卡爾曼增益矩陣：

VIO 狀態(tài)修正以及狀態(tài)協(xié)方差矩陣：

由上述推導(dǎo)過程可以看出，IMU 狀態(tài)傳播與增廣更新了IMU 狀態(tài)向量及其對應(yīng)的協(xié)方差，量測更新建立了針對相機狀態(tài)的殘差觀測模型，而協(xié)方差矩陣增廣將IMU 和相機狀態(tài)關(guān)聯(lián)起來，觀測更新相機狀態(tài)的同時，也會間接地對IMU 狀態(tài)進行更新。MSCKF 算法只有在特征跟蹤丟失或滑動窗口中歷史的相機位姿數(shù)量達到閾值的情況下，才會對特征點進行三角化并執(zhí)行量測更新。

2 交互式多模型MSCKF 算法

交互多模型算法能夠?qū)勺兡Ｐ鸵约霸肼暯y(tǒng)計特性未知的系統(tǒng)進行自適應(yīng)處理，在目標(biāo)跟蹤、圖像處理、故障檢測以及組合導(dǎo)航等領(lǐng)域的應(yīng)用效果顯著。IMM 算法將復(fù)雜的非線性、非高斯問題分解為若干個線性的、高斯問題，本文將MSCKF 作為其子濾波器，構(gòu)建適用于復(fù)雜環(huán)境的雙目視覺/慣導(dǎo)里程計信息融合方法。

2.1 IMM-MSCKF 算法

由于IMU 數(shù)據(jù)更新速度快，且短時測量精度高，因此，針對觀測噪聲統(tǒng)計特性未知的量測方程構(gòu)建多模型。雙目視覺/慣導(dǎo)里程計系統(tǒng)模型可由式(8)～(13)和式(20)～(28)表示，為便于描述，將VIO 系統(tǒng)模型表示如下：

其中，mk∈M為k時刻模型集合M中的有效模型；為VIO 系統(tǒng)狀態(tài)向量；rk為式(28)中定義的觀測殘差向量；Fk為狀態(tài)轉(zhuǎn)移矩陣；Gk為噪聲系數(shù)矩陣；TH,k為測量矩陣；wk和vk分別為過程噪聲和觀測噪聲。

設(shè)M中共有ζ 個模型，各模型匹配子濾波器對應(yīng)的初始馬爾可夫轉(zhuǎn)移概率為：

提出的IMM-MSCKF 算法由ζ 個相互作用的并行子濾波器構(gòu)成，一個完整的狀態(tài)估計周期如下：

（1）模型匹配子濾波器初始化。k+1 時刻，將所有子濾波器k時刻的狀態(tài)估計依照模型轉(zhuǎn)移概率進行混合，可得到各子濾波器的初始條件，初始模型轉(zhuǎn)移概率可表示為：

（2）模型匹配子濾波器狀態(tài)估計。將MSCKF 算法作為模型匹配子濾波器，各子濾波器的狀態(tài)估計過程相互獨立，與實際系統(tǒng)模型越接近的子濾波器的狀態(tài)估計精度會更高，對于每一個模型匹配子濾波器，具體步驟如下：

IMM-MSCKF 是由多個并行的MSCKF 子濾波器構(gòu)成，因此，IMM-MSCKF 算法也只有在特征跟蹤丟失或滑動窗口中歷史相機位姿數(shù)量達到閾值時，才會對特征點進行三角化并執(zhí)行濾波更新。

（3）新息修正。對于每個子濾波器，在k+1 時刻，重新計算新息及其協(xié)方差矩陣

（4）模型轉(zhuǎn)移概率更新。根據(jù)貝葉斯推理，可以推導(dǎo)出k+1 時刻第j個子濾波器的似然函數(shù)。

其中n為系統(tǒng)狀態(tài)向量的維數(shù)，則各子濾波器的模型轉(zhuǎn)移概率可更新為：

（5）輸出交互融合。將更新后的模型轉(zhuǎn)移概率作為匹配子濾波器的權(quán)重，對各子濾波器的狀態(tài)估計進行交互融合，得到最優(yōu)的估計結(jié)果。k+1 時刻融合后的狀態(tài)估計及其對應(yīng)的估計誤差協(xié)方差陣為：

至此，IMM-MSCKF 算法實現(xiàn)了對VIO 系統(tǒng)的狀態(tài)估計，從算法結(jié)構(gòu)可以看出，其主要特征是使用多個并行子濾波器進行估計，通過各子濾波器與實際系統(tǒng)模型的匹配程度自適應(yīng)地更新各子濾波器的模型轉(zhuǎn)移概率。因此，IMM-MSCKF 算法具有較好的魯棒性，在系統(tǒng)模型不確定或噪聲統(tǒng)計特性未知的復(fù)雜環(huán)境中具有較好的穩(wěn)定性和估計精度。

2.2 基于IMM-MSCKF 算法的VIO 狀態(tài)估計流程

提出的IMM-MSCKF 算法結(jié)構(gòu)流程如圖1 所示，主要包括濾波器初始化、IMU 狀態(tài)傳播、雙目視覺圖像數(shù)據(jù)處理、相機狀態(tài)增廣、子濾波器觀測更新以及輸出交互融合等五個步驟，具體如下：

圖1 IMM-MSCKF 算法結(jié)構(gòu)流程Fig.1 Structure flow chart of IMM-MSCKF algorithm

1）濾波器初始化，對左右相機、IMU 傳感器各項參數(shù)進行初始化，并設(shè)置初始馬爾可夫轉(zhuǎn)移概率、模型轉(zhuǎn)移概率以及MSCKF 濾波器各項參數(shù)等。

2）雙目視覺圖像數(shù)據(jù)處理，對新獲取的雙目原始圖像進行處理，包括畸變校正、圖像濾波、特征點的跟蹤、特征點的提取與匹配。特征點的跟蹤采用多尺度LK 光流算法；特征提取與匹配采用FAST 算法。

3）相機狀態(tài)增廣，當(dāng)檢測到雙目視覺相機輸入的新圖像對時，將左相機的位姿參數(shù)初始化為狀態(tài)向量增廣到相機的狀態(tài)方程中。

4）子濾波器觀測更新，當(dāng)檢測到MSCKF 濾波器滿足觀測更新條件時，所有的模型匹配子濾波器均進行1.4 節(jié)中式(25)～(31)所述觀測更新過程。濾波器更新時機有兩種：一是被連續(xù)跟蹤數(shù)幀而具有較強極限約束能力的特征點由于相機運動或視場變化而出現(xiàn)跟蹤丟失的情況，需要解算出它們的位置坐標(biāo)并完成量測更新；另外一種情況是滑動窗口中存儲的相機狀態(tài)達到設(shè)置的閾值上限時，需要刪除那些老舊的相機狀態(tài)，由于這些待刪除的老舊狀態(tài)中包含了大量可用信息，刪除它們之前需要完成量測更新。

5）輸出交互融合，針對每個子濾波器，完成新息修正和模型概率更新，并根據(jù)它們的狀態(tài)估計進行交互融合，得到最優(yōu)的估計結(jié)果。

3 試驗及結(jié)果分析

為了驗證本文提出的IMM-MSCKF 算法對于雙目視覺/慣性里程計的有效性，采用KITTI 公共數(shù)據(jù)集進行了仿真實驗，并將IMM-MSCKF 算法與傳統(tǒng)的EKF 和MSCKF 算法進行了對比分析。KITTI 數(shù)據(jù)集提供的數(shù)據(jù)主要包含兩臺灰度相機、兩臺彩色相機、一臺激光掃描儀和一套高精度GPS/IMU 組合導(dǎo)航系統(tǒng)采集的信息。本實驗中使用了兩個灰度相機和IMU采集的數(shù)據(jù)，并將GPS/IMU 組合系統(tǒng)輸出的位置、速度和姿態(tài)信息作為參考值。實驗選取城市道路場景中編號為“2011_09_29_drive_0071”的數(shù)據(jù)集中圖像幀號為(0,1000)之間的數(shù)據(jù)。該組數(shù)據(jù)中城市道路環(huán)境較為復(fù)雜，存在較多遮擋、陰影等明暗交替的情況。

3.1 特征點跟蹤與邊緣化處理

當(dāng)有新的雙目圖像數(shù)據(jù)輸入時，一部分特征點會跟蹤丟失，其中三角化成功的特征點被用于濾波更新。除了通過這些丟失的觀察特征點更新VIO 狀態(tài)外，還需要對滑動窗口中的老舊狀態(tài)進行定時邊緣化，以確保要估計的參數(shù)數(shù)量不會隨著時間無限增加。圖2 為對選用的KITTI 數(shù)據(jù)集中某時刻圖像數(shù)據(jù)進行特征跟蹤與管理的示意圖。

圖2 特征點跟蹤與管理Fig.2 Feature point tracking and management

圖2 中上、下兩幅圖像為左相機獲取的連續(xù)兩個時刻的圖像數(shù)據(jù)。不同的符號代表特征點的變化，圓表示前后兩個時刻能夠匹配的特征點，星號表示不再被跟蹤的特征點，這些特征點需要從特征變量中移除。

3.2 不同算法在相同條件下的性能比較

在EKF、MSCKF 和IMM-MSCKF 算法中，假設(shè)初始位置誤差為0 m;初始速度誤差為0.2 m/s; INS 初始姿態(tài)誤差角為 0.2 °。陀螺的隨機常值漂移為0.01 °/h，陀螺隨機游走系數(shù)為加速度計常值偏置誤差為1×10-4g，加速度計量測白噪聲標(biāo)準(zhǔn)差為在MSCKF 和IMM-MSCKF 算法中，設(shè)左、右相機在像平面上的重投影量測噪聲為0.001 m，INS 誤差狀態(tài)向量對應(yīng)的過程噪聲協(xié)方差矩陣QI和VIO 系統(tǒng)量測噪聲矩陣R分別設(shè)為：

IMM-MSCKF 算法中模型個數(shù)設(shè)置為3 個，對應(yīng)的3 個子濾波器中量測噪聲矩陣分別設(shè)置為0.25R、R和4R，模型之間的初始馬爾可夫轉(zhuǎn)移概率設(shè)為：

將GPS/IMU 組合導(dǎo)航系統(tǒng)的輸出作為地面真實參考軌跡，如圖3 中黑色實線所示。

圖3 KITTI 數(shù)據(jù)集軌跡與位置估計對比Fig.3 Trajectories of KITTI data set and estimations

圖3 中綠色點劃線、藍色虛線以及粉紅色雙劃線分別為使用EKF、MSCKF 和IMM-MSCKF 算法得到的位置估計軌跡。采用三種算法得到的姿態(tài)、速度和位置估計對比分析結(jié)果如圖4、圖5、圖6 所示。

圖4 姿態(tài)角估計誤差Fig.4 Attitude estimation error

從圖4 姿態(tài)角估計誤差可以看出，采用三種算法進行估計得到的俯仰角和橫滾角估計誤差均能收斂到均值0 附近，三種算法估計結(jié)果較為接近；航向角估計誤差明顯要大于俯仰角和橫滾角估計誤差，而且隨著時間增加，航向角估計誤差有逐漸積累變大的趨勢；上述姿態(tài)估計誤差結(jié)果與VIO系統(tǒng)的可觀測性分析是一致的[20-22]。僅從航向角估計誤差均值來看，MSCKF和IMM-MSCKF 算法估計結(jié)果優(yōu)于EKF 算法，特別是在車輛拐彎機動時，EKF 算法得到的航向角誤差估計變化較為劇烈，而MSCKF 和IMM-MSCKF 算法估計結(jié)果變化不大，這驗證了MSCKF 算法在劇烈運動的場景中有較好的適應(yīng)性。與MSCKF 算法相比，IMM-MSCKF 算法的航向角估計誤差較小，表明相對于單一模型濾波器，多模型濾波算法具有更好的估計精度。

從圖5 可以看出，采用三種算法得到的水平速度估計誤差均收斂于0，EKF 速度估計誤差均值大于MSCKF 和IMM-MSCKF 算法。與MSCKF 算法相比，本文提出的IMM-MSCKF 算法速度估計誤差較小，表現(xiàn)優(yōu)于MSCKF。這也表明基于多模型的濾波器比單一模型濾波器能提供更精確、更穩(wěn)健的狀態(tài)估計。

圖5 水平速度估計誤差Fig.5 Estimation error of horizontal velocity

圖6 展示了采用三種算法進行濾波的經(jīng)度和緯度估計誤差。與速度估計誤差情況類似，采用EKF 算法得到的經(jīng)度和緯度估計誤差明顯大于另外兩種算法；與IMM-MSCKF 算法相比，MSCK 算法得到的位置估計誤差累積速度更快，表明IMM-MSCKF 算法對系統(tǒng)中存在的不確定性提供了更好的估計。

圖6 水平位置估計誤差Fig.6 Estimation error of horizontal position

為了進一步分析IMM-MSCKF 算法的性能，以GPS/IMU 系統(tǒng)輸出的位置信息為參考，將上述三種不同算法得到的水平位置均方根誤差(RMSE)進行比較。針對N個濾波更新時刻，均方根誤差（RMSE）計算如下：

其中qest為方位、水平速度或水平位置中的某項參數(shù)，qgt為KITTI 數(shù)據(jù)集中對應(yīng)的參考真值。采用三種算法得到的狀態(tài)估計RMSE 如表1 所示。

表1 三種算法的均方根誤差Tab.1 RMSEs of the three different methods

由表1 可以看出，IMM-MSCKF 的位姿狀態(tài)估計最為準(zhǔn)確，其姿態(tài)估計RMSE 為0.36 °，MSCKF 和EKF 算法對應(yīng)的姿態(tài)估計RMSE 分別為0.94 °和1.93 °；IMM-MSCKF 算法水平速度估計RMSE 為0.26 m/s，MSCKF 為0.35 m/s，EKF 為0.38 m/s；IMM-MSCKF 算法水平位置估計RMSE 為11.62 m，MSCKF 和EKF 算法對應(yīng)的水平位置估計RMSE 分別為19.68 m 和26.33 m。

IMM-SCKF 算法中三種模型對應(yīng)的模型轉(zhuǎn)移概率變化情況如圖7 所示，由于受到可變量測噪聲的影響，三種模型對應(yīng)的轉(zhuǎn)移概率變化具有一定的隨機性，這體現(xiàn)了交互式多模型算法中匹配模型的自適應(yīng)調(diào)整特性。

圖7 IMM-MSCKF 算法中模型轉(zhuǎn)換概率變化情況Fig.7 Model probability variation in IMM-MSCKF algorithm

4 結(jié) 論

針對雙目視覺/慣性里程計系統(tǒng)模型復(fù)雜、非線性強的特點，研究了基于濾波器估計的緊耦合VIO 系統(tǒng)信息融合方法。經(jīng)典的MSCKF 算法采用滑動窗口結(jié)構(gòu)，通過連續(xù)時間段內(nèi)的相機位姿狀態(tài)建立非線性三角化約束，實現(xiàn)濾波更新和狀態(tài)估計。然而，由于該算法中系統(tǒng)模型采用單一固定模型，不均勻光照、陰影等噪聲容易導(dǎo)致濾波器穩(wěn)定性和估計精度下降。因此，本文將交互式多模型估計與MSCKF 算法相結(jié)合提出了一種交互式多模型MSCKF 算法。該算法以MSCKF 為模型匹配子濾波器，將各子濾波器的輸入、輸出進行交互融合，實現(xiàn)對VIO 系統(tǒng)的狀態(tài)估計。最后，通過KITTI 數(shù)據(jù)集對提出的IMM-MSCKF 算法進行了實驗驗證，結(jié)果表明該算法相比于 EKF 和MSCKF 算法具有更好的估計精度和魯棒性，能夠?qū)崿F(xiàn)高精度和一致性的實時VIO 系統(tǒng)信息融合。