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

摘要：在這篇文章中，我們提出了一種新的方法，以融合具有挑戰性的城市環境中陸地車輛的視覺慣性測量，在這種環境中，GNSS信號既不可用也不可靠。受MEMSIMU大偏差導致的退化情況的激勵，我們在擴展卡爾曼濾波器的框架內重新設計了視覺慣性里程計的系統模型。特別地，系統模型通過由三軸陀螺儀、兩軸加速計和單軸里程表組成的簡化慣性傳感器系統傳播。分析可觀測性推導揭示了我們估計器的不可觀測基礎，這些方向通過使用GNSS接收機的間歇性位置測量來解決。此外，我們在蒙特卡洛模擬中檢查了狀態向量的不確定性，這與我們的理論結果一致。通過KITTI基準數據集和廣泛的現場測試驗證了所提出的方法，顯示隧道中的平均位置漂移為1.25%，街道峽谷中6.7km的平均位置誤差為2.81m。

1 引言

在城市地區，充滿挑戰的環境促使研究人員和技術人員融合了內感知和外感知傳感器，以進行自我運動估計。在定位的背景下，這些環境包括全球導航衛星系統（GNSS）中斷和GNSS的多路徑區域以及視覺系統的低亮度/紋理區域。融合了GNSS信號的慣性導航系統（INS）稱為INS/GNSS集成系統[1]，[2]是利用INS和GNSS的互補誤差特性的著名解決方案。然而，它的局限性是顯而易見的：當GNSS信號不可用或不可靠時，來自慣性測量單元（IMU）的噪聲測量的集成不能確保有界定位誤差。

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

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

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

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

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

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

本文組織如下。在第二節中回顧了之前關于陸地車輛視覺輔助定位的工作之后，第三節中介紹了坐標系定義、符號和MSCKF的初步內容。此外，還研究了常規VIO在遠程陸地車輛應用中的缺點。在第四節中，開發了所提出的VIWO，并通過推導其基來識別其不可觀測子空間。接下來，討論了虛擬數據集中的數值模擬結果，并在第五節中通過城市地區的驅動數據集驗證了所提出的方法。

2 相關工作 在過去幾十年中，通過集成各種傳感器的地面車輛導航系統已經成為熱門的研究領域。由于有大量關于地面車輛定位的相關研究，我們涵蓋了視覺輔助或基于視覺的方法以及基于深度學習的端到端方法的相關工作。

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

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

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

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

3 多狀態約束卡爾曼濾波器

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

圖1陸地車輛視覺慣性系統的坐標系定義，前進方向為右。

A.坐標系和符號

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

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

其中上小三角和上波浪線表示估計和誤差狀態，是單位矩陣，是斜對稱算子，使得

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

B.狀態矢量

MSCKF的狀態向量由主體和滑動窗口相關狀態組成，即

車體相關狀態由加速度計和陀螺儀的單位四元數、位置、速度和零偏組成，依次如下：

滑動窗口狀態是車輛探索時隨機克隆的先前車體姿勢，并定義為先前的車體姿勢，即

EKF的誤差狀態為

其中每個誤差狀態遵循（1）和（3）中的誤差定義。

我們假設與一致，不失一般性，以簡化符號，校準參數已知，但將在第四節中建模。

C.系統模型

IMU的測量值被建模為具有加性時變偏差和噪聲的真實讀數。

在這個表達式中，是車輛的真實加速度，是假設為,和分別是加速度計和陀螺儀的零平均白高斯過程。此外，真實比力為，真實角速度表示為。請注意，我們忽略了由于地球自轉而產生的附加項，因為我們假設車輛配備了低級別IMU，其中偏置水平遠高于自轉產生的讀數。

車體相關狀態由以下一組非線性方程控制，

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

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

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

我們將協方差矩陣劃分如下

由于滑動窗口姿態不受IMU讀數的影響，在離散時間內傳播方程為，

其中，是狀態轉移矩陣，

是從IMU的噪聲密度確定的離散時間中的噪聲協方差矩陣。

D.測量模型

視覺測量是從空間不同位置觀察的一系列特征點投影。數學上，它寫為

其中是第個特征到第個視圖的投影，是建模為與無關的零均值高斯噪聲的測量噪聲。在我們的實驗中為考慮熱噪聲、跟蹤器性能等，將的標準偏差設置為2.5像素。函數是第個特征的投影，。

最新估計時線性化的誤差模型為

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

為了消除對特征位置誤差的依賴，將（21）投影到的左零點空間。注意，由于，其中是視圖的數量，因此只有當且車輛移動時，總是存在維度為的左零空間。設，則有

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

E.關于常規VIO的問題

在介紹我們的方法之前，我們強調了VIO（不僅僅是MSCKF）對于使用低級別IMU的陸上車輛的挑戰，應該解決這些挑戰以擴大其通用性。

車輛靜止檢測

轉彎行駛時的定位精度

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

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

我們觀察到，當車輛進行圓形軌跡（這是陸地車輛的常見機動）時，定位精度尤其降低。非正式地，由于速度估計不準確，軌跡并不平滑。事實上，這是車輪視覺慣性系統的主要缺點，因為在恒定加速度分布中存在額外的不可觀察方向[21]。與顯示出明顯平滑轉彎的地面實況（GT）相反，MSCKF不僅導致了曲折的軌跡，而且在彎道駕駛中的最大位置誤差為8.33米。為了解決這一問題，我們使用嵌入式車輪里程表重新設計了估計器中的系統模型，該里程表輸出連續圖像幀之間的絕對比例。

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

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

4 方法提出

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

在下文中，我們詳細描述了濾波器結構，并推導了誤差狀態方程中的解析雅可比矩陣。然后，我們分析地證明了可觀測性，以通過利用封閉形式的線性化模型加深對所提出系統的理解。

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

A.狀態矢量

VIWO的過濾器狀態由主體、校準（主體到凸輪）和滑動窗口狀態組成，即

其中是車輪里程表的比例因子，它與視覺比例因子等效，因為特征深度由平移運動縮放。和是和的外參，在(6)中定義。由于我們只知道從攝像機的安裝角度和位置猜測的的近似值，因此我們在在線將它們精細地估計為過濾器狀態。

因此，誤差狀態是

其中在（9）中定義。旋轉誤差的定義類似于（1），

因此，誤差狀態的維數為。

B.使用輪子的系統模型

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

陀螺儀的測量如（10）所示建模，里程表讀數通過比例因子和加性零均值白高斯過程進行縮放。回顧以非轉向軸為中心的的定義，車身速度和噪聲如下

因為NHC在中確實有效。注意，我們假設和是一致的，而不喪失一般性。

在實踐中，我們在開始算法之前，通過使用加速計的傾斜角度來調整IMU相對于垂直于重力方向的表面的讀數。然后，連續時間非線性系統模型被表示為

在此公式中，我們將比例因子和校準參數建模為隨機常數。將泰勒級數擴展到一階，很容易看出誤差狀態方程為

其中我們省略了輸入參數，以簡化符號。

（29）的連續時間表示在上

狀態轉移矩陣的封閉形式和噪聲矢量在附錄A中導出。濾波器協方差基于（30）傳播。

C.狀態增強

當滿足特定標準時（例如，當前和最新滑動窗口姿勢之間的距離超過用戶定義的閾值，或者每當新圖像可用時），當前身體姿勢被增強到過濾狀態。假設是無偏電流濾波器狀態，且，則擴充執行以下計算

雅可比矩陣為

其中是滑動窗口的當前數量。

D.在線校準的測量模型

VIWO利用了三種嵌入式傳感器的測量結果：攝像頭、加速計和GNSS接收器。由于我們添加了校準參數，視覺相關模型將不同于（22）。我們描述測量方程并依次導出每個雅可比矩陣。

1）視覺特征跟蹤：從第幀觀察到的第個特征位置參數化為

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

由誤差狀態擾動(33)并忽略一階項產量

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

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

這里，和分別是測量車輛的側傾角和俯仰角，是加速度計測量值。由于我們的濾波器狀態不是用歐拉角表示的，因此我們必須推導出姿態誤差和歐拉角誤差之間的誤差狀態關系。

根據歐拉角速率和體速率之間的關系，我們有以下等式，

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

為零平均高斯噪聲。

然而，比力包括重力和身體加速度，其中身體加速度作為計算傾角的擾動。我們采用[47]中的適應方案：如果，協方差矩陣被增加，使得方差在中被預先調整的值膨脹。根據[47]中的數值模擬，我們設置。

3）GNSS位置：通過GNSS接收機計算車輛的全球位置，每當GNSS的導航解決方案可用時，濾波器就更新其位置。很容易看出，測量模型是

其中是位置測量，是零均值高斯噪聲。當GNSS位置可用時，比例因子是可觀察的，因此可以恢復視覺特征的深度。知道視覺特征的深度意味著單眼視覺模糊性已經得到解決。在實踐中，沒有GNSS位置的尺度模糊性起著次要作用，因為在我們的現場測試中，的變化僅小于1.5%。

E.可觀測性分析

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

因此，我們考慮以下線性化系統

EKF-SLAM狀態為

這里，是特征點的個數。

系統的可觀察性矩陣為

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

證明：第個特征的可觀測性矩陣的第塊是

其中點表示零矩陣，并且在（35）中導出雅可比矩陣。顯而易見

第一個不可觀測方向和第二個不可觀察方向分別對應于全球平移和旋轉，第三個基礎是移動平臺的尺度。

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

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

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

是對應于圍繞重力方向的旋轉的的第三列。

命題3：給定視覺、旋轉角度和全局位置測量，（40）的所有狀態都是可觀察的。

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

將可觀測性塊矩陣右側的、和相乘

如果車輛在中停留在的原點，則（51），（52）是零向量。然而，這在我們的案例中很難實現，因為我們考慮的是在給定時間內不靜止的車輛。

5 模擬實驗

我們在虛擬和現實世界環境中評估所提出的方法。對于前者，為了反映陸地車輛在物理上可行的導航剖面，我們根據KITTI數據集[44]中的地面真實速度和姿態生成模擬軌跡。對于后者，我們將我們的方法與KITTI基準數據集中最先進的VIO算法進行比較。此外，我們在數十公里的駕駛序列中廣泛評估了我們的方法，其中包括隧道和街道峽谷中的GNSS信號中斷。

A.虛擬軌跡

該測試的目的是驗證VIWO的可觀察性特征。我們采用來自KITTI的20110930_0027序列的真實速度和姿態，并生成IMU、里程計和特征測量。圖4繪制具有地標的導航剖面，其中最大滾動角和俯仰角分別為5.13和2.86度，平均速度為21.6公里/小時。我們將和設置為零來模擬NHC。此外，我們為每個連續的8個滑動窗口集生成10個虛擬特征軌跡。換句話說，我們假設隨機生成的特征點在8個視圖上得到了很好的跟蹤。

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

模擬傳感器的規格基于我們的測試車輛。請注意，IMU的性能基于BOSCHSMI130（安裝在我們的測試車輛中）以使模擬條件盡可能真實，并且我們在生成IMU測量時添加了偏置穩定性。規范總結在表I中。對于里程計測量，我們添加了一個密度為的高斯白噪聲，并將比例因子設置為。對于虛像，空間分辨率為1280×800視野(FOV)為57.2度，采樣率為7fps，測量噪聲為2.5像素。和之間的空間標定參數的初始不確定性分別設置為1度和30厘米作為標準偏差。

表 1 低級IMU規格

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

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

B.KITTI基準數據集

我們將我們的方法與最先進的VIO進行比較：OpenVINS[20]是基于MSCKF的具有長期SLAM特征和在線傳感器標定的VIO，沒有VINS-Mono[8]使用IMU預集成的基于優化的VIO的閉環，以及基于關鍵幀的VIO優化視覺和慣性因素的OKVIS[15]。該實驗的目的是針對最先進的VIO方法定量評估VIWO。

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

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

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

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

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

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

所提出的方法的強度是明顯的：當使用車輪里程計和帶有幾十mg偏差的低級別imu時，VIWO提供了最有效的融合結構之一。僅限加速度計偏差可以退化傾斜角。我們認為這是一個小缺點，因為陀螺儀偏差在VIWO中很好，如圖8所示。但是，需要注意的是，VIWO不能糾正位置漂移如我們的可觀測性分析顯示，在沒有絕對位置測量的里程計中的比例因子。為了解決尺度模糊，我們使用間歇的GNSS位置更新真實世界的驅動數據集。

C.實車實驗

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

圖9后備箱內安裝了輸入模塊和參考模塊，攝像頭安裝在擋風玻璃上

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

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

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

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

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

表 2 隧道數據中的航向和位置出口誤差

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

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

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

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

表 3 建筑數據中的平均和最大位置誤差

圖13顯示了一個GNSS中斷區域，其中VIO產生的最大位置誤差為16.2m，而VIWO在位置校正之前僅給出1.0m的誤差。VIO中的這個大錯誤部分源于零速度更新。盡管可以從里程表讀數中感知到車輛是靜止的，但VIO的狀態向量會根據速度的測量不確定性進行校正。結果，車輛的位置偏離了真實位置。相反，VIWO在車輛停止時輸出靜止運動，因為它基于速度讀數傳播位置。

圖13建筑數據集中的GNSS中斷區域：VIWO和VIO隨時間漂移的位置估計，其中GNSS位置在黃色圓圈中可用

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

6結論

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

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