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

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

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

純視覺SLAM常用的包括單目、雙目以及RGBD相機。Mono SLAM[1]是2007年提出的基于單目視覺的SLAM框架，后端使用基于EKF（擴展卡爾曼濾波）的方法，追蹤前端稀疏的特征點，以相機的當前狀態(tài)和所有路標點為狀態(tài)量，更新其均值和協(xié)方差。PTAM[2]（parallel tracking and mapping）首次區(qū)分出前后端，是第一個使用非線性優(yōu)化作為后端的方案。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]相比之前的工作，支持單目、雙目、深度相機三種相機模式，該系統(tǒng)主要使用了跟蹤、地圖構(gòu)建、閉環(huán)檢測三個線程。由于其應(yīng)用廣，結(jié)構(gòu)完善，被認為是現(xiàn)代視覺SLAM系統(tǒng)中最為經(jīng)典的系統(tǒng)之一。

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

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

1 方法流程

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

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

2 數(shù)據(jù)預(yù)處理

2.1 視覺處理部分

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

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

2.2 IMU預(yù)積分

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

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

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

在IMU數(shù)據(jù)預(yù)處理階段，使用中值積分的方法計算離散時間的IMU預(yù)積分值，還計算了測量狀態(tài)的協(xié)方差矩陣以及計算了預(yù)積分殘差對旋轉(zhuǎn)四元數(shù)，速度和位移的雅可比矩陣。

至此數(shù)據(jù)預(yù)處理完成，進入下一模塊。

3 多目視覺IMU聯(lián)合初始化

3.1 主相機視覺慣性聯(lián)合初始化

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

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

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

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

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

其中權(quán)重計算的依據(jù)為兩條路徑的接近程度，這里用旋轉(zhuǎn)軸與旋轉(zhuǎn)角來衡量。旋轉(zhuǎn)角度誤差為：

Huber魯棒核權(quán)重為：

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

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

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

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

3.2 基于主相機的多目VIO聯(lián)合初始化

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

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

至此，完成多目與IMU的聯(lián)合初始化。

4 優(yōu)化定位

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

4.1 基于滑動窗口的非線性優(yōu)化

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

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

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

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

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

4.2 在線優(yōu)化時間戳

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

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

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

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

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

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

5 實驗

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

5.1 實驗介紹

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

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

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

5.2 實驗結(jié)果

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

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

5.2.1 小圈復雜環(huán)境實驗結(jié)果

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

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

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

圖6 數(shù)據(jù)1實驗結(jié)果Fig.6 Experimental results of data 1

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

5.2.2 大圈簡單環(huán)境實驗結(jié)果

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

圖7 數(shù)據(jù)2實驗結(jié)果Fig.7 Experimental results of data 2

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

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

如圖7（c）所示為文本提出的多目VIO算法和真實軌跡的絕對軌跡誤差曲線，結(jié)果顯示，多目VIO算法的RMSE為1.08 m，平均誤差為0.94 m，其他參數(shù)如表2所示?？梢钥吹胶碗p目的算法相比，多目VIO算法在初始階段仍存在初始化偏移大的問題，此時APE最大為3.38 m。在后續(xù)的運行中明顯比雙目算法更穩(wěn)定?？偟膩碚f，在簡單穩(wěn)定的環(huán)境下，多目VIO算法的各項參數(shù)Max、Mean、Median、Min、RMSE均低于雙目VIO算法，算法魯棒性更強，定位精度更高。要注意的是，與上個數(shù)據(jù)集實驗結(jié)果不同，多目的最大值Max與標準差Std均優(yōu)于雙目，此處雙目的Max出現(xiàn)于路程的后半段，Max值是多目的兩倍之多是由于機器人運行時間長、IMU累積誤差造成。而初始階段，多目VIO最大值Max還是由于初始化不良造成。

6 結(jié)束語

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

通過兩個不同的數(shù)據(jù)集對MCSI-VINS進行測試，得到以下結(jié)論：

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

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

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