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

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

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

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

基于MEMS的足部慣性導(dǎo)航系統(tǒng)(foot inertial navigation system,FINS)因?yàn)槠涑杀镜?體積小,重量輕且功耗低的特點(diǎn),已成為行人導(dǎo)航定位領(lǐng)域的研究熱點(diǎn)[5]. 雙足步行機(jī)器人的外表酷似人體的結(jié)構(gòu)和形態(tài)[6],因此也多采用足部慣性導(dǎo)航方式,其主要思路是通過對微慣性導(dǎo)航系統(tǒng)進(jìn)行零速修正或航位推算的方式實(shí)現(xiàn)導(dǎo)航定位. 通過對系統(tǒng)誤差狀態(tài)量的可觀測性分析發(fā)現(xiàn),微慣性足部導(dǎo)航方式的航向角誤差的可觀測性弱,卡爾曼濾波器將估計(jì)其航向角誤差. 因此,足部航向角誤差將逐步積累,而無法通過零速修正的方式對航向角進(jìn)行修正.

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

1 組合導(dǎo)航定位方案及系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

本文提出的導(dǎo)航方案如圖1所示.

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

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

2 導(dǎo)航機(jī)理分析

2.1 基于緊耦合優(yōu)化的VINS導(dǎo)航機(jī)理

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

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

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

(1)

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

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

(2)

2.2 基于零速修正的VINS導(dǎo)航機(jī)理

基于慣性測量組件足部安裝方式的雙足步行機(jī)器人導(dǎo)航系統(tǒng),以捷聯(lián)慣性導(dǎo)航為主提供導(dǎo)航信息,通過機(jī)器人足部運(yùn)動信息進(jìn)行零速檢測觸發(fā)基于卡爾曼濾波的零速修正,補(bǔ)償導(dǎo)航系統(tǒng)誤差,并通過四元數(shù)算法提供機(jī)器人行進(jìn)過程中的姿態(tài)信息[11-12]. 機(jī)器人處于不同步態(tài)時,系統(tǒng)通過足部加速度計(jì)和陀螺儀的輸出,設(shè)計(jì)相應(yīng)的閾值,對機(jī)器人足部運(yùn)動進(jìn)行零速檢測.

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

(3)

(4)

(5)

(6)

2.3 VINS/FINS信息雙向融合導(dǎo)航機(jī)理

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

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

卡爾曼濾波器的量測:

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

(7)

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

定義導(dǎo)航系統(tǒng)的觀測矢量:

(8)

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

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

3 組合導(dǎo)航系統(tǒng)的性能實(shí)驗(yàn)與分析

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

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

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

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

圖4 實(shí)驗(yàn)環(huán)境及路線圖Fig.4 Experimental environment and roadmap

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

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

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

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

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

4 結(jié)論

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