基于磁力計(jì)、IMU和單目視覺的自主定位方法

樊建彪，孫 劍，樊歡歡,陳 偉

(1.西安交通大學(xué)航天航空學(xué)院，西安 710049；2.西安交通大學(xué)機(jī)械結(jié)構(gòu)強(qiáng)度與振動(dòng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安 710049)

0 引言

近年來，隨著社會(huì)發(fā)展和科技技術(shù)的進(jìn)步，移動(dòng)機(jī)器人越來越多地應(yīng)用于日常生活中。導(dǎo)航技術(shù)不僅是移動(dòng)機(jī)器人的核心技術(shù)，同時(shí)也是移動(dòng)機(jī)器人自主完成任務(wù)的基礎(chǔ)。目前移動(dòng)機(jī)器人的導(dǎo)航系統(tǒng)主要依靠GPS(全球定位系統(tǒng))與 IMU進(jìn)行融合解算位姿信息，而對(duì)于復(fù)雜環(huán)境此方案無法保證GPS信號(hào)的實(shí)時(shí)性，同時(shí)IMU模塊的零偏會(huì)造成解算的位姿漂移，因此應(yīng)用場(chǎng)景有限。

SLAM(simultaneous localization and mapping)技術(shù)依靠移動(dòng)機(jī)器人自身攜帶的傳感器對(duì)周圍環(huán)境進(jìn)行觀測(cè)，同時(shí)通過融合算法對(duì)各傳感器的數(shù)據(jù)進(jìn)行處理，可以實(shí)時(shí)得到移動(dòng)機(jī)器人在空間中的位姿，不依賴于外部傳感器信息，是實(shí)現(xiàn)機(jī)器人在復(fù)雜環(huán)境下自主定位導(dǎo)航的關(guān)鍵技術(shù)。SLAM技術(shù)于20世紀(jì)80年代被提出，經(jīng)過幾十年的發(fā)展，已經(jīng)產(chǎn)生多種成熟的SLAM算法。

目前主要的SLAM算法包括單目視覺SLAM[1-5]、雙目視覺SLAM[6-8]、RGB-D視覺SLAM[9]和激光雷達(dá)SLAM[10]。單目視覺SLAM系統(tǒng)僅需一臺(tái)攝像頭和一套低成本IMU，其體積小、成本低、功耗小等優(yōu)點(diǎn)引起了學(xué)術(shù)界和工業(yè)界的極大興趣。自從基于擴(kuò)展卡爾曼濾波的Mono-SLAM[11]算法被提出以后，單目視覺逐漸流行起來。目前有眾多的單目視覺解決方案，包括PTAM[12]、SVO[13]、ORB-SLAM2[14]、MSCKF[15]和VINS-SLAM[16]等。這些方案主要是基于濾波器或者優(yōu)化器來融合IMU和視覺傳感器數(shù)據(jù)對(duì)位姿進(jìn)行解算。而目前的單目視覺SLAM算法主要是依靠IMU數(shù)據(jù)來解決單目視覺解算過程中的尺度問題，但I(xiàn)MU存在漂移，其尺度解算精度不高。同時(shí)單目視覺SLAM算法主要依靠單目視覺數(shù)據(jù)進(jìn)行優(yōu)化得到位姿信息，但是當(dāng)運(yùn)動(dòng)為純旋轉(zhuǎn)時(shí)，則無法通過求解對(duì)極幾何來求解機(jī)器人的姿態(tài)角。該算法在實(shí)際應(yīng)用中存在位姿解算漂移的問題，同時(shí)解算精度不高。

通過將三維磁阻傳感器按照載體坐標(biāo)系安裝，磁力計(jì)可測(cè)量載體空間磁場(chǎng)的三維磁感應(yīng)強(qiáng)度，據(jù)此可以計(jì)算出載體的姿態(tài)角。因此本文首次提出了基于磁力計(jì)、IMU和單目視覺傳感器融合的位姿解算方法，用來對(duì)視覺尺度因子、重力、速度、IMU參數(shù)和載體姿態(tài)進(jìn)行估計(jì)，解決單目視覺在解算位姿時(shí)由于純旋轉(zhuǎn)情況而出現(xiàn)的不穩(wěn)定情況，同時(shí)本方案具有高精確和強(qiáng)魯棒的特性。

1 算法設(shè)計(jì)

本文通過融合磁力計(jì)、IMU和單目視覺傳感器數(shù)據(jù)，設(shè)計(jì)能夠在三維空間中實(shí)時(shí)估計(jì)移動(dòng)機(jī)器人位姿的算法。所設(shè)計(jì)的擴(kuò)展卡爾曼濾波器將磁力計(jì)和IMU數(shù)據(jù)進(jìn)行融合，解算出機(jī)器人的姿態(tài)并對(duì)陀螺儀零偏進(jìn)行了估計(jì)，之后用陀螺儀的零偏估計(jì)量修正陀螺儀數(shù)據(jù)，同時(shí)將估計(jì)的姿態(tài)量用于加速度計(jì)的積分，從而得到機(jī)器人的位置量。通過單目視覺傳感器解算對(duì)極幾何和PnP計(jì)算位置信息。通過圖優(yōu)化算法融合IMU計(jì)算的位置殘差和單目視覺計(jì)算的殘差，得到移動(dòng)機(jī)器人的位置狀態(tài)和加速度計(jì)的零偏量。用計(jì)算得到的加速度計(jì)的零偏修正加速度計(jì)數(shù)據(jù)。至此就可以估計(jì)出移動(dòng)機(jī)器人的位置和姿態(tài)狀態(tài)。算法流程如圖1所示。

圖1 算法流程圖

圖1中，bw和ba分別表示通過算法估計(jì)出來的陀螺儀零偏量和加速度計(jì)零偏量。通過對(duì)IMU中各傳感器的零偏進(jìn)行修正，可以得到更加精確的位姿信息并提高系統(tǒng)的魯棒性。

1.1 磁力計(jì)與IMU卡爾曼姿態(tài)估計(jì)器

1.1.1 IMU傳感器特性

IMU傳感器包括三軸陀螺儀和三軸加速度計(jì)，可以測(cè)量高頻的角速度和線加速度數(shù)據(jù)。但是，IMU的測(cè)量數(shù)據(jù)中包含兩種測(cè)量誤差，即測(cè)量噪聲和零偏。測(cè)量噪聲是高斯白噪聲，零偏服從高斯隨機(jī)游走。因此IMU的測(cè)量模型如式(1)：

(1)

1.1.2 基于卡爾曼濾波器的姿態(tài)解算算法

建立基于非線性卡爾曼濾波器的姿態(tài)解算算法，其狀態(tài)方程和量測(cè)方程[17]如下

Z=h(X)+V

(2)

式中，狀態(tài)向量X=[q0，q1，q2，q3，bwx，bwy，bwz]，分別表示四元數(shù)和陀螺儀的偏差，W為系統(tǒng)噪聲，服從高斯分布；V為測(cè)量噪聲，服從高斯分布，四元數(shù)微分方程為

對(duì)上式進(jìn)行離散化可以得到如下狀態(tài)轉(zhuǎn)移方程

XK+1=AKXK

(3)

式中

AK=(I+TF)=I+

XK(i)=qi,i=0,1,2,3。T為IMU傳感器數(shù)據(jù)采集時(shí)間間隔。

對(duì)于加速度的測(cè)量數(shù)據(jù)可以通過方向余弦矩陣進(jìn)行估計(jì)，其中假設(shè)重力加速度為常數(shù)|g|，可以根據(jù)式(4)得到加速度計(jì)在載體坐標(biāo)下的測(cè)量方程。

(4)

式中

(5)

通過視覺和IMU優(yōu)化模塊中式(17)可以得到優(yōu)化后加速度計(jì)的零偏估計(jì)并對(duì)加速度計(jì)進(jìn)行修正，因此修正后的測(cè)量值為

磁力計(jì)的測(cè)量方程與加速度計(jì)測(cè)量方程相同。對(duì)磁場(chǎng)進(jìn)行歸一化，并假設(shè)其指向y軸。磁力計(jì)的測(cè)量方程見式(6)

(6)

根據(jù)式(2)～(6)所得到的狀態(tài)方程和測(cè)量方程進(jìn)行卡爾曼更新，求解后驗(yàn)狀態(tài)估計(jì)和后驗(yàn)狀態(tài)協(xié)方差矩陣見式(7)：

(7)

1.2 基于視覺傳感器和IMU的非線性圖優(yōu)化

1.2.1 IMU測(cè)量殘差

首先對(duì)IMU的測(cè)量數(shù)據(jù)進(jìn)行積分，為IMU的殘差計(jì)算提供約束關(guān)系。在給定的兩幀連續(xù)圖像bk和bk+1之間，可以通過加速度計(jì)、陀螺儀的測(cè)量數(shù)據(jù)和式(7)中解算的姿態(tài)角和陀螺儀偏差估計(jì)值計(jì)算bk+1幀時(shí)刻的位置，速度和姿態(tài)角信息，見式(8)[16]：

(8)

(9)

式中

(10)

對(duì)于式(10)采用歐拉法進(jìn)行離散處理求解積分值，同時(shí)進(jìn)行泰勒一階近似。就可以計(jì)算得到bk和bk+1之間的位移、速度和姿態(tài)角的增量，如式(11)所示：

(11)

對(duì)于攝像機(jī)視頻窗口內(nèi)兩個(gè)連續(xù)幀圖像bk和bk+1之間的IMU測(cè)量數(shù)據(jù)，根據(jù)式(9)～式(11)，IMU積分過程的測(cè)量殘差為[16]

(12)

1.2.2 視覺測(cè)量殘差

(13)

式中，s為待求的尺度因子，表示與實(shí)際移動(dòng)距離尺寸相關(guān)的縮放比例。

定義單目視覺在圖像平面上的重投影誤差，通過將第i幀圖像中觀測(cè)到的特征點(diǎn)n重投影到第j幀圖像中，則視覺測(cè)量殘差為

(14)

(15)

1.2.3 圖優(yōu)化狀態(tài)方程

定義需要優(yōu)化的狀態(tài)向量如下

(16)

由式(12)和(14)定義代價(jià)函數(shù)為

(17)

式中，B為所有IMU測(cè)量數(shù)據(jù)的集合，C為解算關(guān)鍵幀內(nèi)所有圖像特征點(diǎn)的集合。通過Ceres Solver優(yōu)化器，對(duì)式(17)求解最小二乘問題，就可以得到位置估計(jì)量、陀螺儀和加速度計(jì)零偏。至此，完成了由視覺和IMU傳感器求解位置的過程。

2 實(shí)驗(yàn)結(jié)果

2.1 實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)平臺(tái)采用optor慣性相機(jī)、Pixhawk飛控硬件和深圳玩智商科技有限公司生產(chǎn)的Dashgo D1移動(dòng)機(jī)器人平臺(tái)。optor慣性相機(jī)帶有一個(gè)MPU-6050模塊，集成了三軸陀螺儀和三軸加速度計(jì)，還帶有兩個(gè)30fps、分辨率為752×480的攝像頭。用Pixhawk飛控中的HMC5883L三軸磁力計(jì)采集地磁場(chǎng)數(shù)據(jù)。Dashgo D1移動(dòng)機(jī)器人的移動(dòng)精度低于1%，作為移動(dòng)平臺(tái)驗(yàn)證算法的移動(dòng)精度和旋轉(zhuǎn)精度。所有傳感器都通過USB連接到NUC機(jī)載電腦上，進(jìn)行數(shù)據(jù)采集，并在機(jī)載電腦上運(yùn)行算法進(jìn)行位姿估計(jì)，如圖1所示。

圖2 實(shí)驗(yàn)平臺(tái)

2.2 實(shí)驗(yàn)結(jié)果

通過Dashgo D1移動(dòng)機(jī)器人搭載實(shí)驗(yàn)設(shè)備在室內(nèi)分別沿著XY軸直線運(yùn)動(dòng)15m和繞z軸旋轉(zhuǎn)360°，分別運(yùn)行本文算法和VINS-SLAM算法，進(jìn)行實(shí)驗(yàn)并記錄實(shí)驗(yàn)數(shù)據(jù)。將本文算法解算結(jié)果和當(dāng)前解算精度高的VINS-SLAM算法解算結(jié)果進(jìn)行對(duì)比。

首先進(jìn)行沿XY軸直線運(yùn)動(dòng)的實(shí)驗(yàn)，使移動(dòng)機(jī)器人以0.05m/s的速度分別沿著XY軸直線勻速移動(dòng)15m。記錄并對(duì)比本文算法和VINS-SLAM算法解算結(jié)果的誤差。實(shí)驗(yàn)結(jié)果如圖3～圖6所示，橫軸表示時(shí)間，縱軸表示移動(dòng)距離。

圖3 X軸實(shí)驗(yàn)結(jié)果

圖4 X軸實(shí)驗(yàn)誤差

圖5 Y軸實(shí)驗(yàn)結(jié)果

圖6 Y軸實(shí)驗(yàn)誤差

將Dashgo D1移動(dòng)機(jī)器人繞z軸按2°/s勻速旋轉(zhuǎn)360°進(jìn)行Yaw姿態(tài)角解算實(shí)驗(yàn)。記錄并對(duì)比本文算法和VINS-SLAM算法解算結(jié)果的誤差，如圖7、圖8所示。

圖7 Yaw角實(shí)驗(yàn)結(jié)果

圖8 Yaw角實(shí)驗(yàn)誤差

從圖(8)可以看出，由于VINS-SLAM算法只用IMU和單目視覺進(jìn)行位姿解算，在純旋轉(zhuǎn)的情況下，當(dāng)旋轉(zhuǎn)180°后由于此時(shí)視覺傳感器無法解算對(duì)極幾何而解算失效，此時(shí)只依賴陀螺儀積分進(jìn)行姿態(tài)解算，就會(huì)出現(xiàn)姿態(tài)解算漂移。而本文算法主要依靠磁力計(jì)和IMU進(jìn)行姿態(tài)解算，有效避免了單目視覺SLAM算法中存在的姿態(tài)解算漂移的問題。

表1 實(shí)驗(yàn)誤差結(jié)果

通過表1對(duì)比本文算法與不使用磁力計(jì)的VINS-SLAM算法可以看出，本文算法通過使用磁力計(jì)使位置解算誤差降低到0.4m以內(nèi)。而解算的姿態(tài)角克服了VINS-SLAM算法中由于純旋轉(zhuǎn)情況而產(chǎn)生的漂移問題，并且解算精度在5°以內(nèi)。

3 結(jié)論

本文首次將磁力計(jì)、單目視覺和 IMU傳感器數(shù)據(jù)進(jìn)行融合，解算移動(dòng)機(jī)器人位姿狀態(tài)，并通過實(shí)驗(yàn)對(duì)設(shè)計(jì)的算法進(jìn)行驗(yàn)證，與VINS-SLAM單目視覺算法進(jìn)行對(duì)比。實(shí)驗(yàn)結(jié)果表明，所設(shè)計(jì)的基于磁力計(jì)、IMU和單目視覺的自主定位算法，不但提高了移動(dòng)機(jī)器人的位姿解算精度，同時(shí)也克服了現(xiàn)有的單目視覺SLAM算法在純旋轉(zhuǎn)情況下姿態(tài)角解算漂移的問題。本算法由于只使用磁力計(jì)、IMU和單目攝像頭，具有質(zhì)量小、成本低、消耗計(jì)算資源少等優(yōu)點(diǎn)，因此可廣泛應(yīng)用于小型移動(dòng)機(jī)器人的自主導(dǎo)航系統(tǒng)中。