黃秀珍，伍一帆，李凱濤

(1.浙江理工大學(xué) 科技與藝術(shù)學(xué)院，浙江 紹興312369；2.杭州中威電子股份有限公司，浙江 杭州310000)

0 引言

如今，機(jī)器人的應(yīng)用空間已走出實(shí)驗(yàn)室面向各個(gè)領(lǐng)域，小到日常生活中的家用吸塵器、快遞分揀機(jī)等，大到國家領(lǐng)域中的深海采礦集礦機(jī)、無人機(jī)偵察等[1]。

在移動(dòng)機(jī)器人領(lǐng)域，根據(jù)給定的環(huán)境地圖有效確定所處位置是一項(xiàng)基本任務(wù)，但在缺少地圖的情況下，機(jī)器人將無法進(jìn)行自主定位[2]。機(jī)器人可以通過攜帶傳感器來獲取地圖以及估計(jì)自身位置和姿態(tài)。最常見的方法是結(jié)合衛(wèi)星信號(hào)完成位姿估計(jì)，但是衛(wèi)星導(dǎo)航容易受到環(huán)境的影響，出現(xiàn)較大誤差。而機(jī)器視覺SLAM算法可以搭載視覺傳感器的主體，在沒有環(huán)境先驗(yàn)信息的情況下，在運(yùn)動(dòng)過程中建立環(huán)境的模型，同時(shí)估計(jì)自己的運(yùn)動(dòng)[3]。因此視覺SLAM算法具有很高的研究價(jià)值，也是目前機(jī)器人視覺的研究熱點(diǎn)。

基于濾波器的后端視覺SLAM定位算法最早出現(xiàn)于2002年，Davison等人[4]開始著手研究完整的視覺SLAM定位系統(tǒng)，實(shí)現(xiàn)了迭代擴(kuò)展卡爾曼濾波(IEKF)的實(shí)時(shí)視覺SLAM系統(tǒng)，該研究首次建立了包含預(yù)測和噪聲模型的視覺SLAM系統(tǒng)?，F(xiàn)在人們以傳統(tǒng)卡爾曼濾波(KF)算法為基礎(chǔ)，涌現(xiàn)出大批基于擴(kuò)展卡爾曼濾波、無跡卡爾曼濾波(UKF)和粒子濾波的位姿估計(jì)算法[5-6]。G.Loianno等人提出一個(gè)基于李群SE (3)的 UKF的視覺慣性里程計(jì)系統(tǒng)，以獲得一個(gè)獨(dú)特的、無奇異性的剛體姿態(tài)表示(SE(3)-UKF算法)[7]。Martin Brossard等人把視覺慣性和基于李群的無跡卡爾曼算法相結(jié)合[8],提出了L-UKF-LG算法和R-UKF-LG算法。本文以文獻(xiàn)[8]的算法為基礎(chǔ)，把UKF的系統(tǒng)狀態(tài)用李群表示，并構(gòu)建視覺慣性緊耦合模型，對文獻(xiàn)[8]的算法進(jìn)行改進(jìn)，并在Matlab環(huán)境下對傳統(tǒng)UKF算法、傳統(tǒng)IEKF算法、SE(3)-UKF算法、L-UKF-LG算法和R-UKF-LG算法進(jìn)行仿真分析。

1 UKF-LG視覺SLAM算法設(shè)計(jì)

本次仿真實(shí)驗(yàn)算法優(yōu)化了傳統(tǒng)UKF的系統(tǒng)狀態(tài)，采用了UKF-LG視覺SLAM算法。算法通過視覺傳感器對數(shù)量固定的三維路標(biāo)進(jìn)行觀測，設(shè)定路標(biāo)不隨機(jī)器人的移動(dòng)而產(chǎn)生變化，并采用慣性傳感器(IMU)觀測值和視覺測量值融合的方法解決了視覺慣導(dǎo)工作頻率不一致的問題，構(gòu)建視覺慣性緊耦合模型[9]。下面將分別介紹視覺慣性緊耦合模型、系統(tǒng)狀態(tài)模型以及UKF-LG視覺SLAM算法流程。

1.1 視覺慣性緊耦合模型

視覺慣性緊耦合模型如圖1所示，IMU提供機(jī)器人當(dāng)前時(shí)刻的線加速度和角加速度信息，相機(jī)提供路標(biāo)點(diǎn)的觀測信息。系統(tǒng)狀態(tài)模型中的位姿信息需要對IMU數(shù)據(jù)計(jì)算得到的估計(jì)結(jié)果以及相機(jī)的觀測結(jié)果，并通過濾波器來得到最優(yōu)的估計(jì)結(jié)果，UKF-LG算法對應(yīng)于圖中的“濾波器”部分。另外，IMU輸出信息的速度和相機(jī)輸出圖像信息的速度并不是保持一致的，為了解決這一問題，設(shè)定IMU的信息輸入周期為濾波周期[10]。如果當(dāng)前時(shí)刻只有IMU信息輸入，則緊耦合模型僅使用IMU的數(shù)據(jù)進(jìn)行狀態(tài)估計(jì)和更新；如果圖像信息和IMU數(shù)據(jù)一起輸入，則緊耦合模型將兩個(gè)信息融合進(jìn)行濾波，估計(jì)系統(tǒng)狀態(tài)并更新。

圖1 視覺慣性緊耦合框架Fig.1 Visual inertial tight coupling frame

1.2 系統(tǒng)狀態(tài)模型

對于一個(gè)裝載了IMU和視覺傳感器的機(jī)器人，想要得到它的位姿信息，就需要估計(jì)它的旋轉(zhuǎn)矩陣R、速度v、位移x、IMU偏差bk=[wk,b,ak,b]，以及空間中的n個(gè)路標(biāo)在世界坐標(biāo)系下的位置信息y=[y1,y2,…,yn][11]，具體的系統(tǒng)狀態(tài)模型為：

(1)

式中，旋轉(zhuǎn)矩陣R(k)表示從IMU坐標(biāo)系到世界坐標(biāo)系的變化，wk,i和ak,i表示k時(shí)刻IMU輸出的角速度和線加速度，gG表示當(dāng)前位置的重力加速度，dt為測量周期。式(1)中的誤差均設(shè)定為方差為0的高斯白噪聲：

n=[nw,na,nbw,naw]～N(0,Q)。

(2)

對于式(1)中的狀態(tài)量，可以聯(lián)合起來構(gòu)成特殊歐式群矩陣Xk∈SE(3)，特殊歐式群是一種李群，可以用來描述三維空間的旋轉(zhuǎn)和平移，Xk的表示形式為：

(3)

特殊歐式群矩陣Xk不能進(jìn)行歐式加法運(yùn)算，只能采用指數(shù)映射的計(jì)算方式，而IMU偏差bk滿足歐式加法運(yùn)算，所以把這兩項(xiàng)寫出分塊矩陣的形式，系統(tǒng)的狀態(tài)方程表示為：

[Xk,bk]=f(Xk-1,Uk-bk-1,n),

(4)

式中，Uk=[wk,b,ak,b]為k時(shí)刻IMU測量到的角速度和線加速度。

觀測模型用來描述k時(shí)刻機(jī)器人自身位姿與觀測到的路標(biāo)之間的關(guān)系，所以只有在視覺傳感器檢測到了路標(biāo)后才有觀測方程[12]，k時(shí)刻的觀測方程中存放的是當(dāng)前時(shí)刻所有特征點(diǎn)的空間信息。下面給出系統(tǒng)的觀測模型，對環(huán)境中路標(biāo)的觀測值為Z(k)，觀測噪聲為ny：

(5)

式中，K為相機(jī)的內(nèi)參矩陣；RC和tc分別表示從IMU 坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣和平移量。

1.3 UKF-LG視覺SLAM算法流程

下面給出UKF-LG視覺SLAM算法流程。

1.3.1 預(yù)測階段

① 首先計(jì)算在李代數(shù)空間上2k+1維(k的取值由[Xk,bk]的維數(shù)確定)的sigma點(diǎn)的分布，sigma點(diǎn)的計(jì)算公式如下：

(6)

取λ=3-k，計(jì)算權(quán)值：

(7)

(8)

③ 求協(xié)方差矩陣P-，具體步驟為：

(9)

1.3.2 觀測更新階段

① 根據(jù)得到的協(xié)方差矩陣，重新計(jì)算sigma點(diǎn)：

(10)

(11)

(12)

1.3.3 濾波更新階段

① 計(jì)算卡爾曼增益Kk：

(13)

(14)

2 UKF-LG視覺SLAM算法Maltab實(shí)現(xiàn)

2.1 初始參數(shù)說明

本次仿真使用的代碼使用Euroc數(shù)據(jù)集[14]，在Matlab2018b環(huán)境下仿真。對傳統(tǒng)UKF算法、傳統(tǒng)IEKF算法[15]、SE(3)-UKF算法、L-UKF-LG算法和R-UKF-LG算法進(jìn)行對比仿真以驗(yàn)證算法準(zhǔn)確度，視覺傳感器的采集頻率為20 Hz，IMU的采集頻率為200 Hz。相機(jī)的校準(zhǔn)矩陣如下：

(15)

2.2 算法模塊框圖

算法的整體流程分為四大模塊：環(huán)境搭建、濾波法初始化、進(jìn)行定位算法以及計(jì)算并顯示誤差。四大模板塊的具體內(nèi)容如下：

① 環(huán)境搭建：設(shè)置了相機(jī)的采樣頻率和IMU采樣頻率；讀取數(shù)據(jù)集中通過相機(jī)拍攝的圖片、相機(jī)位姿的真實(shí)值以及IMU數(shù)據(jù)。

② 濾波器初始化：協(xié)方差矩陣的初始化以及過程噪聲和測量噪聲的設(shè)置；系統(tǒng)狀態(tài)的初始值以及相機(jī)的參數(shù)設(shè)置。

③ 進(jìn)行定位算法：前文提到的幾種濾波法的預(yù)測階段、更新階段、系統(tǒng)狀態(tài)估計(jì)值的保存以及特征點(diǎn)集的更新。

④ 計(jì)算并顯示誤差：計(jì)算5種濾波法對機(jī)器人位置和姿態(tài)估計(jì)的標(biāo)準(zhǔn)誤差(RMSE)，并且繪制出曲線。主函數(shù)的流程圖如圖2所示。

圖2 主函數(shù)流程圖Fig.2 Main function flow chart

圖2中5個(gè)濾波法預(yù)測階段的代碼由**Propagation.m實(shí)現(xiàn)，其中的**表示對應(yīng)的濾波法名稱(比如L-UKF的m文件是lUKFPropagation)；特征點(diǎn)檢測的代碼由ObserveLandmarks.m函數(shù)實(shí)現(xiàn)；狀態(tài)更新階段的代碼和預(yù)測階段類似，由**Update.m函數(shù)實(shí)現(xiàn)；記錄估計(jì)的姿態(tài)由UpdateTraj.m函數(shù)賦值給各個(gè)濾波法的狀態(tài)矩陣；特征點(diǎn)的補(bǔ)充由manageAmers.m函數(shù)運(yùn)用線性三角形法實(shí)現(xiàn)。

3 仿真結(jié)果分析

為了比較5種濾波法在估計(jì)位姿上的性能，以5種濾波法對位置和姿態(tài)估計(jì)的均方根誤差(RMSE)作為評價(jià)指標(biāo)。數(shù)據(jù)集以微型飛行器作為載具，并配置了視覺慣性傳感器在Vicon Room環(huán)境下采集數(shù)據(jù)，選擇V1_02_medium數(shù)據(jù)集中的cam0進(jìn)行仿真。

圖3為在cam0數(shù)據(jù)集下5種濾波法在各個(gè)時(shí)刻對無人機(jī)的空間位置估計(jì)結(jié)果的誤差，可以看處幾種濾波器的位置誤差差距并不是很明顯。從圖中可以看出，在整個(gè)時(shí)刻里，本文算法對于位置的估計(jì)要低于傳統(tǒng)UKF算法，特別是在40～48 s后無人機(jī)發(fā)生大幅度的旋轉(zhuǎn)(從數(shù)據(jù)集拍攝的視頻可知)后，L-UKF-LG比其他濾波法能保持的誤差更低。

圖3 位置估計(jì)結(jié)果Fig.3 Location estimation results

圖4為5種濾波法在cam0數(shù)據(jù)集下的姿態(tài)估計(jì)結(jié)果的誤差。UKF花費(fèi)了一定的時(shí)間才收斂在RMSE=1附近；40～48 s后無人機(jī)發(fā)生的大幅度旋轉(zhuǎn)對R-UKF-LG算法和IEKF算法的姿態(tài)比較結(jié)果RMSE產(chǎn)生了影響，數(shù)值明顯上升；SE(3)-UKF雖然使用了李群來表示狀態(tài)信息，但是沒有使用IMU來提供包括角速度在內(nèi)的各種關(guān)于位姿的物理量信息，導(dǎo)致產(chǎn)生的誤差一直累計(jì)，所以它的姿態(tài)RMSE值隨著時(shí)間上升。相對于位置的比較結(jié)果而言，L-UKF-LG算法的姿態(tài)RMSE誤差比較小，特別是30 s以后，能很好地保持在0.6以內(nèi)，收斂性比較好。

圖4 姿態(tài)估計(jì)結(jié)果Fig.4 Attitude estimation results

4 結(jié)束語

本文實(shí)現(xiàn)了UKF-LG視覺SLAM算法，并構(gòu)建視覺慣性緊耦合模型，在Matlab環(huán)境下實(shí)現(xiàn)UKF-LG算法,給出了5種濾波器在兩個(gè)Euroc數(shù)據(jù)集中的仿真結(jié)果對比圖。以RMSE為判斷標(biāo)準(zhǔn)，分析了濾波器的定位性能，得出L-UKF-LG相對于傳統(tǒng)的UKF算法，有更低的位置和姿態(tài)的RMSE，以及更好的定位精確性。本文中UKF對移動(dòng)機(jī)器人的定位雖然取得了一定效果，但仍有很多的情況沒有考慮，在狀態(tài)向量初始值等參數(shù)的選擇問題上還需要進(jìn)一步研究和測試，找到能使算法性能達(dá)到最優(yōu)或接近最優(yōu)的參數(shù)組合。