凌有鑄，郭俊陽，陳孟元，陳何寶，袁學(xué)超

（1. 安徽工程大學(xué) 電氣工程學(xué)院，蕪湖 241000； 2. 高端裝備先進(jìn)感知與智能控制教育部重點(diǎn)實(shí)驗(yàn)室，蕪湖 241000； 3. 蕪湖市固高自動(dòng)化技術(shù)有限公司，蕪湖 241000）

移動(dòng)機(jī)器人基于視覺的同步定位與建圖（Visual simultaneous localization and mapping, VSLAM）主要研究機(jī)器人如何在復(fù)雜多變的環(huán)境中準(zhǔn)確地確定位置并建立當(dāng)前場(chǎng)景地圖的問題。然而傳統(tǒng)VSLAM 算法僅依靠單一傳感器已無法滿足科研的需求，也無法在復(fù)雜快速運(yùn)動(dòng)的環(huán)境中及時(shí)捕捉機(jī)器人的運(yùn)動(dòng)信息并反饋給主控機(jī)進(jìn)行運(yùn)動(dòng)補(bǔ)償?？蒲泄ぷ髡唛_始融合兩種不同傳感器的信息意圖互補(bǔ)兩者的不足，期望慣性測(cè)量單元（Inertial Measurement Unit, IMU）可以在快速運(yùn)動(dòng)時(shí)替代相機(jī)捕捉相機(jī)運(yùn)動(dòng)的物理信息提高VSLAM 定位精度，而相機(jī)也可以彌補(bǔ)IMU 在慢速運(yùn)動(dòng)下因使用時(shí)間過長(zhǎng)使得累計(jì)誤差漂移的問題。

文獻(xiàn)[1]建立IMU 位置和速度積分模型，利用三個(gè)連續(xù)圖像幀和一個(gè)特征點(diǎn)匹配的視覺信息求解理想IMU 模型閉式解，但測(cè)量值未考慮噪聲對(duì)IMU 測(cè)量值的影響。文獻(xiàn)[2]提出利用歐拉角參數(shù)化旋轉(zhuǎn)誤差，避免重復(fù)積分。文獻(xiàn)[3]在[2]的基礎(chǔ)上對(duì)旋轉(zhuǎn)角進(jìn)行流形參數(shù)化和數(shù)據(jù)處理，提取初值數(shù)據(jù)避免重復(fù)更新，減少積分帶來的誤差累計(jì)，但該方法未考慮IMU 隨機(jī)游走誤差的影響。2017 年，國(guó)內(nèi)沈劭劼團(tuán)隊(duì)[4]在IMU初始化階段將特征點(diǎn)深度信息、相機(jī)平移與重力、速度一起估計(jì)，該方法計(jì)算量大且精準(zhǔn)度低。同年，MUR-ARTAL R 提出VI-ORB（Visual Inertial Oriented FAST and Rotated BRIEF）算法，利用松耦合的方法在原有基礎(chǔ)上估計(jì)零偏，但沒有考慮外補(bǔ)參數(shù)的影響[5]。

IMU 預(yù)積分的提出極大地促進(jìn)了基于圖優(yōu)化的視覺慣性聯(lián)合導(dǎo)航系統(tǒng)。Klein G 等人[6]提出的PTAM算法能夠同時(shí)對(duì)某個(gè)時(shí)間段內(nèi)的相關(guān)狀態(tài)進(jìn)行位姿和誤差優(yōu)化，具有較高的計(jì)算精度，然而具有極高的局限性與依賴性。這類方法只有在初始狀態(tài)已知的情況下才能使用，計(jì)算結(jié)果的準(zhǔn)確度也與初始狀態(tài)的數(shù)據(jù)準(zhǔn)確度緊緊相關(guān)[7,8]。文獻(xiàn)[9]在VSLAM 階段以相同計(jì)算資源為條件對(duì)比實(shí)驗(yàn)，得出優(yōu)化算法比濾波算法更能得出準(zhǔn)確的效果。文獻(xiàn)[10]提出的OKVIS 是一種緊耦合的雙目慣性里程計(jì)，將視覺和IMU 的誤差項(xiàng)和狀態(tài)量在一起優(yōu)化，但這種算法不支持重定位，也不包含閉環(huán)檢測(cè)功能。

2015 年Leutenegger 等人[11]提出了一種基于關(guān)鍵幀的算法，在局部?jī)?yōu)化中優(yōu)化相機(jī)位姿和IMU 誤差，跟蹤和重建圖像中興趣點(diǎn)（interest- point）的3D 位置。2016 年Usenko 等人[12]提出了一種類似于非線性優(yōu)化的方法，通過在局部?jī)?yōu)化中使用直接圖像對(duì)齊而不是間接使用興趣點(diǎn)測(cè)量值來得到全局的連續(xù)地圖。RaulMur-Artal[13]在ORB-SLAM 的追蹤前段添加IMU模塊，并將視覺慣性聯(lián)合初始化，優(yōu)化位姿信息，減少誤差多次使用導(dǎo)致的數(shù)據(jù)漂移，但其無法在相機(jī)劇烈運(yùn)動(dòng)下正常使用。2017 年Forster 等人[14]提出了一種基于預(yù)積分理論的視覺慣性里程計(jì)，將預(yù)積分理論應(yīng)用于SO(3)模型中，將IMU 測(cè)量數(shù)據(jù)與圖像中稀疏子集直接跟蹤相結(jié)合，從而加速了優(yōu)化計(jì)算。文獻(xiàn)[15]以O(shè)KVIS 為主要框架，添加基于視覺慣性融合的前端，實(shí)現(xiàn)視覺和慣性數(shù)據(jù)緊耦合，但未計(jì)算出尺度變化和旋轉(zhuǎn)角偏差，也沒有考慮到IMU 自身攜帶的誤差，定位精度和魯棒性較差。Forster[16]開發(fā)一種預(yù)積分理論，恰當(dāng)?shù)亟鉀Q旋轉(zhuǎn)群的多種結(jié)構(gòu)。同理，單目視覺慣性狀態(tài)估計(jì)器（Monocular Visual-inertial State Estimator, VINS-mono）通過增加IMU 偏置校正在連續(xù)時(shí)間擴(kuò)展IMU 預(yù)積分[17]。

本文以VINS-mono 算法為基礎(chǔ)，提出一種基于雙重初始化和關(guān)鍵幀分級(jí)優(yōu)化的改進(jìn)視覺慣性SLAM方法（Double Initiatlation and Covisibility Graph-VINS，DICG-VINS）。針對(duì)IMU 預(yù)積分帶來的誤差累計(jì)的問題，在視覺慣導(dǎo)初始化階段結(jié)合費(fèi)雪矩陣構(gòu)建情形評(píng)估函數(shù)對(duì)初始化信息矩陣進(jìn)行度量，使得重力信息和尺度信息具有可觀性，更加準(zhǔn)確地表征局部地圖；在滑動(dòng)窗口優(yōu)化前，對(duì)關(guān)鍵幀按照共視關(guān)系進(jìn)行分級(jí)處理，聯(lián)合重投影誤差和IMU 預(yù)積分誤差構(gòu)建誤差代價(jià)函數(shù)，優(yōu)化分級(jí)得到的強(qiáng)共視關(guān)鍵幀，提高運(yùn)動(dòng)精度的同時(shí)減少軌跡誤差。

1 系統(tǒng)框架

本文設(shè)計(jì)的系統(tǒng)框架如圖1 所示。

圖1 系統(tǒng)框架Fig.1 System framework diagram

2 視覺慣性初始化

2.1 IMU 誤差分析

IMU 通常測(cè)量載體的角速度wm和加速度am的離散值，測(cè)量值受到傳感器隨時(shí)間緩慢變化的偏置和測(cè)量白噪聲的影響，故測(cè)量值可以由真實(shí)值、隨機(jī)游走偏差和高斯白噪聲三個(gè)部分組成[18]，為：

其中，wm和am分別表示慣性測(cè)量單元IMU 中陀螺儀和加速度計(jì)的測(cè)量值；wB和wW分別表示陀螺儀和加速度計(jì)的真實(shí)值；bg和ba分別表示陀螺儀和加速度計(jì)的隨機(jī)游走偏差；表示世界坐標(biāo)系（World）到IMU 坐標(biāo)系（Body）的旋轉(zhuǎn)矩陣；ηg和ηa分別表示陀螺儀和加速度計(jì)的高斯白噪聲，gW表示世界坐標(biāo)系下的重力加速度矢量，本文選擇東北天坐標(biāo)系作為世界坐標(biāo)，gW=[0,0,g]T。IMU 坐標(biāo)系變換示意圖如圖2 所示。

圖2 IMU 坐標(biāo)系轉(zhuǎn)換Fig.2 IMU coordinate system transformation

定義IMU 運(yùn)動(dòng)狀態(tài)如下：

其中ZF為IMU 狀態(tài)變量。F為圖像幀，W為世界坐標(biāo)系，p、v和q分別表示IMU 位置、速度和姿態(tài)四元數(shù)。

2.2 IMU 初始化終止條件

單一的視覺里程計(jì)無法獲得絕對(duì)尺度，聯(lián)合慣性數(shù)據(jù)能夠使得整個(gè)視覺系統(tǒng)的尺度與重力方向可觀測(cè)[19]。VINS-mono 在視覺慣性系統(tǒng)運(yùn)行的前15 s 進(jìn)行聯(lián)合初始化，以確保所有參數(shù)可觀，但固定初始化時(shí)長(zhǎng)的方法存在不確定性，導(dǎo)致初始化時(shí)間延長(zhǎng)。而IMU 無法直接從靜止?fàn)顟B(tài)開始運(yùn)動(dòng)，需要有足夠的運(yùn)動(dòng)激勵(lì)[20]。因此需要保證IMU 和相機(jī)的觀測(cè)值是機(jī)器人在充分運(yùn)動(dòng)時(shí)測(cè)量得到，觀測(cè)值的好壞充分影響著系統(tǒng)的魯棒性程度[21]。

則狀態(tài)變量費(fèi)雪信息矩陣可以表示為

將視覺慣導(dǎo)觀測(cè)樣本情形評(píng)估函數(shù)定義如下：

其中，Ωk(F)是從關(guān)于評(píng)估狀態(tài)ZF的尺度和重力加速度等相關(guān)狀態(tài)獲得的信息矩陣。F為運(yùn)行過程中某個(gè)圖像幀，Δτ是與圖像幀相關(guān)的圖像信息矩陣。

為了最小化置信度的局域半徑，需要最小化協(xié)方差的行列式，由于得到的最小化置信度局域半徑與最大化信息矩陣的對(duì)數(shù)行列式相同，從而得到觀測(cè)樣本情形評(píng)估函數(shù)的性能度量：

定義情形評(píng)估函數(shù)值小于閾值Sth來衡量初始化狀態(tài)具有較高的置信度，認(rèn)為此時(shí)的ZF具有高確定性，終止初始化，將相應(yīng)的預(yù)積分和初始化數(shù)據(jù)融合，進(jìn)入下一步的視覺慣性實(shí)時(shí)優(yōu)化。為避免因情形評(píng)估函數(shù)計(jì)算時(shí)間過長(zhǎng)使得IMU 誤差累積，故在初始化過程中額外增加一個(gè)15 s 時(shí)間設(shè)定，若在15 s 內(nèi)未達(dá)到較高的置信度，在第15 s 時(shí)直接終止初始化，將當(dāng)前初始化結(jié)果送入下一階段實(shí)時(shí)優(yōu)化。情形評(píng)估函數(shù)算法如算法1 所示。

算法1：Double initialization of case evaluation function Input：關(guān)鍵幀狀態(tài)變量 ZF 聯(lián)合初始化時(shí)間t最差情形評(píng)估閾值 Sth Output：聯(lián)合初始化后的狀態(tài)變量ZFbest 視覺慣性聯(lián)合初始化 for 聯(lián)合初始化時(shí)間 t＜ 15s 計(jì)算樣本情形評(píng)估函數(shù) fλ(F )隨初始化時(shí)間t 的變化 iffλ( F)＜Sth聯(lián)合初始化終止 end if 聯(lián)合初始化 end for 聯(lián)合初始化終止 計(jì)算聯(lián)合初始化后的狀態(tài)變量ZFbest

3 基于共視約束的視覺慣性緊耦合優(yōu)化

基于共視關(guān)鍵幀優(yōu)化的視覺慣性緊耦合示意圖如圖2 所示，其中相機(jī)數(shù)據(jù)與IMU 預(yù)積分統(tǒng)一在一個(gè)非線性優(yōu)化框架中進(jìn)行優(yōu)化。在關(guān)鍵幀進(jìn)入滑動(dòng)窗口優(yōu)化之前，對(duì)關(guān)鍵幀進(jìn)行分級(jí)處理，按照幀間公共觀測(cè)地圖點(diǎn)稀疏程度將關(guān)鍵幀分為強(qiáng)共視關(guān)鍵幀和弱共視關(guān)鍵幀兩類，后續(xù)滑動(dòng)窗口優(yōu)化只優(yōu)化強(qiáng)共視關(guān)鍵幀。

圖2 基于共視關(guān)鍵幀優(yōu)化的視覺慣性緊耦合示意圖Fig.2 Schematic diagram of tightly coupled visual inertia based on covisibility keyframe optimization

3.1 分級(jí)關(guān)鍵幀篩選

視覺SLAM后端一般都是利用滑動(dòng)窗口最小化重投影誤差作為約束項(xiàng)去優(yōu)化關(guān)鍵幀，但每次進(jìn)入滑動(dòng)窗口的關(guān)鍵幀數(shù)量固定，處理信息的實(shí)時(shí)性低，移動(dòng)舊關(guān)鍵幀的時(shí)候也會(huì)對(duì)關(guān)鍵幀攜帶的信息有不利影響。文獻(xiàn)[22]在后端增加雙步邊緣化處理，對(duì)高維度矩陣分布拆解后進(jìn)行優(yōu)化求解以保證計(jì)算稀疏化，計(jì)算效率比傳統(tǒng)的邊緣化快，但仍存在計(jì)算速度慢、實(shí)時(shí)性低的缺點(diǎn)。Mur-Aetal R[23]在處理后端優(yōu)化時(shí)利用關(guān)鍵幀之間所有的共視關(guān)系作為滑動(dòng)窗口優(yōu)化的約束項(xiàng)，充分發(fā)揮先驗(yàn)信息的作用，但該方法容易造成滑動(dòng)窗口關(guān)鍵幀信息冗余，處理效率低下。

在關(guān)鍵幀進(jìn)入滑動(dòng)窗口優(yōu)化之前，本文借助Covisibility Graph 的概念構(gòu)建基于共視約束的關(guān)鍵幀局部?jī)?yōu)化概率圖模型，按共視強(qiáng)弱對(duì)關(guān)鍵幀進(jìn)行分級(jí)處理，保留強(qiáng)共視關(guān)鍵幀，忽略弱共視關(guān)鍵幀，將幀間觀測(cè)信息及關(guān)鍵幀上地圖點(diǎn)等信息作為約束項(xiàng)優(yōu)化強(qiáng)共視關(guān)鍵幀。此時(shí)共視約束概率圖模型可以看成一個(gè)無向加權(quán)圖，其聯(lián)合概率分布即可分解為多個(gè)團(tuán)Q勢(shì)能函數(shù)（因子）的乘積：

該模型僅利用強(qiáng)共視關(guān)鍵幀構(gòu)建局部場(chǎng)景地圖，提高觀測(cè)信息的準(zhǔn)確性，減少了滑動(dòng)窗口的優(yōu)化時(shí)間。共視關(guān)鍵幀局部?jī)?yōu)化圖模型如圖3 所示。左側(cè)圖中關(guān)鍵幀之間含有公共觀測(cè)地圖點(diǎn)，為了解關(guān)鍵幀間信息變化，將幀間關(guān)系簡(jiǎn)單化，得到右側(cè)的位姿圖，虛線表示關(guān)鍵幀之間的共視關(guān)系。設(shè)定含150 個(gè)及以上共同觀測(cè)點(diǎn)的關(guān)鍵幀為強(qiáng)共視關(guān)鍵幀，少于150 個(gè)共同觀測(cè)點(diǎn)的關(guān)鍵幀為弱共視關(guān)鍵幀。

圖3 共視關(guān)鍵幀局部?jī)?yōu)化圖模型Fig.3 Covisibility keyframe local optimization graph model

在滑動(dòng)窗口優(yōu)化之前先進(jìn)行共視關(guān)鍵幀篩選可以將進(jìn)入滑動(dòng)窗口關(guān)鍵幀數(shù)量固定在一定的范圍內(nèi)，減少優(yōu)化的計(jì)算量；忽略弱共視關(guān)鍵幀可以消除因關(guān)鍵幀數(shù)量過多導(dǎo)致位姿信息冗余；保留弱共視關(guān)鍵幀的地圖點(diǎn)及相關(guān)位姿信息并作為約束項(xiàng)優(yōu)化強(qiáng)共視關(guān)鍵幀，提高局部地圖的精準(zhǔn)性。

3.2 滑動(dòng)窗口緊耦合優(yōu)化

在滑動(dòng)窗口優(yōu)化時(shí)，僅優(yōu)化強(qiáng)共視關(guān)鍵幀，同時(shí)保留弱共視關(guān)鍵幀的位姿信息作為約束項(xiàng)。視覺位姿估計(jì)誤差項(xiàng)、IMU 預(yù)積分誤差項(xiàng)分量構(gòu)建一個(gè)目標(biāo)函數(shù)中，最小化重投影誤差和IMU 誤差優(yōu)化狀態(tài)量[24]：

Eproj( k , k+ 1)為第k 和k+1 個(gè)關(guān)鍵幀之間的重投影誤差；Eimu( i , j )為時(shí)間i 到j(luò) 之間的IMU 誤差項(xiàng)，由視覺位姿估計(jì)和IMU 預(yù)積分測(cè)量組成，如式(9)所示：

IMU 測(cè)量誤差包括傳統(tǒng)的位姿誤差 ep、 ev和 eq及偏差eb，各誤差分量如式(10)所示：

其中∑η為測(cè)量噪聲的信息矩陣，∑i為預(yù)積分信息矩陣，是在IMU 預(yù)積分階段計(jì)算殘差得出的雅克比矩陣，ρ（?）為魯棒核函數(shù)。

利用g2o（general graph optimization）方法構(gòu)建圖優(yōu)化模型處理后端優(yōu)化的問題，借助列文伯格-馬夸爾特法（Levenberg-Marquardt method，L-M）求解誤差 狀態(tài)增量的最小值使得目標(biāo)函數(shù)最小化。在滑動(dòng)窗口算法中實(shí)現(xiàn)狀態(tài)更新，便于后續(xù)SLAM 系統(tǒng)的運(yùn)動(dòng)跟蹤和位姿估計(jì)。

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

4.1 視覺慣性聯(lián)合初始化參數(shù)收斂性分析

本文算法（DICG-VINS）在VINS 初始化階段基礎(chǔ)上借助費(fèi)雪矩陣構(gòu)建觀測(cè)樣本情形評(píng)估函數(shù)，評(píng)估視覺慣性聯(lián)合初始化誤差變化，使用Ceres Solver 軟件來實(shí)現(xiàn)牛頓高斯迭代法在黎曼流形面上的迭代搜索，將加速度計(jì)偏差和陀螺儀偏差快速穩(wěn)定在基準(zhǔn)值附近，達(dá)到快速收斂的目的。對(duì)比DICG-VINS 與VINS算法的初始化算法在MH_04 數(shù)據(jù)集的偏差變化，多次實(shí)驗(yàn)取平均值測(cè)得的陀螺儀和加速度計(jì)偏置數(shù)據(jù)繪制散點(diǎn)圖進(jìn)行分析，如圖4 和圖5 所示。

圖4 陀螺儀偏置收斂過程示意圖Fig.4 Schematic diagram of the convergence process by the gyroscope bias

陀螺儀偏置收斂過程示意圖如圖4 所示，VINS和本文算法（DICG-VINS）初始化算法都能實(shí)現(xiàn)偏差的快速收斂，但VINS 算法未對(duì)偏差進(jìn)行實(shí)時(shí)優(yōu)化處理，而DICG-VINS 算法構(gòu)建了情形評(píng)估函數(shù)實(shí)時(shí)評(píng)估當(dāng)前偏差的狀態(tài)，因此精度和收斂性都在一定程度高于VINS-mono 算法。加速度偏置收斂過程示意圖如圖5 所示，DICG-VINS 加速度收斂過程整體上與陀螺儀偏置收斂過程類似，最終都趨向定值，僅個(gè)別估計(jì)值存在誤差，但收斂速度較VINS-mono 更快更準(zhǔn)確。

圖5 加速度偏置收斂過程Fig.5 Schematic diagram of the convergence process by the accelerometer bias

本文設(shè)定情形評(píng)估函數(shù)可以滿足待估計(jì)參數(shù)收斂的要求，能夠使得IMU 達(dá)到快速收斂的目的，在一定程度上有效的抑制了IMU 偏差的增大。與VINS-mono在初始化階段以時(shí)間為終止條件的情況相比，有效地節(jié)約了初始化時(shí)間，減少了IMU 測(cè)量值誤差，提高了后續(xù)階段的優(yōu)化精度。

4.2 滑動(dòng)窗口優(yōu)化方法性能分析

本文在后端使用了共視約束的滑動(dòng)窗口優(yōu)化方法對(duì)局部軌跡進(jìn)行校正。為驗(yàn)證方法改進(jìn)后的效果，在MH_01_easy、MH_03_medium、MH_05_difficult 三個(gè)數(shù)據(jù)集上對(duì)DICG-VINS 和VINS-mono 的滑動(dòng)窗口優(yōu)化方法進(jìn)行測(cè)試。為排除其他因素的干擾，在軌跡評(píng)估時(shí)使用擬合的尺度，每組實(shí)驗(yàn)測(cè)試10 次。統(tǒng)計(jì)其軌跡誤差，對(duì)比結(jié)果見圖6。其中不難看出，對(duì)于MH_01數(shù)據(jù)集和MH_03 數(shù)據(jù)集來說，經(jīng)過滑動(dòng)窗口優(yōu)化以后，位置漂移最大值在± 100 mm 左右，比傳統(tǒng)的VINS算法的位置誤差減少了34%。對(duì)于復(fù)雜數(shù)據(jù)集（如MH_05_difficult）位置誤差在± 200 mm 以內(nèi)。

圖6 DICG-VINS 算法位置漂移誤差Fig.6 Position drift error of DICG-VINS algorithm

在圖 6(a)中 y 軸的漂移誤差過大是因?yàn)镸H_01_easy 數(shù)據(jù)集是在工廠環(huán)境下拍攝，工廠環(huán)境情況復(fù)雜且障礙物多，在一定程度上阻礙了無人機(jī)的飛行；在圖6(b)中y 軸在0-40 m 的時(shí)候波動(dòng)性較強(qiáng)，此時(shí)IMU 還處于初始化階段，加速度計(jì)由于自身噪聲過大，無法完成初始化任務(wù)；在圖6(c)中z 軸和y 軸位置漂移在50 m 的時(shí)候達(dá)到谷底，是因?yàn)榇藭r(shí)無人機(jī)前方有障礙物，無人機(jī)急速下降導(dǎo)致DICG-VINS 算法無法及時(shí)跟隨，出現(xiàn)短暫的誤差過大情形。

EuRoC 測(cè)試中運(yùn)行時(shí)間的比較如表1 所示，在測(cè)試中，DICG-VINS 相較于VINS-Mono 在運(yùn)行速度上平均提升11.19%。其中，在MH_01_easy、MH_02_easy和V2_01_easy 數(shù)據(jù)集上，DICG-VINS 比VINS-mono的運(yùn)行時(shí)間加快了25 s 左右。在V1_02_medium 和V2_02_medium 數(shù)據(jù)集上，DICG-VINS 僅比VINS- mono 運(yùn)行時(shí)間快了8 s。分級(jí)關(guān)鍵幀篩選后進(jìn)入滑動(dòng)窗口優(yōu)化，節(jié)省滑動(dòng)窗口優(yōu)化的關(guān)鍵幀的時(shí)間，但也提高了算法的軌跡精度。

表1 測(cè)試中各算法運(yùn)行時(shí)間比較Tab.1 Comparison of the running time in each algorithm

4.3 算法精度對(duì)比實(shí)驗(yàn)分析

為了進(jìn)一步驗(yàn)證算法整體性效果，在EuRoC 數(shù)據(jù)集不同環(huán)境下共4 個(gè)序列（MH_01_easy、MH_04 _difficult、V1_01_easy、和V2_03_difficult）驗(yàn)證DICG- VINS 的整體性能，并與當(dāng)前主流的VIO 方法對(duì)比。DICG-VINS 在上述數(shù)據(jù)集上的精度與VI-ORB 接近，優(yōu)于無閉環(huán)情況下的 VI-ORB 算法、無閉環(huán)的VINS-Mono 算法。本文算法軌跡與VINS-Mono（無閉環(huán)）、VI-ORB（無閉環(huán)）軌跡、ORB-SLAM2（無閉環(huán)）軌跡及真實(shí)軌跡在數(shù)據(jù)集上的比較結(jié)果如圖7所示。根據(jù)圖7(a)可看出系統(tǒng)在運(yùn)行MH_01_easy 數(shù)據(jù)集時(shí)，軌跡結(jié)果誤差較小，且累計(jì)誤差得到了很好的消除，這是由于MH_01_easy 序列，數(shù)據(jù)采集時(shí)無人機(jī)速度緩慢，系統(tǒng)在處理關(guān)鍵幀的同時(shí)進(jìn)行了全局優(yōu)化操作。圖7(c)可看出系統(tǒng)運(yùn)行V1_03_difficult 數(shù)據(jù)集時(shí)，軌跡結(jié)果誤差較大，這是因?yàn)樵摂?shù)據(jù)集在房間采集，環(huán)境與工廠環(huán)境不同，能夠采集的特征信息十分稀疏，不利于系統(tǒng)運(yùn)行。

圖7 各算法運(yùn)行軌跡比較（含真值）Fig.7 Comparison of the running trajectory of each algorithm (including true value)

EuRoC 數(shù)據(jù)集下VIO 算法軌跡精度（RMSE）對(duì)比如表2 所示。與VI-ORB、ORB-SLAM2 和VINS- mono 等算法相比，DICG-VINS 算法在EuRoC 大多數(shù)據(jù)集上的軌跡精度都與主流算法保持在同一水準(zhǔn)或者有所提高，定位精度普遍有所提高。對(duì)比VINS-mono算法，DICG-VINS 在算法軌跡精度上平均提高29.47%，其中在MH_01_easy、MH_03_medium 和V1_03_difficult 上，DICG-VINS 取得了最佳性能，具有良好的表現(xiàn)。對(duì)比ORB-SLAM2 算法，本文算法還是略有不足，主要是因?yàn)榍岸喂饬鞣m然提高了圖像提取速率，提取地圖點(diǎn)的質(zhì)量與特征法相比明顯較差，但后端分級(jí)優(yōu)化對(duì)關(guān)鍵幀信息進(jìn)行約束優(yōu)化，彌補(bǔ)了部分不足，甚至效果比ORB-SLAM2 更佳。

表2 VIO 算法軌跡精度（RMSE）對(duì)比（單位：m）Tab.2 VIO algorithm set Trajectory accuracy (RMSE) comparison (unit: m)

5 結(jié) 論

為提高VINS-mono 在初始化階段和局部?jī)?yōu)化階段的定位精度和系統(tǒng)魯棒性，本文提出了一種改進(jìn)初始化和共視約束緊耦合優(yōu)化的視覺慣性SLAM 方法。利用費(fèi)雪矩陣構(gòu)建最差情形誤差函數(shù)反推狀態(tài)矩陣的信息量期望值，加快初始化偏差收斂速度。在關(guān)鍵幀緊耦合優(yōu)化階段，通過共視關(guān)系分級(jí)關(guān)鍵幀，優(yōu)化強(qiáng)共視關(guān)鍵幀的同時(shí)保留弱共視關(guān)鍵幀的幀間信息作為滑動(dòng)窗口的約束項(xiàng)，降低了運(yùn)算難度，并提高了運(yùn)算精度。實(shí)驗(yàn)分析環(huán)節(jié)，采用EuRoC 數(shù)據(jù)集對(duì)DICG- VINS 與其他主流算法進(jìn)行對(duì)比實(shí)驗(yàn)分析，結(jié)果表明，DICG-VINS 與VINS-mono 相比在平均運(yùn)行速度方面提高了11.19%，精度方面提高了29.47%；但對(duì)比ORB-SLAM2 算法，DICG-VINS 圖像提取精度低，后續(xù)關(guān)鍵幀優(yōu)化效率提升不明顯。DICG-VINS 后續(xù)會(huì)進(jìn)一步完善視覺慣性初始化終止條件的設(shè)定，在情形評(píng)估時(shí)能夠更快更準(zhǔn)確地評(píng)估當(dāng)前狀態(tài)的期望值，完成初始化的同時(shí)減少誤差的增加，構(gòu)建更加完備的視覺慣性SLAM 系統(tǒng)。