基于改進MSCKF 算法的室內(nèi)機器人定位方法①

孫 弋, 張雪麗

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

引言

機器人實現(xiàn)應(yīng)用的關(guān)鍵可概括為3 個核心問題:我在哪、我要去哪和如何去[1]的問題, 其中“我在哪”的定位問題是首要問題[2], 而且隨著機器人在礦井[3]、醫(yī)療[4]、服務(wù)[5]等領(lǐng)域的應(yīng)用, 定位精準性的需求逐漸成為人們研究和關(guān)注的重點[6,7].由于單一傳感器的定位精度和可適用性有限, 目前, 機器人定位普遍采用將多種傳感器數(shù)據(jù)進行融合的方式實現(xiàn)[8-11], 其中慣性測量單元(IMU)和相機(camera)存在互補性[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 等提出的算法, 該算法采用擴展卡爾曼(EKF)[14]將IMU 和單目Camera 傳感器數(shù)據(jù)進行EKF 融合, 融合方法是用IMU 運動模型構(gòu)建EKF 狀態(tài)方程, 用單目Camera 的重投影誤差建立EKF 量測模型.

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

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

1 MSCKF 算法

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

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

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

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

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

2 MSCKF 算法的改進與實現(xiàn)

2.1 改進MSCKF 算法

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

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

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

2.2 MSCKF 與改進MSCKF 算法的實現(xiàn)

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

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

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

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

(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) 對新圖像進行光流跟蹤、匹配, 并提取新的特征點.

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

(5) 特征點處理.

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

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

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

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

① 對特征點進行三角化, 精確得到當前路標點在全局坐標系的位置.

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

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

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

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

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

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

2.2.2 MSCKF 與改進MSCKF 算法的實現(xiàn)

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

圖2 Turtlebot2 機器人

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

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

3 實驗結(jié)果分析

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

圖3 實驗環(huán)境

3.1 規(guī)定軌跡運動

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

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

3.2 封閉軌跡運動

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

圖4 規(guī)定路線運動軌跡

表1 閉環(huán)誤差

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

4 結(jié)束語

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