閔華松，楊 杰

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

0 引 言

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

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

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

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

1 三輪全向機(jī)器人的系統(tǒng)平臺和運(yùn)動模型

1.1 系統(tǒng)平臺

本文搭建的實(shí)驗(yàn)驗(yàn)證平臺為三輪全向移動機(jī)器人，其實(shí)物圖和系統(tǒng)結(jié)構(gòu)圖如圖1、圖2所示。

圖1 移動機(jī)器人實(shí)物

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

圖2 移動機(jī)器人系統(tǒng)結(jié)構(gòu)

1.2 三輪全向機(jī)器人的運(yùn)動學(xué)方程

三輪全向機(jī)器人在平面上可以獨(dú)立控制機(jī)器人對應(yīng)重心的平移與旋轉(zhuǎn)，并且具有全向移動的優(yōu)勢，能夠同時(shí)進(jìn)行旋轉(zhuǎn)和平移運(yùn)動。其運(yùn)動學(xué)模型如圖3所示。

圖3 三輪全向輪的運(yùn)動學(xué)模型

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

則，三輪全向機(jī)器人的運(yùn)動學(xué)方程為

其中

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

2 基于IMU－Kinect的定位算法

2.1 坐標(biāo)系變換

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

首先給定全局坐標(biāo)系，在系統(tǒng)靜止的時(shí)候，分別測量機(jī)器人、IMU、Kinect坐標(biāo)系與全局坐標(biāo)系的歐拉角（φ，θ，ψ），其中IMU 的歐拉角可以通過讀取傳感器數(shù)據(jù)進(jìn)行姿態(tài)估計(jì)來計(jì)算。則各坐標(biāo)系之間的轉(zhuǎn)換矩陣Cxy通過式（3）得到

2.2 IMU 位姿估計(jì)

（1）姿態(tài)解算

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

DCM 算法先對全局坐標(biāo)系和IMU 載體坐標(biāo)系構(gòu)建方向余弦矩陣，然后將該矩陣求導(dǎo)，得到方向余弦矩陣的微分方程，通過解微分方程，得到矩陣變量，最后轉(zhuǎn)化成歐拉角，即為所求的姿態(tài)角。具體描述如下

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

則有微分矩陣

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

（2）位移估計(jì)

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

采用加速度計(jì)構(gòu)建測量模型，狀態(tài)向量x ＝（x，y，vx，vy，anx，any），因?yàn)闄C(jī)器人的運(yùn)動是在平面上運(yùn)動，故z 軸位移和速度、加速度不參與計(jì)算。anx，any是加速度的偏移量，包括重力加速度的分量和偏置量。

對加速度的測量值a進(jìn)行分解可得

式中：ag——全局坐標(biāo)系的加速度分解量，aI——加速度計(jì)的測得量，CIg——IMU 的坐標(biāo)系到全局坐標(biāo)系的轉(zhuǎn)換矩陣，該矩陣隨著IMU 的姿態(tài)而變化，受IMU 姿態(tài)解算的影響。wa——噪聲。

則構(gòu)建觀測模型

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

2.3 Kinect位姿估計(jì)

使用Kinect進(jìn)行定位與三維構(gòu)圖的算法，即RGBDSLAM，包含如下幾個步驟：

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

（2）初始運(yùn)動估計(jì)：根據(jù)特征匹配的結(jié)果求出相機(jī)的運(yùn)動參數(shù)，即采用RANSAC算法，通過迭代，得到一個最優(yōu)的初始位姿估計(jì)值，同時(shí)有效的去除誤匹配的外點(diǎn)。

（3）位姿優(yōu)化：通過點(diǎn)云數(shù)據(jù)的配準(zhǔn)，改善初始位姿估計(jì)的效果。算法中采用的是廣義的ICP （GICP）算法。

（4）圖優(yōu)化：為得到全局一致的地圖，需要進(jìn)行圖優(yōu)化。算法中將得到的位姿作為節(jié)點(diǎn)，和鄰近幀之間的位姿關(guān)系作為邊，采用Hog－man算法進(jìn)行優(yōu)化。

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

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

式中：pi——相機(jī)移動前測得的匹配點(diǎn)，qi——移動后測得的對應(yīng)匹配點(diǎn)。R——旋轉(zhuǎn)矩陣，T——平移向量。

根據(jù)剛體變換計(jì)算得到當(dāng)前位姿為

則根據(jù)狀態(tài)向量寫出觀測模型為

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

2.4 基于IMU－Kinect的算法框架

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

圖4 IMU－Kinect的算法框架

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

在進(jìn)行位姿融合時(shí)有如下考慮：

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

（2）指令不為0，當(dāng)Kinect特征點(diǎn)豐富時(shí)，進(jìn)行位姿融合處理。

（3）指令不為0，當(dāng)Kinect特征點(diǎn)小于閾值時(shí)，舍棄Kinect數(shù)據(jù)，采用IMU 進(jìn)行位姿估計(jì)。

位姿融合包含兩個部分：姿態(tài)和位移。對姿態(tài)的融合為：當(dāng)Kinect與IMU 姿態(tài)測量相差小于閾值時(shí)，將二者的數(shù)據(jù)進(jìn)行1：1的加權(quán)平均；當(dāng)二者的姿態(tài)測量大于閾值時(shí)，將二者數(shù)據(jù)進(jìn)行擴(kuò)展卡爾曼濾波。對位移的融合為：根據(jù)預(yù)估模型方程（8）和觀測方程（12），進(jìn)行EKF融合。

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

3 實(shí)驗(yàn)結(jié)果及分析

3.1 軟件設(shè)計(jì)

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

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

本文在該算法的基礎(chǔ)上添加了慣性傳感的信息和位姿的融合，對應(yīng)的功能包和節(jié)點(diǎn)信息見表1。

表1 ROS下的功能包

表1中imosen功能包涉及本文機(jī)器人在ROS 下的驅(qū)動，imu功能包涉及razorIMU 的驅(qū)動，openni系列功能包涉及kinect的驅(qū)動。telekey節(jié)點(diǎn)接收遙控信息并發(fā)布cmd＿vel主題，即速度、加速度指令；node.py接收慣性傳感器的數(shù)據(jù)，并發(fā)布imu主題，即姿態(tài)角、9軸數(shù)據(jù)；control節(jié)點(diǎn)訂閱cmd＿vel、imu主題并發(fā)布o(jì)dom 主題，即里程信息，同時(shí)通過藍(lán)牙發(fā)送指令控制機(jī)器人運(yùn)行；openni相關(guān)節(jié)點(diǎn)發(fā)布rgb／depth相關(guān)的主題；rgbdslam 節(jié)點(diǎn)接收odom、rgb／depth主題，在原算法基礎(chǔ)上，對位姿進(jìn)行EKF融合，輸出位姿和3D 地圖。

3.2 實(shí)驗(yàn)結(jié)果和分析

本文實(shí)驗(yàn)環(huán)境為一個約4＊5m 的實(shí)驗(yàn)室，內(nèi)有辦公桌椅，部分區(qū)域?yàn)榘讐?。設(shè)定機(jī)器人的線速度約0.2m／s，機(jī)器人的運(yùn)行由上位機(jī)控制。機(jī)器人的初始位置是 （0，0）。

（1）折線運(yùn)動

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

圖6 折線運(yùn)動下Kinect與改進(jìn)算法定位結(jié)果的比較

（2）橢圓運(yùn)動

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

圖7 橢圓運(yùn)動下Kinect與改進(jìn)算法定位結(jié)果的比較

4 結(jié)束語

本文提出了一種以RGBD－SLAM 算法為基礎(chǔ)的改進(jìn)的定位與構(gòu)圖算法，結(jié)合九軸慣性傳感器IMU 對姿態(tài)的解算和對加速度的測量，對Kinect位姿估計(jì)的結(jié)果進(jìn)行EKF數(shù)據(jù)融合，實(shí)現(xiàn)最優(yōu)運(yùn)動估計(jì)。通過實(shí)驗(yàn)對以上兩種算法進(jìn)行了比較和分析，實(shí)驗(yàn)結(jié)果表明相對于傳統(tǒng)的RGBDSLAM 算法，本文提出的算法提高了計(jì)算效率和定位精度，能更準(zhǔn)確的追蹤機(jī)器人的位姿。

［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，F(xiàn)rancois 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.