黃蘇軍，魏國亮，管 啟，李 卓，趙 珊

1(上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海 200093)

2(上海理工大學(xué) 理學(xué)院，上海 200093)

1 引 言

近年來，各式各樣的自主移動機(jī)器人、無人機(jī)、自動駕駛汽車、穿戴式ARVR設(shè)備等智能硬件產(chǎn)品逐漸進(jìn)入人們的日常生活中.其背后所蘊(yùn)含的SLAM(Simultaneous Localization and Mapping)技術(shù)也逐漸成為研究熱點(diǎn).SLAM技術(shù)即同時定位與地圖構(gòu)建技術(shù)，是指搭載特定傳感器的主體，在沒有環(huán)境先驗(yàn)信息的情況下，于運(yùn)動過程中建立環(huán)境的模型，同時估計(jì)自己的運(yùn)動[1，2].

在SLAM技術(shù)中，如果僅以相機(jī)為傳感器來完成機(jī)器人的實(shí)時定位與建圖，對應(yīng)的SLAM技術(shù)被稱為視覺SLAM.迄今為止，已有許多視覺SLAM系統(tǒng)[3-5]被提出，它們能夠支持單目、雙目、RGB-D 3種相機(jī)中的一種或多種來進(jìn)行機(jī)器人的實(shí)時定位與建圖.然而，在視覺SLAM中，使用單目相機(jī)存在尺度不確定性的問題，使用雙目相機(jī)存在對大量圖像特征進(jìn)行匹配導(dǎo)致計(jì)算負(fù)擔(dān)重的問題，使用RGB-D相機(jī)存在受物體表面影響導(dǎo)致準(zhǔn)確性降低的問題，而且這3種類型相機(jī)都存在采集的圖像視野范圍有限，在相機(jī)快速運(yùn)動時容易出現(xiàn)圖像特征不足或缺失等問題[6，7].因此，視覺SLAM在實(shí)際應(yīng)用中存在一定弊端.針對視覺SLAM中存在的問題，可以通過多傳感器融合技術(shù)來解決.

視覺慣性SLAM是一項(xiàng)融合相機(jī)傳感器和慣性傳感器數(shù)據(jù)的SLAM技術(shù).視覺慣性SLAM中的相機(jī)傳感器可以獲取所拍攝物體的圖像信息，價格低、功耗低，可長時間穩(wěn)定工作，但易受外部環(huán)境影響.而視覺慣性SLAM中的IMU傳感器可以測量載體的角速度和加速度，具有功耗低、采樣頻率高和不受外部環(huán)境干擾等優(yōu)點(diǎn)，但在長時間工作情況下存在累積漂移問題.相機(jī)可以長時間提供較好的位姿估計(jì)，IMU能在短時間內(nèi)對物體位姿進(jìn)行良好的估計(jì)，且不少受環(huán)境干擾.二者具有明顯的互補(bǔ)性，因而經(jīng)常被聯(lián)合使用于多傳感器融合技術(shù)中.以相機(jī)和IMU為傳感器的視覺慣性SLAM技術(shù)廣泛應(yīng)用于多個領(lǐng)域，而如何更好地融合相機(jī)傳感器和IMU傳感器實(shí)現(xiàn)更準(zhǔn)確地定位，則是當(dāng)下SLAM技術(shù)研究的重中之重[8].

目前，許多視覺慣性SLAM系統(tǒng)采用單個IMU和相機(jī)作為傳感器采集數(shù)據(jù).其中，MSCKF(Multi-State Constraint Kalman Filter)[9]是一種經(jīng)典的基于濾波的緊耦合視覺慣性SLAM系統(tǒng)，不同于傳統(tǒng)的基于擴(kuò)展卡爾曼濾波的方式，它并不將特征點(diǎn)加入到狀態(tài)向量中，而是當(dāng)某個特征點(diǎn)多次被相機(jī)觀測到時，將特征點(diǎn)與多個相機(jī)位姿之間的關(guān)聯(lián)約束用于擴(kuò)展卡爾曼濾波更新.基于MSCKF這一經(jīng)典框架，Mourikis和Li通過首次估計(jì)雅可比(FEJ)修正可觀性并在線標(biāo)定IMU和相機(jī)的外參，提出了MSCKF2.0[10].Ke Sun等人將MSCKF中的單目相機(jī)擴(kuò)展到雙目相機(jī)并將其代碼開源，提出了S-MSCKF[11].Patrick等人在MSCKF基礎(chǔ)上增加了特征點(diǎn)的表示形式和相機(jī)內(nèi)外參的在線標(biāo)定，提出了OpenVINS[12].Kevin等人在MSCKF框架下引入了多個相機(jī)的測量信息并在線標(biāo)定相機(jī)的內(nèi)外參，提出了mc-VINS[13].

雖然上述視覺慣性導(dǎo)航系統(tǒng)使用單個IMU和相機(jī)已經(jīng)能夠較好的估計(jì)物體位姿，但在系統(tǒng)運(yùn)行過程中，可能受環(huán)境影響出現(xiàn)撞擊或劇烈震動造成IMU傳感器連接斷開或損壞[14]，這將讓使用IMU約束物體位姿估計(jì)問題的系統(tǒng)定位精度迅速下降甚至不能正確定位.因此，不能僅靠單個IMU對物體位姿估計(jì)進(jìn)行約束，需要引入冗余的慣性傳感器來應(yīng)對其故障.而隨著硬件制造工藝水平的不斷提高，基于MEMS制造工藝的IMU產(chǎn)品體積、價格以及功耗大大降低，這使得在視覺慣性導(dǎo)航系統(tǒng)中配備多個IMU成為可能.多個IMU的使用可以保證當(dāng)某些IMU故障時，系統(tǒng)仍能正常運(yùn)行.

對于多個IMU的使用方法已有大量的研究[15，16]，但早期的工作研究局限于傳感器故障檢測和隔離(FDI)，而不是提高傳感器的定位精度.

在視覺慣性SLAM系統(tǒng)中，目前使用多個IMU和相機(jī)組合進(jìn)行定位研究的工作較少.Kevin[17]等人提出了mi-VINS(multi-IMU visual-inertial navigation system)，mi-VINS利用3個不同型號的IMU傳感器與相機(jī)組合用于定位.它將每個IMU的狀態(tài)都加入到系統(tǒng)的狀態(tài)向量中并推導(dǎo)了各IMU之間的相對姿態(tài)約束關(guān)系.實(shí)驗(yàn)證明，該方法定位精度較高，但由于系統(tǒng)狀態(tài)維數(shù)較大導(dǎo)致計(jì)算復(fù)雜度較高，對于需要進(jìn)行高頻率定位的應(yīng)用如無人機(jī)的實(shí)時控制存在一定局限性.Zhang[18]等人將收集的由9個IMU組成的IMU陣列測量值，通過最小二乘估計(jì)器將IMU陣列測量值以概率形式映射到一個虛擬的IMU，并推導(dǎo)了相應(yīng)的傳播方程.該方法定位精度較高，計(jì)算速度較快，但未考慮定位魯棒性.

綜上所述，目前使用多個IMU或由多個IMU組成的IMU陣列結(jié)合相機(jī)進(jìn)行定位的算法較少，已有的算法框架定位整體性能表現(xiàn)不佳.本文在OpenVINS框架下展開IMU陣列和相機(jī)信息融合技術(shù)研究，在此基礎(chǔ)上提出了一種使用IMU陣列和相機(jī)作為傳感器的定位算法框架.與使用單個IMU的方法相比，該算法能夠?qū)崿F(xiàn)更高的定位精度而不明顯增加額外的計(jì)算時間，且對IMU傳感器異常具有一定的魯棒性.

本文的主要創(chuàng)新如下：1)基于三次均勻B樣條，提出了一種IMU陣列測量模型.2)基于分布式擴(kuò)展卡爾曼濾波器，通過IMU陣列在幾何上的約束關(guān)系，進(jìn)行多IMU傳感器的數(shù)據(jù)融合，這可以在提高系統(tǒng)定位精度的同時增強(qiáng)系統(tǒng)的魯棒性.3)將所提出的IMU陣列引入OPENVINS開源框架，構(gòu)建了一種基于IMU陣列的視覺慣性SLAM系統(tǒng).

2 算法框架與概述

本文結(jié)構(gòu)可分為基于IMU陣列的虛擬IMU測量模型和基于IMU陣列的視覺慣性SLAM系統(tǒng)兩大部分，系統(tǒng)整體框架如圖1所示.第1部分為虛擬IMU測量模型，該部分首先根據(jù)輸入包含時間戳的位姿數(shù)據(jù)，利用三次均勻B樣條得到單個IMU的角速度和加速度測量值，然后根據(jù)IMU陣列信號模型對得到的單個IMU測量值進(jìn)行模型轉(zhuǎn)換得到IMU陣列測量值，最后使用卡爾曼濾波器將IMU陣列測量值進(jìn)行融合得到一個虛擬IMU(VIMU)的加速度和角速度測量值.第2部分為基于IMU陣列的視覺慣性SLAM系統(tǒng).每當(dāng)相機(jī)捕獲一幀圖像數(shù)據(jù)時便對圖像特征進(jìn)行提取并跟蹤；每收到一幀經(jīng)第一大部分融合后的VIMU數(shù)據(jù)，根據(jù)IMU運(yùn)動模型對虛擬IMU狀態(tài)進(jìn)行估計(jì)，并對系統(tǒng)狀態(tài)協(xié)方差矩陣進(jìn)行更新，最后在OpenVINS框架下融合VIMU和相機(jī)數(shù)據(jù)更新系統(tǒng)狀態(tài)，輸出對物體位姿的估計(jì)結(jié)果.

圖1 系統(tǒng)整體框架

3 基于IMU陣列的虛擬IMU測量模型

為了系統(tǒng)在運(yùn)行過程中出現(xiàn)IMU異常情況時，仍能有效定位，引入由多個IMU組成的IMU陣列來共同約束物體位姿估計(jì)問題.基于B樣條曲線原理可將物體運(yùn)動的位姿軌跡轉(zhuǎn)化為對應(yīng)的IMU測量值，以此IMU測量值為基準(zhǔn)，根據(jù)IMU陣列信號模型可獲取IMU陣列中各個IMU的測量值，利用卡爾曼濾波器將IMU陣列中各IMU的測量值進(jìn)行融合可獲取一個虛擬IMU的測量值.本節(jié)將介紹如何根據(jù)虛擬IMU測量模型來獲取一個虛擬IMU測量值的具體步驟.

3.1 單IMU測量模型

根據(jù)三次均勻B樣條原理，在一系列時間均勻分布的SE(3)位姿給定的條件下，可沿著給定軌跡計(jì)算在任何時間步長的位姿，速度和加速度[19]，則當(dāng)位姿和時間步長ts給定時，位姿{(lán)S}可以被插值為：

(1)

(2)

(3)

為樣條插值常數(shù)，exp(·)，log(·)是SE(3)上的指數(shù)和對數(shù)映射.坐標(biāo)系符號如圖2所示.

圖2 通過B樣條插值出位姿

IMU測量物體的三軸角速度和加速度，測量值通常受高斯白噪聲和零偏影響，其測量模型如下：

ωm(t)=ω(t)+bw(t)+nw(t)

(4)

(5)

根據(jù)三次均勻B樣條，可得到單個IMU任意時刻的角速度和加速度，式(4)和式(5)可寫為：

(6)

(7)

其中(·)∨表示從反對稱矩陣中提取向量.

3.2 IMU陣列測量模型

在獲取單個IMU測量值后，引入IMU陣列測量模型獲取IMU陣列的測量值，為系統(tǒng)提供更加豐富的物體運(yùn)動信息，使系統(tǒng)能更好的完成定位功能.由物體基本運(yùn)動學(xué)原理可知，旋轉(zhuǎn)坐標(biāo)系中某一點(diǎn)的比力可以分解為另一點(diǎn)的比力，離心力以及歐拉力，則如圖3所示的IMU陣列中某個IMU輸出的比力等于陣列坐標(biāo)系原點(diǎn)比力，離心力與歐拉力之和，計(jì)算如下[20]：

圖3 IMU陣列信號模型

(8)

IMU陣列中每個IMU的陀螺儀測量的角速度與每個IMU在IMU陣列中的位置無關(guān)，因此IMU陣列中所有陀螺儀的測量值均相同.

ωj=ω

(9)

式中ωj為陣列坐標(biāo)系下位置為rj處IMU的角速度；ω表示陣列坐標(biāo)系原點(diǎn)的角速度.

將基于三次均勻B樣條計(jì)算得到的單個IMU測量值作為基準(zhǔn)并記作Ib，則基準(zhǔn)IMU的角速度IbωIb和角加速度IbαIb為：

(10)

(11)

(12)

(13)

基于上述運(yùn)動方程式，可計(jì)算得到IMU陣列中每個IMU的真實(shí)慣性測量值，然后使用隨機(jī)游走偏差和白噪聲將其破壞，可模擬生成實(shí)際的IMU陣列測量值.

3.3 IMU陣列測量數(shù)據(jù)融合方法

利用三次均勻B樣條得到IMU陣列中各位置IMU測量值后，對得到的測量值分別進(jìn)行卡爾曼濾波處理得到濾波后各IMU的角速度和加速度估計(jì)值.為了使用卡爾曼濾波器，將IMU陀螺儀和加速度計(jì)測量的真實(shí)角速度和加速度建模為隨機(jī)游走過程，因此：

(14)

式中nω和na為零均值高斯白噪聲.

IMU陣列測量值融合系統(tǒng)中第i個IMU的狀態(tài)空間模型定義如下：

(15)

對IMU陣列中每個IMU測量值進(jìn)行卡爾曼濾波，步驟如下：

1)狀態(tài)一步預(yù)測：

(16)

2)狀態(tài)一步預(yù)測均方誤差陣：

(17)

3)濾波增益：

(18)

4)狀態(tài)估計(jì)：

(19)

5)狀態(tài)估計(jì)均方誤差：

(20)

(21)

4 基于IMU陣列的視覺慣性SLAM系統(tǒng)

本文將OpenVINS框架應(yīng)用于IMU陣列和相機(jī)的融合處理，使用經(jīng)過卡爾曼濾波處理后得到的虛擬IMU狀態(tài)替代原框架中單個IMU的狀態(tài).改進(jìn)后的系統(tǒng)框架可分為兩大部分，第1部分執(zhí)行狀態(tài)傳播，每當(dāng)獲取一個虛擬IMU(VIMU)測量值，便執(zhí)行一次狀態(tài)傳播，并計(jì)算系統(tǒng)狀態(tài)和協(xié)方差矩陣，第2大部分執(zhí)行狀態(tài)更新，每當(dāng)收到一幅圖像數(shù)據(jù)，在狀態(tài)向量滑動窗口擴(kuò)增一個相機(jī)位姿估計(jì)狀態(tài)并相應(yīng)的擴(kuò)充協(xié)方差矩陣，在測量模型得到特征點(diǎn)提取結(jié)果后對狀態(tài)進(jìn)行更新.

本節(jié)首先給出整個系統(tǒng)的狀態(tài)向量，然后根據(jù)誤差狀態(tài)表示和IMU運(yùn)動學(xué)方程給出了系統(tǒng)狀態(tài)傳播方程，最后根據(jù)卡爾曼濾波得到系統(tǒng)狀態(tài)更新方程.

4.1 狀態(tài)向量

系統(tǒng)的狀態(tài)向量Xk由虛擬IMU狀態(tài)、歷史虛擬IMU位姿、特征點(diǎn)位置、相機(jī)外參和內(nèi)參、虛擬IMU和相機(jī)的時間偏移5個部分組成：

(22)

為了執(zhí)行擴(kuò)展卡爾曼濾波進(jìn)行狀態(tài)估計(jì)，對四元數(shù)采用左乘擾動形式定義誤差，對位置、速度、偏置采用“真實(shí)值=估計(jì)值+誤差”的標(biāo)準(zhǔn)加性誤差定義，對應(yīng)的關(guān)系式為：

(23)

(24)

4.2 狀態(tài)傳播

根據(jù)IMU的運(yùn)動學(xué)方程，虛擬IMU狀態(tài)估計(jì)傳播方程為：

(25)

由于式(25)是連續(xù)時間情況下虛擬IMU狀態(tài)估計(jì)傳播方程，實(shí)際應(yīng)用中根據(jù)四階龍格庫塔法對其進(jìn)行離散化可得到離散時間虛擬IMU誤差狀態(tài)傳播方程：

(26)

式中Φ(tk+1，tk)和Gk分別表示系統(tǒng)的雅可比矩陣和噪聲雅可比矩陣；

從k時刻到k+1時刻的誤差協(xié)方差為：

(27)

式中Qd為離散時間噪聲協(xié)方差.

4.3 狀態(tài)更新

(28)

(29)

(30)

代入式(29)可得：

(31)

式(31)可寫成：

(32)

式中nn為噪聲向量，協(xié)方差矩陣為Rn,k.

接下來便可執(zhí)行EKF更新：

卡爾曼增益為：

(33)

協(xié)方差矩陣為：

(34)

狀態(tài)估計(jì)值為：

(35)

(36)

5 實(shí)驗(yàn)結(jié)果評估與分析

本文算法以O(shè)penVINS框架中提供的視覺慣性仿真器為基礎(chǔ)，獲取了由4個參數(shù)相同的IMU按照一定距離排列組成的IMU陣列測量值，并假設(shè)IMU陣列中每個IMU都是硬件同步的，即它們收集數(shù)據(jù)的時間戳一致，系統(tǒng)參數(shù)設(shè)置如表1所示，IMU陣列排列方式如圖4所示.實(shí)驗(yàn)運(yùn)行環(huán)境配置為Intel Xeon Silver 4110 CPU，主頻2.1GHz，32GB內(nèi)存，GeForce GTX1080顯卡，Ubuntu16.04 LTS操作系統(tǒng).

圖4 IMU陣列示意圖

表1 系統(tǒng)參數(shù)設(shè)置

為了驗(yàn)證本文算法的定位精度和魯棒性能，分別在EUROC數(shù)據(jù)集[21]和TUM VI數(shù)據(jù)集[22]進(jìn)行了多次實(shí)驗(yàn)測試.EUROC數(shù)據(jù)集是由微型飛行器在工業(yè)環(huán)境和室內(nèi)環(huán)境下采集的視覺慣性數(shù)據(jù)集，并按照飛行器速度的快慢和所采集圖像質(zhì)量劃分?jǐn)?shù)據(jù)集難易程度，同時提供由徠卡動作捕捉系統(tǒng)獲取的6D姿態(tài)真實(shí)值以及各個傳感器的標(biāo)定參數(shù).TUM VI數(shù)據(jù)集是由慕尼黑工業(yè)大學(xué)David Schuber等人手持視覺慣性傳感器在室內(nèi)和戶外場景下采集的數(shù)據(jù)系列，包含原始數(shù)據(jù)和校準(zhǔn)后的數(shù)據(jù)，并使用動作捕捉系統(tǒng)記錄物體姿態(tài)真實(shí)值.本文選取了EUROC數(shù)據(jù)集中MH_02_easy、MH_04_difficult、V1_01_easy等不同環(huán)境下具有不同難度的數(shù)據(jù)序列以及TUM VI數(shù)據(jù)集中room1、room3等室內(nèi)環(huán)境下的數(shù)據(jù)序列進(jìn)行多次實(shí)驗(yàn)測試，并將實(shí)驗(yàn)計(jì)算得到的位置均方根誤差用于評估系統(tǒng)定位精度.

5.1 定位精度

為了驗(yàn)證本文算法的定位精度，首先分別繪制了EUROC和TUM VI數(shù)據(jù)集中MH_02_easy和room3數(shù)據(jù)序列在本文算法和OPENVINS下的運(yùn)動軌跡，同時與提供的物體真實(shí)運(yùn)動軌跡進(jìn)行比較，軌跡對比如圖5和圖6所示.

圖5 EUROC/MH_02_easy數(shù)據(jù)集軌跡對比

圖6 TUM VI/ room3數(shù)據(jù)集軌跡對比

由圖5和圖6可看出本文算法在MH_02_easy和room3序列中得到的物體運(yùn)動軌跡相比較OpenVINS更加接近真實(shí)軌跡，具有更小的軌跡誤差.

然后選取均方根誤差這一常見的定位精度評價指標(biāo)進(jìn)行對比，在各數(shù)據(jù)集上分別運(yùn)行本文算法和OpenVINS得到物體位姿估計(jì)結(jié)果，與提供的物體真實(shí)位姿進(jìn)行比較，計(jì)算兩者的位置均方根誤差，從而驗(yàn)證本文算法的定位精度.各數(shù)據(jù)集上位置均方根誤差對比如表2和表3所示.

表2 EUROC數(shù)據(jù)集位置均方根誤差對比

表3 TUM VI數(shù)據(jù)集位置均方根誤差對比

根據(jù)表2和表3的對比結(jié)果可看出在EUROC和TUM VI數(shù)據(jù)集中，本文算法與OpenVINS相比位置均方根誤差更小，定位精度更高.特別是在EUROC和TUM VI數(shù)據(jù)集的V1_03_difficult和room5序列中位置均方根誤差降低達(dá)到29.03%、30.26%，有效的提升了物體的定位精度.物體定位精度的提升，主要是因?yàn)楸疚脑贠penVINS基礎(chǔ)上加入IMU陣列后使物體運(yùn)動的角速度和加速度信息能更加充分有效的獲取，而不再是僅使用一個IMU來獲取物體運(yùn)動的角速度和加速度信息.利用IMU陣列能夠獲取豐富的角速度和加速度信息，對其進(jìn)行融合能夠得到物體運(yùn)動更加精確的角速度和加速度信息，進(jìn)而改善物體位姿估計(jì)精度.

5.2 運(yùn)行時間

在將IMU陣列引入到基于單個IMU和相機(jī)的視覺慣性SLAM系統(tǒng)中后，定位精度明顯提升，而且并沒有因此影響系統(tǒng)實(shí)時性.表4和表5是本文算法與OpenVINS平均運(yùn)行時間結(jié)果對比，平均運(yùn)行時間是指在本文實(shí)驗(yàn)環(huán)境下，處理數(shù)據(jù)集中每一幀平均所耗費(fèi)的時間.

表4 EUROC數(shù)據(jù)集運(yùn)行時間對比

從表4和表5可看出，在EUROC和TUM VI數(shù)據(jù)集各序列上本文算法和OpenVINS每幀平均消耗的時間相差很小.在EUROC和TUM VI數(shù)據(jù)集各序列上本文算法每幀平均消耗的時間較OpenVINS相比最大僅相差1.7ms和8ms.時間的增加主要是因?yàn)樵诒疚乃惴ㄖ幸肓烁嗟腎MU測量值并且對其進(jìn)行融合，系統(tǒng)需要一定的時間進(jìn)行運(yùn)算處理，但這種運(yùn)算處理計(jì)算量較小并不會導(dǎo)致系統(tǒng)在運(yùn)行過程中實(shí)時性下降.

表5 TUM VI數(shù)據(jù)集運(yùn)行時間對比

為了更直觀的對比本文算法和OpenVINS在運(yùn)行時間上的差異，繪制出了在EUROC數(shù)據(jù)集MH_02_easy序列和TUM VI數(shù)據(jù)集room3序列上運(yùn)行時間曲線對比圖，如圖7和圖8所示.

圖7 EUROC/MH_02_easy運(yùn)行時間對比

圖8 TUM VI/room3運(yùn)行時間對比

在MH_02_easy序列的起始時刻到終止時刻整個范圍內(nèi)，本文算法和OpenVINS處理每幀的時間消耗曲線分別如圖7中的黑色虛線和黑色實(shí)線所示，在整個時間段內(nèi)每幀平均的運(yùn)行時間分別為0.0068668s、0.0068232s.兩者每幀平均的運(yùn)行時間相差僅0.0436ms.在room3序列的起始時刻到終止時刻整個范圍內(nèi)，本文算法和OpenVINS處理每幀的時間消耗曲線分別如圖8中的黑色虛線和黑色實(shí)線所示，在整個時間段內(nèi)每幀平均的運(yùn)行時間分別為0.0086689s、0.0085889s.兩者每幀平均的運(yùn)行時間相差僅0.08ms.由圖7和圖8以及表4和表5可看出，本文算法相比較于OpenVINS在EUROC和TUM VI數(shù)據(jù)集各序列上對每幀的處理時間十分接近，并沒有對系統(tǒng)實(shí)時性造成太大影響.而在時間消耗相當(dāng)?shù)那闆r下，本文算法能明顯降低物體位姿估計(jì)誤差，提高物體定位精度.

5.3 系統(tǒng)魯棒性能

在機(jī)器人運(yùn)動過程中，可能由于機(jī)器人快速運(yùn)動、劇烈抖動、撞擊物體或者周圍環(huán)境高溫，引起慣性傳感器測量出現(xiàn)異常，從而導(dǎo)致物體位姿估計(jì)精度下降甚至系統(tǒng)崩潰.針對上述情況，本文引入的虛擬IMU模型能夠很大程度上減少在某個IMU測量出現(xiàn)異常時對系統(tǒng)造成的影響.本文通過增大IMU陣列中各IMU測量噪聲并對IMU4測量值進(jìn)行移除，以模擬機(jī)器人受到環(huán)境影響下慣性傳感器測量出現(xiàn)異常的運(yùn)動狀況.表6和表7顯示的是將IMU陣列中各IMU測量噪聲增大并移除IMU4測量值后運(yùn)行本文算法，在EUROC和TUM VI各數(shù)據(jù)集序列上得到的位置均方根誤差對比.

表6 EUROC數(shù)據(jù)集位置均方根誤差對比

表7 TUM VI數(shù)據(jù)集位置均方根誤差對比

由于OpenVINS使用單個 IMU狀態(tài)約束物體位姿估計(jì)問題，在IMU沒有測量值時即出現(xiàn)異常時，系統(tǒng)便不能正確進(jìn)行定位.從表6和表7得到的位置均方根誤差對比結(jié)果來看，當(dāng)IMU陣列中IMU測量值出現(xiàn)異常時，本文算法仍能對物體位姿做出正確估計(jì)，位置均方根誤差雖有所增加但依舊能保持良好的定位精度.

為了更加全面的評估本文算法的魯棒性能，將本文算法在IMU正常情況下以及IMU測量值出現(xiàn)異常情況下得到的EUROC和TUM VI數(shù)據(jù)集各序列的位姿估計(jì)結(jié)果和提供的位姿真實(shí)值進(jìn)行相對位置誤差計(jì)算，將所有計(jì)算得到的相對位置誤差在各子段長度上取平均值，結(jié)果如表8所示.

表8 本文算法相對位置誤差對比

從表8可看出即使在IMU測量出現(xiàn)異常情況下，本文算法在各子段長度上的相對位置誤差仍然較小.從整個子段長度區(qū)間上看，當(dāng)IMU陣列中IMU出現(xiàn)異常時，本文算法所得到的相對位置誤差相較于IMU正常工作時有所增加，但最多也只在子段長度為16m上增加了29.63%.

將上述計(jì)算得到的各字段長度上相對位置誤差數(shù)據(jù)，繪制相對位置誤差箱型圖，如圖9所示.

圖9 本文算法相對位置誤差對比

如圖9繪制的本文算法在IMU正常和異常兩種情況下各子段長度上的相對位置誤差箱型圖所示，其中各子段長度上箱型圖的中位數(shù)值與表8中的數(shù)值相同.如圖9中黑色虛線箱型圖所示，當(dāng)IMU出現(xiàn)異常時，本文算法在各子段長度上的相對位置誤差相比與IMU正常情況平均增加了24.63%.

從表6-表8和圖9可以看出在IMU異常情況下，本文算法相對位置誤差雖有所上升，但仍能對物體位姿進(jìn)行良好的估計(jì)，獲得較高的定位精度.這是因?yàn)樵贠penVINS框架下引入IMU陣列后，借助本文所提出的虛擬IMU模型能夠有效解決由于IMU異常導(dǎo)致系統(tǒng)不能正確進(jìn)行定位的問題，達(dá)到提升系統(tǒng)魯棒性的目的.

6 結(jié) 論

本文針對基于單個IMU和相機(jī)的定位方法進(jìn)行了改進(jìn)，提出了基于IMU陣列和相機(jī)數(shù)據(jù)融合的定位方法，該方法旨在保證系統(tǒng)實(shí)時性的條件下進(jìn)一步提高物體位姿估計(jì)精度和魯棒性.該算法由兩大部分組成，首先，使用基于IMU陣列的虛擬IMU測量模型得到IMU陣列融合后的虛擬IMU.然后，在OpenVINS框架下融合IMU陣列測量數(shù)據(jù)和相機(jī)數(shù)據(jù)實(shí)現(xiàn)對物體的位姿估計(jì).實(shí)驗(yàn)結(jié)果表明，在平均運(yùn)行時間相當(dāng)情況下，本文所提出的方法與OpenVINS方法相比具有更好的定位精度，并且在IMU陣列中IMU出現(xiàn)異常情況時仍能保證良好的定位精度，具有更強(qiáng)的魯棒性.未來還需要在此基礎(chǔ)之上，進(jìn)一步優(yōu)化IMU陣列數(shù)據(jù)融合方法，實(shí)際搭建IMU陣列傳感器并測試本算法的性能表現(xiàn).