敖龍輝，郭 杭

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

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

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

1.1 IMU預積分技術

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

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

(1)

(2)

(3)

(4)

(5)

(6)

其中

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

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

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

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

(16)

(17)

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

(18)

1.3 IMU殘差

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

(19)

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

2 雙目圖像處理

2.1 圖像處理

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

2.2 視覺殘差

(20)

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

2.3 IMU初始化

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

假設時刻k和時刻k+1同時觀測到編號為j的特征點，k為初始幀。對應單個點的觀測，有特征點觀測量與IMU觀測量之間的直接關系

(21)

(22)

3 視覺慣導融合

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

3.1 目標函數(shù)

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

(23)

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

(24)

3.2 邊緣化信息

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

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

(25)

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

(26)

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

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

(27)

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

4 試驗及分析

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

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

4.2 試驗結果

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

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

5 結 語

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