孫 弋, 張雪麗

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

引言

機(jī)器人實(shí)現(xiàn)應(yīng)用的關(guān)鍵可概括為3 個核心問題:我在哪、我要去哪和如何去[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], 其中慣性測量單元(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)動模型構(gòu)建EKF 狀態(tài)方程, 用單目Camera 的重投影誤差建立EKF 量測模型.

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

本文針對傳統(tǒng)MSCKF 算法使用的IMU 中加速度計(jì)傳感器的自身缺點(diǎn): 測量信息中包含重力信息, 且算法在執(zhí)行時無法很好的將重力加速度有效去除[19];速度和位置狀態(tài)方程經(jīng)積分解算得到, 存在偏置漂移累計(jì)和距離測量精度隨時間惡化[20]等問題, 提出了改進(jìn)MSCKF 算法.改進(jìn)的MSCKF 算法用輪式里程計(jì)(Wheeled Odometer, 后續(xù)使用W-Odom 表示)取代加速度計(jì)傳感器, 利用W-Odom 對機(jī)器人運(yùn)動的平移數(shù)據(jù)測量較為穩(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ù)測更有效, 從而提升定位精度.

1 MSCKF 算法

MSCKF 是基于EKF 濾波的IMU 和Camera 的數(shù)據(jù)融合算法.在介紹MSCKF 算法之前, 首先需要對相關(guān)的坐標(biāo)系進(jìn)行定義和約定.在本文中, I 表示IMU 坐標(biāo)系, 又稱為載體坐標(biāo)系, 與載體固連, 隨載體運(yùn)動而變化; 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ù)對系統(tǒng)狀態(tài)向量進(jìn)行預(yù)測.IMU 測量輸出的數(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)方程.通過對上述方程采用積分方法即可實(shí)現(xiàn)對系統(tǒng)狀態(tài)向量的預(yù)測.

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

2.1 改進(jìn)MSCKF 算法

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

首先分析W-Odom.已知本文實(shí)驗(yàn)使用的Turtlebot2機(jī)器人采用兩輪差速底盤控制, 輪子半徑為r, 左右輪之間距離為 L, 輪子行走一圈編碼器的總脈沖數(shù)為sum,設(shè) Δt 時間內(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)動.

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

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

MSCKF 算法通過IMU 數(shù)據(jù)預(yù)測狀態(tài)向量, 通過單目Camera 對環(huán)境進(jìn)行觀測, 提取自然路標(biāo), 并運(yùn)用路標(biāo)信息的重投影誤差約束對預(yù)測的狀態(tài)信息進(jìn)行修正更新.如圖1 是傳統(tǒng)MSCKF 算法的流程圖, 圖中的英文對應(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ù)測.傳統(tǒng)MSCKF 算法輸入為IMU 數(shù)據(jù), 調(diào)用根據(jù)第1 節(jié)EKF 狀態(tài)方程編寫的函數(shù), 輸出系統(tǒng)狀態(tài)向量的預(yù)測值和對應(yīng)的協(xié)方差矩陣.

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

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

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

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

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

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

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

① 對特征點(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)前幀時刻系統(tǒng)狀態(tài)向量的最優(yōu)解.

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

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

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

由此, 可以完成一次對當(dāng)前時刻機(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)測試.如圖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ī)器人平臺(如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)算法有兩個比較重要的文件:src/ros_interface.cpp 和src/cornet_detector.cpp.其中src/ros_interface.cpp 是算法主體, 包括狀態(tài)預(yù)測、狀態(tài)增廣和測量更新等步驟.src/cornet_detector.cpp 是視覺前端處理部分, 主要進(jìn)行光流跟蹤及特征點(diǎn)處理等操作.

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

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

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

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

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

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

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

遙控使機(jī)器人在走廊內(nèi)從起點(diǎn)(0.00, 0.00)開始運(yùn)動, 最后回到起點(diǎn)(0.00, 0.00), 即做閉環(huán)軌跡運(yùn)動, 且實(shí)驗(yàn)中機(jī)器人的運(yùn)動為直線和轉(zhuǎn)彎的結(jié)合.為更好的分析閉環(huán)定位誤差, 減小由于運(yùn)動時間、環(huán)境、操作誤差和偶然性對實(shí)驗(yàn)結(jié)果的干擾, 本文在兩種環(huán)境下進(jìn)行了運(yùn)動時間不同的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)動閉環(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)動軌跡

表1 閉環(huán)誤差

對比實(shí)驗(yàn)1-實(shí)驗(yàn)4 的結(jié)果可以看出, 實(shí)驗(yàn)2 和實(shí)驗(yàn)4 的運(yùn)動時間較長, 但其定位精度優(yōu)于實(shí)驗(yàn)1 和實(shí)驗(yàn)3, 說明在特征紋理豐富環(huán)境, 單目Camera 在更新時可選取的有效特征點(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ì)對平移運(yùn)動的約束比加速度計(jì)好,改進(jìn)MSCKF 算法的定位結(jié)果更優(yōu).

4 結(jié)束語

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