多相機與IMU融合的室外機器人定位方法研究

董 榮，厲茂海，林 睿，劉仕琦，丁 文

蘇州大學 機電工程學院，江蘇 蘇州 215021

在人類社會和科技水平不斷進步的背景下，信息技術、人工智能、互聯網、機械電子等相關產業取得快速發展，室外移動機器人在工業和生活中的應用也越來越多。機器人的狀態估計問題一直是研究熱點。SLAM技術也就是同時定位和地圖創建，是實現室外移動機器人自主定位移動導航的關鍵技術。近年來，由于相機具有廉價、采集信息豐富、重量輕、安裝方便、易于與其他傳感器融合等優勢，基于視覺的SLAM方法受到研究生的廣泛關注，大量的相關方法也應運而生。

純視覺SLAM常用的包括單目、雙目以及RGBD相機。Mono SLAM[1]是2007年提出的基于單目視覺的SLAM框架，后端使用基于EKF（擴展卡爾曼濾波）的方法，追蹤前端稀疏的特征點，以相機的當前狀態和所有路標點為狀態量，更新其均值和協方差。PTAM[2]（parallel tracking and mapping）首次區分出前后端，是第一個使用非線性優化作為后端的方案。DTAM[3]（dense tracking and mapping）、LSD-SLAM[4]（large-scale direct monocular SLAM）、SVO[5]（fast semi-directmonocular visual odometry）、DSO[6]（direct sparse odometry）等都是基于直接法的單目或雙目SLAM方案。ORBSLAM[7-8]相比之前的工作，支持單目、雙目、深度相機三種相機模式，該系統主要使用了跟蹤、地圖構建、閉環檢測三個線程。由于其應用廣，結構完善，被認為是現代視覺SLAM系統中最為經典的系統之一。

而視覺部分，無論是單目、雙目還是RGBD相機均具有朝向單一、采集視野有限、受遮擋、信息不充分等缺點，研究表明，使用多相機可以有效地提高視覺SLAM的性能。2003年，Pless等人首次實現了在SFM領域使用多相機系統[9]。隨后，Ragab[10]基于EKF，在機器人上放了兩對背對背的立體相機實現了SLAM問題。Kaess等人[11]基于Harris角點[12]提出了多相機的SLAM系統。2013年，Lee團隊在無人駕駛領域使用了多相機系統[13]，實現了無人車的自定位。Harmat使用多相機對PTAM性能進行了改善[14]，改進了透視相機模型并在無人機上安裝了一個朝地的立體相機和一個朝天的廣角相機，提出了經典的MCPTAM（multi-camera parallel tracking and mapping）方法[15]。Tribou同樣基于PTAM以及MCPTAM的方法研究了具有非重疊視場角的SLAM方法[16]，改善了無人機定位的魯棒性。此外，Urban等人[17]基于ORBSLAM開展相關研究，國內楊紹武等人通過配置相機個數和朝向提高了無人機系統的魯棒性[18-19]。

多目SLAM受益于多相機不同朝向下信息的豐富性，改善了單、雙目相機的SLAM方法的魯棒性，尤其在面對弱紋理以及視線受到遮擋的情形下。然而僅使用多相機無法改善圖像模糊，運動過快等缺陷。視覺與慣導融合后能彌補純視覺下快速運動、圖像模糊、過度曝光造成的定位精度低、運動丟失的缺點。視覺與IMU（慣性測量單元）融合的算法有MSF[20]（multi-sensor fusion）、OKVIS[21]、MSCKF[22]（multi-state constraint Kalman filter）、ROVIO[23]（robust visual inertial odometry）、VINS等[24]。其中VINS Fusion支持多種視覺慣性傳感器類型（單目攝像機+IMU，雙目攝像機+IMU，甚至僅限單目雙目攝像機）。相機和IMU具有良好的互補性，高頻率IMU可以彌補快速運動對相機的影響，反之相機能夠有效修正IMU的漂移。

本文在此基礎上，提出了一種基于VINS-MONO算法的多目視覺與慣導緊耦合的定位方法MCSI-VINS，在數據預處理階段提出了一種基于隊列數據結構構建緩沖區的方法，并創新性地提出了一種在線的根據主相機的多相機初始化方法，前端使用Harris角點和LK（Lucas-Kanade）光流跟蹤。后端使用基于滑動窗口的非線性優化的方法。此次工作在后端部分增加了多相機的誤差項，誤差項中增加了時間戳的優化。此方案為室外移動機器人的長期、大規模定位問題提供了保障。最后通過實驗驗證了本文提出的算法較一些單目、雙目與IMU結合的算法相比，魯棒性更強，精度更高。

1 方法流程

本文提出的多目視覺與慣導緊耦合的定位方法中，多相機提供豐富的視覺信息，慣性測量單元提供高頻率運動信息。方法流程大體分成三步，數據預處理、多目VIO聯合初始化，優化定位。如圖1所示，紅色虛線方框部分為數據預處理階段，此階段的視覺處理部分對采集到的第一幀圖像進行Harris角點的提取并對之后每一幀進行光流追蹤，此階段的IMU部分對數據進行預積分。數據預處理的結果傳遞給下一模塊。在初始化模塊中，首先是主相機的視覺慣性聯合初始化，對光流跟蹤得到的特征點進行處理，求解幀與幀之間位姿的變換，融合IMU預積分信息后最終可以得到尺度信息并將所有量拉到世界坐標系下，完成主相機初始化。隨后根據主相機來完成多目相機與慣性的聯合初始化。在優化定位模塊，基于滑動窗口對視覺重投影誤差以及IMU誤差進行緊耦合聯合優化，其中增加了其他相機的誤差項并對時間戳也進行了在線的優化，不僅保證了系統的魯棒性，也能夠更加精確得到機器人位姿。

圖1 多目視覺與慣導融合方法流程圖Fig.1 Flow chart of multi-vision and inertial navigation fusion method

2 數據預處理

2.1 視覺處理部分

多目相機采集到的圖像信息豐富，在視覺處理部分，各個相機獨立采集圖像并進行數據處理。接下來以主相機為例，簡述主要流程。首先，對第一幀圖像進行Harris角點的提取。隨后對之前圖像進行LK金字塔光流跟蹤，在這個過程中，若光流跟蹤成功，則該特征點被跟蹤次數增加1，反之被剔除，并通過基礎矩陣剔除外點。基礎矩陣為一幅圖像上一個點到另一幅圖像上對極線L的一種映射。一般地，從匹配的點對中選擇8個點，使用歸一化估算出基礎矩陣F，隨后計算其余點對到其對應對極線的距離，如果距離小于閾值則為內點，否則為外點，迭代k次當某次去除外點后內點的數目占總數目的比例大于95%時，停止迭代。接著，尋找新的特征點進行數量的補齊，同時通過設置mask來保證特征點均勻分布。最后，對特征點進行去畸變矯正和深度歸一化，計算出角點速度，將上述特征點的相關信息傳遞給下一模塊。

在視覺處理過程中，其余相機同時進行數據預處理工作，在這里，提出了一種利用隊列這種數據結構，構造緩沖區的方法，由此來解決數據匹配問題。

2.2 IMU預積分

IMU部分，加速度計獲得機器人運動的加速度，陀螺儀獲得角速度。由于外部存在噪聲，將IMU的真實值記為ω、a，測量值記為、，則有：

上標g表示陀螺儀，a表示加速度計，w表示世界坐標系world，b表示IMU坐標系。

將相鄰圖像時間間隔內的IMU數據進行積分就可以得到這段時間內機器人位姿的變化。預積分使得和IMU測量值無關的部分無需重復積分計算，大大降低了計算量。預積分量僅僅跟IMU測量值有關，表示如下：

在IMU數據預處理階段，使用中值積分的方法計算離散時間的IMU預積分值，還計算了測量狀態的協方差矩陣以及計算了預積分殘差對旋轉四元數，速度和位移的雅可比矩陣。

至此數據預處理完成，進入下一模塊。

3 多目視覺IMU聯合初始化

3.1 主相機視覺慣性聯合初始化

默認選取前相機作為主相機，如果當前相機提取的特征點數量小于一個閾值時，包括相機受遮擋的情況，則通過判斷當前時刻各相機特征點數量而定，此時選取特征點最多的相機作為主相機。

基于主相機的初始化流程圖如圖2所示。

圖2 主相機初始化流程圖Fig.2 Main camera initialization flow chart

假設前端能夠跟蹤到足夠多的特征點，幀與幀之間的相機之間的旋轉變換可以通過滑動窗口的SFM（structure from motion）、經典五點算法、三角化、PNP（perspective-n-point），在滑動窗口中進行整個BA（bundle adjustment）的優化調整得到。與此同時可以通過IMU陀螺儀數據預積分得到相對應的旋轉變換。下面的式子表示任意兩幀k、k+1之間可以有兩條“路徑”來得到兩幀之間的旋轉，這里用四元數表示旋轉。

將多個時刻線性方程式（7）累計起來，并加上Huber魯棒核權重得到：

其中權重計算的依據為兩條路徑的接近程度，這里用旋轉軸與旋轉角來衡量。旋轉角度誤差為：

Huber魯棒核權重為：

公式（8）采用SVD分解，即最小奇異值對應的奇異向量。可以得到相機與IMU之間的旋轉約束。利用在線標定的相機與IMU之間的外參以及離線標定的外參，可估計陀螺儀bias，通過求解一個最小二乘得到：

對于n維變量x∈Rn，尋找一個最優值x*使代價函數F()

x取最小值，代價函數通常具有如下形式：

至此可以將第一幀圖像坐標系下的所有變量都轉換為世界坐標系下的值，并把相機平移和特征點尺度恢復到米制單位，完成一個相機的全部初始化。

3.2 基于主相機的多目VIO聯合初始化

上一節求得的世界坐標系與主相機第一幀相機坐標系之間的旋轉矩陣記為。除了在線標定和離線標定主相機與IMU之間的外參外，其余相機的外參也被在線和離線標定同時被優化。優化得到的任意相機與IMU之間的外參記為T b c j，這里用j來代表第j個相機。通過對齊重力方向求得的不同相機第一幀相機坐標系與世界坐標系的旋轉矩陣，這里要注意世界坐標系并非同一個，因此提出了一種基于主相機的多目初始化方法。

利用主相機，將所有不同相機中的位置、姿態、速度都轉化到同一個世界坐標系下，推導公式如下：

至此，完成多目與IMU的聯合初始化。

4 優化定位

隨著移動機器人不斷像新環境探索，就會有新的相機姿態以及看到新的特征點，最小二乘殘差就會越來越多，信息矩陣越來越大，計算量將不斷增加。為了保持優化變量的個數在一定范圍內，使用滑動窗口算法來動態移除變量。初始化之后，執行一個基于滑動窗口的緊耦合多目VIO，得到精度更高魯棒性更好的狀態估計值。在優化中還增加了其他相機的誤差項并對時間戳也進行了在線的優化。

4.1 基于滑動窗口的非線性優化

初始化之后，執行一個基于滑窗的緊耦合多目VIO，得到精度更高魯棒性更好的狀態估計值。與3.2節中的推導過程類似，可以求出世界坐標系與IMU坐標系之間的外參

其中，c1為主相機。在建立多目VIO狀態量以及殘差方程時，都以IMU坐標系作為參考系，不同的相機盡管優化的狀態量以及殘差方程完全不相同，但求得的狀態量的增量也就是機器人的運動姿態幾乎是相同的。

使用基于滑動窗口的緊耦合VIO，滑窗中的狀態向量定義為：

x k是某一時刻捕獲第k幀圖像時IMU的狀態。多目滑窗狀態變量共包括滑動窗口內的n+1個IMU的狀態，所有相機和IMU的外參，不同相機下mi+1個3d點的逆深度。目標函數如下：

其中三個殘差項分別是邊緣化的先驗信息，IMU測量殘差，視覺的重投影殘差，cn為相機的個數。

4.2 在線優化時間戳

基于相機與IMU發布頻率不同，相機IMU觸發的延遲，相機圖片傳輸的延遲，相機與IMU不同的時鐘以及相機曝光時間等因素，即使在系統運行前進行了時間戳的標定，也很難保證時間戳的同步，從而造成IMU預積分時間段錯誤導致里程計的失效。通過將時間差納入到重投影誤差中與IMU積分誤差一同優化來實現時間差的在線標定，是削減IMU與相機時鐘差對VINS精度影響的最佳方案。

假設在很短時間內，特征點的速度是恒定的。如圖3所示，在k時刻，相機檢測到這個點，在下一時刻又檢測到了這個點記為，則特征點的速度可表示為：

圖3 相鄰時刻特征點速度Fig.3 Velocity of characteristic point at adjacent moments

用這個速度對觀測進行補償可表示為：

將i時刻的觀測進行補償后得到，在圖4中以黃點表示，再反投到j時刻的圖像中，得到新的補償后的反投點，記為，在圖4中以紅點表示。原先j時刻的觀測也進行時間戳的補償，得到的點記為，在圖4中以綠點表示，則新的殘差可表示為：

圖4 時間戳補償構建殘差示意圖Fig.4 Timestamp compensates for build residuals

5 實驗

本文的前幾部分介紹了多目視覺與慣導緊耦合的定位方法的數據預處理、初始化以及優化定位模塊。本章對上文提出的方法進行實驗，展示了本文提出的多目和慣導融合的方案MCSI-VINS的實驗結果，并且對實驗結果進行對比分析。

5.1 實驗介紹

本實驗使用的室外機器人移動平臺通過無線鍵盤人工遙控，時間延遲小，可以做到實時控制。圖5為安裝在小車上的多目相機示意圖，面向車前的相機為小覓彩色版S2110-95/Color，一左一右為小覓S1030。

圖5 相機安裝位置Fig.5 Location of camera

為了更好地分析本文提出的MCSI-VINS和經典的VINS雙目和單目算法精度的優劣，實驗錄制了兩個不同的數據集。在數據集1中，小車圍繞某產業園行駛了約600 m的小圈并回到初始位置。在錄制過程中，人為加入了一些使環境更為復雜的因素。如，行人的遮擋、車輛的快速通過、遇到障礙物突然停止、接近于純旋轉的轉彎等。在數據集2中，小車圍繞同一產業園行駛了約1 800 m的大圈并回到初始位置。與數據1完全相反，在錄制過程中，盡量保證了環境因素的穩定。

5.2 實驗結果

為了更簡單地表示各個算法、約定，經典的VINS雙目算法簡稱為2CVIO，VINS單目算法簡稱為1CVIO，本文提出的MCSI-VINS簡稱為MCVIO。RTK-GPS得到的軌跡為真實軌跡，用groundtruth表示。在本章節的圖中，各自算法得到的軌跡也遵循上述約定表示。

實驗結果將算法得到的機器人運動軌跡和真實軌跡進行對比來評價。本文采用絕對軌跡誤差（absolute trajectory error，ATE）來對比，也稱為絕對位姿誤差（absolute position error，APE）。原理為將算法解算的軌跡與真實軌跡進行絕對距離評估，兩者以一定的標準對齊，然后計算偏差。同時，基于計算絕對位姿誤差，用RMSE的大小來評估最終算法的精度。也就是計算所有時間上的均方根誤差（root mean square error，RMSE）。

5.2.1 小圈復雜環境實驗結果

該實驗使用數據1進行實驗。利用1CVIO、2CVIO、MCVIO得到的機器人位姿軌跡和GPS得到的groundtruth軌跡如圖6（a）所示。其中藍色為單目V

IO算法得到的軌跡，綠色為雙目VIO算法得到的軌跡，紅色為本文提出算法的軌跡，虛線為GPS的軌跡。根據圖6（a）可以直觀地看出單目VIO算法由于復雜的數據集環境，其估計的機器人軌跡與真實的軌跡相差較大，而雙目VIO和本文提出的算法得到的軌跡和真實軌跡較為接近。注意后半段，紅色和虛線仍然保持著高度的重合，而雙目VIO算法也由于IMU累計誤差產生了些許的漂移。

如圖6（b）的evo結果顯示，雙目VIO算法的RMSE為1.71 m，平均誤差為1.42 m，其他參數如表1所示。如圖6（c）的evo結果顯示，多目VIO算法的RMSE為1.56 m，平均誤差為1.09 m，其他參數如表1所示。可以看到和雙目的算法相比，多目VIO算法在初始階段的誤差較大，這與雙側兩個相機的初始化不充分有關，在后續的運行中明顯比雙目算法更穩定，最后的累積漂移也明顯減少。總的來說，在復雜的環境下，多目VIO算法的各項參數Mean、Median、Min、RMSE均低于雙目VIO算法，算法魯棒性更強，定位精度更高。要注意的是，多目的最大值Max和標準差Std都大于雙目，多目的最大值出現在一開始，是由于錄制的數據集并沒有一個良好的初始化而造成。而由于離散程度的不同，多目的最大值與平均值相差較大而造成Std也偏大。但總的來說，多目VIO算法更優。

圖6 數據1實驗結果Fig.6 Experimental results of data 1

表1 雙目多目VIO與真實軌跡的絕對軌跡誤差的各項參數（數據1）Table 1 Parameters of absolute trajectory error between binocular multivision VIO and groundtruth（data 1）m

5.2.2 大圈簡單環境實驗結果

該實驗使用數據2進行實驗。利用1CVIO、2CVIO、MCVIO得到的機器人位姿軌跡和GPS得到的groundtruth軌跡如圖7（a）所示。其中紅色為單目VIO算法得到的軌跡，藍色為雙目VIO算法得到的軌跡，綠色為本文提出算法的軌跡，虛線為GPS的軌跡。根據圖7（a）可以直觀地看出，單目VIO算法于簡單穩定的環境下，其軌跡與真實軌跡相比復雜環境更為相近，但由于其初始化不充分而造成單目VIO算法與其余算法相比仍舊存在定位精度上的差距。而雙目VIO和本文提出的算法得到的軌跡和真實軌跡較為接近。綠色軌跡和虛線始終保持著高度的重合，而雙目VIO算法也由于IMU累計誤差產生了些許的漂移，在各個轉彎時漂移稍大。

圖7 數據2實驗結果Fig.7 Experimental results of data 2

雖然可以直觀地看出單目VIO算法的定位精度明顯不佳于其余算法，但卻無法明確地看出雙目VIO和多目VIO的優劣。如圖7（b）為雙目VIO算法和真實軌跡的evo評估結果，雙目VIO算法的RMSE為2.78 m，平均誤差為2.48 m，其他參數如表2所示。

表2 雙目多目VIO與真實軌跡的絕對軌跡誤差的各項參數（數據2）Table 2 Parameters of absolute trajectory error between binocular multivision VIO and groundtruth（data 2）m

如圖7（c）所示為文本提出的多目VIO算法和真實軌跡的絕對軌跡誤差曲線，結果顯示，多目VIO算法的RMSE為1.08 m，平均誤差為0.94 m，其他參數如表2所示。可以看到和雙目的算法相比，多目VIO算法在初始階段仍存在初始化偏移大的問題，此時APE最大為3.38 m。在后續的運行中明顯比雙目算法更穩定。總的來說，在簡單穩定的環境下，多目VIO算法的各項參數Max、Mean、Median、Min、RMSE均低于雙目VIO算法，算法魯棒性更強，定位精度更高。要注意的是，與上個數據集實驗結果不同，多目的最大值Max與標準差Std均優于雙目，此處雙目的Max出現于路程的后半段，Max值是多目的兩倍之多是由于機器人運行時間長、IMU累積誤差造成。而初始階段，多目VIO最大值Max還是由于初始化不良造成。

6 結束語

在未知環境下的定位問題中，本文針對單目相機在室外復雜環境下易受到遮擋、相機移動過快、圖像模糊以及機器人純旋轉下算法精度和魯棒性下降、低精度IMU的累積偏移等問題，提出一種多目相機與IMU融合的方案。首先在數據預處理階段提出了一種基于隊列數據結構構建緩沖區的方法，并創新性地提出了一種在線的根據主相機的多相機初始化方法，前端使用Harris角點和LK光流跟蹤。后端使用基于滑動窗口的非線性優化的方法。此次工作在后端部分增加了多相機的誤差項，誤差項中增加了時間戳的優化。此算法保證了系統的魯棒性，也能夠更加精確得到機器人位姿。

通過兩個不同的數據集對MCSI-VINS進行測試，得到以下結論：

（1）多目相機可以提供一個更廣闊的視野，提供了不同方位的特征點信息，在某相機被遮擋的情況下，仍能保證定位算法的準確性和魯棒性。

（2）本文提出的MCSI-VINS方法不論室外是簡單穩定的環境還是復雜環境，較經典的單目、多目VINS算法來說，其魯棒性更好，定位精度更高。

（3）對于數據集的錄制，給本文提出的算法一個更好的初始化過程能夠使其性能發揮更好。