一種魯棒的多目視覺慣性即時定位與建圖方法

傅 博，焦艷梅，丁夏清，吳 俊，熊 蓉

(1.浙江大學(xué)智能系統(tǒng)與控制研究所，杭州310027；2.浙江大學(xué)工業(yè)控制技術(shù)國家重點(diǎn)實(shí)驗(yàn)室，杭州310027)

1 引言

深空探測任務(wù)對空間機(jī)器人提出了全自主漫游的要求，其關(guān)鍵是機(jī)器人的精確自定位能力。深空探測作為一種機(jī)器人特殊應(yīng)用場景，與地面環(huán)境存在很多相似，可以借鑒目前快速發(fā)展的地面即時定位與建圖(SLAM)技術(shù)來滿足上述新需求。

目前公開的美國火星探測機(jī)器人(MER)的定位主要通過視覺實(shí)現(xiàn)[1-2]，由雙目相機(jī)通過匹配當(dāng)前時刻與前一時刻的圖像實(shí)現(xiàn)對自身前后兩個時刻位姿變換的估計(jì)[3]，并利用慣性測量單元(IMU)對翻滾角和俯仰角能觀的特性，提升了視覺里程計(jì)的性能[4]。雙目相機(jī)利用兩個相機(jī)之間的基線，相比于單目相機(jī)要更加穩(wěn)定和精確[5]，但視野較小，難以形成全位置、豐富視角的回訪定位；若遇到相機(jī)視野被障礙物遮擋、視覺特征不明顯、特征紋理重復(fù)性高以致難以匹配等情況，往往會丟失定位[3]，也難以用于機(jī)器人運(yùn)動速度過快以致成像模糊的情況[4]。

多目相機(jī)能夠利用多個視角的信息提供一個更廣闊的視野，可以解決視野問題。其中，Multi-Col-SLAM[6]將ORB-SLAM[7-8]擴(kuò)展到了多目的情況，提出了多目SLAM優(yōu)化方法，并且實(shí)現(xiàn)了多目情況下的回環(huán)檢測。Yang等[9-10]將 PTAMSLAM[11]擴(kuò)展到了多目的情況，給出了多目視覺SLAM中測量誤差雅克比的求解方法，并且基于上述方法在SLAM的數(shù)值優(yōu)化中引入了多個相機(jī)獲取的局部視覺特征。相比于單目、雙目視覺定位方法，多目視覺方法需要配置更多的硬件和運(yùn)算資源。以MultiCol-SLAM為例，在i7處理器上采集多目相機(jī)數(shù)據(jù)，定位可以獲得3 Hz的信息，而采集雙目相機(jī)數(shù)據(jù)并運(yùn)行ORB-SLAM定位，可以獲得15 Hz信息。不過實(shí)際使用中，3 Hz足夠完成實(shí)時定位，而多目視覺定位可以帶來魯棒性和定位精度的提升。

針對機(jī)器人快速運(yùn)動帶來的問題，Liu等[12]提出了一種適用于車體的多目視覺慣性算法，在計(jì)算相對位姿時，利用IMU獲得的翻滾角和俯仰角的信息簡化需要求解的每一次運(yùn)動的旋轉(zhuǎn)矩陣，進(jìn)而簡化了位姿估計(jì)過程。然而由于IMU的信息帶有噪聲，直接用于運(yùn)動估計(jì)會帶來一定的誤差；另外，其方法是分別求出相機(jī)和IMU的位姿，再在此基礎(chǔ)上進(jìn)行聯(lián)合優(yōu)化，這是一種松耦合的優(yōu)化方法，無法得出精確的優(yōu)化結(jié)果。

為解決上述問題，本文提出一種新的多目視覺慣性定位方法，結(jié)合多目相機(jī)的廣闊視野和IMU提供的高頻運(yùn)動信息來完成定位。與現(xiàn)有的松耦合優(yōu)化方法[12-13]不同，所提方法基于緊耦合優(yōu)化，將圖像的特征點(diǎn)加入至狀態(tài)向量，直接優(yōu)化出相機(jī)和IMU的位姿，得到精確的機(jī)器人位姿。

2 系統(tǒng)流程

如圖1所示，在多目VINS系統(tǒng)中，多相機(jī)提供視覺信息，IMU提供高頻運(yùn)動信息。算法流程分為3步：數(shù)據(jù)采集預(yù)處理、初始化以及優(yōu)化定位。數(shù)據(jù)采集模塊對采集到的圖像進(jìn)行特征提取，對IMU數(shù)據(jù)進(jìn)行預(yù)積分輸出給下一模塊。在初始化模塊，視覺前端進(jìn)行不同時刻不同相機(jī)之間的位姿解算，得到機(jī)器人相鄰兩個時刻的位姿更新。IMU完成初始化，得到世界坐標(biāo)系相對于慣性系的相對位姿，完成視覺慣性配準(zhǔn)。在優(yōu)化定位模塊，融合視覺重投影誤差和IMU誤差進(jìn)行緊耦合聯(lián)合優(yōu)化，輸出機(jī)器人位姿。

圖1 系統(tǒng)流程圖Fig.1 System flowchart

3 系統(tǒng)建模與位姿更新

3.1 坐標(biāo)系定義

世界坐標(biāo)系定義為W，機(jī)器人坐標(biāo)系定位為B，并定義旋轉(zhuǎn)矩陣R和平移向量t，則k時刻機(jī)器人坐標(biāo)系相對于世界坐標(biāo)系的變換矩陣WBkT可以表示為式(1)：

考慮硬件通用性，定義機(jī)器人固定相機(jī)數(shù)為N，第i個相機(jī)的相機(jī)坐標(biāo)系為Ci。 本文統(tǒng)一使用機(jī)器人坐標(biāo)系來表示機(jī)器人的運(yùn)動。因?yàn)橄鄼C(jī)之間都是剛性固定的，假定外參系數(shù)已知，那么各相機(jī)的位姿和觀測到的特征都可以通過外參轉(zhuǎn)到機(jī)器人坐標(biāo)系下，從而統(tǒng)一了待優(yōu)化量的坐標(biāo)系。

3.2 多目系統(tǒng)投影

參考Yang等[9-10]的工作，多目的投影模型(第j個特征點(diǎn)在第i個相機(jī)坐標(biāo)系下的圖像投影)如式(2)：

式中：π(·)為任意第n個相機(jī)的相機(jī)坐標(biāo)系相對于像素坐標(biāo)系的投影變換；為機(jī)器人在世界坐標(biāo)系的位姿；為第i個相機(jī)相對于機(jī)器人坐標(biāo)系的位姿(也就是外參)，由于傳感器剛性固定于機(jī)器人上，可以認(rèn)為BCiT是不變的；pj為第j個世界坐標(biāo)系下的空間3D點(diǎn)；uij為投影之后獲得的第ij個像素點(diǎn)。投影原理如圖2所示。圖中五角星為空間中3D點(diǎn)，紅色圓點(diǎn)為2D像素點(diǎn)。

圖2 投影原理圖Fig.2 Schematic diagram of projection

3.3 相機(jī)位姿更新

不失一般性，先計(jì)算機(jī)器人本體的位姿更新，再經(jīng)由外參的轉(zhuǎn)化更新為每一個相機(jī)的位姿更新。基于機(jī)器人從時刻k到時刻k+1的位姿變換，和機(jī)器人在時刻k的位姿，可以推導(dǎo)出機(jī)器人在時刻k+1的位姿，如式(3)：

同理，在已知相機(jī)與機(jī)器人之間外參的前提下，第i個相機(jī)從時刻k到時刻k+1的位姿更新可以表示為式(4)：

4 視覺慣性緊耦合與IMU初始化

4.1 緊耦合優(yōu)化

在本文方法中，SLAM系統(tǒng)有2個并行的線程，位姿跟蹤和地圖優(yōu)化。考慮空間環(huán)境的復(fù)雜與大尺度，為了能盡可能的保持算法的穩(wěn)定性，位姿跟蹤在前端實(shí)時完成，而地圖優(yōu)化部分則在后端運(yùn)行。

IMU預(yù)積分得到兩幀之間的約束，放入地圖中完成聯(lián)合優(yōu)化。本方法所提SLAM系統(tǒng)的地圖包含了N個相機(jī)在不同時刻的位姿、一組被這些相機(jī)觀測到的地圖3D點(diǎn)以及2個位姿之間的預(yù)積分約束。圖優(yōu)化框架如圖3所示?？梢钥吹?，機(jī)器人上固定的多相機(jī)使得機(jī)器人在任意時刻都可以觀測到更多的路標(biāo)點(diǎn)，而不同時刻之間的觀測也有了更密集的數(shù)據(jù)關(guān)聯(lián)，從而提升定位的準(zhǔn)確性和魯棒性。優(yōu)化變量的損失函數(shù)可以表示為式(5)：

式中：Eproj為重投影誤差，由像素點(diǎn)的測量值與公式(2)得到的像素點(diǎn)的估計(jì)值計(jì)算得到；Eimu為IMU項(xiàng)的誤差。兩項(xiàng)誤差同時優(yōu)化并更新機(jī)器人的位姿和空間中路標(biāo)點(diǎn)的位置。

圖3 圖優(yōu)化框架Fig.3 Framework of graph optimization

4.2 自動靜止初始化

多目VINS在使用之前需要完成IMU的初始化，初始化時要求一定的旋轉(zhuǎn)和平移運(yùn)動，而且對于運(yùn)動的大小和方式有一定的要求，導(dǎo)致在初始化時容易因不恰當(dāng)?shù)倪\(yùn)動而失敗，進(jìn)而影響多目VINS的跟蹤定位效果。另外，多目VINS的初始化要求在一些情況下無法被滿足，例如安裝在無人車上時，初始化采集的數(shù)據(jù)缺少俯仰角和翻滾角的信息，從而容易導(dǎo)致初始化失敗。本文提出一種穩(wěn)定的靜止初始化算法，在機(jī)器人運(yùn)行前先靜置幾秒完成初始化，從而完成穩(wěn)定的多目VINS算法。

對于多目VINS系統(tǒng)的自動IMU初始化的算法，IMU的測量值加速度 3×1向量 a滿足式(6)[12]：

式中：einit為需要優(yōu)化的誤差向量，g為重力加速度3×1向量，矩陣為IMU坐標(biāo)系相對于世界坐標(biāo)系的旋轉(zhuǎn)3×3矩陣，其偏航角設(shè)為零。在靜止?fàn)顟B(tài)下采集多組IMU數(shù)據(jù)，然后聯(lián)合優(yōu)化求解得到初始的俯仰角和翻滾角，從而解出旋轉(zhuǎn)矩陣完成靜止初始化，得到世界坐標(biāo)系相對于慣性系的相對位姿。

5 實(shí)驗(yàn)驗(yàn)證

5.1 平臺搭建

在V-REP中搭建仿真環(huán)境，在移動機(jī)器人上安裝多目相機(jī)與IMU，采集數(shù)據(jù)完成了實(shí)驗(yàn)，如圖4所示。多目相機(jī)的固定方式為：一個朝前的雙目相機(jī)和一個斜朝后的單目相機(jī)。

圖4 移動機(jī)器人傳感器配置Fig.4 Configuration of mobile robot

5.2 結(jié)果分析

由于移動機(jī)器人只能在平面上運(yùn)動，無法進(jìn)行各角度的充分運(yùn)動來完成初始化。用靜止初始化方法，先采集靜置機(jī)器人的數(shù)據(jù)，再控制機(jī)器人繞圈運(yùn)動采集多目圖像和IMU數(shù)據(jù)。實(shí)驗(yàn)結(jié)果如圖5所示。圖中紅色、黑色的點(diǎn)為地圖路標(biāo)點(diǎn)，藍(lán)色的框?yàn)闄C(jī)器人位姿，綠色線為相機(jī)不同時刻之間的共視。從圖中可以看到，即使機(jī)器人只運(yùn)動了較短的一段距離，已經(jīng)能建出全局的地圖，并且不同相機(jī)之間產(chǎn)生了豐富的數(shù)據(jù)關(guān)聯(lián)，所以重建出的路標(biāo)點(diǎn)較為稠密，且輪廓分明。此重建結(jié)果可以說明，多目相機(jī)提供的更為廣闊的視野對于定位是有益的。

圖5 移動機(jī)器人實(shí)驗(yàn)結(jié)果Fig.5 Experiment results of mobile robot

保存所提定位算法的軌跡，而軌跡真值則由V-REP仿真平臺提供，計(jì)算相對軌跡誤差(RPE)如圖6所示，上圖為平移誤差，單位是m，下圖是旋轉(zhuǎn)誤差，取一個角度的值，單位是deg。

圖6 相對軌跡誤差Fig.6 Relative pose error

可以看到，本文所提方法的平移誤差略好約ORB-SLAM方法，旋轉(zhuǎn)誤差遠(yuǎn)優(yōu)于ORB-SLAM方法。主要原因是融合IMU的高頻運(yùn)動信息可以帶來更好的旋轉(zhuǎn)精度，而且多目相機(jī)提供了更廣闊的視野和更豐富的數(shù)據(jù)關(guān)聯(lián)，可以增加回環(huán)的概率從而修正旋轉(zhuǎn)誤差。

多目相機(jī)帶來了新的觀測的同時也帶來了新的外點(diǎn)，這些外點(diǎn)的存在一定程度上降低了定位算法的精度。下一部分的工作是利用算法在引入觀測的同時去除外點(diǎn)，從而使得新增的都是有用的觀測，并且優(yōu)化算法，提高后端優(yōu)化的速度來提升定位精度。

6 結(jié)論

1)多目相機(jī)可以提供一個更廣闊的視野，提供更多的觀測和更豐富的數(shù)據(jù)關(guān)聯(lián)，提升定位算法的準(zhǔn)確性和魯棒性。

2)多目視覺慣性緊耦合優(yōu)化可以提高定位方法的魯棒性，更好地應(yīng)用于快速移動、急停急走的應(yīng)用場景，并且可以在視覺丟失的情況下輔助機(jī)器人繼續(xù)完成定位。

3)自動靜止初始化算法，使得所提方法不受應(yīng)用場景的限制，提高了所提定位方法的穩(wěn)定性。