一種基于VINS/FINS組合導航方法

袁 珊,萬 游,孟佳杰,汪雨婷,錢偉行,古翠紅

(南京師范大學南瑞電氣與自動化學院,江蘇 南京 210023)

近年來,隨著智能型機器人技術的快速發展,自主式移動機器人在各行各業逐漸顯現出自己的作用和優勢. 按移動方式進行劃分,機器人可分為雙足步行機器人、輪式移動機器人和履帶式移動機器人等. 與其他種類移動機器人相比,雙足步行機器人與支撐點的接觸是非連續的,是目前的研究熱點之一[1]. 但是要想普及雙足步行機器人仍有許多問題需要解決,環境感知與導航定位問題尤為重要.

同時定位與地圖構建(simultaneous localization and mapping,SLAM)在機器人導航定位領域有著舉足輕重的作用[2]. 視覺傳感器以較高的分辨率和相對較快的速度感知豐富的外部環境信息,但也存在著對運動變化敏感等問題,而慣性傳感器(inertial measurement unit,IMU)可以提供高頻率的精確運動信息,彌補視覺跟蹤運動模糊的問題[3]. 當前研究的視覺慣性導航系統(visual-inertial navigation system,VINS)多使用集成度高的低精度IMU,其加速度計和陀螺儀的零偏較大,器件誤差會對組合導航系統的定位精度造成影響[4].

基于MEMS的足部慣性導航系統(foot inertial navigation system,FINS)因為其成本低,體積小,重量輕且功耗低的特點,已成為行人導航定位領域的研究熱點[5]. 雙足步行機器人的外表酷似人體的結構和形態[6],因此也多采用足部慣性導航方式,其主要思路是通過對微慣性導航系統進行零速修正或航位推算的方式實現導航定位. 通過對系統誤差狀態量的可觀測性分析發現,微慣性足部導航方式的航向角誤差的可觀測性弱,卡爾曼濾波器將估計其航向角誤差. 因此,足部航向角誤差將逐步積累,而無法通過零速修正的方式對航向角進行修正.

針對以上問題,本文研究了一種基于信息雙向融合的VINS/FINS多傳感器組合導航方法. 將視覺傳感器和消費級慣性測量組件安裝于機器人軀干部分,另一套器件精度較高的慣性測量組件安裝于機器人足部,利用足部FINS系統零速修正后的位置信息實時修正軀干VINS系統中的低精度慣性器件誤差,同時軀干VINS系統可以獲得相對準確的航向角. 基于以上特性構建信息雙向融合的組合導航系統結構,提高雙足步行機器人的航向精度和導航定位精度.

1 組合導航定位方案及系統結構設計

本文提出的導航方案如圖1所示.

圖1 基于VINS和FINS相互輔助的定位系統工作流程圖Fig.1 The principle chart of positioning system based on VINS and FINS

圖1左邊部分為VINS導航系統,由安裝于軀干的慣性測量組件和視覺傳感器分別采集慣性信息和視覺圖像信息. 對圖像特征點進行特征提取與匹配,之后進行位姿估計,利用相鄰的地圖點,尋找更多能夠成果匹配的特征點,并選取關鍵幀完成地圖構建,系統使用基于流形的IMU預積分思想進行慣性數據預處理,以解決兩種傳感器數據輸出頻率不一致的問題,從而完成視覺信息和慣性信息的緊耦合. 圖1右側部分為FINS導航系統,通過步態相位檢測對機器人足部導航系統進行間斷性的零速誤差修正. 中間部分為誤差修正系統,以軀干慣性信息誤差方程作為狀態變量,其狀態量與足部慣導系統相同,前行過程中系統輸出位置信息時,將FINS系統解算的位置信息作為基準,建立卡爾曼濾波器,此時VINS中慣性信息捷聯解算得到的位置與FINS位置信息的差值即為位置誤差觀測量;根據軀干VINS系統提供的圖像信息和慣導信息判斷雙足步行機器人的行走方式,當判斷為直線行走方式時,使用VINS系統中精度較高的航向角替代FINS中的低精度航向角. 利用VINS/FINS導航系統不同的誤差發散特性進行信息交互,完成導航信息雙向融合,同步修正系統誤差,提高雙足步行機器人的導航定位與航向精度.

2 導航機理分析

2.1 基于緊耦合優化的VINS導航機理

純視覺SLAM方案在工作過程中,通常采用勻速運動模型預測當前幀位姿. 當相機運動比較緩慢時,該模型算法有效,但是視覺范圍內出現快速運動的物體或相機突然抖動,可能會導致跟蹤失敗[6]. 初始化是保證導航系統正常運行的重要前提,視覺和慣性信息的緊耦合優化是將視覺傳感器對狀態的約束量與IMU對狀態的約束量來進行聯合優化來估計機器人位姿[7]. 不同于松耦合方式的是將IMU和相機分別對自身運動估計后再進行融合. 而緊耦合方法通過將視覺約束和IMU約束聯立進行非線性優化,更能達到傳感器之間的優勢互補的目的[8]. IMU可以為單目視覺提供尺度信息,視覺位姿可以矯正IMU的漂移問題. 本文采用的視覺慣性信息緊耦合優化方案框架如圖2所示.

圖2 視覺慣性緊耦合方案框架Fig.2 Visual/inertial tight coupling scheme framework

采集視覺傳感器圖像信息,提取具有方向信息的多尺度FAST特征點,并使用32字節的BRIEF作為描述符,前后幀圖像中的特征點進行快速匹配,通過匹配特征點滿足的極線約束關系建立前后幀圖像幾何關系,從而實時獲取相機初始位姿信息. 慣性測量組件提供角速度和加速度的離散時間樣本,其測量頻率通常遠高于視覺測量頻率,IMU預積分的目的在于將相鄰兩個視覺關鍵幀間的IMU測量值進行統一積分得到運動狀態的相對增量,避免因為之前狀態量的變化引起的重復積分問題. IMU預積分理論建立在歐拉積分基礎上[9-10],記i時刻和j時刻相鄰兩幀IMU觀測數據的時間間隔為Δt,并定義相對旋轉增量ΔRij、相對速度增量ΔVij和相對位置增量ΔPij,由加速度測量數據aB和角速度測量數據wB計算得到的預積分項如下:

(1)

式中,下標a和g分別表示IMU中的加速度計和陀螺儀,bg和ba分別為兩個傳感器的零偏,ηa和ηg為測量高斯白噪聲. 上標k表示當前時刻,Gw表示世界坐標系下的重力矢量,RwB表示IMU系到世界坐標系的旋轉矩陣的轉置.

通過控制局部地圖規模大小來控制非線性優化的計算量,定義視覺慣性緊耦合優化方案中的各誤差代價函數,主要分為3部分誤差:視覺重投影誤差、預積分誤差和由于滑動窗口產生的先驗信息. 通過最小化各測量誤差馬氏距離的平方和可以得到系統的最優估計:

(2)

2.2 基于零速修正的VINS導航機理

基于慣性測量組件足部安裝方式的雙足步行機器人導航系統,以捷聯慣性導航為主提供導航信息,通過機器人足部運動信息進行零速檢測觸發基于卡爾曼濾波的零速修正,補償導航系統誤差,并通過四元數算法提供機器人行進過程中的姿態信息[11-12]. 機器人處于不同步態時,系統通過足部加速度計和陀螺儀的輸出,設計相應的閾值,對機器人足部運動進行零速檢測.

零速檢測算法由以下4個條件組成,C1、C2、C3和C4分別是根據加速度計的矢量和、加速度計Z軸的值、陀螺儀的矢量和和陀螺儀Y軸的值來判斷是否滿足零速的條件.C1=1表示滿足該條零速檢測的條件,C1=0表示不滿足該條零速檢測的條件,C2、C3和C4類似. 對于每個時刻的IMU信息,零速檢測條件為C1&C2&C3&C4,即4個邏輯條件必須同時滿足,才能判斷足部是靜止的.

(3)

(4)

(5)

(6)

2.3 VINS/FINS信息雙向融合導航機理

采用足部慣性測量組件進行機器人定位時,由于航向角誤差的可觀測性很差,卡爾曼濾波器不能精確地將航向角誤差估計出來[13]. 視覺/慣性組合導航系統通過相機圖像讀取、視覺里程計、后端優化和回環檢測等步驟后可以獲得比較準確的航向角[14]. 另外,視覺/慣性組合導航系統的IMU多為低精度傳感器,其加速度偏差較大. IMU初始化階段,若忽略加速度偏差,將其加到重力加速度中,容易導致重力加速度發散,進而導致初始化失敗,VINS導航系統不僅無法獲得準確的初始位姿,還會為非線性優化帶來錯誤的IMU約束,IMU的器件誤差很有可能導致VINS導航系統無法定位[15].

為了提高視覺/慣性組合導航系統中的IMU器件精度,本文以軀干慣導誤差方程作為狀態方程,其狀態量與足部INS相同. 機器人行進過程中每當系統輸出位置信息,以足部INS位置信息作為基準,將VINS中的慣性信息捷聯解算得到的位置與足部INS位置信息作差值即為位置誤差觀測量,建立卡爾曼濾波器.

卡爾曼濾波器的量測:

Zk=(δL,δλ,δh).

(7)

式中,δL、δλ和δh分別表示經度、緯度和高度誤差觀測量.

定義導航系統的觀測矢量:

(8)

式中,λb,Lb,hb分別為VINS中的較低精度慣性器件提供的經度、緯度和高度,λf,Lf,hf分別為FINS提供的經度、緯度和高度,Z(t)為t時刻的觀測矢量,H(t)為t時刻的觀測矩陣,X(t)表示t時刻的狀態量,Nf(t)為t時刻的觀測噪聲.

首先利用視覺/慣性組合導航系統中的加速度計和陀螺儀采集的慣性信息進行導航解算,更新15維狀態量和相應的四元數,根據狀態量和四元數推算出卡爾曼濾波器的狀態轉移和協方差矩陣. 同時,每當FINS導航系統和軀干消費級慣性測量組件經捷聯導航解算后輸出位置信息時,將二者的位置作差作為系統的位置誤差觀測量,計算卡爾曼增益,修正軀干慣性測量組件捷聯導航解算后的速度、姿態轉移陣及位置誤差和軀干消費級慣性測量組件的器件誤差. 通過提高軀干IMU的器件精度,即扣除加速度計的零偏,使軀干VINS導航系統能成功初始化,獲得準確的初始位姿并且在非線性優化階段提供正確的慣性測量約束. 同時通過VINS組合導航系統的圖像信息和慣性信息判斷行人的行走方式,當行人為直線行走時,用VINS系統的精度較高的航向角替代足部慣性導航系統的低精度航向角. 通過以上步驟,完成兩種導航信息的雙向融合.

3 組合導航系統的性能實驗與分析

本文實驗中安裝于雙足步行機器人軀干部分的傳感器為小覓S1000-120-ir,主要包括視覺傳感器和消費級慣性測量組件,單目相機的幀率是20 fps,分辨率是640*480,IMU的數據采集頻率為200 Hz. 安裝于雙足步行機器人足部的慣性測量組件的型號為XSENS MTI-300. 通過采集靜態IMU數據,進行靜態數據分析,得到兩組陀螺儀和加速度計的零偏穩定性如表1所示,MTi-300為工業級慣性測量組件,其陀螺儀和加速度計的零偏值較小,說明其器件精度明顯優異于小覓S1000-120-ir中的消費級慣性測量組件. 實驗采用的計算機配置為i7-9750U,2.60GHz CPU,8GB RAM,操作系統為Ubuntu16.04,傳感器具體安裝位置如圖3所示.

表1 MTi-300與小覓S-1000中慣性測量組件的基本性能參數Table 1 Basic performance parameters of inertial measurement unit in mti-300 and Xiaomi S-1000

行進路線選取某地下停車場的一條閉合路線,并使實驗起點與終點重合,實驗環境及行進路線如圖4所示,實驗總路程約為220 m,除了拐彎處,其他路段均保持直線行走,標準軌跡由激光測距儀和人工測量得到. 采用如下導航解算方案驗證基于VINS/FINS導航信息雙向融合方案的有效性:

圖3 傳感器安裝位置示意圖Fig.3 Installation position diagram of sensor

圖4 實驗環境及路線圖Fig.4 Experimental environment and roadmap

圖5 誤差修正前后軌跡結果對比圖Fig.5 Comparison of positioning results before and after error correction of VINS system

(1)驗證VINS系統中的低精度IMU器件誤差得到修正,將修正前后的VINS系統在同一條閉合路線上進行實驗,實驗結果如圖5(a)所示. 實驗選用的開源VINS方案為Mur等[3]于2017年提出的VIORB-SLAM. 由于單目VINS的器件誤差較大,導致初始化失敗,初始化階段無法獲得準確的初始位姿,非線性優化階段的IMU約束為VINS帶來較大的誤差,行進軌跡如圖5(a)中點劃線所示,最大的定位誤差為 5.3 m,占總行進路程的2.4%. 以足部慣性導航系統的位置信息為觀測量,估計VINS中的IMU的加速度零偏,當加速度零偏被扣除后,加速度計的精度得到提高,單目VINS中低精度IMU誤差得到修正后,系統的定位精度得到明顯提升,最大定位誤差為3 m,占總行進距離的1.3%. 如圖5(a)中虛線所示. 說明本文所研究的器件誤差修正方法可有效提高VINS系統的定位精度.

(2)驗證FINS系統的航向角誤差得到修正,將修正前后的FINS系統在同一條閉合路線上進行實驗,實驗結果如圖5(b)所示. 實驗中選用的改進前的FINS系統為趙小明等[5]研究的鞋式慣性導航系統,FINS系統的航向角被修正之前的定位曲線如圖5(b)中點劃線所示航向角被修正之前,其航向角誤差較大,并且隨著時間的延長逐漸增大,最大航向角誤差為4.2°. 在直線行走階段,利用單目VINS的航向角替代足部慣性導航系統的航向角,定位曲線如圖5(b)中虛線所示,最大航向角誤差為1.1°. 實驗說明本文所研究的航向角修正方法可有效提高FINS系統的航向角精度.

圖6 不同方案軌跡對比結果圖Fig.6 Trajectory comparison of different schemes

(3)為驗證VINS/FINS導航信息雙向融合方案的有效性,將誤差修正前的VINS系統和FINS系統與修正后的VINS/FINS組合導航方案在同一條路線上進行實驗,實驗結果如圖6所示. 在實驗結束時刻VINS/FINS組合導航方案定位誤差約為1.8 m,占總行進距離的0.82%,最大航向角誤差為0.9°. 由圖6可知,與VINS和FINS方案相比,本文研究的VINS/FINS雙向信息融合方案導航軌跡波動較為平緩,魯棒性更高,且定位精度和航向精度有了明顯的提升,綜合驗證了本文研究方案的有效性.

4 結論

本文研究了一種VINS/FINS導航信息雙向融合的多傳感器組合導航方案. 首先,針對FINS導航系統的航向角誤差逐漸累積導致的航向角誤差較大的問題,提出了在某種行走方式下,通過VINS組合導航系統的航向角替代足部慣性導航系統的航向角的方法. 其次,針對VINS組合導航系統的IMU器件精度差的問題,以FINS導航系統的位置信息為觀測量,構建卡爾曼濾波器,以估計并修正VINS導航系統的低精度IMU器件誤差. 最后,設計了雙足步行機器人多傳感器組合導航系統的性能驗證實驗,綜合驗證了本文研究方案的有效性.