孫 弋, 張雪麗

(西安科技大學(xué) 通信與信息工程學(xué)院, 西安 710054)

引言

機(jī)器人實(shí)現(xiàn)應(yīng)用的關(guān)鍵可概括為3 個(gè)核心問題:我在哪、我要去哪和如何去[1]的問題, 其中“我在哪”的定位問題是首要問題[2], 而且隨著機(jī)器人在礦井[3]、醫(yī)療[4]、服務(wù)[5]等領(lǐng)域的應(yīng)用, 定位精準(zhǔn)性的需求逐漸成為人們研究和關(guān)注的重點(diǎn)[6,7].由于單一傳感器的定位精度和可適用性有限, 目前, 機(jī)器人定位普遍采用將多種傳感器數(shù)據(jù)進(jìn)行融合的方式實(shí)現(xiàn)[8-11], 其中慣性測(cè)量單元(IMU)和相機(jī)(camera)存在互補(bǔ)性[12], 因此IMU和Camera 傳感器的數(shù)據(jù)融合方式受到了更多青睞.

IMU 和Camera 數(shù)據(jù)融合問題又稱為VIO (Visual-Inertial Odometry)問題.目前, 基于濾波的主流VIO 算法有: Multi-Sensor Fusion (MSF)、Multi-State Constraint Kalman Filter (MSCKF)、Robust Visual Inertial Odometry (ROVIO)等.其中MSCKF[13]是2007 年由Mourikis 等提出的算法, 該算法采用擴(kuò)展卡爾曼(EKF)[14]將IMU 和單目Camera 傳感器數(shù)據(jù)進(jìn)行EKF 融合, 融合方法是用IMU 運(yùn)動(dòng)模型構(gòu)建EKF 狀態(tài)方程, 用單目Camera 的重投影誤差建立EKF 量測(cè)模型.

近年來, 隨著機(jī)器人行業(yè)的迅速發(fā)展, 關(guān)于MSCKF算法的研究有許多進(jìn)展.文獻(xiàn)[15]從計(jì)算代價(jià)角度, 集成了EKF-SLAM 和MSCKF 兩種算法.首先預(yù)測(cè)每個(gè)特征分配策略的計(jì)算成本, 然后制定目標(biāo)函數(shù), 最后用目標(biāo)函數(shù)的最小化決定定位系統(tǒng)所使用的融合策略.該算法對(duì)狀態(tài)向量也進(jìn)行了增廣, 使得當(dāng)系統(tǒng)進(jìn)行更新時(shí), 長(zhǎng)時(shí)間跟蹤的特征點(diǎn)能及時(shí)得到利用.文獻(xiàn)[16]是將原來的單目Camera 改為立體Camera, 提高定位的穩(wěn)健性, 并且邊緣化采用Two-way Marginalization策略, 考慮了無人機(jī)應(yīng)用中懸停等特性.文獻(xiàn)[17]利用了全方位攝像機(jī)獲得球形圖像, 從更寬的視眼內(nèi)獲得更多的視覺特征與IMU 測(cè)量相結(jié)合, 避免因環(huán)境紋理不足造成定位效果不佳的問題.文獻(xiàn)[18]在初始化階段應(yīng)用sigma 點(diǎn)濾波器, 使得當(dāng)機(jī)器人初始化階段遭受沖擊時(shí)定位與導(dǎo)航性能仍然可靠.

本文針對(duì)傳統(tǒng)MSCKF 算法使用的IMU 中加速度計(jì)傳感器的自身缺點(diǎn): 測(cè)量信息中包含重力信息, 且算法在執(zhí)行時(shí)無法很好的將重力加速度有效去除[19];速度和位置狀態(tài)方程經(jīng)積分解算得到, 存在偏置漂移累計(jì)和距離測(cè)量精度隨時(shí)間惡化[20]等問題, 提出了改進(jìn)MSCKF 算法.改進(jìn)的MSCKF 算法用輪式里程計(jì)(Wheeled Odometer, 后續(xù)使用W-Odom 表示)取代加速度計(jì)傳感器, 利用W-Odom 對(duì)機(jī)器人運(yùn)動(dòng)的平移數(shù)據(jù)測(cè)量較為穩(wěn)定和準(zhǔn)確的優(yōu)點(diǎn)[19], 改進(jìn)傳統(tǒng)MSCKF算法的EKF 狀態(tài)方程.改進(jìn)的EKF 狀態(tài)方程使用陀螺儀構(gòu)建姿態(tài)方程, 使用輪式里程計(jì)構(gòu)建速度和位置方程, 避免狀態(tài)方程受重力干擾和由于積分產(chǎn)生累計(jì)誤差的問題, 使預(yù)測(cè)更有效, 從而提升定位精度.

1 MSCKF 算法

MSCKF 是基于EKF 濾波的IMU 和Camera 的數(shù)據(jù)融合算法.在介紹MSCKF 算法之前, 首先需要對(duì)相關(guān)的坐標(biāo)系進(jìn)行定義和約定.在本文中, I 表示IMU 坐標(biāo)系, 又稱為載體坐標(biāo)系, 與載體固連, 隨載體運(yùn)動(dòng)而變化; C 表示相機(jī)坐標(biāo)系; G 表示全局坐標(biāo)系, 是固定不變的坐標(biāo)系, 本文定義G 系在算法的初始位置, 即是以機(jī)器人中心點(diǎn)位置為原點(diǎn), z 軸為垂直載體向上,x 軸指向載體的前部, y 軸指向載體的左側(cè).上述坐標(biāo)系均滿足右手定則.

MSCKF 算法的系統(tǒng)狀態(tài)向量X 定義為:

MSCKF 算法的EKF 狀態(tài)方程使用IMU 數(shù)據(jù)對(duì)系統(tǒng)狀態(tài)向量進(jìn)行預(yù)測(cè).IMU 測(cè)量輸出的數(shù)據(jù)是角速度 ω和線加速度 f , 將其經(jīng)坐標(biāo)轉(zhuǎn)換可得到角速度和線加速度在IMU 坐標(biāo)系下的表示, 這其中包含了陀螺儀和加速度計(jì)的零偏 bω、 bf以及噪聲 wω、 wf, 因此真實(shí)的角速度和加速度定義為:

MSCKF 算法的狀態(tài)方程推導(dǎo)較為復(fù)雜, 本文直接給出連續(xù)狀態(tài)方程, 具體推導(dǎo)過程見文獻(xiàn)[21]:

式(4)為姿態(tài)狀態(tài)方程, 式(6)為速度狀態(tài)方程,式(8)為位置狀態(tài)方程.通過對(duì)上述方程采用積分方法即可實(shí)現(xiàn)對(duì)系統(tǒng)狀態(tài)向量的預(yù)測(cè).

2 MSCKF 算法的改進(jìn)與實(shí)現(xiàn)

2.1 改進(jìn)MSCKF 算法

由傳統(tǒng)MSCKF 算法的EKF 狀態(tài)方程可以看出,速度方程受重力影響, 且是對(duì)加速度計(jì)測(cè)量的線加速度數(shù)據(jù)和重力加速度進(jìn)行積分得到, 位置狀態(tài)方程是對(duì)速度再次積分, 即對(duì)線加速度數(shù)據(jù)進(jìn)行兩次積分得到, 這使得速度和位置方程受重力干擾, 且位置漂移隨時(shí)間二次增長(zhǎng).針對(duì)此問題, 本文利用W-Odom 傳感器以及IMU 中陀螺儀的互補(bǔ)性: 陀螺儀測(cè)量的角速度短時(shí)間內(nèi)精度較高, W-Odom 對(duì)平移測(cè)量較為準(zhǔn)確, 以及室內(nèi)環(huán)境特點(diǎn): 機(jī)器人在二維平面內(nèi)運(yùn)動(dòng), 替換EKF狀態(tài)方程中的式(6)和式(8), 改進(jìn)MSCKF 算法.

首先分析W-Odom.已知本文實(shí)驗(yàn)使用的Turtlebot2機(jī)器人采用兩輪差速底盤控制, 輪子半徑為r, 左右輪之間距離為 L, 輪子行走一圈編碼器的總脈沖數(shù)為sum,設(shè) Δt 時(shí)間內(nèi)左右編碼器輸出的脈沖數(shù)為 NL、 NR, 左右輪的線速度為 VL、 VR, 機(jī)器人的速度v 與角速度 ω0, 由式(11)-式(17)可計(jì)算出機(jī)器人的速度 v 以及位置x、y.其中式(13)為速度方程, 式(16)和式(17)是WOdom 位置方程.

改進(jìn)MSCKF 算法首先用W-Odom 的速度方程式(13)替換EKF 速度狀態(tài)方程式(6); 其次用EKF 姿態(tài)狀態(tài)方程式(4)求得姿態(tài)角中的航向角, 并用代替W-Odom 的位置方程式(16)和式(17)中的θ, 改進(jìn)位置方程, 最后用改進(jìn)后的位置方程替換EKF 位置狀態(tài)方程式(8), 即得到改進(jìn)的MSCKF 算法.改進(jìn)MSCKF 算法舍棄IMU 中加速度計(jì)的使用, 綜合利用IMU 中陀螺儀和W-Odom 的各自特點(diǎn), 將兩種傳感器數(shù)據(jù)進(jìn)行融合, 改進(jìn)傳統(tǒng)MSCKF 算法的EKF 狀態(tài)方程, 適用于機(jī)器人在室內(nèi)的運(yùn)動(dòng).

2.2 MSCKF 與改進(jìn)MSCKF 算法的實(shí)現(xiàn)

2.2.1 MSCKF 與改進(jìn)MSCKF 算法的實(shí)現(xiàn)流程

MSCKF 算法通過IMU 數(shù)據(jù)預(yù)測(cè)狀態(tài)向量, 通過單目Camera 對(duì)環(huán)境進(jìn)行觀測(cè), 提取自然路標(biāo), 并運(yùn)用路標(biāo)信息的重投影誤差約束對(duì)預(yù)測(cè)的狀態(tài)信息進(jìn)行修正更新.如圖1 是傳統(tǒng)MSCKF 算法的流程圖, 圖中的英文對(duì)應(yīng)的是程序里不同的函數(shù).

判斷10 s 內(nèi)是否有新一幀圖像.若在10 s 內(nèi)無圖像幀到來, 則MSCKF 算法停止?fàn)顟B(tài)量更新, 結(jié)束; 若有新一幀圖像到來, 則進(jìn)行下一步.

(1) 判斷是否初始化.若沒有完成初始化, 則進(jìn)行初始化, 并構(gòu)造MSCKF 狀態(tài)向量, 進(jìn)行下一步; 若已經(jīng)完成初始化, 則直接進(jìn)行下一步.

(2) EKF 預(yù)測(cè).傳統(tǒng)MSCKF 算法輸入為IMU 數(shù)據(jù), 調(diào)用根據(jù)第1 節(jié)EKF 狀態(tài)方程編寫的函數(shù), 輸出系統(tǒng)狀態(tài)向量的預(yù)測(cè)值和對(duì)應(yīng)的協(xié)方差矩陣.

(3) 對(duì)新圖像進(jìn)行光流跟蹤、匹配, 并提取新的特征點(diǎn).

(4) 將新一幀圖像的位姿pose 加入到狀態(tài)向量和協(xié)方差中(增廣).

(5) 特征點(diǎn)處理.

① 若之前視圖中的特征點(diǎn)在當(dāng)前幀中觀測(cè)不到(看不見的特征點(diǎn)), 則將該特征的跟蹤列表加入到measurementUpdate 中, 用于更新MSCKF 狀態(tài)向量.

② 若當(dāng)前幀觀測(cè)到的特征是之前在視圖中已經(jīng)觀測(cè)到的特征點(diǎn)(成熟特征點(diǎn)), 則將該特征點(diǎn)加入到跟蹤列表.

③ 對(duì)新提取的特征點(diǎn)分配新的featureID, 加入到跟蹤列表.

(6) 循環(huán)遍歷所有加入到measurementUpdate 中的特征點(diǎn), 進(jìn)行EKF 更新, 得到MSCKF 系統(tǒng)狀態(tài)向量的最優(yōu)解.

① 對(duì)特征點(diǎn)進(jìn)行三角化, 精確得到當(dāng)前路標(biāo)點(diǎn)在全局坐標(biāo)系的位置.

② 特征點(diǎn)的邊緣化: 將重投影誤差中的關(guān)于特征點(diǎn)誤差的約束進(jìn)行邊緣化(左零空間投影), 將特征點(diǎn)誤差約束轉(zhuǎn)化為系統(tǒng)狀態(tài)向量的約束(待優(yōu)化變量).

③ 利用重投影誤差, 采用視覺更新, 進(jìn)行一次線性化, 求解當(dāng)前幀時(shí)刻系統(tǒng)狀態(tài)向量的最優(yōu)解.

(7) 將滑動(dòng)窗口滑圖像幀進(jìn)行邊緣化, 使其維持在固定的長(zhǎng)度.

圖1 傳統(tǒng)MSCKF 算法流程圖

以上是MSCKF 算法的流程圖.改進(jìn)MSCKF 算法是在原算法基礎(chǔ)上, 對(duì)第二步EKF 預(yù)測(cè)部分的狀態(tài)方程進(jìn)行改進(jìn).改進(jìn)算法的輸入為W-Odom 數(shù)據(jù)和IMU 中陀螺儀的數(shù)據(jù), 且執(zhí)行的函數(shù)(方程)不同, 執(zhí)行的函數(shù)為im_propagate.

由此, 可以完成一次對(duì)當(dāng)前時(shí)刻機(jī)器人位置的估計(jì), 當(dāng)新一幀圖像到來之后, 循環(huán)執(zhí)行此步驟, 即實(shí)現(xiàn)MSCKF 算法及其改進(jìn)算法.

2.2.2 MSCKF 與改進(jìn)MSCKF 算法的實(shí)現(xiàn)

本文使用Turtlebot2 機(jī)器人, 在裝有Ubuntu 16.04機(jī)器人操作系統(tǒng)(ROS)的Lenovo G50 筆記本電腦上進(jìn)行實(shí)驗(yàn)測(cè)試.如圖2 所示, 是帶有kokuki 底盤的Turtlebot2 機(jī)器人, 內(nèi)置了IMU 和輪式里程計(jì)傳感器,在此基礎(chǔ)上外加單目相機(jī)和核心處理器Intel i5, 就組成了實(shí)驗(yàn)硬件.ROS 是實(shí)現(xiàn)機(jī)器人定位的軟件部分.ROS 可提供用戶、計(jì)算機(jī)操作系統(tǒng)以及外部設(shè)備間的通信功能, 并集成了一系列定位、導(dǎo)航等行為的工具和庫, 能夠在不同的機(jī)器人平臺(tái)(如Turtlebot)上輔助研究人員進(jìn)行機(jī)器人系統(tǒng)相關(guān)的研究和開發(fā), 創(chuàng)建定位、導(dǎo)航等復(fù)雜的機(jī)器人行為.

圖2 Turtlebot2 機(jī)器人

本文使用的MSCKF 及其改進(jìn)算法以msckf 和im_msckf 包封裝在ROS 內(nèi).MSCKF 及其改進(jìn)算法首先訂閱陀螺儀、輪式里程計(jì)、單目Camera 發(fā)布的話題, 然后執(zhí)行根據(jù)2.1.1 節(jié)編寫的算法處理數(shù)據(jù), 并以msg 格式發(fā)布msckf 和im_msckf 話題.最后在Ubuntu 16.04 系統(tǒng)上編寫msckf.py, 該文件訂閱MSCKF 及其改進(jìn)算法發(fā)布的話題, 并以txt 形式輸出MSCKF 及其改進(jìn)算法計(jì)算的機(jī)器人位置坐標(biāo).

MSCKF 及改進(jìn)算法有兩個(gè)比較重要的文件:src/ros_interface.cpp 和src/cornet_detector.cpp.其中src/ros_interface.cpp 是算法主體, 包括狀態(tài)預(yù)測(cè)、狀態(tài)增廣和測(cè)量更新等步驟.src/cornet_detector.cpp 是視覺前端處理部分, 主要進(jìn)行光流跟蹤及特征點(diǎn)處理等操作.

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

本文采用兩種策略驗(yàn)證改進(jìn)MSCKF 算法的有效性: 一種是使機(jī)器人做規(guī)定軌跡運(yùn)動(dòng), 用Matlab 讀取TXT 數(shù)據(jù), 繪制運(yùn)動(dòng)軌跡圖, 對(duì)比真實(shí)軌跡與算法軌跡, 直觀的觀察機(jī)器人運(yùn)動(dòng)過程中的定位精度; 第二種策略不關(guān)注機(jī)器人的真實(shí)運(yùn)動(dòng)軌跡, 關(guān)注機(jī)器人運(yùn)動(dòng)時(shí)間長(zhǎng)短與閉環(huán)定位精度的關(guān)系, 因此使機(jī)器人做閉環(huán)軌跡運(yùn)動(dòng), 并將(0.00, 0.00)與算法計(jì)算的終點(diǎn)位置的距離作為衡量定位準(zhǔn)確度的指標(biāo)(閉環(huán)誤差), 繪制閉環(huán)誤差表, 統(tǒng)計(jì)平均閉環(huán)誤差.其中機(jī)器人運(yùn)動(dòng)的實(shí)驗(yàn)環(huán)境為實(shí)驗(yàn)室和走廊兩種環(huán)境, 如圖3 所示.

圖3 實(shí)驗(yàn)環(huán)境

3.1 規(guī)定軌跡運(yùn)動(dòng)

遙控使機(jī)器人做封閉規(guī)定軌跡運(yùn)動(dòng), 實(shí)驗(yàn)運(yùn)動(dòng)環(huán)境為圖3(a).實(shí)驗(yàn)前預(yù)先在路徑上設(shè)置4 個(gè)坐標(biāo)點(diǎn)(0.00,0.00)、(3.80, 0.00)、(3.80, 2.70)、(2.70, 0.00), 規(guī)定機(jī)器人路線為矩形, 運(yùn)動(dòng)距離為13 m, 且各坐標(biāo)點(diǎn)之間的運(yùn)動(dòng)為直線, 運(yùn)動(dòng)一圈后回到起點(diǎn)位置(0.00, 0.00).

圖4 為一次實(shí)驗(yàn)Matlab 繪制的運(yùn)動(dòng)軌跡圖.圖中五角星為機(jī)器人起點(diǎn)及終點(diǎn)位置, 直線軌跡為規(guī)定的機(jī)器人運(yùn)動(dòng)軌跡, ×號(hào)軌跡為使用傳統(tǒng)MSCKF 算法的IMU/Camera 運(yùn)動(dòng)軌跡, 圓圈軌跡為改進(jìn)MSCKF 算法的IMU/W-Odom/Camera 運(yùn)動(dòng)軌跡.圖中可直觀的看出, 改進(jìn)MSCKF 算法的整體運(yùn)動(dòng)軌跡更接近真實(shí)軌跡, 計(jì)算的終點(diǎn)位置也更接近起點(diǎn)(0.00, 0.00).

3.2 封閉軌跡運(yùn)動(dòng)

遙控使機(jī)器人在走廊內(nèi)從起點(diǎn)(0.00, 0.00)開始運(yùn)動(dòng), 最后回到起點(diǎn)(0.00, 0.00), 即做閉環(huán)軌跡運(yùn)動(dòng), 且實(shí)驗(yàn)中機(jī)器人的運(yùn)動(dòng)為直線和轉(zhuǎn)彎的結(jié)合.為更好的分析閉環(huán)定位誤差, 減小由于運(yùn)動(dòng)時(shí)間、環(huán)境、操作誤差和偶然性對(duì)實(shí)驗(yàn)結(jié)果的干擾, 本文在兩種環(huán)境下進(jìn)行了運(yùn)動(dòng)時(shí)間不同的5 次實(shí)驗(yàn), 其中, 實(shí)驗(yàn)2 和實(shí)驗(yàn)4 實(shí)驗(yàn)環(huán)境為圖3(a), 其余實(shí)驗(yàn)環(huán)境為圖3(b), 統(tǒng)計(jì)的運(yùn)動(dòng)閉環(huán)誤差如表1.表中x、y 表示算法計(jì)算得到的終點(diǎn)坐標(biāo), 閉環(huán)誤差是該終點(diǎn)坐標(biāo)與起點(diǎn)(0.00, 0.00)的距離.

圖4 規(guī)定路線運(yùn)動(dòng)軌跡

表1 閉環(huán)誤差

對(duì)比實(shí)驗(yàn)1-實(shí)驗(yàn)4 的結(jié)果可以看出, 實(shí)驗(yàn)2 和實(shí)驗(yàn)4 的運(yùn)動(dòng)時(shí)間較長(zhǎng), 但其定位精度優(yōu)于實(shí)驗(yàn)1 和實(shí)驗(yàn)3, 說明在特征紋理豐富環(huán)境, 單目Camera 在更新時(shí)可選取的有效特征點(diǎn)較多, 使得MSCKF 算法定位效果優(yōu)于特征貧乏環(huán)境.由表1 中可以看出, 改進(jìn)MSCKF 算法定位精度優(yōu)于傳統(tǒng)MSCKF 算法, 平均閉環(huán)誤差提升了0.081 m, 定位精度提升, 說明改進(jìn)MSCKF算法使用輪式里程計(jì)對(duì)平移運(yùn)動(dòng)的約束比加速度計(jì)好,改進(jìn)MSCKF 算法的定位結(jié)果更優(yōu).

4 結(jié)束語

針對(duì)傳統(tǒng)MSCKF 算法實(shí)現(xiàn)機(jī)器人室內(nèi)定位時(shí)加速度計(jì)傳感器的固有缺點(diǎn): 受重力干擾和需積分解算得到速度和位置方程的問題, 本文提出了改進(jìn)MSCKF算法.改進(jìn)算法使用輪式里程計(jì)取代IMU 中加速度計(jì)傳感器, 替換傳統(tǒng)MSCKF 算法中EKF 速度和位置狀態(tài)方程, 使EKF 預(yù)測(cè)更加準(zhǔn)確.同時(shí)將MSCKF 及其改進(jìn)算法封裝在ROS 內(nèi), 結(jié)合Turtlebot2 機(jī)器人實(shí)現(xiàn)算法并進(jìn)行實(shí)驗(yàn)驗(yàn)證.通過對(duì)實(shí)驗(yàn)結(jié)果分析得到, MSCKF算法及改進(jìn)MSCKF 算法在特征紋理豐富環(huán)境中的定位效果要優(yōu)于特征貧乏環(huán)境, 且改進(jìn)MSCKF 算法的整體運(yùn)動(dòng)軌跡更接近真實(shí)軌跡, 閉環(huán)誤差也優(yōu)于傳統(tǒng)的MSCKF 算法, 這為后續(xù)室內(nèi)機(jī)器人進(jìn)行自主導(dǎo)航和路徑規(guī)劃提供了更好的前提.