基于改進MSCKF 算法的室內機器人定位方法①

孫 弋, 張雪麗

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

引言

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

IMU 和Camera 數據融合問題又稱為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 傳感器數據進行EKF 融合, 融合方法是用IMU 運動模型構建EKF 狀態方程, 用單目Camera 的重投影誤差建立EKF 量測模型.

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

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

1 MSCKF 算法

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

MSCKF 算法的系統狀態向量X 定義為:

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

MSCKF 算法的狀態方程推導較為復雜, 本文直接給出連續狀態方程, 具體推導過程見文獻[21]:

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

2 MSCKF 算法的改進與實現

2.1 改進MSCKF 算法

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

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

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

2.2 MSCKF 與改進MSCKF 算法的實現

2.2.1 MSCKF 與改進MSCKF 算法的實現流程

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

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

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

(2) EKF 預測.傳統MSCKF 算法輸入為IMU 數據, 調用根據第1 節EKF 狀態方程編寫的函數, 輸出系統狀態向量的預測值和對應的協方差矩陣.

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

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

(5) 特征點處理.

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

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

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

(6) 循環遍歷所有加入到measurementUpdate 中的特征點, 進行EKF 更新, 得到MSCKF 系統狀態向量的最優解.

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

② 特征點的邊緣化: 將重投影誤差中的關于特征點誤差的約束進行邊緣化(左零空間投影), 將特征點誤差約束轉化為系統狀態向量的約束(待優化變量).

③ 利用重投影誤差, 采用視覺更新, 進行一次線性化, 求解當前幀時刻系統狀態向量的最優解.

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

圖1 傳統MSCKF 算法流程圖

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

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

2.2.2 MSCKF 與改進MSCKF 算法的實現

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

圖2 Turtlebot2 機器人

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

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

3 實驗結果分析

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

圖3 實驗環境

3.1 規定軌跡運動

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

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

3.2 封閉軌跡運動

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

圖4 規定路線運動軌跡

表1 閉環誤差

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

4 結束語

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