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

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

蘇州大學(xué) 機(jī)電工程學(xué)院，江蘇 蘇州 215021

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

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

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

多目SLAM受益于多相機(jī)不同朝向下信息的豐富性，改善了單、雙目相機(jī)的SLAM方法的魯棒性，尤其在面對弱紋理以及視線受到遮擋的情形下。然而僅使用多相機(jī)無法改善圖像模糊，運(yùn)動(dòng)過快等缺陷。視覺與慣導(dǎo)融合后能彌補(bǔ)純視覺下快速運(yùn)動(dòng)、圖像模糊、過度曝光造成的定位精度低、運(yùn)動(dòng)丟失的缺點(diǎn)。視覺與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支持多種視覺慣性傳感器類型（單目攝像機(jī)+IMU，雙目攝像機(jī)+IMU，甚至僅限單目雙目攝像機(jī)）。相機(jī)和IMU具有良好的互補(bǔ)性，高頻率IMU可以彌補(bǔ)快速運(yùn)動(dòng)對相機(jī)的影響，反之相機(jī)能夠有效修正IMU的漂移。

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

1 方法流程

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

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

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

2.1 視覺處理部分

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

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

2.2 IMU預(yù)積分

IMU部分，加速度計(jì)獲得機(jī)器人運(yùn)動(dòng)的加速度，陀螺儀獲得角速度。由于外部存在噪聲，將IMU的真實(shí)值記為ω、a，測量值記為、，則有：

上標(biāo)g表示陀螺儀，a表示加速度計(jì)，w表示世界坐標(biāo)系world，b表示IMU坐標(biāo)系。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 優(yōu)化定位

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

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

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

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

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

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

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

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

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

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

圖3 相鄰時(shí)刻特征點(diǎn)速度Fig.3 Velocity of characteristic point at adjacent moments

用這個(gè)速度對觀測進(jìn)行補(bǔ)償可表示為：

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

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

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

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

5.1 實(shí)驗(yàn)介紹

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

圖5 相機(jī)安裝位置Fig.5 Location of camera

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

5.2 實(shí)驗(yàn)結(jié)果

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

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

5.2.1 小圈復(fù)雜環(huán)境實(shí)驗(yàn)結(jié)果

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

IO算法得到的軌跡，綠色為雙目VIO算法得到的軌跡，紅色為本文提出算法的軌跡，虛線為GPS的軌跡。根據(jù)圖6（a）可以直觀地看出單目VIO算法由于復(fù)雜的數(shù)據(jù)集環(huán)境，其估計(jì)的機(jī)器人軌跡與真實(shí)的軌跡相差較大，而雙目VIO和本文提出的算法得到的軌跡和真實(shí)軌跡較為接近。注意后半段，紅色和虛線仍然保持著高度的重合，而雙目VIO算法也由于IMU累計(jì)誤差產(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所示。可以看到和雙目的算法相比，多目VIO算法在初始階段的誤差較大，這與雙側(cè)兩個(gè)相機(jī)的初始化不充分有關(guān)，在后續(xù)的運(yùn)行中明顯比雙目算法更穩(wěn)定，最后的累積漂移也明顯減少?？偟膩碚f，在復(fù)雜的環(huán)境下，多目VIO算法的各項(xiàng)參數(shù)Mean、Median、Min、RMSE均低于雙目VIO算法，算法魯棒性更強(qiáng)，定位精度更高。要注意的是，多目的最大值Max和標(biāo)準(zhǔn)差Std都大于雙目，多目的最大值出現(xiàn)在一開始，是由于錄制的數(shù)據(jù)集并沒有一個(gè)良好的初始化而造成。而由于離散程度的不同，多目的最大值與平均值相差較大而造成Std也偏大。但總的來說，多目VIO算法更優(yōu)。

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

表1 雙目多目VIO與真實(shí)軌跡的絕對軌跡誤差的各項(xiàng)參數(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)境實(shí)驗(yàn)結(jié)果

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

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

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

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

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

6 結(jié)束語

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

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

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

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

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