袁 珊,萬(wàn) 游,孟佳杰,汪雨婷,錢偉行,古翠紅

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

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

同時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)在機(jī)器人導(dǎo)航定位領(lǐng)域有著舉足輕重的作用[2]. 視覺傳感器以較高的分辨率和相對(duì)較快的速度感知豐富的外部環(huán)境信息,但也存在著對(duì)運(yùn)動(dòng)變化敏感等問(wèn)題,而慣性傳感器(inertial measurement unit,IMU)可以提供高頻率的精確運(yùn)動(dòng)信息,彌補(bǔ)視覺跟蹤運(yùn)動(dòng)模糊的問(wèn)題[3]. 當(dāng)前研究的視覺慣性導(dǎo)航系統(tǒng)(visual-inertial navigation system,VINS)多使用集成度高的低精度IMU,其加速度計(jì)和陀螺儀的零偏較大,器件誤差會(huì)對(duì)組合導(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)航方式,其主要思路是通過(guò)對(duì)微慣性導(dǎo)航系統(tǒng)進(jìn)行零速修正或航位推算的方式實(shí)現(xiàn)導(dǎo)航定位. 通過(guò)對(duì)系統(tǒng)誤差狀態(tài)量的可觀測(cè)性分析發(fā)現(xiàn),微慣性足部導(dǎo)航方式的航向角誤差的可觀測(cè)性弱,卡爾曼濾波器將估計(jì)其航向角誤差. 因此,足部航向角誤差將逐步積累,而無(wú)法通過(guò)零速修正的方式對(duì)航向角進(jìn)行修正.

針對(duì)以上問(wèn)題,本文研究了一種基于信息雙向融合的VINS/FINS多傳感器組合導(dǎo)航方法. 將視覺傳感器和消費(fèi)級(jí)慣性測(cè)量組件安裝于機(jī)器人軀干部分,另一套器件精度較高的慣性測(cè)量組件安裝于機(jī)器人足部,利用足部FINS系統(tǒng)零速修正后的位置信息實(shí)時(shí)修正軀干VINS系統(tǒng)中的低精度慣性器件誤差,同時(shí)軀干VINS系統(tǒng)可以獲得相對(duì)準(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),由安裝于軀干的慣性測(cè)量組件和視覺傳感器分別采集慣性信息和視覺圖像信息. 對(duì)圖像特征點(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ù)輸出頻率不一致的問(wèn)題,從而完成視覺信息和慣性信息的緊耦合. 圖1右側(cè)部分為FINS導(dǎo)航系統(tǒng),通過(guò)步態(tài)相位檢測(cè)對(duì)機(jī)器人足部導(dǎo)航系統(tǒng)進(jìn)行間斷性的零速誤差修正. 中間部分為誤差修正系統(tǒng),以軀干慣性信息誤差方程作為狀態(tài)變量,其狀態(tài)量與足部慣導(dǎo)系統(tǒng)相同,前行過(guò)程中系統(tǒng)輸出位置信息時(shí),將FINS系統(tǒng)解算的位置信息作為基準(zhǔn),建立卡爾曼濾波器,此時(shí)VINS中慣性信息捷聯(lián)解算得到的位置與FINS位置信息的差值即為位置誤差觀測(cè)量;根據(jù)軀干VINS系統(tǒng)提供的圖像信息和慣導(dǎo)信息判斷雙足步行機(jī)器人的行走方式,當(dāng)判斷為直線行走方式時(shí),使用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方案在工作過(guò)程中,通常采用勻速運(yùn)動(dòng)模型預(yù)測(cè)當(dāng)前幀位姿. 當(dāng)相機(jī)運(yùn)動(dòng)比較緩慢時(shí),該模型算法有效,但是視覺范圍內(nèi)出現(xiàn)快速運(yùn)動(dòng)的物體或相機(jī)突然抖動(dòng),可能會(huì)導(dǎo)致跟蹤失敗[6]. 初始化是保證導(dǎo)航系統(tǒng)正常運(yùn)行的重要前提,視覺和慣性信息的緊耦合優(yōu)化是將視覺傳感器對(duì)狀態(tài)的約束量與IMU對(duì)狀態(tài)的約束量來(lái)進(jìn)行聯(lián)合優(yōu)化來(lái)估計(jì)機(jī)器人位姿[7]. 不同于松耦合方式的是將IMU和相機(jī)分別對(duì)自身運(yùn)動(dòng)估計(jì)后再進(jìn)行融合. 而緊耦合方法通過(guò)將視覺約束和IMU約束聯(lián)立進(jìn)行非線性優(yōu)化,更能達(dá)到傳感器之間的優(yōu)勢(shì)互補(bǔ)的目的[8]. IMU可以為單目視覺提供尺度信息,視覺位姿可以矯正IMU的漂移問(wèn)題. 本文采用的視覺慣性信息緊耦合優(yōu)化方案框架如圖2所示.

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

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

(1)

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

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

(2)

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

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

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

(3)

(4)

(5)

(6)

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

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

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

卡爾曼濾波器的量測(cè):

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

(7)

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

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

(8)

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

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

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

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

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

行進(jìn)路線選取某地下停車場(chǎng)的一條閉合路線,并使實(shí)驗(yàn)起點(diǎn)與終點(diǎn)重合,實(shí)驗(yàn)環(huán)境及行進(jìn)路線如圖4所示,實(shí)驗(yàn)總路程約為220 m,除了拐彎處,其他路段均保持直線行走,標(biāo)準(zhǔn)軌跡由激光測(cè)距儀和人工測(cè)量得到. 采用如下導(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é)果對(duì)比圖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)致初始化失敗,初始化階段無(wú)法獲得準(zhǔn)確的初始位姿,非線性優(yōu)化階段的IMU約束為VINS帶來(lái)較大的誤差,行進(jìn)軌跡如圖5(a)中點(diǎn)劃線所示,最大的定位誤差為 5.3 m,占總行進(jìn)路程的2.4%. 以足部慣性導(dǎo)航系統(tǒng)的位置信息為觀測(cè)量,估計(jì)VINS中的IMU的加速度零偏,當(dāng)加速度零偏被扣除后,加速度計(jì)的精度得到提高,單目VINS中低精度IMU誤差得到修正后,系統(tǒng)的定位精度得到明顯提升,最大定位誤差為3 m,占總行進(jìn)距離的1.3%. 如圖5(a)中虛線所示. 說(shuō)明本文所研究的器件誤差修正方法可有效提高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)劃線所示航向角被修正之前,其航向角誤差較大,并且隨著時(shí)間的延長(zhǎng)逐漸增大,最大航向角誤差為4.2°. 在直線行走階段,利用單目VINS的航向角替代足部慣性導(dǎo)航系統(tǒng)的航向角,定位曲線如圖5(b)中虛線所示,最大航向角誤差為1.1°. 實(shí)驗(yàn)說(shuō)明本文所研究的航向角修正方法可有效提高FINS系統(tǒng)的航向角精度.

圖6 不同方案軌跡對(duì)比結(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é)束時(shí)刻VINS/FINS組合導(dǎo)航方案定位誤差約為1.8 m,占總行進(jìn)距離的0.82%,最大航向角誤差為0.9°. 由圖6可知,與VINS和FINS方案相比,本文研究的VINS/FINS雙向信息融合方案導(dǎo)航軌跡波動(dòng)較為平緩,魯棒性更高,且定位精度和航向精度有了明顯的提升,綜合驗(yàn)證了本文研究方案的有效性.

4 結(jié)論

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