室內(nèi)環(huán)境下立體視覺慣導(dǎo)融合定位

敖龍輝，郭 杭

(南昌大學(xué)，江西 南昌 330031)

居家服務(wù)機(jī)器人是未來機(jī)器人發(fā)展的重要方向，狀態(tài)估計(jì)是機(jī)器人技術(shù)的基礎(chǔ)。持續(xù)且準(zhǔn)確的狀態(tài)估計(jì)在家庭服務(wù)機(jī)器人運(yùn)用上格外重要。為了提高機(jī)器人的穩(wěn)健性和自主能力，越來越多的傳感器安裝在機(jī)器人上。IMU模組與相機(jī)模組憑借低成本、小型化，在導(dǎo)航定位領(lǐng)域也有著大量的應(yīng)用。在眾多研究人員的共同努力下，提出了許多基于純視覺里程計(jì)的算法，如SVO[1],LSD-SLAM[2],DSO[3],ORB-SLAM[4]。除了相機(jī)以外，IMU是另一個流行的狀態(tài)估計(jì)選項(xiàng)。IMU通過測量解算高頻率下加速度和角速度，實(shí)現(xiàn)對運(yùn)載體導(dǎo)航定位的目的，這對于實(shí)時應(yīng)用中的低延遲姿態(tài)反饋是必要的[5]。在室內(nèi)自主機(jī)器人的研究中，結(jié)合視覺及慣導(dǎo)的組合定位導(dǎo)航方法有著極大的研究前景[6-10]。視覺導(dǎo)航方法融合慣性信息能夠有效地克服相機(jī)快速運(yùn)動造成的問題，同時視覺信息又能夠在慢速運(yùn)動時解決IMU的漂移問題?？紤]雙目相機(jī)和IMU具有天然的互補(bǔ)特性，立視覺慣導(dǎo)組合導(dǎo)航成為機(jī)器人定位導(dǎo)航的重要選擇[11]。本文研究基于緊耦合非線性優(yōu)化的立體視覺慣性融合導(dǎo)航方法，提出一種穩(wěn)健的系統(tǒng)初始化方法，實(shí)現(xiàn)了基于該方法的視覺慣性融合導(dǎo)航系統(tǒng)，并設(shè)計(jì)試驗(yàn)進(jìn)行驗(yàn)證，系統(tǒng)能夠得到很好的初始化效果，達(dá)到相當(dāng)高的定位精度，非常適合運(yùn)用于居家場景機(jī)器人的導(dǎo)航定位。

1 數(shù)據(jù)處理方法

1.1 IMU預(yù)積分技術(shù)

IMU預(yù)積分技術(shù)[10]在基于緊耦合優(yōu)化的視覺慣性融合導(dǎo)航方法中具有舉足輕重的作用。在雙目視覺慣性導(dǎo)航系統(tǒng)工作時，IMU的數(shù)據(jù)采集頻率遠(yuǎn)遠(yuǎn)大于圖像關(guān)鍵幀的獲取頻率，如圖1所示。

假設(shè)給定時刻k和k+1的兩幀圖像，以及k到k+1時間內(nèi)的慣性測量值，則兩個時刻的狀態(tài)有如下關(guān)系

(1)

(2)

(3)

(4)

(5)

(6)

其中

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

式中，i為對應(yīng)于k到k+1時間內(nèi)的IMU測量的離散時刻；δt為i到i+1之間的時間間隔。

1.2 協(xié)方差矩陣計(jì)算

在k到k+1之間的兩個離散時刻，計(jì)算不同測量的協(xié)方差矩陣，用來做加權(quán)融合。通過非線性系統(tǒng)的線性過程，t時刻誤差項(xiàng)的導(dǎo)數(shù)為

(16)

(17)

式(17)表示下一時刻IMU測量誤差與上一時刻的測量誤差呈線性關(guān)系。根據(jù)當(dāng)前時刻的狀態(tài)，就可以預(yù)測下一時刻的IMU均值和協(xié)方差，協(xié)方差預(yù)測公式如下

(18)

1.3 IMU殘差

IMU的殘差表示兩幀之間的狀態(tài)約束關(guān)系，可以用IMU的狀態(tài)變化量來表示

(19)

式中第3行的[]im表示對四元數(shù)的取虛部，是四元數(shù)誤差的近似表示。IMU殘差項(xiàng)的協(xié)方差矩陣即是預(yù)積分迭代后增量誤差的協(xié)方差矩陣。

2 雙目圖像處理

2.1 圖像處理

經(jīng)過離線校準(zhǔn)每個相機(jī)的固有參數(shù)和相機(jī)之間的非固有變換，在雙目攝像頭獲取圖像信息之后；對每一幀圖像提取Harris角點(diǎn)[12]，對左右目圖像提取的點(diǎn)進(jìn)行匹配，采用Lucas-Kanade光流法[13]跟蹤相鄰幀的角點(diǎn)；最后將跟蹤的結(jié)果發(fā)送至優(yōu)化后端進(jìn)行處理。圖像處理的具體流程如圖2所示。

2.2 視覺殘差

(20)

圖像觀測的結(jié)果決定了視覺殘差的確定性，協(xié)方差矩陣與相機(jī)的標(biāo)定結(jié)果有關(guān)，在標(biāo)定相機(jī)內(nèi)參時，重投影誤差一般在0.5～1像素之間。

2.3 IMU初始化

由于非線性系統(tǒng)對系統(tǒng)初值精度的要求，對比于VINS-Mono提出的松耦合的初始化方法，本文提出了一種緊耦合穩(wěn)健的IMU初始化方法。通過求解視覺慣性系統(tǒng)的解析解(如圖3所示)，可以算得初始的重力矢量、速度和三維點(diǎn)的深度。為了有效避免IMU漂移的情況，在后端優(yōu)化中使用這些結(jié)果作為初值。

假設(shè)時刻k和時刻k+1同時觀測到編號為j的特征點(diǎn)，k為初始幀。對應(yīng)單個點(diǎn)的觀測，有特征點(diǎn)觀測量與IMU觀測量之間的直接關(guān)系

(21)

(22)

3 視覺慣導(dǎo)融合

采用一種滑動窗口的緊耦合優(yōu)化方法進(jìn)行穩(wěn)健且高精度的狀態(tài)估計(jì)。基于緊耦合優(yōu)化的視覺慣性融合方法將滑動窗口內(nèi)所有的待估計(jì)變量(包括位置、姿態(tài)、速度、IMU零偏、相機(jī)-IMU外參、所有點(diǎn)的深度)都放在一個優(yōu)化框架進(jìn)行優(yōu)化。

3.1 目標(biāo)函數(shù)

整個滑動窗內(nèi)的狀態(tài)向量定義如下

(23)

狀態(tài)估計(jì)的本質(zhì)是一個極大似然估計(jì)問題，然后對于一個符合高斯噪聲模型的SLAM問題進(jìn)行最大后驗(yàn)概率推斷，求解一個非線性最小二乘問題。有如下形式表達(dá)

(24)

3.2 邊緣化信息

隨著時間的增加，系統(tǒng)的狀態(tài)數(shù)也隨之增加，計(jì)算復(fù)雜度也越來越高。為了約束計(jì)算的復(fù)雜性，再保留之前有用信息，本文采用了邊緣化技術(shù)。

邊緣化技術(shù)是由概率論中的邊緣分布所產(chǎn)生的，給定聯(lián)合概率分布P(x,y)，求出邊緣概率分布P(x)。在該非線性優(yōu)化問題中，邊緣化技術(shù)通過一系列操作將某些不需要的狀態(tài)標(biāo)量進(jìn)行消元。由高斯牛頓法求解非線性最小二乘問題可以表示為

(25)

式中，xa、xb為系統(tǒng)的兩個狀態(tài)變量,通過高斯消元，將H的左下角消掉可得

(26)

通過式(26)可以直接計(jì)算Δxb

(B-CTA-1C)Δxb=bb-CTA-1ba

(27)

由此式，即可舍棄狀態(tài)Δxa，直接求取Δxb，這就是邊緣化先驗(yàn)。

4 試驗(yàn)及分析

4.1 試驗(yàn)環(huán)境說明

本文在試驗(yàn)室中搭建模擬居家環(huán)境(如圖4所示)，在HUSKY A200平臺上搭載LeaDsense雙目相機(jī)，LeaDsense雙目攝像頭設(shè)備包含兩個快門攝像機(jī)及一個九軸IMU傳感，相機(jī)頻率為60幀，IMU頻率為200幀，操作系統(tǒng)為Ubuntu16.04版本。為驗(yàn)證本文中提到方法的可行性，首先對提出的初始化方法進(jìn)行了驗(yàn)證，其次驗(yàn)證了本文方法運(yùn)用于室內(nèi)居家服務(wù)機(jī)器人的定位精度及穩(wěn)定性。在模擬環(huán)境中分別隨機(jī)驅(qū)動機(jī)器人行走半圈、一圈及兩圈并得到試驗(yàn)數(shù)據(jù)。

4.2 試驗(yàn)結(jié)果

圖5為系統(tǒng)啟動時VINS-Mono初始化方法與本文初始化方法的效果及對定位精度的影響對比圖。其中圖5(a)為VINS-Mono初始定位結(jié)果，圖5(b)為本文方法在同等場景下的初始定位結(jié)果。本文方法的初值估計(jì)較為準(zhǔn)確，定位效果也更加出色，軌跡基本重合于RTS的真值軌跡。

圖6為采用本文方法驅(qū)動機(jī)器人行駛半圈、一圈和兩圈的試驗(yàn)結(jié)果圖，其中淺色為RTS高精度光學(xué)動作捕捉設(shè)備采集的真值數(shù)據(jù)軌跡，深色為本文方法的試驗(yàn)結(jié)果軌跡。從試驗(yàn)結(jié)果可以看出，本文提出的視覺慣導(dǎo)融合導(dǎo)航方法運(yùn)用在室內(nèi)服務(wù)機(jī)器人的精度較高，平均誤差能控制在0.1 m以內(nèi)，試驗(yàn)結(jié)果可到達(dá)亞厘米級。

5 結(jié) 語

本文提出了一種基于緊耦合非線性優(yōu)化的立體視覺慣性融合導(dǎo)航方法，并在自己搭建的居家環(huán)境下，分別對系統(tǒng)的初始化效果、定位精度及穩(wěn)定性進(jìn)行了驗(yàn)證，得到了很好的結(jié)果，在室內(nèi)條件下，定位平均誤差能控制在0.1 m以內(nèi)。為視覺慣導(dǎo)在居家服務(wù)機(jī)器人的定位途徑提供了參考；同時系統(tǒng)也存在需要改進(jìn)的地方，在硬件方面，本文中的IMU數(shù)據(jù)與相機(jī)數(shù)據(jù)時間同步精確度有待提高，系統(tǒng)在初始化時可以將該參數(shù)一起聯(lián)合進(jìn)行估計(jì)，從而完成更加精確的系統(tǒng)狀態(tài)估計(jì)。