室內環境下立體視覺慣導融合定位

敖龍輝，郭 杭

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

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

1 數據處理方法

1.1 IMU預積分技術

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

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

(1)

(2)

(3)

(4)

(5)

(6)

其中

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

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

1.2 協方差矩陣計算

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

(16)

(17)

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

(18)

1.3 IMU殘差

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

(19)

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

2 雙目圖像處理

2.1 圖像處理

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

2.2 視覺殘差

(20)

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

2.3 IMU初始化

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

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

(21)

(22)

3 視覺慣導融合

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

3.1 目標函數

整個滑動窗內的狀態向量定義如下

(23)

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

(24)

3.2 邊緣化信息

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

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

(25)

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

(26)

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

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

(27)

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

4 試驗及分析

4.1 試驗環境說明

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

4.2 試驗結果

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

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

5 結 語

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