IMU松耦合的KinectFusion算法改進(jìn)

應(yīng)宏鐘，高宏力，宋興國，楊林志

（西南交通大學(xué)，四川 成都 610031）

1 引言

在未知環(huán)境中實現(xiàn)移動機器人的增量式地圖構(gòu)建（Mapping）與實時定位（Localization），是移動機器人智能化不可缺少的重要核心技術(shù)，這兩項技術(shù)被統(tǒng)稱為同時定位與地圖構(gòu)建SLAM（Simultaneous Localization and Mapping），是移動機器人領(lǐng)域熱門的研究方向之一。

SLAM最早在1987年由R.Smith等人提出，創(chuàng)造了SLAM的第一個分類—基于擴展卡爾曼濾波器的EKF-SLAM[1]。而后文獻(xiàn)[2]使用超聲波傳感器構(gòu)建了第一套移動機器人SLAM系統(tǒng)。而后隨著激光雷達(dá)的出現(xiàn)，SLAM系統(tǒng)的實用性得到了極大的提升，文獻(xiàn)[3]利用激光雷達(dá)制作了一套十分優(yōu)秀的無人機室內(nèi)導(dǎo)航SLAM系統(tǒng)，但是具有較高的使用成本。

近年來，隨著計算機性能的快速提升，基于機器視覺的VSLAM獲得了巨大的成功，目前主要有三類V-SLAM：單目相機SLAM（Mono SLAM）、雙目相機SLAM（Stereo Vision SLAM）以及RGB-D SLAM。單目相機SLAM只需要單個攝像頭就能實現(xiàn)成本極低，但是只能獲取相對深度，然后在相似變換空間中求解，不能在傳統(tǒng)的歐式空間中求解，存在尺度漂移問題[4]。雙目相機SLAM可以通過多個相機估計點的位置，且不受運動的影響，但是帶來的是較大的計算量，實時性能較差。RGB-D SLAM是通過RGB-D相機實現(xiàn)的，相機在采集RGB信息的同時通過Time-of-Flight原理或者結(jié)構(gòu)光直接獲取各個像素點的深度信息，以較低的成本和較小的計算量獲取環(huán)境信息，其中較為經(jīng)典的是KinectFusion[5]算法。該算法通過Kinect深度攝像頭獲取RGB圖像和深度數(shù)據(jù)，調(diào)用CUDA對數(shù)據(jù)進(jìn)行多線程處理，具有較高的是實時性，并且首次實現(xiàn)了實時稠密的三維地圖重建與自身定位。但是存在ICP（Iterative Closest Point）定位失效的問題，容易丟失攝像頭的位姿變換信息。通過加入慣性傳感器IMU來獲取攝像頭的運動狀態(tài)，解決ICP定位失效的問題，提升算法的魯棒性和定位精度。

2 KinectFusion算法描述

2.1 Kinect攝像頭

Kinect攝像頭主要由紅外激光投射器、紅外相機、RGB相機組成，其采用的深度獲取原理Light Coding為：由紅外激光投射器投射出經(jīng)過“體編碼”的激光散斑（Laser Speckle），散斑在不同距離有著不同的結(jié)構(gòu)形式，投射在物體表面時形成獨特唯一的形狀結(jié)構(gòu)，再由紅外相機捕捉這些物體表面的光斑結(jié)構(gòu)，解算出深度信息。

Kinect攝像頭可以獲取深度范圍為（0.4～3.5）m，水平檢測范圍 57.5°，垂直檢測范圍 43.5°，3m 處的距離誤差為（±3.5）cm，分辨率為3cm[6]。

2.2 KinectFusion算法

KinectFusion算法的基本流程，如圖1所示。

圖1 KinectFusion算法基本流程Fig.1 Flow Chart of KinectFusion Algorithm

主要包括四個部分：

（1）深度數(shù)據(jù)處理：使用雙邊濾波（Bilateral Filtering）對原始深度數(shù)據(jù)進(jìn)行處理，在保留邊緣的同時對邊緣進(jìn)行平滑。用深度圖像幀中每個像素點u=（x，y）的深度值Di（u）以及相機內(nèi)部矩陣K來計算頂點向量Pi（u），再通過Pi（u）計算每個點的法向量ni（u），將深度圖像中帶深度值的二維像素點轉(zhuǎn)化為以相機為原點坐標(biāo)系中的三維點；

（2）相機跟蹤：使用 ICP（Iterative Closest Point）算法，對相機進(jìn)行跟蹤，通過當(dāng)前幀的點云與已有模型點云之間的匹配，求解出相機的相對位移與轉(zhuǎn)角；

（3）體積融合：使用 TSDF（Truncated Signed Distance Function）方法實現(xiàn)體積融合，將得到的點云數(shù)據(jù)融合到網(wǎng)格模型中。在網(wǎng)格模型中共有n3個柵格單元被稱為體素，每個體素中存有一個介于［-1，1］之間的值，該值表示了體素到測量面的距離，當(dāng)值為正時表示體素在測量面的后面即在物體內(nèi)部，反之當(dāng)值為負(fù)時表示體素在測量面的前面，即物體外部。每當(dāng)有新的數(shù)據(jù)需要融合進(jìn)網(wǎng)格模型時，通過式（2）對網(wǎng)格模型進(jìn)行更新，當(dāng)場景變化快或者有新場景出現(xiàn)時會給出較大的權(quán)值，以快速的更新網(wǎng)格模型，當(dāng)場景較為穩(wěn)定時會給出較小的權(quán)值，來減小噪聲對模型的影響。

式中：TSDFavg—加權(quán)融合后的網(wǎng)格模型；TSDFi-1—原有的網(wǎng)格模型；TSDFi—新增的網(wǎng)格模型；wi—權(quán)值。

（4）地圖渲染：使用光線投影算法對網(wǎng)格模型進(jìn)行渲染，將模型可視化的同時，渲染出的模型點云可以用于下一幀輸入深度圖的ICP配準(zhǔn)，就此完成一個循環(huán)。

3 IMU松耦合的ICP算法

3.1 ICP算法

在KinectFusion中點云與已有模型之間的匹配是一個3D-3D的位姿估計問題，采用投影法來計算每幀畫面與模型之間匹配點的位置變化。在相機坐標(biāo)系中Ps（u）為K時刻待匹配的原始點云，Pd（u）為K-1時刻融合成的TSDF模型上的目標(biāo)點云，ni（u）為目標(biāo)點云Pd（u）的單位法向量，在ICP算法中使用如下目標(biāo)函數(shù)求解位姿變換，如圖2所示。

上式構(gòu)建了一個最小二乘問題，當(dāng)目標(biāo)函數(shù)E取得最小值時，式中的矩陣M即為所求的位姿變換矩陣，且矩陣M由一個旋轉(zhuǎn)矩陣 R（α，β，γ）和一個平移矩陣 T（tx，ty，tz）組成。

圖2 模型與點云匹配示意圖Fig.2 Schematic Diagram of Model and Point Cloud Matching

在實際程序中，由于深度攝像頭采樣頻率為30幀每秒，兩幀之間的變化較小，所以可以通過線性化的方式來求解，做以下近似：sinθ≈θ，cosθ≈1，將位姿變換矩陣 M 簡化為M?=T（tx，ty，tz），原目標(biāo)函數(shù)就可以通過線性方程組（4）來求得最優(yōu)解。

在實際測試中，上述的ICP算法經(jīng)常會定位失效，當(dāng)Kinect相機快速移動時，K+1時刻的深度幀與K時刻已有模型之間的位置變化較大，能匹配的點較少，直接使用ICP進(jìn)行最近點迭代容易得到錯誤的位姿變換矩陣，甚至無法求得位姿變換矩陣。當(dāng)相機視野中大部分區(qū)域為平面時，有可能無法求得正確的與平面平行的兩個坐標(biāo)軸方向上的位姿變換，造成建模中斷或者建模錯亂。目前已有的解決方法有通過邊線關(guān)系的ICP算法改進(jìn)[8]，結(jié)合RGB信息進(jìn)行位姿估計[9-10]等。

3.2 IMU松耦合的ICP算法改進(jìn)

IMU慣性傳感器能夠測得角加速度和加速度且具有較高的采樣頻率，當(dāng)相機運動過快或者場景中存在大平面區(qū)域?qū)е翴CP算法失效時，可以通過IMU采集到的數(shù)據(jù)進(jìn)行位姿估計，彌補純視覺SLAM產(chǎn)生的缺陷。

IMU慣性傳感器由三軸陀螺儀、三軸加速度計和三軸磁力計組成，在測量時能直接測得角速度、線加速度和磁偏角。理論上對角速度進(jìn)行積分就能獲取轉(zhuǎn)過的角度，對加速度進(jìn)行二次積分就能獲取位移，但實際使用中傳感器存在漂移、噪聲和測量誤差，需要使用互補濾波（CF）、擴展卡爾曼濾波（EKF）等算法，對采集到的數(shù)據(jù)進(jìn)行濾波和數(shù)據(jù)融合，減小數(shù)據(jù)噪聲和測量誤差造成的影響，提升傳感器精度。

3.2.1 計算姿態(tài)四元數(shù)與位移估計

通過較為成熟的Madgwick算法[7]來實現(xiàn)姿態(tài)估計，該算法屬于互補濾波算法，使用四元數(shù)微分方程和測得的角速度來求解姿態(tài)，然后利用地磁計和加速度計通過梯度下降算法進(jìn)行補償。在位移估計時，利用已經(jīng)求解的姿態(tài)將三軸加速度中的重力加速度分量去除，然后進(jìn)行二次積分得到位移向量。由于長時間內(nèi)的位移估計存在極大的累積誤差并且難以消除，所以只使用短時間的位移估計。將所用IMU的采樣頻率設(shè)置為150Hz，相機獲取深度幀的頻率為30Hz，每五次IMU采樣時間等同于一次深度攝像頭的采樣時間，具體描述如下：

式中：qest，t—t時刻的姿態(tài)四元數(shù)；q?est，t-1—t-1 時刻的姿態(tài)四元數(shù)；q˙ω，t—由測得的角速度計算得到的姿態(tài)四元數(shù)的變化率；β—陀螺儀的測量誤差；fg，m（q?，a?，m?）—加速度和磁偏角相關(guān)的多元向量誤差函數(shù)；Δt—IMU的采樣時間間隔；at—測得的三軸加速度向量；Rq，t—t時刻求得的由姿態(tài)四元數(shù)表示的旋轉(zhuǎn)矩陣；g—重力加速度向量；TK—五個采樣周期內(nèi)的位移向量。

3.2.2 坐標(biāo)系變換

IMU傳感器與Kinect相機的相對位置固定不變，存在一個固定不變的坐標(biāo)系變換矩陣，將IMU測得的位姿變換轉(zhuǎn)化到相機上。公式如下：

3.2.3 數(shù)據(jù)融合

使用最優(yōu)加權(quán)融合對ICP算法和IMU測量得到的位姿變換矩陣進(jìn)行融合，記由IMU得到的位姿估計方差為，ICP 算法計算得到的位姿估計方差為，對應(yīng)的加權(quán)系數(shù)分別為w1和w2，為使融合后的方差最小，構(gòu)造輔助函數(shù)如下：

4 實驗

在室內(nèi)場景中對所述算法進(jìn)行實際測試，所用的實驗設(shè)備，如圖3所示。由一個Kinect攝像頭和一塊9軸IMU傳感器組成。所用程序在PCL開源庫的基礎(chǔ)上進(jìn)行開發(fā)，運行環(huán)境為ubuntu14.04操作系統(tǒng)，Intel i7 4790K CPU，16G 內(nèi)存，NVIDIA GTX970顯卡4G顯存。

圖3 攝像頭和IMU實物Fig.3 Camera and IMU Suit

實際實驗所使用的參數(shù)，如表1所示。在實際實驗中IMU所得位姿變換矩陣的標(biāo)準(zhǔn)差由IMU傳感器的標(biāo)準(zhǔn)差來近似，取σ1=0.0992.σ2用 σ2=σcamek（Δθ/θ+ΔT/T）計算得出，其中 σcam為 Kinect深度傳感器的初始標(biāo)準(zhǔn)差參考文獻(xiàn)[6]取σcam=0.0233，k為自行給定的放大系數(shù)，取k=20，Δθ和ΔT為ICP算法與IMU求解出的三軸轉(zhuǎn)動角度和位移的差值，θ和T分別為IMU求解出的三軸轉(zhuǎn)動角度和位移。

當(dāng)ICP算法和IMU求解的位姿相近時，以ICP算法求解的位姿為主，減小IMU漂移產(chǎn)生的誤差，如圖4所示。當(dāng)快速移動或者場景匹配像素點較少時，ICP算法誤差增大甚至失效，兩個算法得到的位姿估計差值增大，降低ICP算法求解結(jié)果的加權(quán)系數(shù)，當(dāng)差值超過20%后，舍棄ICP求解的結(jié)果。

表1 實驗參數(shù)Tab.1 Experiment Parameters

圖4 標(biāo)準(zhǔn)差σFig.4 Standard Deviation σ

改進(jìn)后算法對室內(nèi)場景的重建結(jié)果，如圖5所示。原算法運行結(jié)果，如圖6所示。當(dāng)相機視角從下往上移動通過平整墻面時，改進(jìn)后算法順利通過并完成了對部分天花板的地圖重構(gòu)，沒有明顯形變；原算法在通過平整墻面時由于大部分區(qū)域為平面，求解出了錯誤的位姿估計，最后地圖重建和定位都發(fā)生了錯亂。

圖5 改進(jìn)算法重建結(jié)果Fig.5 Map of Improved Algorithm

圖6 原算法重建結(jié)果Fig.6 Map of Original Algorithm

5 結(jié)論

使用慣性測量傳感器IMU對較為經(jīng)典的實時稠密RGB-D SLAM算法—KinectFusion算法做了改進(jìn)。使用IMU與ICP算法松耦合實現(xiàn)最終的位姿估計，解決了快速移動或有效配準(zhǔn)點稀少時造成的ICP算法失效問題。通過實驗證明了，改進(jìn)算法在魯棒性優(yōu)于原有算法的情況下，解決了原有算法存在的問題，且沒有明顯的實時性問題，能順利的實時完成地圖重構(gòu)與定位。