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

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

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

1 引言

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

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

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

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

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

2 系統(tǒng)流程

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

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

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

3.1 坐標系定義

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

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

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

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

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

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

3.3 相機位姿更新

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

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

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

4.1 緊耦合優(yōu)化

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

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

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

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

4.2 自動靜止初始化

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

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

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

5 實驗驗證

5.1 平臺搭建

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

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

5.2 結果分析

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

圖5 移動機器人實驗結果Fig.5 Experiment results of mobile robot

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

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

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

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

6 結論

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

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

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