深度相機與微機電慣性測量單元松組合導航算法

張福斌， 林家昀

(1.西北工業大學 航海學院, 陜西 西安 710129; 2.湖南航天機電設備與特種材料研究所, 湖南 長沙 410200)

0 引言

隨著機器人技術的不斷發展，在室內反恐、排爆以及搜救等活動中機器人的使用量日益增加。對于一個自主運動的物體，無論其執行何種任務，準確獲取自身位置和姿態(簡稱位姿)信息是一個必不可少的功能，只有知道自身位姿，才能更好地完成任務[1]。所以室內機器人高精度導航與定位一直是學術研究和工程實踐上的一個重點問題。

全球定位系統(GPS)作為一種機器人常用定位方式，由于信號容易被墻壁等障礙物遮擋，在室內幾乎不能正常使用[2]?；诶锍逃嫷暮轿煌扑闶且环N輪式機器人常用的定位方式[3]，但是由于建筑物內部經常有樓梯等，導致輪式機器人使用范圍受限，需要使用雙足等非輪式機器人，這種類型的機器人通常無法攜帶里程計，因此必須依靠其他手段進行導航定位。

微機電系統(MEMS)慣性測量單元(IMU)是一種機器人常用的慣性導航傳感器，具有低功耗、體積小、抵抗外界干擾能力強等優點[4]，但單純基于MEMS IMU的導航系統存在定位誤差快速增加等不足[5]。視覺導航作為一種新興的導航與定位技術，優點在于視覺獲取的信息量大[6-7]，能夠還原三維場景，并且可以通過回環消除累積的定位誤差，但視覺受環境特征影響大，圖像匹配存在隨機誤差[8-9]。因此，慣性導航與視覺導航能夠實現傳感器之間的優勢互補，抑制定位誤差發散速度，提升導航系統的魯棒性和定位精度[10]。

視覺與MEMS IMU組合導航有兩種組合方式，分別為松組合與緊組合[11]。

松組合算法優點在于兩種傳感器獨立工作，計算速度快，但不足之處在于通常只使用視覺導航的位置信息作為量測，導航精度較低。目前主流的視覺與MEMS IMU松組合算法由孫彤等[12]提出，該算法將視覺輸出的位姿作為擴展卡爾曼濾波(EKF)的量測值，對載體位姿等狀態量進行估計，得到更加準確的位姿信息。

緊組合算法優點在于將兩種傳感器信息之間的耦合關系計算在內，得到更加精確的導航結果，但不足之處在于算法復雜，計算量大。目前主流的視覺與MEMS IMU緊組合導航中，有基于單目相機的視覺慣性組合導航系統(VINS)、基于雙目相機的視覺慣性緊耦合系統(OKVIS)以及基于深度相機的深度圖像與慣性器件組合導航系統(RGB-D-Inertial SLAM)[13]等多個視覺慣性組合導航系統。

VINS通過對相機及由IMU輸出的慣性導航數據進行前端預處理、相機初始化、光束平差法(BA)聯合優化、全局優化和回環檢測，實現視覺慣性的緊耦合[14]。但由于單目相機本身缺乏尺度信息，需要將慣性導航信息預積分進行尺度初始化。這一操作會將慣性導航誤差引入尺度計算，故降低了自身定位與三維重構的精度。

OKVIS通過慣性導航預積分的值作為預測量，依據預測提取和匹配特征點，使用雙目相機計算得到具有尺度的圖像信息，再對圖像使用非線性優化方法進行重投影優化，預測和修正慣性導航系統的誤差[15]。該方法能夠得到較高精度的載體位姿信息，并且包含尺度；但計算量大，對硬件要求較高，且不能實時重構三維環境，且沒有重定位的功能，無法消除累積誤差。

在基于深度相機和由IMU構成的慣性導航系統融合算法的研究中， Brunetto等[16]提出的基于移動設備的彩色- 深度(RGB-D)視覺測量與慣性測量融合算法，通過一組卡爾曼濾波器，消除慣性導航位姿測量數據噪聲，對視覺與慣性導航的測量數據進行融合。

Kim等[17]提出的高動態環境中基于RGB-D傳感器和IMU的視覺里程計算法，針對高動態環境，基于加速穩健特征(SURF)描述子，將深度點云與彩色圖像結合生成三維特征點，并使用IMU對兩幅連續圖像之間的旋轉分量進行補償，得到特征點的運動矢量。

Zhu等[18]開發了基于RGB-D和慣性輸入的室內場景實時重建算法，將彩色圖像、深度點云和慣性測量結果通過EKF相融合，并采用迭代的方法進行聯合優化，從而提高傳感器快速運動時的重建魯棒性。

上述3種算法均為視覺與由IMU構成的慣性導航系統緊耦合的算法，計算精度高，系統魯棒性強，但是系統工作時同時處理彩色圖像、深度點云以及慣性導航系統測量數據，算法復雜，計算量大，運算時間長。

基于深度相機的工作原理，本文綜合考慮室內環境的特點，在保證系統精度和魯棒性的前提下，從減少計算量、降低對導航算法處理器運算能力的角度，提出了一種適用于室內的深度相機與MEMS IMU松組合導航算法。該算法使用MEMS IMU預積分得到的位姿變化量作為改進迭代最近點(ICP)的初值，而不是傳統ICP中將初值設置為0，從而減少了迭代次數和計算時間；使用視覺導航得到的位置信息減去慣性導航得到的位置信息，得到二者的位置差，以此差值作為量測量，對姿態誤差、速度誤差、位置誤差以及等效陀螺儀的常值漂移和加速度計的常值零偏進行估計，最后用估計值修正姿態誤差等，從而得到更加準確的載體位姿信息，提升導航系統的定位精度和三維重構精度。

1 深度相機與MEMS IMU松組合導航算法設計

1.1 松組合導航算法工作流程

深度相機與MEMS IMU松組合導航算法工作流程如圖1所示。工作時深度相機與MEMS IMU同時啟動，深度相機對載體所處環境的彩色圖像和深度點云進行采集，MEMS IMU對載體運動的角速度和加速度進行采集。

圖1 深度相機與MEMS IMU松組合導航算法流程Fig.1 Flowchart of loose coupled navigation algorithm based on depth camera/MEMS IMU

深度相機采集到彩色圖像和深度點云數據后，如果能夠從前后兩幀彩色圖像之間檢測并提取出大于等于12對特征點，則認為當前環境特征點豐富，僅用彩色圖像進行位姿推算就能夠保證導航系統的正常運行，所以為了減少計算量，這種情況下只用深度相機的彩色圖像信息進行導航計算；對完成特征點提取的相鄰兩幀彩色圖像使用隨機一致性采樣(RANSAC)算法剔除和優化特征點匹配結果，然后運用PnP算法計算出兩幀之間載體在導航坐標系下的位姿變化量。

如果小于12對特征點，則認為載體當前處于特征點缺失的區域，此時單靠深度相機輸出的彩色圖像信息導航系統無法正常工作。為了保證系統的魯棒性，使用深度相機輸出的彩色圖像和深度點云數據，采用基于MEMS IMU輔助的改進ICP算法計算載體位姿變化。首先對MEMS IMU采集到的角速度和加速度信息進行預積分，由于兩幀之間的時間間隔只有0.1 s，所以MEMS IMU預積分得到的載體位姿變化量包含的誤差較小。將MEMS IMU預積分的結果作為深度點云迭代中ICP算法的初值，從而在保證計算精度的情況下減少迭代次數和計算時間，得到載體的位姿變化量。

最后將視覺導航計算得到的載體運動信息與MEMS IMU預積分得到的載體運動信息作為量測量，送入深度相機與MEMS IMU組合算法，估計MEMS IMU解算的姿態誤差、速度誤差、位置誤差、陀螺常值漂移和加速度計常值零偏。最后用估計得到的誤差對計算結果進行修正，從而得到更加準確的載體位姿變化信息，進而完成對載體的導航與定位；同時彩色圖像對深度點云中的點進行上色，然后運用兩幀之間的位姿變化關系，對深度點云進行拼接，得到清晰平滑的三維重構模型。

1.2 基于MEMS IMU輔助的改進ICP算法

(1)

式中：R為旋轉矢量；v為速度矢量；g為當地重力加速度；d為平移矢量。

MEMS IMU測量得到的角速度和加速度雖然本身含有常值漂移和零偏等誤差，但通過積分得到的位姿信息在短時間內有較高的可信度，而Δt僅為0.1 s，所以可以通過積分計算在Δt內載體的旋轉矩陣變化量ΔR與平移向量變化量Δd：

(2)

得到旋轉矩陣變化量ΔR與平移向量變化量Δd后，將其代入ICP算法中待優化的目標函數：

(3)

式中：kbi為旋轉后的點云集合；kri為旋轉前的點云集合。改進后的ICP算法將IMU測量值作為初值代入ICP算法，使ICP算法的迭代初值不為0，而是從一個較為準確的旋轉矩陣ΔR和平移向量Δd開始迭代，這樣能夠在不降低迭代精度的情況下，有效減少ICP算法的迭代次數。

1.3 深度相機與MEMS IMU組合算法

該算法中，視覺導航系統使用彩色圖像或深度點云計算得到機器人的旋轉矩陣和平移向量，使用MEMS IMU輸出信息計算機器人的角增量和速度增量，然后通過預積分計算機器人的旋轉矩陣和平移向量，將二者計算得到的位置作差運算，將差值作為EKF的量測量，進行誤差估計，最后使用誤差估計的結果反饋修正旋轉矩陣和平移向量，從而得到更加精確的機器人位姿信息。

設置EKF濾波器的系統狀態變量為

(4)

由常規捷聯慣性導航系統的誤差模型可以得到狀態方程為

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

取量測值為捷聯慣性導航系統計算得到的位置Pi與視覺計算得到的位置Pv之間的差值，所以量測方程為

Z=(Pi-Pv)=HX+V，

(17)

式中：

(18)

V=Vp，

(19)

Vp為零均值高斯白噪聲[19-20]。

通過EKF估計系統的狀態變量，從而得到姿態誤差、速度誤差、位置誤差、等效陀螺的常值漂移和加速度計常值零偏，然后用估計得到的誤差修正當前機器人的位姿信息，并用修正后的位姿信息完成航位推算與點云拼接。

2 算法實驗驗證

2.1 基于MEMS IMU輔助的改進ICP算法實驗驗證

實驗在數學仿真軟件MATLAB平臺下進行，首先在網上下載10幅不同的深度點云圖片，然后對它們進行旋轉和平移，由于算法中時間間隔為0.1 s，為了保證實驗的可靠性和真實性，對每一幅圖片進行小幅度的旋轉和平移，并記錄每一幅圖片的旋轉量和平移量；然后參考MEMS IMU(如荷蘭XSENS公司生產的的MTI 100-IMU)在0.1 s可能產生的位姿誤差量，給上述記錄的旋轉量和平移量添加誤差；最后實驗組使用添加誤差后的旋轉和平移作為初值，對旋轉前后的兩幅深度點云進行計算，當迭代結果E(ΔR,Δd)，即重投影誤差小于0.01時，結束并記錄結束時的迭代次數，對照組不使用添加誤差后的旋轉和平移作為初值，直接將初值設置為0，對旋轉前后的兩幅深度點云進行計算，當迭代結果E(ΔR,Δd)小于0.01時，結束并記錄結束時的迭代次數。實驗結果如表1所示。

表1 兩種算法各自的迭代次數

通過實驗結果可以非常明顯看出，當滿足重投影誤差小于0.01時，在對10幅深度點云的計算中，基于MEMS IMU輔助的改進ICP算法完成運算時的迭代次數，明顯小于傳統ICP算法完成運算時的迭代次數，說明改進的ICP算法是有效的。這是由于改進的ICP算法有了先驗信息，即旋轉和平移的加入，使得ICP算法在迭代初始時，更接近于真值，迭代次數才會明顯下降。

2.2 松組合導航實驗結果與分析

如圖2所示，使用美國Microsoft公司產Kinect v1深度相機和 荷蘭XSENS公司產MTI 100-IMU中的MEMS IMU作為傳感器，在速度可以設定的遙控小車上搭建深度相機與MEMS IMU松組合導航實驗平臺。

圖2 實驗平臺Fig.2 Experimental platform

載體以0.1 m/s的速度在學院大樓走廊運動，期望的載體運動軌跡為一個長4.7 m、寬1.0 m的矩形，如圖3所示。

圖3 期望的載體運動軌跡Fig.3 Expected trajectory of vehicle

實驗分為兩個部分：第1部分為室內導航實驗，通過對比不同方法生成的載體軌跡，驗證本文設計的圖像選擇邏輯，基于位置差值的深度相機與MEMS IMU松組合算法以及適用于室內小場景的回環檢測方法的有效性；第2部分為室內三維重構實驗，通過對比不同方法生成的走廊三維重構模型，驗證本文設計的算法能夠有效提高三維重構的精度和平滑度。

室內導航實驗分為兩組：第1組同時使用Kinect v1相機的彩色圖像和MTI 100-IMU，基于目前認可度較高的VINS算法計算并繪制載體運動軌跡；第2組使用Kinect v1相機和MTI 100-IMU，基于本文設計的深度相機與MEMS IMU松組合導航算法計算并繪制載體運動軌跡。兩種方法在使用相同的傳感器前提下，計算載體的運動軌跡并比較定位誤差的大小，驗證本文設計的深度相機與MEMS IMU松組合導航算法的有效性。兩組實驗的載體運動軌跡結果，分別如圖4和圖5所示。

圖4 基于VINS算法解算的載體運動軌跡Fig.4 Vehicle trajectory based on VINS

圖5 基于本文松組合導航算法解算的載體運動軌跡Fig.5 Vehicle trajectory resolved by the proposed loose coupled integrated navigation algorithm

第1組基于VINS算法的實驗中，使用Kinect v1相機輸出的彩色圖像和MTI 100-IMU組合，得到的航位推算軌跡沒有發生混亂，但效果并不理想，最終得到的位置為(7.162 m,1.032 m)，與出發點(0 m,0 m)之間的距離誤差為7.236 m，總里程為11.4 m，誤差占總里程數的63.45%. 因回環檢測未能正常工作，導航過程中的累積誤差沒有得到修正?？赡苁且驗槭褂肒inect v1相機采集的彩色圖像和MTI 100-IMU輸出的角速度和加速度的精度達不到VINS算法的要求，所以最終計算得到的載體運動軌跡與真實軌跡相差較大，但由于VINS為視覺與MEMS IMU緊組合系統，所以繪制的軌跡比較平滑。

第2組本文設計的深度相機與MEMS IMU松組合導航系統較好地完成了導航任務。當載體運動到走廊墻壁上有展板部分時，特征點清晰，程序使用深度相機與MEMS IMU松組合導航算法計算小車的姿態和位移。當載體運動到白色墻壁部分時，視覺特征點丟失，程序切換為基于MEMS IMU輔助的改進ICP算法，保證了系統在視覺特征丟失時的魯棒性，較好地解決了特征點缺失和重復場景導致的錯誤重定位問題，沒有出現軌跡混亂，也沒有出現較大的抖動與跳變。由于實驗用的遙控車結構穩定性較弱，存在較大的抖動，所以軌跡存在輕微抖動。運動過程中使用航位推算法，因此存在累積誤差。實驗中遙控車回到出發點時，計算得到結束位置到出發點的距離誤差為0.273 m，總里程為11.4 m，誤差占總里程數的2.40%. 由軌跡圖可以看出，回到出發點后，本文設計的基于導航先驗信息的回環檢測方法完成了重定位，修正了累積誤差。

基于相同的實驗場地和實驗儀器，對兩種算法分別進行了10次重復實驗。載體回到初始位置時，記錄航位推算的載體結束位置與初始位置之間的位置偏差，并計算位置偏差在總里程中的占比，結果如表2和表3所示。

從表2和表3中可以看出，VINS組合導航算法在軌跡平滑方面表現較好，但在計算載體旋轉時很難正確估計姿態變化角度，且航位推算誤差較大，位置誤差大致占總里程的60.97%～81.07%之間。

表2 載體回到初始位置時的航位推算位置偏差

表3 位置偏差占總里程的百分比

本文設計的深度相機與MEMS IMU松組合導航算法有效地融合了深度相機和MEMS IMU的優點，位置誤差大致占總里程的1.61%～3.14%之間。與VINS算法相比，在使用相同傳感器的情況下，定位精度更高，但由于是松組合系統，所以軌跡平滑度不如VINS算法。

3 結論

本文從室內非輪式機器人對于非結構化環境感知與精確自身定位的需求出發，在分析視覺導航和慣性導航以及視覺與MEMS IMU組合導航不足的基礎上，結合深度相機與MEMS IMU的優點，設計了一種深度相機與MEMS IMU松組合導航算法，具體如下：

1)針對傳統ICP算法計算量大等問題，設計了一種基于MEMS IMU輔助的改進ICP算法。該算法中，通過MEMS IMU預積分為ICP算法提供迭代初值，從而減少迭代次數和計算時間，最后設計并進行了10組實驗，驗證了基于MEMS IMU輔助的改進ICP算法能夠將迭代次數大約縮小至原來的一半。

2)針對單一傳感器導航誤差大，傳統松組合只使用視覺信息作為量測，信息運用不充分的問題，提出了一種基于位置差值的深度相機與MEMS IMU松組合導航算法。該算法將視覺導航和慣性導航得到的載體位置差值作為量測量，對系統誤差進行估計與修正，從而抑制誤差增長速度，得到更加精確的導航信息，最后通過實驗驗證了基于位置差值的深度相機與MEMS IMU松組合導航算法的有效性。