基于拉普拉斯分布的雙目視覺里程計

范涵奇 吳錦河

視覺里程計(Visual odometry,VO)是在未知環境中只通過相機獲得的圖像信息來實現機器人定位的技術.近些年來,VO 廣泛應用于機器人導航[1]、無人機[2-3]和虛擬現實/增強現實[4]等領域.

視覺里程計使用單目或多目相機作為視覺傳感器.單目相機為主要傳感器的VO 系統雖然成本較低,但面臨的主要問題是單目尺度的不確定性,在計算過程中會發生尺度漂移[5-6].該問題通常使用多傳感器來解決,例如雷達和多目相機.相比于單目相機,雙目相機可以直接測量三維空間點的位置,避免了尺度的不確定性,因此本文使用雙目相機作為視覺傳感器.

VO 系統分為前端和后端[7].前端通過特征點匹配構造數據關聯來為后端優化提供初始位姿.數據關聯(Data association)是指在幀與幀之間的特征點、特征點與地圖點以及地圖點與地圖點之間構建特征對應關系[8].數據關聯錯誤是VO 系統失敗的主要原因之一.基于圖像特征點的VO 系統數據關聯方式主要分為兩種.一種是通過計算特征點的描述子來構建數據關聯.采用描述子匹配特征點的準確性與魯棒性較高,但是特征描述子的計算非常耗時.Mur-Artal等[9]測試尺度不變特征變換(Scaleinvariant feature teansform,SIFT)[10]和加速健壯特征(Speed-up robust feature,SURF)[11]提取耗時約為300 ms,像這類比較耗時的特征提取算法會影響VO 系統的實時性.為了提高實時性,Mur-Artal等在ORB (Oriented fast and rotated brief)-SLAM(Simultaneous localization and mapping)[9]中選擇計算速度較快的ORB 特征點作為圖像特征,幀與幀之間特征點通過特征描述子匹配.Cvi?i?等[12]在SOFT (Stereo odometry based on feature tracking)中提取Corner 角點和Blob 角點同時計算特征描述子,并通過在連續幀中追蹤同一特征點,如果該特征能被追蹤到則使用初始的描述子來提高數據關聯的精確性.由于相機幀率和圖像分辨率越來越高,導致特征提取的計算量越來越大,即使使用ORB 這類速度較快的特征描述子也可能會影響VO 系統的實時性.另一種方式只提取角點而不計算描述子,角點之間的匹配關系通過稀疏的光流(Optical flow)跟蹤來構建[13-14].稀疏的光流算法計算速度快,但光流容易導致特征點誤匹配從而使得數據關聯錯誤,因此本文使用光流來構建數據關聯的同時采用其他技術盡可能剔除錯誤的數據關聯.

VO 系統的后端優化前端估計的相機初始位姿,本文在后端只優化相機位姿而不維護一個全局地圖.在優化過程中目標函數的設計影響著系統魯棒性.在經典的Bundle adjustment (BA)和擴展的卡爾曼濾波(Extended Kalman filter,EKF)算法中都假設誤差服從高斯分布,優化過程中對噪聲敏感,因而導致位姿估計的誤差較大.與高斯分布相比,拉普拉斯分布對大噪聲不敏感.同時,對于長尾數據(Long tail data)來說,拉普拉斯分布比高斯分布更適合對大幅噪音的似然描述[15],從而對異常點數據更加魯棒.

本文在VO 后端假設誤差服從拉普拉斯分布,進而構造優化問題的目標函數.Casafranca等[16]在此假設下構造因子圖優化問題.Bustos等[17]通過構造旋轉的平均只優化相機朝向,在相機朝向已知的情況下得到全局最優的相機位置和三維空間點.該方法優化相機朝向時不受相機位置與三維空間點的影響因此更簡單并且能夠處理純旋轉的相機運動,但是該方法運算速度慢,并且在求解相機位置時由于同時優化三維空間點和相機位置,誤差較大的三維點會影響相機位置的求解.與Casafranca等[16]和Bustos等[17]的方法不同,本文在后端分開優化求解相機朝向、三維空間點以及相機平移,在此過程中假設相機位姿與三維空間點的誤差都服從拉普拉斯分布.SOFT 等多個不同的算法證實分開估計相機的朝向與位置可以提高相機位姿估計的精確性.

近些年來,基于直接法的雙目視覺里程計越來越受研究者的歡迎,例如,Stereo DSO (Stereo direct sparse odometry)[18]、SVO2 (Semidirect visual odometry 2)[19]和FMD stereo SLAM (Fusing MVG (multiple view geometry)and direct SLAM)[20].Stereo DSO 算法通過采用梯度較大的稀疏像素,使用Bundle adjustment優化來得到精度較高的相機位姿.該算法速度較快、魯棒性好,并且可以生成稠密的三維點云,但是對于場景的光照變化比較敏感.SVO2 擴展了半稠密直接法單目視覺里程計SVO[13],SVO2 算法只對選擇的關鍵幀提取特征點并且采用稀疏圖像對齊算法匹配特征點,因此SVO2 速度快適用于計算平臺受限場合的定位.SVO 采用深度濾波器估計深度,由于初始深度估計具有較大誤差在優化時可能無法收斂到真實深度,進而影響相機位姿估計.FMD stereo SLAM 方法融合了多視角幾何和直接法,在前端采用直接法估計初始位姿而在后端采用多視角幾何的方法來估計三維結構,這種直接法和多視角幾何法融合的方法同時兼顧了速度與精度.與以上幾種方法相比,本文算法采用特征點法并且引入了誤差服從拉普拉斯分布的假設來優化相機位姿.

本文的組織結構如下:第1 節簡要介紹本文算法的框架.第 2 節中詳細闡述特征點的提取以及如何剔除錯誤的特征匹配并篩選穩定的特征點.第3節為相機位姿的估計與優化.第4 節中通過實驗驗證了本文算法的有效性.第5 節為本文的結論.

1 系統概述

本節主要從整體上概述本文提出的算法.算法主要由數據關聯和相機位姿優化估計兩部分組成.數據關聯是相機位姿估計的預處理過程.在構造數據關聯時,特征點的選擇與數據關聯的準確性影響著相機位姿估計的精度.在兼顧速度與精度的情況下如何選擇穩定的特征點并剔除錯誤的關聯是VO 算法重要的一步.位姿估計部分接收篩選的穩定特征點來優化求解相機位姿并重建出稀疏的環境地圖.

1.1 特征選擇與數據關聯

為了提高VO 算法的實時性,需要盡可能快地提取每幀圖像的特征點,因此本文選擇FAST(Features from accelerated segment test)[21]角點作為圖像特征點.數據關聯主要通過稀疏光流算法來構建.雖然稀疏的光流計算速度快,但是往往會導致特征點的誤匹配,因此本文采用三個策略,即平滑的運動約束、視差一致性檢測以及環形匹配來盡可能剔除錯誤的關聯,進而提高算法的魯棒性,并在此基礎上選擇穩定的特征點.

1.2 相機位姿優化

本文算法的主要流程如圖1 所示.其中,P表示三維空間點的位置,p表示三維空間點對應的在二維圖像上的投影坐標,R,t分別表示相機的旋轉和平移.下標l,r,k為左右相機和圖像幀的索引.在相機位姿估計的過程中,通常對于相機朝向的優化估計是比較困難的,因此本文首先去優化估計相機的朝向以及三維空間點,然后固定已經求得的相機朝向和三維空間點來優化求解相機平移.在光流構建數據關聯的過程中,同一特征點可以連續地在多幀中被跟蹤.本文基于特征點能被連續跟蹤的幀數選擇不同的參考幀從而獲得當前幀相機位姿的多個估計.對于當前幀的多個相機位姿估計,在相機位姿誤差服從拉普拉斯分布的假設下構造優化問題的目標函數,進而得到位姿的最優估計.

2 圖像特征匹配

光流是三維運動在二維圖像平面上的投影.在使用光流構建數據關聯時,需要剔除錯誤的關聯以提高數據關聯的精確性,并且需要在此基礎上篩選穩定的特征點.如果一個特征點能在連續多幀中跟蹤到,那么該特征點是穩定的.

一般而言,相機連續幀之間的運動是平滑的,即前后幀之間的運動量較小,那么圖像幀Ik的運動與前一幀Ik-1的運動是高度相似的,這種現象稱為平滑的運動約束(Smooth motion constraint,SMC).Badino等[22]將該思想應用到三維空間中來剔除三維點的誤匹配,本文應用到二維的圖像特征點上以提高光流跟蹤的準確性.

由于當前幀的運動與前一幀是相似的,因此可以使用前一幀的運動去預測特征點在當前幀上的位置.即

式中,Pk,Pk-1分別代表同一組三維空間點在當前幀Ik和前一幀Ik-1視角下的位置表示,表示已經估計的前一幀的相對運動,即幀Ik-1相對于幀Ik-2的相對位姿變換,π(·)表示投影函數,將三維空間點投影到二維的圖像上,pk為三維空間點Pk在圖像幀Ik上的投影.

由于pk更接近于光流收斂的位置,因此本文使用pk作為光流跟蹤過程中預測的前一幀圖像特征點在當前幀上的初始位置,在特征跟蹤過程中可以消除一定特征點的誤匹配,從而提供更為可靠的特征點匹配信息.

在平滑的運動約束下單純使用光流跟蹤特征點依然存在誤匹配,為了剔除誤匹配增加特征點匹配的正確性,本文采用強約束環形匹配(Circle matching)[12].圖2 中描述了環形匹配的整個過程.如圖2 所示,假設在圖像幀上檢測到某一特征點x,在連續幀之間的左右圖像上使用光流按照的順序跟蹤特征點x,跟蹤結束時在幀上的特征點記為x′.本文通過計算特征點x與x′之間的距離d(x,x′)來判斷該環能否閉合.如果距離d不大于一個像素,則認為該環能閉合,否則認為該特征點匹配出錯,進而剔除誤匹配的特征點.

圖2 環形匹配 (使用光流依照箭頭所示順序跟蹤特征點)Fig.2 Circular matching (Use optical flow to track feature points following the order as arrows direct)

雙目相機系統中,視差是一個非常重要的量.在使用以上兩種方法剔除特征點的誤匹配后,為了進一步提高特征點匹配的精確性,通過檢測雙目相機的視差來再次剔除誤匹配.本文在光流跟蹤過程中只計算稀疏特征點之間的視差.對于已經校正好的左右相機的圖像幀來說,左右匹配的特征點一定位于水平的極線(Epipolar line)上.從圖像特征點的角度來看,假設 (xl,yl)與 (xr,yr)是左右相機上一對匹配的特征點,則視差的y分量應等于0.即:yl=yr,但實際上在圖像校正與特征點跟蹤過程中由于噪聲的存在,等式不是嚴格成立的.因此,本文采用閾值thd來篩除掉大于閾值的匹配點.此外,本文以左邊相機為參考系,則視差在x方向上的分量應該為正.本文使用視差一致性(Disparity consistency)來描述這種現象.即

其中,(xl,yl),(xr,yr)為左右圖像上匹配的特征點.如圖3 所示,使用特征選擇策略后能明顯地剔除誤匹配,進而提高數據關聯的精確性.

圖3 特征點匹配的對比Fig.3 Comparison of feature matching

為了精確地估計相機位姿,本文盡可能選擇穩定的特征點.特征點的穩定性通過該特征能被觀測到的圖像幀數來度量.一個特征點能在連續幀中被觀測到的次數越多,那么該特征點越穩定.如圖4所示,使用光流構建數據關聯時同一特征點可以被連續跟蹤多幀.SOFT 使用age來描述特征點的穩定性,本文同樣采用age來表示一個特征點的穩定性.以往的幾項研究[12,23]證實選擇穩定的特征點可以提高相機位姿估計的精確性.

圖4 光流特征跟蹤與age說明(同一特征點可以在連續幀中被跟蹤,age值越大該特征點越穩定,T表示兩幀之間的位姿變換)Fig.4 Optical flow feature tracking andagedescription(The same feature point can be tracked in consecutive frames.The larger theage,the more stable the feature point,Trepresents the pose transformation between two frames)

3 相機位姿估計

本節主要描述了相機位姿估計與稀疏點云的重建.本文將位姿估計分為兩部分.首先,根據幀與幀之間二維的匹配點直接估計相機的相對旋轉;然后固定已知的相機旋轉來估計平移.

3.1 拉普拉斯分布假設下目標函數的構造

一般而言,SLAM 可以表示為最大后驗估計(Maximum a posteriori estimation,MAP)問題.假定要估計的未知變量為X,其中,X包括相機位姿和環境中路標點的位置.由觀測方程zj=hj(Xj)+?j可得到一個觀測集合Z={zj:j=1,2,···,m},其中,Xj?X是估計變量的子集,hj(·)為觀測模型,?j為觀測的噪音.

在MAP 估計中,通過最大化后驗概率來估計變量X,即

其中,p(Z|X)為似然,p(X)為先驗.在沒有先驗信息的情況下,p(X)是一個常量,MAP 退化為最大似然估計(Maximum likelihood estimation,MLE).假定觀測的噪音互不相關,即觀測Z相互獨立,則式(3)分解為

假設觀測的噪音?j服從具有信息矩陣 Ωj的零均值拉普拉斯分布,則式(4)中的似然為

由于最大后驗估計等價于最小化后驗的負對數,則有

由式(6)可知,在噪音服從拉普拉斯分布的假設下構造了以L1范數來度量誤差的非線性優化問題.因此在相機位姿與三維點云優化時,本文在拉普拉斯分布的假設下采用L1范數來度量誤差.

傳統上在高斯分布假設下采用L2范數作為目標函數來優化估計相機位姿.盡管L2范數應用廣泛,但是拉普拉斯分布假設下的L1范數提供了另一種選擇.Moreno等[24]詳細對比了L1范數和L2范數在不同噪音水平下的表現,證實了L1范數對較大的異常值更加魯棒.

為了提高L2范數對較大誤差的魯棒性,通常結合魯棒的核函數(例如Huber)來減小誤差較大的數據對優化結果的影響.Casafranca等[25]在后端采用L1范數度量誤差實現了因子圖SLAM,并在實驗中證實了L1范數在消除大噪音數據影響方面比Huber 核更加魯棒.同時相比于Huber 核函數,L1范數在優化時并不需要調整任何內核參數.Bahreinian等[26]研究了在位姿圖優化中通過最小化L1范數能有效剔除離群點的影響.綜上,盡管與魯棒Huber 核函數類似,但是L1范數在消除大噪音數據的影響方面仍然更加有效,即L1范數對大噪音數據優化的魯棒性更好.

3.2 旋轉的估計

不同于一般的雙目VO 算法只使用左側圖像信息來估計相機位姿,為了盡可能多地估計當前幀的旋轉,本文使用左右相機的圖像信息來分別估計當前幀的旋轉.

如圖4 所示,本文根據當前幀上特征點的age值選擇不同的參考幀來估計當前幀的相機朝向.對于首次提取的某一特征點的age值記為0,如果該特征點能跟蹤到下一幀則age值加1,以此類推,age值越大說明該特征點能被連續跟蹤的幀數越多.如果當前幀中某一特征點的age值為n,那么說明該特征點可以在前n幀中被跟蹤到,因此本文選擇前n個圖像幀分別作為參考幀來估計當前幀相機的朝向.對于當前幀Ik相機朝向的n個不同估計,使用集合{1Rk,2Rk,···,nRk}來表示,其中Rk代表當前幀Ik相機的絕對朝向.為了得到相機朝向的最優估計,假設噪聲服從拉普拉斯分布,則有

其中,Ri∈{1Rk,2Rk,···,n-1Rk,nRk},S? 為相機旋轉.通過采用SO(3)上的迭代加權最小二乘(Iterative reweighted least squares,IRLS)算法獲得相機朝向的最優估計S*.算法1 描述了SO(3)上的IRLS 算法.

算法 1.SO(3)上的 IRLS 算法

為了估計當前幀Ik的絕對旋轉Rk,本文首先估計選擇的參考幀Ik-i與當前幀Ik之間的相對旋轉由于參考幀Ik-i的絕對相機旋轉Rk-i已經求得,因此可以得到當前幀Ik絕對的相機旋轉,即:

對于參考幀與當前幀之間的相對旋轉,傳統上應用對極約束根據幀與幀之間二維特征點的匹配關系,通過分解本質矩陣(Essential matrix)[27]來恢復,但是對于純旋轉的相機運動來說,無法從本質矩陣中恢復相機的運動.針對這個問題,Fathian等[28]使用四元數來表示相機的相對旋轉,提出了QuEst算法來從二維的匹配點中估計相機的運動.該算法需要5 對點來求解相機旋轉,但通常匹配特征點的數量多于5 對,因此本文采用QuEst 結合隨機采樣一致性算法(Random sample consensus,RANSAC)[29]來精確地估計兩個相機視角之間相對的旋轉變換.

3.3 稀疏點云重建

重建的三維空間點的精確性影響著相機位置的估計.由第3.2 節,在相機運動的過程中基于特征點的age值,可以在前n幀中多次觀測到同一三維空間點.如圖5 所示,首先根據雙目相機的視差可以三角化得到同一三維空間點的n個位置表示.然后由于前n幀的相機位姿已知,因此可以使用左右相機前后幀之間特征點的匹配關系再次三角化得到 2n-2 個同一三維空間的位置,即三角化得到同一三維空間點的 3n-2 個位置.本文使用集合{1P,2P,···,3n-2P}表示同一空間點的多次觀測.為了得到最優三維空間點的位置,本文將三維空間點視為歐氏空間 R3中的三維向量,在同一三維空間點的多次觀測中得到該三維空間點的最優位置,即

圖5 同一三維空間點的多次三角化(基于特征點的age值,在前n幀中根據雙目相機的視差以及左右相機連續幀間特征點的匹配關系多次三角化同一三維空間點)Fig.5 Multiple triangulations of the same 3D space point (Based on theageof the features,the same 3D space point is triangulated multiple times in the firstnframes according to the disparity of the stereo camera and the matching relationship between the feature points of the left and right camera consecutive frames)

其中,Pi∈{1P,2P,···,3n-2P}.通過采用 R3上的IRLS 算法優化三維空間點,得到三維空間點的最優位置P*.

3.4 平移的估計

本文使用優化后的三維點云與二維點的匹配關系來求相機的平移.三維空間點投影到二維圖像平面的整個過程為

其中,[u,v,1]T為投影點的齊次坐標表示,f為相機焦距,投影函數用π來表示,(αx,αy)為圖像的主點,[R|t]為相機兩個視角之間的運動.[x,y,z,1]T為三維點云的齊次坐標表示.

本文固定已經估計的旋轉和優化后的三維空間點云來優化估計相機的平移,即

其中,l,r表示左右相機.整個過程中IRLS 算法只迭代優化相機的平移,通常僅需迭代少數幾次即可收斂.與SO(3)上的 IRLS 算法相比,對于R3上的IRLS 算法,只在于計算誤差以及迭代更新的方式不同,R3上直接通過兩個向量之差來定義誤差.

4 實驗結果與分析

本文在KITTI 數據集[30]以及New Tsukuba數據集[31]上測試提出的算法.實驗環境為裝有Ubuntu 18.04,配置為Intel? CoreTMi7-4 770 CPU,8 GB RAM 的計算機.KITTI 數據集提供了大尺度動態場景下的22 個立體圖像序列.其中前11 序列(00～10)提供了真實軌跡.New Tsukuba 數據集為人工合成的小場景的靜態室內環境.

本文主要在精度與速度兩個方面來評估算法.在精度方面,主要通過評估估計軌跡與真實軌跡之間的均方根誤差(Root mean square error,RMSE)、均值(Mean)與標準差(Standard deviation,STD)來反映本文算法的精確性,其中均方根誤差代表絕對軌跡誤差,均方根誤差越小則位姿估計越準確.本文使用光流跟蹤特征點選擇的窗口塊大小為9×9,視差一致性檢測的閾值thd=1,驗證是否構成回環匹配的閾值d ≤1.

由于VISO2-S[32]與ORB-SLAM2[33]的作者開放了源代碼,因此本文選擇這兩種算法來與本文算法(算法1)進行對比.表1 中對比了本文算法與ORB-SLAM2在KITTI 數據集00～10 序列共11個序列上的測試結果.從表1 中可知,除了01 序列外,本文算法在其余的10 組序列上位姿估計的結果較精確.01 序列采集的是高速公路的場景,由于場景紋理較少并且場景紋理相似,導致提取的特征點分布不均勻,且大部分集中在中間的一條線上,只使用圖像特征點在此場景中估計相機位姿是困難的,其他算法在此序列上也存在同樣的問題,導致位姿估計的誤差較大.相比于ORB-SLAM2,本文算法的RMSE 在多數序列上更小.由于在05和06序列上有閉環,ORB-SLAM2 通過閉環檢測與校正來進一步提高位姿估計的準確性,而本文算法沒有回環校正.雖然在05和06 序列上ORB-SLAM2的RMSE 較小,但是本文算法的RMSE 非常接近ORB-SLAM2.

表1 中同時對比了本文算法與VISO2-S 在KITTI數據集00～10 序列上估計的相機運動軌跡與真實軌跡的RMSE、Mean和STD.VISO2-S 算法通過最小化重投影誤差并采用高斯牛頓法迭代優化求解相機的旋轉與平移.通常對于相機的旋轉優化是比較困難的,相機旋轉影響著相機平移的估計,因此采用迭代法同時優化求解得到相機旋轉與平移的誤差較大.本文算法通過分開獨立的優化相機的旋轉與平移提高來位姿估計的精確性,從表中數據可知本文算法優于VISO2-S 算法.

表1 本文算法、ORB-SLAM2 以及VISO2-S 估計的軌跡與真實軌跡之間RMSE、Mean、STD 的對比Table 1 Comparison of RMSE,Mean,STD between the trajectory estimated by ours,ORB-SLAM2,and VISO2-S and the real trajectory

表2 中對比了不同算法在大場景的 KITTI數據集上的運行時間,部分數據來源于文獻[34]與KITTI Benchmark[30].本文只統計特征處理與位姿優化估計所耗費的時間.從表中數據可知,由于ORB-SLAM2 采用局部BA 與全局BA 來優化相機位姿,因此ORB-SLAM2 位姿優化估計部分耗時較多.由于VISO2-S 對左右每幅圖像提取Coner角點和Blob 角點并計算描述子,通過描述子來構建圖像幀之間的數據關聯,最后通過最小化重投影誤差求解相機位姿,因此VISO2-S 主要耗時在特征處理部分.FB-KLT 方法提取Shi-Tomasi 角點并通過高斯牛頓法求解相機位姿,從表中實驗數據看出該方法主要耗時在特征處理部分.本文算法在特征處理部分速度最快,耗時主要在位姿優化估計部分.總體而言,從表2 中可以看出本文算法耗時最少.

表2 計算時間的對比(部分數據摘自KITTI Benchmark[30])(ms)Table 2 Computation time comparison(Partial data from KITTI Benchmark[30])(ms)

在室內場景的New Tsukuba 數據集上,本文算法估計的RMSE 為0.389,ORB-SLAM2 估計的RMSE 為1.015,VISO2-S 估計的RMSE 為0.916,可以看出本文算法表現較好.

在圖6 (a)和圖7 (a)中,給出了本文算法、ORBSLAM2和VISO2-S 估計的相機運動軌跡與真實相機軌跡在KITTI 數據集上的對比.而圖6 (b)和圖7 (b)為本文算法在KITTI 數據集上重建的周圍環境的稀疏地圖.

圖6 在 KITTI 數據集序列 00～05 上的實驗Fig.6 Experiments on the KITTI sequence 00～05

圖7 在 KITTI 數據集序列 06～10 上的實驗Fig.7 Experiments on the KITTI sequence 06～10

在圖8(a)中,給出了New Tsukuba 數據集上本文算法、ORB-SLAM2 以及VISO2-S 估計的軌跡與真實軌跡的對比.從軌跡對比圖中可以看出,本文算法估計的軌跡更接近于真實軌跡.圖8(b)為本文算法在New Tsukuba 數據集上重建的稀疏環境地圖.

圖8 在 New Tsukuba 數據集上的實驗Fig.8 Experiments on New Tsukuba sequence

5 結束語

本文基于拉普拉斯分布提出了一種快速精確的雙目視覺里程計算法.為了提高相機位姿與三維空間點估計的精確性,在相機旋轉、三維空間點以及相機平移這些量的誤差服從拉普拉斯分布的假設下來構造優化問題的目標函數.通過在SO(3)以及R3上的優化來得到位姿和三維空間點的最優估計.KITTI 以及New Tsukuba 數據集上的實驗結果表明本文提出的算法能夠快速準確地估計相機位姿與三維空間點的位置.本文算法尚有一定的局限性,在構建數據關聯時只使用了光流跟蹤,由于光流自身的原因,對于亮度變化明顯的環境不夠魯棒.未來的工作中,將進一步研究提高光流跟蹤的精確性并嘗試將視覺信息與慣性測量結合起來,增加系統的魯棒性.