融合IMU 的RGBD－SLAM 算法改進研究

閔華松，楊 杰

（武漢科技大學 冶金自動化與檢測技術教育部工程研究中心，湖北 武漢430081）

0 引 言

基于RGB－D傳感器的定位技術成為近幾年來的熱門研究問題。RGB－D 傳 感 器 （如Kinect）可 同 時 提 供 彩 色（RGB）和深度 （Depth）圖像信息，相比普通相機，連續性好，環境信息更加豐富，因此定位的精度更高，且更容易進行三維地圖重建。

基于RGB－D 傳感器的定位算法有很多種。如Henry等［1］和Endres 等［2］提 出 的 基 于 圖 像 匹 配 技 術 的RGBDSLAM 算法，Kerl等［3］和Audras等［4］提出的基于光度模型的密集特征追蹤方法，Stuckler等［5］提出的基于點云匹配技術的方法。本文的定位算法是基于Endres等提出的RGBDSLAM 算法，即采用SIFT／SURF／ORB算法進行特征提取和匹配，然后采用RANSAC算法去除離群點得到初始位姿估計，采用ICP算法進行點云匹配改善位姿估計。由于采用稀疏特征點匹配技術對特征點依賴很大，當特征點數量小于閾值時，匹配會失效；同時由于匹配并非對全部圖像信息的匹配，使用RANSAC算法進行運動估計并不能完全代表兩幀圖像匹配的結果，因此誤差必然存在。

對此，一些學者提出使用先驗運動估計來提高定位算法的精度和魯棒性能。如Bouvrie等［6］將IMU 姿態解算值和初始姿態估計進行加權平均融合，得到一個新的姿態估計值，將該值參與到ICP 點云匹配；Hjelmare等［7］將輪式里程計、單軸陀螺儀與視覺里程計進行擴展卡爾曼濾波（EKF），利用運動模型構建預測模型，利用傳感器模型構建觀測模型；Hervier等［8］則提出基于剛體變換量的Lie群狀態方程，以Fisher（費希爾）信息陣描述ICP 算法的協方差，對IMU 與ICP的數據進行了IEKF （廣義的擴展卡爾曼濾波）融合。上述方法都在一定程度上提高了RGB－D 定位算法的精度，但是大多都著眼于姿態的融合，常采用輪式里程計測量的速度或者速度常量對位移進行預估，當系統存在漂移或者運動變換量很大時，系統的性能會嚴重降低。

針對上述問題，本文基于RGBD－SLAM 的定位的算法，提出了如下改進方案：引入IMU 傳感器，利用機器人的運動模型、IMU 的測量數據以及測量偏差構建預估模型，采用ICP位姿估計結果構建觀測模型，同時將機器人的運動指令和運動限制作為約束條件，進行擴展卡爾曼濾波融合。

1 三輪全向機器人的系統平臺和運動模型

1.1 系統平臺

本文搭建的實驗驗證平臺為三輪全向移動機器人，其實物圖和系統結構圖如圖1、圖2所示。

圖1 移動機器人實物

實物圖最上層是筆記本，是整個系統的核心處理單元，它接收傳感單元檢測的環境信息和機器人本體的運動信息，同時控制機器人的運行。中間層是Kinect和九軸慣性傳感器razorIMU［9］，是系統的傳感單元。Kinect每秒可以獲取30幀640＊480像素的RGB／Depth圖像，具有57度的水平視角和43度的垂直視角。razorIMU 具有三軸陀螺儀，三軸加速度計和三軸地磁計，可以檢測到的三維坐標系下角速度、線加速度和磁場的實時信息。最底層是全向移動機器人，是整個系統的執行單元。該單元由STM32作主控，具有藍牙、紅外、巡線傳感器、夏普紅外測距模塊、超聲波測距模塊等外圍設備，最大速度為0.6m／s。

圖2 移動機器人系統結構

1.2 三輪全向機器人的運動學方程

三輪全向機器人在平面上可以獨立控制機器人對應重心的平移與旋轉，并且具有全向移動的優勢，能夠同時進行旋轉和平移運動。其運動學模型如圖3所示。

圖3 三輪全向輪的運動學模型

其中，全局坐標系為Xa～Ya，機器人坐標系為Xr～Yr，θ為Xa與Xr之間的夾角，為1號輪子與Yr之間的夾角，為30°，L為機器人中心到輪子中心的距離。vi（i＝1，2，3）為輪子i沿驅動力方向的速度。

則，三輪全向機器人的運動學方程為

其中

則在給定機器人前進速度和角速度的情況下，可以通過式 （1）求出三輪沿驅動力方向的速度，進而計算三輪的驅動力，驅使電機的運行。對式 （1）求逆運算，可以在已知三輪速度的情況下，求出機器人的前進速度和角速度，這也是輪式里程計的原理。本文中機器人運行過程中存在漂移現象，故舍棄輪式里程計，采用慣性傳感器IMU。

2 基于IMU－Kinect的定位算法

2.1 坐標系變換

本平臺涉及到的坐標系有全局坐標系、機器人坐標系、IMU 坐標系以及Kinect坐標系。從各個傳感器獲取的數據都是基于其自身坐標系而言的，所以將多個傳感器數據進行融合計算時必須先進行坐標轉換。本文中將各坐標系數據均轉化到全局坐標系進行運算。

首先給定全局坐標系，在系統靜止的時候，分別測量機器人、IMU、Kinect坐標系與全局坐標系的歐拉角（φ，θ，ψ），其中IMU 的歐拉角可以通過讀取傳感器數據進行姿態估計來計算。則各坐標系之間的轉換矩陣Cxy通過式（3）得到

2.2 IMU 位姿估計

（1）姿態解算

IMU 慣性傳感器具有三軸加速度計、三軸陀螺儀和三軸地磁儀。陀螺儀測的是角速度的值，進行一次積分得到對應軸的角度，但是由于陀螺儀的漂移和測量噪聲的存在，角度的計算會失效，必須進行濾波處理。IMU 常用的姿態解算算法有互補濾波 （CF）和擴展卡爾曼濾波 （EKF）算法。其中EKF算法是將姿態角和偏移角作為狀態向量，陀螺儀的測量數據作為輸入，而加速度計的測量作為觀測模型，進行濾波處理。DCM［10］算法，即direction cosine matrix，是一種互補濾波的算法，目前在IMU 姿態解算中經常使用，可以對6軸或9軸慣性傳感單元進行解算，相對于EKF算法，后者更加簡單、有效。

DCM 算法先對全局坐標系和IMU 載體坐標系構建方向余弦矩陣，然后將該矩陣求導，得到方向余弦矩陣的微分方程，通過解微分方程，得到矩陣變量，最后轉化成歐拉角，即為所求的姿態角。具體描述如下

式中：C——方向余弦矩 陣，i′，j′，k′——載體坐標系的單位向量，它是由加速度計和電子羅盤的測量值來確定的，對其求導得到

則有微分矩陣

使用DCM 算法求解姿態比EKF 算法更為普遍，其測量結果也更為精確。IMU 姿態解算的結果具備一定的準確性，因此在多傳感器融合的過程中，可以予以較多的考慮。

（2）位移估計

通過IMU 進行航位推算，即由線加速度值去除重力分量，然后進行二次積分得到位移值，理論上可以得到航位信息，但是由于噪聲的存在、運動過程中的抖動以及姿態解算的誤差，加速度的分量無法準確去除，且會造成累積誤差，隨著運動時間的增長，位移的累積誤差會越來越大。普遍的做法是將慣性導航系統與其它導航系統 （如視覺、無線電等）進行組合導航，提高定位的精度。本文采用EKF算法將慣性傳感器與視覺傳感器進行數據融合，同時將機器人的運動指令作為約束條件。

采用加速度計構建測量模型，狀態向量x ＝（x，y，vx，vy，anx，any），因為機器人的運動是在平面上運動，故z 軸位移和速度、加速度不參與計算。anx，any是加速度的偏移量，包括重力加速度的分量和偏置量。

對加速度的測量值a進行分解可得

式中：ag——全局坐標系的加速度分解量，aI——加速度計的測得量，CIg——IMU 的坐標系到全局坐標系的轉換矩陣，該矩陣隨著IMU 的姿態而變化，受IMU 姿態解算的影響。wa——噪聲。

則構建觀測模型

實驗結果表明，在IMU 移動的情況下，加速度的偏移量（anx，any）并非為常量，但在短期運動的情況下仍可以常量計算，在機器人靜止的時候，可以對該偏移量進行修正，即在運動指令為0時，用式 （9）計算

2.3 Kinect位姿估計

使用Kinect進行定位與三維構圖的算法，即RGBDSLAM，包含如下幾個步驟：

（1）特征提取與匹配：首先從RGB圖像中提取出特征點，如SIFT、SURF［11］、ORB、SIFTGPU 等算法，SIFTGPU 算法需要系統配備高性能的顯卡，然后使用FLANN算法進行特征匹配。

（2）初始運動估計：根據特征匹配的結果求出相機的運動參數，即采用RANSAC算法，通過迭代，得到一個最優的初始位姿估計值，同時有效的去除誤匹配的外點。

（3）位姿優化：通過點云數據的配準，改善初始位姿估計的效果。算法中采用的是廣義的ICP （GICP）算法。

（4）圖優化：為得到全局一致的地圖，需要進行圖優化。算法中將得到的位姿作為節點，和鄰近幀之間的位姿關系作為邊，采用Hog－man算法進行優化。

通過上述步驟得到了最佳位姿估計值和3D 點云地圖，該算法后期通過處理可以得到三維柵格地圖。

從RGBD－SLAM 算法中可以看出，根據Kinect進行位姿估計的關鍵是找到一個剛體變換量（R，T），使得代價函數f（R，T）具有最小值

式中：pi——相機移動前測得的匹配點，qi——移動后測得的對應匹配點。R——旋轉矩陣，T——平移向量。

根據剛體變換計算得到當前位姿為

則根據狀態向量寫出觀測模型為

其中，hk＝I，vk為噪聲，滿足高斯白噪聲分布，vk～N（0，Rv（v otrust）），噪聲協方差Rv（v otrust）［8］的值取決于攝像機掃描到的數據的可信度，由特征點數、RANSAC算法計算的內點數所占比例來確定，可信度越小，噪聲協方差值越大。

2.4 基于IMU－Kinect的算法框架

本文提出的改進算法框架如圖4所示，在ICP 點云匹配后，將IMU 的預測模型考慮進去，進行位姿融合，得到定位的結果。

圖4 IMU－Kinect的算法框架

機器人的運動指令包含速度和角速度信息，由于三輪全向機器人運行過程中存在漂移，且3個輪子受到的阻力未知，未知的阻力可能會極大的改變運動的方向，因此運動指令內容的參考意義不大。本文主要利用運動指令判斷機器人是否靜止，將運動指令和運動限制 （機器人線速度＜0.6m／s）作為約束條件，構建觀測方程。

在進行位姿融合時有如下考慮：

（1）指令為0時，不做濾波處理，位姿信息保持，Kinect測量數據舍棄，對IMU 的加速度偏移量按式 （9）進行修正。

（2）指令不為0，當Kinect特征點豐富時，進行位姿融合處理。

（3）指令不為0，當Kinect特征點小于閾值時，舍棄Kinect數據，采用IMU 進行位姿估計。

位姿融合包含兩個部分：姿態和位移。對姿態的融合為：當Kinect與IMU 姿態測量相差小于閾值時，將二者的數據進行1：1的加權平均；當二者的姿態測量大于閾值時，將二者數據進行擴展卡爾曼濾波。對位移的融合為：根據預估模型方程（8）和觀測方程（12），進行EKF融合。

融合算法的具體流程如圖5所示。

3 實驗結果及分析

3.1 軟件設計

本文軟件的設計是基于ROS系統。ROS是一款開源的機器人軟件操作平臺，起源于斯坦福大學和Willow Garage公司合作的機器人項目。它提供類似操作系統所提供的功能，同時也提供一些工具程序和庫用于獲取、建立、編寫和運行多機整合的程序。ROS的軟件是以功能包的方式組織起來，每個功能包提供節點、消息、服務以及庫和其他工具。每個節點相當于一個執行運算任務的進程，節點與節點之間的通信則是采用主題或是服務的形式，如通過發布主題，將消息廣播出去，通過訂閱主題獲取其它節點的消息。

本文的算法是對RGBD－SLAM 算法定位部分的改進，該算法僅僅使用彩色 （RGB）和深度 （Depth）信息進行定位與制圖。Endres在ROS上創建了該算法的功能包，它提供了一個rgbdslam 節點。該節點訂閱了kinect相關的主題，通過計算機視覺計算，最終給出相機的運動軌跡和3D地圖。

本文在該算法的基礎上添加了慣性傳感的信息和位姿的融合，對應的功能包和節點信息見表1。

表1 ROS下的功能包

表1中imosen功能包涉及本文機器人在ROS 下的驅動，imu功能包涉及razorIMU 的驅動，openni系列功能包涉及kinect的驅動。telekey節點接收遙控信息并發布cmd＿vel主題，即速度、加速度指令；node.py接收慣性傳感器的數據，并發布imu主題，即姿態角、9軸數據；control節點訂閱cmd＿vel、imu主題并發布odom 主題，即里程信息，同時通過藍牙發送指令控制機器人運行；openni相關節點發布rgb／depth相關的主題；rgbdslam 節點接收odom、rgb／depth主題，在原算法基礎上，對位姿進行EKF融合，輸出位姿和3D 地圖。

3.2 實驗結果和分析

本文實驗環境為一個約4＊5m 的實驗室，內有辦公桌椅，部分區域為白墻。設定機器人的線速度約0.2m／s，機器人的運行由上位機控制。機器人的初始位置是 （0，0）。

（1）折線運動

圖6為兩種算法在折線運動中定位結果的比較。機器人的運行路線為沿Y 軸運動一段距離，之后做一段折線運動，然后再沿X 軸運動。折線區域的特點是，由于Kinect離障礙物很近，特征點急劇減少，甚至降為0。圖6 （a）為兩種算法下位移的對比，其中waypoint為事先測定好的路徑關鍵點。從圖中可以看出，在折線區域，由于特征點不足，RGBD 算法定位失效，沒有捕捉到這部分的軌跡，而IMU－Kinect算法利用IMU 數據參與運算，能夠捕捉到折線運動。此外從圖中可以看到改進的算法比RGB－D 算法定位追蹤的效果更好。圖6 （b）為姿態角的對比，機器人的初始姿態角為0 度，停止點的姿態角為－90 度，即－1.57rad，在折線區域沒有改變姿態角的大小，仍為0 度。從圖中可以看出兩種算法都能捕捉到角度的改變，但是改進的算法捕捉的細節更加完善，得到的結果也更加準確。

圖6 折線運動下Kinect與改進算法定位結果的比較

（2）橢圓運動

圖7為兩種算法在橢圓運動中定位結果的比較。機器人的運行路線為一段橢圓的運動。從圖7 （a）的右上角可以看出兩種算法都能回歸到起點，RGBD 算法比改進的算法得到的軌跡更加平滑，但改進的算法比RGB－D 算法更貼近于路徑關鍵點。圖7 （b）為姿態角的對比，橢圓運動中機器人初始姿態角為0度，隨著旋轉慢慢運動到－180度，再到－360度。從圖中可以看到兩種算法都能捕捉到角度的改變，具有收斂性。

圖7 橢圓運動下Kinect與改進算法定位結果的比較

4 結束語

本文提出了一種以RGBD－SLAM 算法為基礎的改進的定位與構圖算法，結合九軸慣性傳感器IMU 對姿態的解算和對加速度的測量，對Kinect位姿估計的結果進行EKF數據融合，實現最優運動估計。通過實驗對以上兩種算法進行了比較和分析，實驗結果表明相對于傳統的RGBDSLAM 算法，本文提出的算法提高了計算效率和定位精度，能更準確的追蹤機器人的位姿。

［1］Henry P，Krainin M，Herbst E，et al.RGB－D mapping：U－sing Kinect－style depth cameras for dense 3Dmodeling of indoor environments ［C］／／International Journal of Robotics Research，2012，31 （5）：647－663.

［2］Felix Endres，Jürgen Hess，Daniel Cremers，et al.An evaluation of the RGB－D SLAM system ［C］／／In Proc of the IEEE Int Conf on Robotics and Automation，2012.

［3］Kerl C，Sturm J，Cremers D.Robust odometry estimation for RGB－D cameras［C］／／In Proc of the IEEE Int Conf on Robotics and Automation，2013.

［4］Audras C，Comport AI，Meilland M，et al.Real－time dense appearance－based SLAM for RGB－D sensors ［C］／／In Proc Australian Conf Robotics and Automation，2011.

［5］Stuckler J，Behnke S.Model learning and real－time tracking using multi－resolution surfel maps［C］／／In AAAI Conf on Artificial Intelligence，2012.

［6］Bouvrie BD.Improving RGBD indoor mapping with IMU data［D］.Holland：Delft University of Technology，2011.

［7］Fredrik Hjelmare，Jonas Rangjs.Simultaneous localization and mapping using a Kinect in a sparse feature indoor environment［D］.Sweden：Linkping University，2012.

［8］Thibault Hervier，Silvere Bonnabel，Francois Goulette.Accurate 3D maps from depth images and motion sensors via nonlinear Kalman filtering ［C］／／In Proc of the IEEE／RSJ Int Conf on Intelligent Robots and Systems，2012.

［9］Sparkfun Electronics.9 degrees of freedom－razor imu ［EB／OL］.https：／／www.sparkfun.com／products／10736，2014.

［10］Starlino Electronics.DCM tutorial－An introduction to orientation kinematics ［EB／OL］. http：／／www.starlino.com／dcm＿tutorial.html，2011.

［11］Tseng SP，Li WL，Sheng CY，et al.Motion and attitude estimation using inertial measurements with complementary filter［C］／／In Proceedings of 8th Asian Control Conference，2011：863－868.