在城市地區(qū)使用低等級(jí)IMU的單目視覺慣性車輪里程計(jì)

編者按：高精度定位模塊作為智能網(wǎng)聯(lián)車輛的核心技術(shù)之一，為環(huán)境感知、決策規(guī)劃、運(yùn)動(dòng)控制等提供了重要的參數(shù)信息，是自動(dòng)駕駛的重要支撐。智能網(wǎng)聯(lián)汽車搭載多種類型的傳感器，如全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System, GNSS）、慣性測(cè)量單元（Inertial Measurement Unit, IMU）、攝像頭等，為車輛定位系統(tǒng)提供了多種信息源。近年來利用多傳感器融合進(jìn)行組合定位實(shí)現(xiàn)可靠的位姿估計(jì)是當(dāng)下的研究熱點(diǎn)，其中視覺慣性里程計(jì)以性能互補(bǔ)特性被廣泛應(yīng)用。但是在城市峽谷等復(fù)雜地區(qū)中，GNSS信號(hào)易被遮擋導(dǎo)致定位信息不夠精確。本文基于此提出了一種應(yīng)對(duì)城市復(fù)雜地區(qū)GNSS信號(hào)被遮擋情況下的融合定位系統(tǒng)，通過分析不可觀測(cè)子空間得到系統(tǒng)設(shè)計(jì)的有效性，并通過在城市地區(qū)收集的開源基準(zhǔn)數(shù)據(jù)集和廣泛的真實(shí)駕駛數(shù)據(jù)集，驗(yàn)證了所提出的方法。

摘要：在這篇文章中，我們提出了一種新的方法，以融合具有挑戰(zhàn)性的城市環(huán)境中陸地車輛的視覺慣性測(cè)量，在這種環(huán)境中，GNSS信號(hào)既不可用也不可靠。受MEMSIMU大偏差導(dǎo)致的退化情況的激勵(lì)，我們?cè)跀U(kuò)展卡爾曼濾波器的框架內(nèi)重新設(shè)計(jì)了視覺慣性里程計(jì)的系統(tǒng)模型。特別地，系統(tǒng)模型通過由三軸陀螺儀、兩軸加速計(jì)和單軸里程表組成的簡化慣性傳感器系統(tǒng)傳播。分析可觀測(cè)性推導(dǎo)揭示了我們估計(jì)器的不可觀測(cè)基礎(chǔ)，這些方向通過使用GNSS接收機(jī)的間歇性位置測(cè)量來解決。此外，我們?cè)诿商乜?a href="http://ttokpm.com/analog/" target="_blank">模擬中檢查了狀態(tài)向量的不確定性，這與我們的理論結(jié)果一致。通過KITTI基準(zhǔn)數(shù)據(jù)集和廣泛的現(xiàn)場(chǎng)測(cè)試驗(yàn)證了所提出的方法，顯示隧道中的平均位置漂移為1.25%，街道峽谷中6.7km的平均位置誤差為2.81m。

1 引言

在城市地區(qū)，充滿挑戰(zhàn)的環(huán)境促使研究人員和技術(shù)人員融合了內(nèi)感知和外感知傳感器，以進(jìn)行自我運(yùn)動(dòng)估計(jì)。在定位的背景下，這些環(huán)境包括全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS）中斷和GNSS的多路徑區(qū)域以及視覺系統(tǒng)的低亮度/紋理區(qū)域。融合了GNSS信號(hào)的慣性導(dǎo)航系統(tǒng)（INS）稱為INS/GNSS集成系統(tǒng)[1]，[2]是利用INS和GNSS的互補(bǔ)誤差特性的著名解決方案。然而，它的局限性是顯而易見的：當(dāng)GNSS信號(hào)不可用或不可靠時(shí)，來自慣性測(cè)量單元（IMU）的噪聲測(cè)量的集成不能確保有界定位誤差。

另一方面，如果正確處理了高維測(cè)量，視覺傳感器會(huì)為定位問題輸出豐富的信息[3]。無論是通過將視覺測(cè)量與已知地標(biāo)匹配[4]–[6]，還是通過構(gòu)建環(huán)境并估計(jì)自我運(yùn)動(dòng)（SLAM）[7]–[9]，視覺傳感器都是一個(gè)有吸引力的解決方案，不僅是一個(gè)獨(dú)立的系統(tǒng)，也是一個(gè)集成的系統(tǒng)。

[10]中引入了視覺慣性里程計(jì)（VIO）這一術(shù)語，這意味著它通過融合視覺和慣性測(cè)量來逐步估計(jì)車輛的姿態(tài)。視覺傳感器的相對(duì)測(cè)量降低了INS的誤差累積率。另一方面，IMU讀數(shù)（角速度和比力）在短時(shí)間內(nèi)提供了絕對(duì)比例和精確姿態(tài)，采樣時(shí)間比圖像快得多。基本上，所有VIO方法都通過遞歸（濾波）或批估計(jì)（非線性優(yōu)化）來解決最大后驗(yàn)（MAP）問題。

[11]的一項(xiàng)開創(chuàng)性工作提出了多狀態(tài)約束卡爾曼濾波器（MSCKF），該濾波器隨機(jī)克隆其過去的姿態(tài)，以建模視覺特征軌跡。[12]-[14]的后續(xù)研究解決了MSCKF中的可觀測(cè)性失配問題，并提高了濾波的準(zhǔn)確性和一致性。對(duì)于非線性優(yōu)化框架，Leutenger等人[15]提出了聯(lián)合優(yōu)化視覺重投影誤差和慣性誤差項(xiàng)的VIO。另一方面，預(yù)積分[16]為迭代優(yōu)化中融合慣性測(cè)量開辟了一條有效途徑。[17]–[19]的作品采用了這種技術(shù)，并顯示了在VIO中的潛在用途。在這項(xiàng)工作中，我們選擇了一種基于濾波的估計(jì)器，因?yàn)樗谳^小的計(jì)算負(fù)載下顯示出優(yōu)于非線性優(yōu)化方法的競(jìng)爭性能[20]。

對(duì)于陸地車輛應(yīng)用，單目VIO由于其特定的運(yùn)動(dòng)輪廓，如無旋轉(zhuǎn)的恒定加速度，具有額外的不可觀察方向[21]。因此，定位性能降低。里程表的速度測(cè)量傳遞絕對(duì)刻度信息，之前的工作[21]、[22]利用里程表作為輔助INS的額外測(cè)量。然而，在該方案中，在每次獲取速度測(cè)量值或指定間隔時(shí)，更新估計(jì)器以執(zhí)行矩陣反演。在包含地標(biāo)位置或滑動(dòng)窗口的估計(jì)器的情況下，該策略會(huì)帶來巨大的計(jì)算負(fù)擔(dān)。此外，即使通過速度和其他測(cè)量來估計(jì)偏差，MEMSIMU的加速度計(jì)讀數(shù)的雙重積分仍會(huì)累積誤差。

受簡化慣性傳感器系統(tǒng)（RISS）[23]的啟發(fā)，我們開發(fā)了視覺慣性車輪里程計(jì)（VIWO）。具體而言，我們將MSCKF中的系統(tǒng)模型重新設(shè)計(jì)為3DRISS，而不是INS，使用里程表、3軸陀螺儀和2軸（向前和橫向）加速計(jì)的單通道速度測(cè)量。此外，只要可用，VIWO就通過GNSS位置以松耦合方式進(jìn)行更新。本文的主要貢獻(xiàn)有三個(gè)方面：

我們提出了一種新的方法，該方法結(jié)合了簡化慣性系統(tǒng)和MSCKF，其中基于車輪里程表重新設(shè)計(jì)了系統(tǒng)模型，以避免低等級(jí)加速計(jì)的雙重集成；我們通過分析得出VIWO的不可觀測(cè)子空間，以提供濾波器設(shè)計(jì)的有效性；通過在城市地區(qū)（包括隧道和街道峽谷）收集的開源基準(zhǔn)數(shù)據(jù)集和廣泛的真實(shí)駕駛數(shù)據(jù)集，驗(yàn)證了所提出的方法。

本文組織如下。在第二節(jié)中回顧了之前關(guān)于陸地車輛視覺輔助定位的工作之后，第三節(jié)中介紹了坐標(biāo)系定義、符號(hào)和MSCKF的初步內(nèi)容。此外，還研究了常規(guī)VIO在遠(yuǎn)程陸地車輛應(yīng)用中的缺點(diǎn)。在第四節(jié)中，開發(fā)了所提出的VIWO，并通過推導(dǎo)其基來識(shí)別其不可觀測(cè)子空間。接下來，討論了虛擬數(shù)據(jù)集中的數(shù)值模擬結(jié)果，并在第五節(jié)中通過城市地區(qū)的驅(qū)動(dòng)數(shù)據(jù)集驗(yàn)證了所提出的方法。

2 相關(guān)工作 在過去幾十年中，通過集成各種傳感器的地面車輛導(dǎo)航系統(tǒng)已經(jīng)成為熱門的研究領(lǐng)域。由于有大量關(guān)于地面車輛定位的相關(guān)研究，我們涵蓋了視覺輔助或基于視覺的方法以及基于深度學(xué)習(xí)的端到端方法的相關(guān)工作。

Rahman等人[24]在濾波框架中基于3DRISS融合視覺和慣性測(cè)量。他們的貢獻(xiàn)是INS輔助的離群點(diǎn)抑制方案和視覺慣性系統(tǒng)的融合方法，以松耦合的方式利用視覺里程計(jì)（VO）的位置估計(jì)。Lins等人[25]推導(dǎo)了一個(gè)方程式，以計(jì)算給定目標(biāo)的陸上車輛的速度，假設(shè)尺度模糊性得到解決。Karamat等人[26]提出了一種集成算法，將[25]中的VO和3DRISS松散融合。Ma等人[27]開發(fā)了基于雙眼視覺的MSCKF，通過阿克曼測(cè)量進(jìn)行校正；源自陸地車輛運(yùn)動(dòng)的物理約束。Wu等人[21]證明了平面運(yùn)動(dòng)下額外的不可觀測(cè)方向，并提出了一種用道路表面參數(shù)來增強(qiáng)狀態(tài)向量的偽測(cè)量。Wu等人[28]修改了傳統(tǒng)的角檢測(cè)器和跟蹤器，以提高地面車輛VO的精度和計(jì)算時(shí)間。Dong等人[29]對(duì)陸上車輛應(yīng)用的特征檢測(cè)器和描述符的不同組合進(jìn)行了定量比較，VIO基于[30]中的三焦點(diǎn)幾何結(jié)構(gòu)。

通過擴(kuò)展，使用視覺地標(biāo)和先驗(yàn)地圖數(shù)據(jù)庫進(jìn)行地面車輛定位的研究正在進(jìn)行中。Nedevschi等人[32]使用數(shù)字地圖將視覺地標(biāo)與數(shù)字地圖中的相應(yīng)地標(biāo)對(duì)齊。從立體圖像中，地面車輛行駛的車道通過貝葉斯網(wǎng)絡(luò)形式的概率方法使用粒子過濾器進(jìn)行識(shí)別。為了改進(jìn)定位，Gruyer等人[31]將GPS、INS和里程表與EKF相融合，使用了從側(cè)向攝像機(jī)獲得的車道標(biāo)記信息和精確的數(shù)字地圖。Jo等人[33]和Suhr等人[34]利用道路標(biāo)記與數(shù)字地圖的匹配結(jié)果來更新粒子過濾器，而后者通過存儲(chǔ)具有最小點(diǎn)數(shù)的地圖來減少數(shù)字地圖的體積。Wang等人[5]使用EKF融合高精度地圖中交通燈的準(zhǔn)確位置信息和INS信息，以提高定位精度。

近年來，在里程測(cè)量的背景下，出現(xiàn)了使用深度神經(jīng)網(wǎng)絡(luò)（DNN）的端到端方法，取代了手工特征提取器和傳統(tǒng)的基于模型的架構(gòu)。對(duì)于VO，[35]，[36]的工作通過考慮圖像流的時(shí)間相關(guān)性，提出了基于遞歸卷積神經(jīng)網(wǎng)絡(luò)（RCNN）的姿態(tài)估計(jì)架構(gòu)。Li等人[37]通過從光度和幾何一致性設(shè)計(jì)其損失函數(shù)，使用無監(jiān)督深度學(xué)習(xí)方案解決了姿態(tài)估計(jì)問題。在慣性里程計(jì)（IO）應(yīng)用中，Chen等人[38]提出了一種DNN框架，以解決采用長短期記憶（LSTM）的INS的誤差累積。Esfahani等人[39]將IMU讀數(shù)及其在輸入空間中的采樣時(shí)間包括在內(nèi)，后者在慣性里程計(jì)中起著關(guān)鍵作用。Esfahani等人[40]提出了一種僅使用陀螺儀的基于DNN的三自由度姿態(tài)估計(jì)器。Clark等人[41]和Chen等人[42]將VIO視為一個(gè)深度學(xué)習(xí)問題。他們分別向CNN和LSTM提供時(shí)間圖像和窗口IMU測(cè)量。視覺和慣性特征被連接起來，以表示網(wǎng)絡(luò)中的車輛姿態(tài)。

與之前的工作相比，我們的方法以緊耦合的方式融合視覺和慣性測(cè)量，隨機(jī)建模特征和車輛位置之間的相關(guān)性，否則在松耦合的融合中忽略這些相關(guān)性。此外，我們只利用自然出現(xiàn)的點(diǎn)特征，這些點(diǎn)特征是最一般的視覺信息，而沒有任何先前的地圖數(shù)據(jù)庫或訓(xùn)練階段。

3 多狀態(tài)約束卡爾曼濾波器

本節(jié)回顧了多狀態(tài)約束卡爾曼濾波器（MSCKF）[11]，該濾波器通過利用多視圖幾何中的相對(duì)約束來跟蹤相機(jī)姿態(tài)。首先，我們定義將在本文中使用的坐標(biāo)系和符號(hào)。然后，為了本文的完整性，給出了MSCKF的誤差狀態(tài)方程。最后，顯示了MSCKF在陸上車輛應(yīng)用中的缺點(diǎn)，這是我們陸上車輛導(dǎo)航新方法的核心動(dòng)機(jī)。

圖1陸地車輛視覺慣性系統(tǒng)的坐標(biāo)系定義，前進(jìn)方向?yàn)橛摇?/p>

A.坐標(biāo)系和符號(hào)

我們將全局框架定義為，這是與重力對(duì)齊的局部切平面，其原點(diǎn)位于初始身體框架。、和軸指向北、東和向下方向。表示為的相機(jī)框架以針孔相機(jī)模型的焦點(diǎn)為中心，軸遵循右下向前的慣例。我們將傳感器（IMU）框架指定為，將主體框架指定為。的中心與非完整約束（NHC）理想有效的非轉(zhuǎn)向軸的中點(diǎn)重合。的軸分別指向前、右和下。假設(shè)當(dāng)前時(shí)間步長為，則第個(gè)滑動(dòng)窗口幀定義為其中是滑動(dòng)窗口之間的間隔，是窗口的最大大小。圖1總結(jié)了假設(shè)車輛為前輪驅(qū)動(dòng)汽車的坐標(biāo)系定義。

小粗體字母代表向量，例如，代表某個(gè)維向量。矩陣被寫成大寫粗體字母，例如代表某個(gè)的矩陣。我們用上標(biāo)作為參考/解析框架，下標(biāo)作為對(duì)象框架來表示物理量。例如，表示從的原點(diǎn)到的原點(diǎn)的矢量。如果參考幀和解析幀不同，我們將在文本中明確提及。方向余弦矩陣（DCM）是，其將參考幀從轉(zhuǎn)換為，使得。相應(yīng)的單位四元數(shù)表示為。姿態(tài)誤差遵循右不變誤差定義，以避免不期望的可觀測(cè)性特征[43]，即

其中上小三角和上波浪線表示估計(jì)和誤差狀態(tài)，是單位矩陣，是斜對(duì)稱算子，使得

除了姿態(tài)之外的所有其他誤差狀態(tài)被定義為差異，即

B.狀態(tài)矢量

MSCKF的狀態(tài)向量由主體和滑動(dòng)窗口相關(guān)狀態(tài)組成，即

車體相關(guān)狀態(tài)由加速度計(jì)和陀螺儀的單位四元數(shù)、位置、速度和零偏組成，依次如下：

滑動(dòng)窗口狀態(tài)是車輛探索時(shí)隨機(jī)克隆的先前車體姿勢(shì)，并定義為先前的車體姿勢(shì)，即

EKF的誤差狀態(tài)為

其中每個(gè)誤差狀態(tài)遵循（1）和（3）中的誤差定義。

我們假設(shè)與一致，不失一般性，以簡化符號(hào)，校準(zhǔn)參數(shù)已知，但將在第四節(jié)中建模。

C.系統(tǒng)模型

IMU的測(cè)量值被建模為具有加性時(shí)變偏差和噪聲的真實(shí)讀數(shù)。

在這個(gè)表達(dá)式中，是車輛的真實(shí)加速度，是假設(shè)為,和分別是加速度計(jì)和陀螺儀的零平均白高斯過程。此外，真實(shí)比力為，真實(shí)角速度表示為。請(qǐng)注意，我們忽略了由于地球自轉(zhuǎn)而產(chǎn)生的附加項(xiàng)，因?yàn)槲覀兗僭O(shè)車輛配備了低級(jí)別IMU，其中偏置水平遠(yuǎn)高于自轉(zhuǎn)產(chǎn)生的讀數(shù)。

車體相關(guān)狀態(tài)由以下一組非線性方程控制，

其中和分別是加速度計(jì)和陀螺儀的偏置的零平均白高斯過程；這是隨機(jī)行走偏差。矩陣是

濾波器協(xié)方差矩陣通過線性化模型傳播，

雅可比矩陣、是通過擾動(dòng)誤差狀態(tài)并去除依賴于。（13）中的噪聲矢量為

我們將協(xié)方差矩陣劃分如下

由于滑動(dòng)窗口姿態(tài)不受IMU讀數(shù)的影響，在離散時(shí)間內(nèi)傳播方程為，

其中，是狀態(tài)轉(zhuǎn)移矩陣，

是從IMU的噪聲密度確定的離散時(shí)間中的噪聲協(xié)方差矩陣。

D.測(cè)量模型

視覺測(cè)量是從空間不同位置觀察的一系列特征點(diǎn)投影。數(shù)學(xué)上，它寫為

其中是第個(gè)特征到第個(gè)視圖的投影，是建模為與無關(guān)的零均值高斯噪聲的測(cè)量噪聲。在我們的實(shí)驗(yàn)中為考慮熱噪聲、跟蹤器性能等，將的標(biāo)準(zhǔn)偏差設(shè)置為2.5像素。函數(shù)是第個(gè)特征的投影，。

最新估計(jì)時(shí)線性化的誤差模型為

矩陣和分別是滑動(dòng)窗口和特征位置的雅可比矩陣。如果我們讓來自所有視圖的連接殘差為，

為了消除對(duì)特征位置誤差的依賴，將（21）投影到的左零點(diǎn)空間。注意，由于，其中是視圖的數(shù)量，因此只有當(dāng)且車輛移動(dòng)時(shí)，總是存在維度為的左零空間。設(shè)，則有

所有特征點(diǎn)測(cè)量值被疊加并用于計(jì)算卡爾曼增益。過濾器更新遵循（1）和（3）中的誤差定義。

E.關(guān)于常規(guī)VIO的問題

在介紹我們的方法之前，我們強(qiáng)調(diào)了VIO（不僅僅是MSCKF）對(duì)于使用低級(jí)別IMU的陸上車輛的挑戰(zhàn)，應(yīng)該解決這些挑戰(zhàn)以擴(kuò)大其通用性。

車輛靜止檢測(cè)

轉(zhuǎn)彎行駛時(shí)的定位精度

數(shù)十公里行駛時(shí)的無邊界位置和航向不確定性

對(duì)于第一個(gè)挑戰(zhàn)，關(guān)鍵是檢測(cè)靜態(tài)以獲得偽測(cè)量，例如零速度更新（ZUPT）以限制速度誤差。特別是對(duì)于大多數(shù)單眼視覺系統(tǒng)，需要來自純平移的特征點(diǎn)視差來估計(jì)自我運(yùn)動(dòng)。在基于IMU的靜止檢測(cè)方面有幾項(xiàng)先前的工作，例如陸地車輛的頻域方法[45]，以及行人航位推算系統(tǒng)的基于信號(hào)幅度/方差的方法[46]。然而，最簡單有效的方法是使用車輪里程表。這就是為什么我們使用車輪里程表而不是（11）中公式化的慣性導(dǎo)航系統(tǒng)的航位推算方法的原因之一。

我們觀察到，當(dāng)車輛進(jìn)行圓形軌跡（這是陸地車輛的常見機(jī)動(dòng)）時(shí)，定位精度尤其降低。非正式地，由于速度估計(jì)不準(zhǔn)確，軌跡并不平滑。事實(shí)上，這是車輪視覺慣性系統(tǒng)的主要缺點(diǎn)，因?yàn)樵诤愣铀俣确植贾写嬖陬~外的不可觀察方向[21]。與顯示出明顯平滑轉(zhuǎn)彎的地面實(shí)況（GT）相反，MSCKF不僅導(dǎo)致了曲折的軌跡，而且在彎道駕駛中的最大位置誤差為8.33米。為了解決這一問題，我們使用嵌入式車輪里程表重新設(shè)計(jì)了估計(jì)器中的系統(tǒng)模型，該里程表輸出連續(xù)圖像幀之間的絕對(duì)比例。

圖 2 KITTI數(shù)據(jù)集[44]的20110930_drive_0027中的拐角駕駛示例：（a）來自車載攝像頭的圖像；（b） 地面實(shí)況（GT）和估計(jì)的2D位置（MSCKF）報(bào)告為10Hz的圓圈，其中箭頭表示車輛在瞬間的航向

最后，很好地研究了在6自由度一般運(yùn)動(dòng)下有4個(gè)不可觀測(cè)的方向（圍繞重力的全局平移和旋轉(zhuǎn)）[10]。換句話說，這個(gè)方向的不確定性是沒有界限的，阻礙了VIO的長期應(yīng)用。通過在任何可用的情況下以松耦合方式使用GNSS信號(hào)，我們將證明視覺慣性系統(tǒng)可以通過可靠的導(dǎo)航解決方案運(yùn)行數(shù)十公里。

4 方法提出

基于我們之前的工作[47]，其中提出了3DDR/GPS集成系統(tǒng)，我們新設(shè)計(jì)了專門用于陸地車輛的VIO，稱為視覺慣性車輪里程計(jì)（VIWO）。圖3繪制了擬議系統(tǒng)的總體流程圖。我們向估計(jì)器提供4種類型的信息：來自視覺前端的特征軌跡、來自慣性輪系統(tǒng)的先驗(yàn)信息、來自加速計(jì)的傾斜角度以及來自GNSS接收器的全球位置。給定低級(jí)別IMU，我們從系統(tǒng)動(dòng)力學(xué)中排除加速計(jì)讀數(shù)，但僅將其用作傾斜（側(cè)傾和俯仰）角度測(cè)量。優(yōu)雅但簡單的修改避免了噪音加速計(jì)讀數(shù)的雙重集成；相反，它需要車輛速度的單一積分。此外，視覺測(cè)量進(jìn)一步減少了由車輪滑移和陀螺儀的時(shí)變偏差產(chǎn)生的位置漂移。

在下文中，我們?cè)敿?xì)描述了濾波器結(jié)構(gòu)，并推導(dǎo)了誤差狀態(tài)方程中的解析雅可比矩陣。然后，我們分析地證明了可觀測(cè)性，以通過利用封閉形式的線性化模型加深對(duì)所提出系統(tǒng)的理解。

圖 3 所提出方法的概述，視覺慣性車輪里程計(jì)（VIWO）

A.狀態(tài)矢量

VIWO的過濾器狀態(tài)由主體、校準(zhǔn)（主體到凸輪）和滑動(dòng)窗口狀態(tài)組成，即

其中是車輪里程表的比例因子，它與視覺比例因子等效，因?yàn)樘卣魃疃扔善揭七\(yùn)動(dòng)縮放。和是和的外參，在(6)中定義。由于我們只知道從攝像機(jī)的安裝角度和位置猜測(cè)的的近似值，因此我們?cè)谠诰€將它們精細(xì)地估計(jì)為過濾器狀態(tài)。

因此，誤差狀態(tài)是

其中在（9）中定義。旋轉(zhuǎn)誤差的定義類似于（1），

因此，誤差狀態(tài)的維數(shù)為。

B.使用輪子的系統(tǒng)模型

在具有3軸陀螺儀和1軸里程表的慣性輪系統(tǒng)中，傳感器讀數(shù)建模為

陀螺儀的測(cè)量如（10）所示建模，里程表讀數(shù)通過比例因子和加性零均值白高斯過程進(jìn)行縮放?；仡櫼苑寝D(zhuǎn)向軸為中心的的定義，車身速度和噪聲如下

因?yàn)镹HC在中確實(shí)有效。注意，我們假設(shè)和是一致的，而不喪失一般性。

在實(shí)踐中，我們?cè)陂_始算法之前，通過使用加速計(jì)的傾斜角度來調(diào)整IMU相對(duì)于垂直于重力方向的表面的讀數(shù)。然后，連續(xù)時(shí)間非線性系統(tǒng)模型被表示為

在此公式中，我們將比例因子和校準(zhǔn)參數(shù)建模為隨機(jī)常數(shù)。將泰勒級(jí)數(shù)擴(kuò)展到一階，很容易看出誤差狀態(tài)方程為

其中我們省略了輸入?yún)?shù)，以簡化符號(hào)。

（29）的連續(xù)時(shí)間表示在上

狀態(tài)轉(zhuǎn)移矩陣的封閉形式和噪聲矢量在附錄A中導(dǎo)出。濾波器協(xié)方差基于（30）傳播。

C.狀態(tài)增強(qiáng)

當(dāng)滿足特定標(biāo)準(zhǔn)時(shí)（例如，當(dāng)前和最新滑動(dòng)窗口姿勢(shì)之間的距離超過用戶定義的閾值，或者每當(dāng)新圖像可用時(shí)），當(dāng)前身體姿勢(shì)被增強(qiáng)到過濾狀態(tài)。假設(shè)是無偏電流濾波器狀態(tài)，且，則擴(kuò)充執(zhí)行以下計(jì)算

雅可比矩陣為

其中是滑動(dòng)窗口的當(dāng)前數(shù)量。

D.在線校準(zhǔn)的測(cè)量模型

VIWO利用了三種嵌入式傳感器的測(cè)量結(jié)果：攝像頭、加速計(jì)和GNSS接收器。由于我們添加了校準(zhǔn)參數(shù)，視覺相關(guān)模型將不同于（22）。我們描述測(cè)量方程并依次導(dǎo)出每個(gè)雅可比矩陣。

1）視覺特征跟蹤：從第幀觀察到的第個(gè)特征位置參數(shù)化為

與（20）相似，線性化的模型為，

由誤差狀態(tài)擾動(dòng)(33)并忽略一階項(xiàng)產(chǎn)量

遵循與(21)和(22)中相同的過程。

2）傾斜角：從加速度計(jì)獲得的特定力僅用于限定車輛的傾斜角。眾所周知，滾動(dòng)角和俯仰角根據(jù)加速度計(jì)讀數(shù)計(jì)算如下[2]

這里，和分別是測(cè)量車輛的側(cè)傾角和俯仰角，是加速度計(jì)測(cè)量值。由于我們的濾波器狀態(tài)不是用歐拉角表示的，因此我們必須推導(dǎo)出姿態(tài)誤差和歐拉角誤差之間的誤差狀態(tài)關(guān)系。

根據(jù)歐拉角速率和體速率之間的關(guān)系，我們有以下等式，

DCM分解為，代表角速率。因此，測(cè)量模型為

為零平均高斯噪聲。

然而，比力包括重力和身體加速度，其中身體加速度作為計(jì)算傾角的擾動(dòng)。我們采用[47]中的適應(yīng)方案：如果，協(xié)方差矩陣被增加，使得方差在中被預(yù)先調(diào)整的值膨脹。根據(jù)[47]中的數(shù)值模擬，我們?cè)O(shè)置。

3）GNSS位置：通過GNSS接收機(jī)計(jì)算車輛的全球位置，每當(dāng)GNSS的導(dǎo)航解決方案可用時(shí)，濾波器就更新其位置。很容易看出，測(cè)量模型是

其中是位置測(cè)量，是零均值高斯噪聲。當(dāng)GNSS位置可用時(shí)，比例因子是可觀察的，因此可以恢復(fù)視覺特征的深度。知道視覺特征的深度意味著單眼視覺模糊性已經(jīng)得到解決。在實(shí)踐中，沒有GNSS位置的尺度模糊性起著次要作用，因?yàn)樵谖覀兊默F(xiàn)場(chǎng)測(cè)試中，的變化僅小于1.5%。

E.可觀測(cè)性分析

可觀察性是濾波器設(shè)計(jì)期間應(yīng)檢查的最重要的系統(tǒng)屬性之一。換句話說，它問“初始狀態(tài)能否在有限時(shí)間內(nèi)給定系統(tǒng)模型(30)和測(cè)量值(34),(38),(39)唯一地獲得”。我們通過呈現(xiàn)可觀察性特征（包括視覺、傾斜角和位置測(cè)量）來提供我們?yōu)V波器設(shè)計(jì)的有效性，這些是自動(dòng)駕駛汽車中最常見的傳感器之一。由于噪聲不影響可觀察性特征，因此我們?nèi)コ嗽肼曧?xiàng)。此外，還表明MSCKF和EKF-SLAM是線性高斯系統(tǒng)[10]的等效最大后驗(yàn)(MAP)估計(jì)量。因此，我們以EKF-SLAM的形式分析線性化VIWO以簡化分析。從技術(shù)上講，唯一的區(qū)別是濾波器狀態(tài)和視覺測(cè)量：滑動(dòng)窗口被特征位置取代，我們使用零空間投影之前的測(cè)量模型。

因此，我們考慮以下線性化系統(tǒng)

EKF-SLAM狀態(tài)為

這里，是特征點(diǎn)的個(gè)數(shù)。

系統(tǒng)的可觀察性矩陣為

命題1：給定視覺測(cè)量，全局位置和旋轉(zhuǎn)（橫搖、俯仰、偏航）以及尺度是不可觀測(cè)的。

證明：第個(gè)特征的可觀測(cè)性矩陣的第塊是

其中點(diǎn)表示零矩陣，并且在（35）中導(dǎo)出雅可比矩陣。顯而易見

第一個(gè)不可觀測(cè)方向和第二個(gè)不可觀察方向分別對(duì)應(yīng)于全球平移和旋轉(zhuǎn)，第三個(gè)基礎(chǔ)是移動(dòng)平臺(tái)的尺度。

命題2：給定視覺和傾斜角度測(cè)量，全球位置、圍繞重力的旋轉(zhuǎn)（偏航）和尺度是不可觀測(cè)的。

證明：傾斜角度測(cè)量將額外的可觀測(cè)性矩陣添加到（43），即

其中在（38）中導(dǎo)出雅可比矩陣。通過附加的可觀測(cè)性矩陣塊，的第一和第二基不再跨越不可觀測(cè)子空間，但、和是不可觀測(cè)方向，即

是對(duì)應(yīng)于圍繞重力方向的旋轉(zhuǎn)的的第三列。

命題3：給定視覺、旋轉(zhuǎn)角度和全局位置測(cè)量，（40）的所有狀態(tài)都是可觀察的。

證明：為了證明這個(gè)命題，通過增加額外的可觀測(cè)性塊，足以證明、和不再是不可觀測(cè)的方向。

將可觀測(cè)性塊矩陣右側(cè)的、和相乘

如果車輛在中停留在的原點(diǎn)，則（51），（52）是零向量。然而，這在我們的案例中很難實(shí)現(xiàn)，因?yàn)槲覀兛紤]的是在給定時(shí)間內(nèi)不靜止的車輛。

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

我們?cè)谔摂M和現(xiàn)實(shí)世界環(huán)境中評(píng)估所提出的方法。對(duì)于前者，為了反映陸地車輛在物理上可行的導(dǎo)航剖面，我們根據(jù)KITTI數(shù)據(jù)集[44]中的地面真實(shí)速度和姿態(tài)生成模擬軌跡。對(duì)于后者，我們將我們的方法與KITTI基準(zhǔn)數(shù)據(jù)集中最先進(jìn)的VIO算法進(jìn)行比較。此外，我們?cè)跀?shù)十公里的駕駛序列中廣泛評(píng)估了我們的方法，其中包括隧道和街道峽谷中的GNSS信號(hào)中斷。

A.虛擬軌跡

該測(cè)試的目的是驗(yàn)證VIWO的可觀察性特征。我們采用來自KITTI的20110930_0027序列的真實(shí)速度和姿態(tài)，并生成IMU、里程計(jì)和特征測(cè)量。圖4繪制具有地標(biāo)的導(dǎo)航剖面，其中最大滾動(dòng)角和俯仰角分別為5.13和2.86度，平均速度為21.6公里/小時(shí)。我們將和設(shè)置為零來模擬NHC。此外，我們?yōu)槊總€(gè)連續(xù)的8個(gè)滑動(dòng)窗口集生成10個(gè)虛擬特征軌跡。換句話說，我們假設(shè)隨機(jī)生成的特征點(diǎn)在8個(gè)視圖上得到了很好的跟蹤。

圖4使用20110930_0027序列的虛擬軌跡：(a)x-y平面中特征點(diǎn)的位置，(b)歐拉角和物體速度

模擬傳感器的規(guī)格基于我們的測(cè)試車輛。請(qǐng)注意，IMU的性能基于BOSCHSMI130（安裝在我們的測(cè)試車輛中）以使模擬條件盡可能真實(shí)，并且我們?cè)谏蒊MU測(cè)量時(shí)添加了偏置穩(wěn)定性。規(guī)范總結(jié)在表I中。對(duì)于里程計(jì)測(cè)量，我們添加了一個(gè)密度為的高斯白噪聲，并將比例因子設(shè)置為。對(duì)于虛像，空間分辨率為1280×800視野(FOV)為57.2度，采樣率為7fps，測(cè)量噪聲為2.5像素。和之間的空間標(biāo)定參數(shù)的初始不確定性分別設(shè)置為1度和30厘米作為標(biāo)準(zhǔn)偏差。

表 1 低級(jí)IMU規(guī)格

最后，我們假設(shè)身體框架的全局位置是用來自GNSS信號(hào)的的不確定性來測(cè)量的。我們僅使用視覺測(cè)量(VIS)和視覺、傾斜和位置測(cè)量(VIS+ARS+GNSS)在兩個(gè)場(chǎng)景中進(jìn)行了50次蒙特卡羅模擬。圖4繪制了僅使用視覺測(cè)量時(shí)位于不可觀察子空間上的選定誤差狀態(tài)，即和。在圖5(a)中，姿態(tài)和比例因子的不確定性分別單調(diào)增加并保持不變。在某些情況下，位置不確定性會(huì)降低，但這并不一定意味著由于測(cè)量而導(dǎo)致的不確定性降低，因?yàn)椋?3）的狀態(tài)轉(zhuǎn)移矩陣包括產(chǎn)生波動(dòng)的旋轉(zhuǎn)矩陣。相比之下，所選狀態(tài)不再停留在圖5（b）中的不可觀察子空間中，正如我們?cè)诘贗V-E節(jié)中的分析所示。在實(shí)踐中，只要GNSS信號(hào)可靠，我們就會(huì)更新VIWO；因此，不可觀察的模式不會(huì)對(duì)估計(jì)器造成惡化的影響。

圖5選定狀態(tài)的50次Monte-Carlo模擬結(jié)果；具有3σ包絡(luò)的姿態(tài)、位置和比例因子誤差：(a)僅使用視覺測(cè)量VIS，(b)使用視覺、加速度計(jì)和GNSS位置測(cè)量, VIS + ARS + GNSS

B.KITTI基準(zhǔn)數(shù)據(jù)集

我們將我們的方法與最先進(jìn)的VIO進(jìn)行比較：OpenVINS[20]是基于MSCKF的具有長期SLAM特征和在線傳感器標(biāo)定的VIO，沒有VINS-Mono[8]使用IMU預(yù)集成的基于優(yōu)化的VIO的閉環(huán)，以及基于關(guān)鍵幀的VIO優(yōu)化視覺和慣性因素的OKVIS[15]。該實(shí)驗(yàn)的目的是針對(duì)最先進(jìn)的VIO方法定量評(píng)估VIWO。

使用原始KITTI[44]數(shù)據(jù)的5個(gè)住宅序列。我們使用原始IMU測(cè)量(TacticalIMU)進(jìn)行了兩個(gè)實(shí)驗(yàn)，其中OXTSRT3003報(bào)告其偏置可重復(fù)性為1mg和0.01deg/s，偏置添加的IMU(LowIMU)具有70mg和1deg/s偏置以研究性能退化取決于IMU等級(jí)。注意為了盡可能公平地比較方法，我們提取每張圖像最多150個(gè)點(diǎn)的特征，并將初始車輛速度設(shè)置為地面實(shí)況。估計(jì)器參數(shù)根據(jù)每個(gè)算法的IMU的偏差水平進(jìn)行調(diào)整。

圖6顯示了3D位置誤差漂移除以總距離。在TacticalIMU的第一個(gè)實(shí)驗(yàn)中，OpenVINS在5個(gè)序列中的3個(gè)序列中實(shí)現(xiàn)了最小的位置漂移，而我們的方法在圖6（a）中的5個(gè)序列中的2個(gè)序列中顯示了最小的位置漂移。也就是說，由于傳感器偏差低，戰(zhàn)術(shù)級(jí)IMU無法觀察到我們方法的優(yōu)勢(shì)。然而，當(dāng)使用圖6(b)中的低級(jí)IMU時(shí)，我們的方法在整體位置漂移方面記錄了最佳性能。在大傳感器偏差下采用RISS避免了加速度計(jì)測(cè)量的直接集成。

圖6位置RMSE為(a)戰(zhàn)術(shù)級(jí)IMU和(b)低級(jí)別IMU的KITTI住宅序列中的漂移百分比。

圖7繪制了KITTI軌跡的俯視圖以及位置漂移可視化的地面實(shí)況軌跡。陀螺儀偏差都收斂到大致相同的值，如圖8所示。

圖7在KITTI數(shù)據(jù)集中的估計(jì)軌跡：在低級(jí)別IMU設(shè)置下的(a)0930_0020、(b)0930_0027、(c)0930_0033和(d)0930_0034序列。

圖8估計(jì)了低級(jí)別IMU中0930_0033的陀螺儀x、y、z軸偏差

所提出的方法的強(qiáng)度是明顯的：當(dāng)使用車輪里程計(jì)和帶有幾十mg偏差的低級(jí)別imu時(shí)，VIWO提供了最有效的融合結(jié)構(gòu)之一。僅限加速度計(jì)偏差可以退化傾斜角。我們認(rèn)為這是一個(gè)小缺點(diǎn)，因?yàn)橥勇輧x偏差在VIWO中很好，如圖8所示。但是，需要注意的是，VIWO不能糾正位置漂移如我們的可觀測(cè)性分析顯示，在沒有絕對(duì)位置測(cè)量的里程計(jì)中的比例因子。為了解決尺度模糊，我們使用間歇的GNSS位置更新真實(shí)世界的驅(qū)動(dòng)數(shù)據(jù)集。

C.實(shí)車實(shí)驗(yàn)

為了在充滿視覺異常值（如移動(dòng)物體和亮度變化）的遠(yuǎn)程和具有挑戰(zhàn)性的現(xiàn)實(shí)環(huán)境中測(cè)試VIWO，我們使用圖9所示的車輛記錄了一個(gè)駕駛數(shù)據(jù)集。它包括一個(gè)IMU（博世，SMI130)、單目視覺(Onsemiconductor,AR0141CS)、GNSS接收器(Ublox,Neo-M8P)和參考模塊(Novatel,SPAN-CPT)以提供真實(shí)軌跡。我們使用的IMU將陀螺儀和加速度計(jì)的零點(diǎn)偏移報(bào)告為1deg/s、70mg，噪聲RMS報(bào)告為0.02、0.19。與第V-A節(jié)的模擬一樣，圖像傳感器的分辨率為1280×800，F(xiàn)OV為57.2度。

圖9后備箱內(nèi)安裝了輸入模塊和參考模塊，攝像頭安裝在擋風(fēng)玻璃上

我們選擇包括隧道和街道峽谷在內(nèi)的駕駛課程，其中GNSS信號(hào)不可用且間歇性不可靠。隧道數(shù)據(jù)集的整個(gè)軌跡如圖10(a)所示。司機(jī)往返31.3公里，用時(shí)30.4分鐘，往返3條隧道，共6趟。圖10（b，c）分別顯示了穿越和進(jìn)入隧道期間的示例圖像。圖像中的綠點(diǎn)表示跟蹤到的特征，而紅點(diǎn)表示未跟蹤到的特征。同樣，建筑數(shù)據(jù)集的圖11(a)在29.7分鐘內(nèi)繪制了街道峽谷中6.74公里的路線的地面實(shí)況。圖11(b,c)給出了具有駕駛視覺特征的樣本圖像。

圖10隧道數(shù)據(jù)集：(a)3條隧道31.3公里30.4分鐘，隧道(b)樣本圖像，隧道前(c)有綠點(diǎn)（跟蹤特征）和紅點(diǎn)（跟蹤失?。?。

圖11建筑數(shù)據(jù)集：(a)在一個(gè)街道峽谷中，全長6.74公里，持續(xù)29.7分鐘，（b，c）其樣本圖像。

視覺前端通過帶有8點(diǎn)RANSAC的KLT跟蹤器跟蹤150個(gè)FAST角點(diǎn)[48]，以排除一對(duì)連續(xù)圖像中的視覺異常值。此外，為了在圖像上均勻分布角落，我們將每個(gè)角落的最小距離設(shè)置為40像素?；瑒?dòng)窗口數(shù)量設(shè)置為12，并且通過在過濾器更新之前使用Levenberg-Marquardt算法最小化重投影誤差來對(duì)超過3個(gè)視圖的跟蹤特征進(jìn)行三角剖分。VIWO是在JetsonTX2中實(shí)現(xiàn)的，僅使用CPU的一個(gè)實(shí)時(shí)內(nèi)核。在現(xiàn)場(chǎng)測(cè)試中，包括視覺前端和過濾器后端的CPU負(fù)載平均為38%。

我們將提議的VIWO與通過視覺特征跟蹤以及來自里程表和GNSS接收器的速度和位置測(cè)量值更新的MSCKF進(jìn)行比較。請(qǐng)注意，為了使兩種算法盡可能公平，MSCKF還估計(jì){C}和{B}之間的外部標(biāo)定參數(shù)。我們將這種傳統(tǒng)的VIO稱為VIO。

表 2 隧道數(shù)據(jù)中的航向和位置出口誤差

表二總結(jié)了每個(gè)隧道出口處車輛的航向和水平位置誤差，%DT報(bào)告位置誤差除以相應(yīng)隧道的長度。我們之所以選擇出口誤差是為了在GNSS中斷區(qū)域競(jìng)爭位置誤差漂移。VIWO的平均%DT在6次遍歷中為1.25%，而VIO顯示為2.75%。這種改進(jìn)源于以基于里程計(jì)的系統(tǒng)模型取代基于加速度計(jì)的系統(tǒng)模型。盡管VIO中的加速度計(jì)偏差是通過視覺特征和速度更新來估計(jì)的，但其70mg偏差水平的巨大不確定性會(huì)在估計(jì)器上產(chǎn)生很大的線性化誤差。在數(shù)量上，與VIO相比，VIWO將位置誤差降低了54.6%。

圖12在2號(hào)和3號(hào)西行路線出口的隧道數(shù)據(jù)集中，每秒采樣一次的帶有航向（箭頭）的車輛位置（圓圈）。

此外，圖12顯示了西行#2隧道（頂部）和#3隧道（底部）出口處的位置漂移。GNSS測(cè)量在信號(hào)可用時(shí)對(duì)位置進(jìn)行校正，在校正前明顯觀察到地面實(shí)況和估計(jì)軌跡之間的差異。所提出的VIWO表現(xiàn)出比VIO低得多的誤差漂移。

在建筑數(shù)據(jù)集中，我們?cè)u(píng)估了車輛在整個(gè)駕駛過程中的平均和最大最大位置誤差，包括GNSS中斷和開放天空區(qū)域。我們根據(jù)精度稀釋度（DOP）和可見衛(wèi)星數(shù)量整理出可靠的GNSS定位方案，數(shù)據(jù)集中GNSS可用率為55.4%。表III報(bào)告了街道峽谷整個(gè)過程中的平均和最大水平位置誤差。與VIO相比，VIWO將平均位置誤差降低了0.65m。

表 3 建筑數(shù)據(jù)中的平均和最大位置誤差

圖13顯示了一個(gè)GNSS中斷區(qū)域，其中VIO產(chǎn)生的最大位置誤差為16.2m，而VIWO在位置校正之前僅給出1.0m的誤差。VIO中的這個(gè)大錯(cuò)誤部分源于零速度更新。盡管可以從里程表讀數(shù)中感知到車輛是靜止的，但VIO的狀態(tài)向量會(huì)根據(jù)速度的測(cè)量不確定性進(jìn)行校正。結(jié)果，車輛的位置偏離了真實(shí)位置。相反，VIWO在車輛停止時(shí)輸出靜止運(yùn)動(dòng)，因?yàn)樗谒俣茸x數(shù)傳播位置。

圖13建筑數(shù)據(jù)集中的GNSS中斷區(qū)域：VIWO和VIO隨時(shí)間漂移的位置估計(jì)，其中GNSS位置在黃色圓圈中可用

鑒于市區(qū)充滿了視覺異常值，例如移動(dòng)的汽車、行人等，如圖11(c)所示，當(dāng)大部分特征放置在移動(dòng)物體上時(shí)，8-ptRANSAC無法成功消除異常值。結(jié)果，VIWO和VIO都不可避免地被視覺異常值更新。在本實(shí)驗(yàn)中，僅使用圖像來檢測(cè)和擦除移動(dòng)物體上的特征。作為我們未來的工作之一，我們可以利用估計(jì)的自我運(yùn)動(dòng)提供有用的信息來放棄異常值，這將進(jìn)一步提高VIWO的估計(jì)準(zhǔn)確性。

6結(jié)論

在本文中，我們提出了視覺慣性輪里程計(jì)（VIWO），它通過分別從陀螺儀和里程計(jì)獲得的角速率和速度測(cè)量來傳播系統(tǒng)模型。VIWO的優(yōu)勢(shì)和獨(dú)創(chuàng)性在于這種簡單而強(qiáng)大的變化，即在使用低檔IMU時(shí)，從基于加速度計(jì)的方法到基于里程計(jì)的陸地車輛方法。此外，當(dāng)假設(shè)測(cè)量視覺特征、傾斜角和位置時(shí)，我們分析得出了VIWO的不可觀察子空間。它揭示了7個(gè)方向（態(tài)度、位置、尺度）位于不可觀測(cè)的子空間上，當(dāng)全球位置測(cè)量可用時(shí)，這些都變得可觀測(cè)。此外，我們的數(shù)值模擬通過檢查狀態(tài)向量的不確定性證實(shí)了理論結(jié)果。最后，我們?cè)贕NSS信號(hào)不可用且不可靠的隧道和街道峽谷等具有挑戰(zhàn)性的環(huán)境中將VIWO與傳統(tǒng)VIO進(jìn)行了比較。在隧道中，與傳統(tǒng)VIO相比，VIWO將位置誤差平均減少了54.6%，在街道峽谷中減少了0.65m。

我們未來的工作包括使用估計(jì)的自我運(yùn)動(dòng)來進(jìn)一步減少導(dǎo)航誤差的異常值拒絕方案。鑒于我們?cè)谶@項(xiàng)工作中只使用了點(diǎn)特征，因此也可以在圖像中提取額外的視覺信息，如線條、消失點(diǎn)和表面。這些與VIWO融合的測(cè)量將進(jìn)一步提高導(dǎo)航性能。