I. 摘要

大多數6自由度定位和SLAM系統使用靜態地標，因為它們無法有效地將動態目標納入典型的過程中，所以會選擇忽略動態目標。在已經納入動態目標的情況下，典型方法試圖對這些目標進行相對復雜的識別和定位，從而限制魯棒性或通用性。在這項研究中，我們提出了一種中間方案，并使用自動駕駛車輛，在動態車輛提供有限的姿態約束信息，應用于基于逐幀PnP-RANSAC的6自由度定位過程中。我們使用運動模型對初始姿態估計進行更新，并提出了一種計算未來姿態估計質量的預測方法，該方法根據自主車輛的運動是否受到環境中動態車輛幀對幀相對位置的約束而觸發。與最先進的單圖像PnP方法及其虛構姿態濾波相比，我們的方法可檢測和識別合適的動態車輛，以定義這些姿態約束來修改姿態濾波，從而在0.25米至5米的定位公差范圍內提高召回率。我們的約束檢測系統在Ford AV數據集上約35%的時間內是活動的，當約束檢測處于活動狀態時，尤其是定位得到了改善。

II. 引言

為了使自動駕駛車輛安全有效地行駛，準確的定位至關重要，位置精度要求在10-20cm的數量級。行業目前的解決方案通常使用GPS(全球定位系統)、INS(慣性導航系統)和激光雷達的多傳感器融合，使用擴展卡爾曼濾波器等技術進行處理。使用攝像頭的6自由度視覺定位提供了一種替代方法，在自動駕駛場景中可以實現高精度定位，并具有減少對昂貴激光雷達傳感器的依賴，并提供額外的定位冗余。目前最先進的基于特征的可視化定位方法按幀提供定位估計值，容易因三維重建或特征描述和關聯不準確而出現故障。

在這項工作中，我們提出了一種使用觸發約束運動模型來完善幀到幀姿態估計的方法，該模型考慮了環境中動態車輛的相對位置（只要可用）。我們建議利用動態車輛檢測（通常已在AV自主堆棧中出于其他目的執行）作為定位流水線中有限但仍然有益的信息來源。為此，我們考慮對從單幅圖像獲得的6自由度姿態進行順序過濾，基于PnP-RANSAC進行可視化定位，并通過提出的動態車輛運動約束改進過濾器的姿態預測。我們重新使用來自定位過程的局部特征和來自AV感知過程的語義分割掩碼來生成車輛描述符，我們使用這些描述符來比較連續圖像中的動態車輛，以確定它們相對于自主自我車輛的相對位置是否恒定。如果它們的相對位置隨著時間的推移是恒定的，我們就近似認為自主車輛的速度和航向也是恒定的，因此是受約束的。然后，在擴展卡爾曼濾波傳感器融合系統的基礎上，只要有外部姿態約束信息，我們就使用這些信息來修改姿態估計值對濾波器逐幀定位狀態的貢獻。

我們在Ford AV數據集的42公里駕駛中實驗性地驗證了我們的方法，該數據集劃分為247段長度為150米的不同段，涵蓋各種交通條件、道路類型(高速公路、郊區)和時間。

III. 相關工作

在這一節中，我們回顧了最近有關使用語義目標檢測、跟蹤和姿態約束的6自由度視覺定位研究。

A. 6自由度視覺定位

6自由度視覺定位是在給定先驗三維地圖和查詢圖像的情況下估計相機絕對姿態的任務。這是一個具有挑戰性的研究問題，特別是對于自動駕駛車輛的長期和連續運行，因為與數據庫圖像相比，查詢圖像可能會發生顯著的外觀和視角變化。近年來，研究人員探索了不同的方法來解決這些問題，其中包括學習健壯的局部特征、平面、匹配器和全局描述符，利用序列信息，改進2D-3D匹配，以及結合語義和幾何。在場景坐標回歸、魯棒性姿態估計和直接圖像/特征配準方面也開發了基于學習的方法。在這項工作中，我們使用Kapture和HLoc實現了基于特征的視覺定位過程，并利用場景中的動態車輛改進了定位--這是對上述該領域最新創新的補充。

B. 定位和語義場景理解

目前已有幾種方法將語義分割和物體檢測納入視覺地點識別和6自由度度量定位流水線。這包括使用Faster-RCNN構建對象圖或對象匹配；分割特定對象/實體（如建筑物、車道和天際線）以提高識別率；或預選多個對象類別。特別是對于6-自由度定位，在粒子過濾器或P3P-RANSAC循環中采用了三維點及其在查詢圖像上的投影之間的語義標簽一致性，以改進姿態估計。在所有這些方法中，要么假定動態物體在定位過程中無用并因此被移除，要么此類物體在SfM重建過程中不構成三維地圖的一部分。在這項工作中，我們將探索如何利用這些動態物體來改進度量定位。

C. 定位和目標跟蹤

物體跟蹤在增強現實、動作識別和自動駕駛車輛的路徑規劃等多個相關領域都有應用。研究人員還將（自我-車輛）定位和動態物體跟蹤結合起來，前者為后者提供信息; 后者為前者提供信息; 或者兩者共同建模以完成映射任務。

D. 基于約束的相機姿態估計

目前有幾種方法利用額外信息約束相機姿態估計。這包括語義組件和三維點云之間的距離約束、利用重力方向和攝像頭高度、點和線的組合約束、使用地理參考交通標志、基于環境-物體距離的約束和鏡面反射。然而，這些研究的具體方法因應用環境和附加信息的假設而存在很大差異。與這些方法不同，考慮到自動駕駛應用，我們提出使用基于動態車輛相對于自我車輛運動的速度約束來改進自我車輛定位。

IV. 方法

本節首先描述我們方法的關鍵思想，然后描述系統中使用的概念。之后，我們介紹了提出的算法，該算法使用動態車輛施加的運動約束改進姿態濾波。我們方法的流程如下圖所示：

在這項工作中，我們思索是否可以通過考慮在自我車輛前方移動的外部動態車輛的運動來提高自主車輛的定位性能，將這些車輛作為姿態約束。我們提出了以下簡單的約束定義：如果動態車輛的二維像素位置在連續兩幅圖像之間沒有變化，那么我們假設自我車輛和動態車輛的相對位置是恒定的。此外，我們還可以近似認為速度和航向在這段時間內也是恒定的。這使我們能夠創建一個條件算法，以檢測動態車輛在圖像平面中的位置是否靜止，并根據該條件調整非符合人體工程學的運動約束；在沒有約束的情況下，姿態濾波器按原樣運行。這種約束定義雖然簡單，但其優點是不需要其他方法所需的復雜檢測、物體姿態估計和跟蹤機制：而且我們能夠在真實世界的視聽相關實驗中評估其實際效果。

A. SfM視覺定位過程

我們提出的運動約束姿態濾波方法與單幅圖像6自由度姿態的確切來源無關，因此可以使用任何現有的單幅圖像視覺定位方法。我們選擇了使用局部特征的視覺定位過程HLoc 與R2D2 局部特征，因為我們重新使用這些特征進行運動約束檢測。HLoc使用PyCOLMAP 進行SfM重建和PnP-RANSAC估計單幅圖像的3D姿態，我們在順序濾波器中使用它，如下后文所述。

B. 使用IMU的EKF姿態優化-基線系統

我們使用自行車模型對自動駕駛車輛建模，并實現了6自由度誤差狀態EKF濾波器與運動模型相結合來濾波估計的逐幀姿態。我們還假設自動駕駛平臺具有IMU(慣性測量單元)，并將IMU讀數集成到EKF中。IMU數據包括線加速度(Ia)和角速度(Iv)，其狀態向量為：

EKF的預測更新如下：

其中，δ是濾波器連續迭代之間的時間差異;是車輛當前取向的旋轉矩陣;，， 是我們濾波器的三個狀態變量，分別表示x，y，z位置，速度和四元數。濾波器相對于圖像時間戳運行，并且IMU向量是通過在捕獲圖像的時間戳處數值積分原始IMU數據來計算的。按照，我們使用符號?表示四元數乘法，q{}表示從旋轉向量到四元數表示的變換。

我們計算過程雅可比矩陣如下：

其中，表示角軸表示的旋轉矩陣表示，表示作為反對稱矩陣的向量。過程噪聲雅可比矩陣通過過程方差定義：

協方差和卡爾曼增益更新如下：

其中，和分別表示運動模型噪聲和測量模型的雅可白矩陣：

從這里開始，我們將狀態變量，，表示為，以簡化表示。現在我們定義測量更新方程。首先計算狀態誤差δ-這是我們估計的狀態變量與傳入的測量狀態變量之間的差異:

然后我們通過更新計算校正狀態變量，更新的表示為：

注意，在實踐中，這可能是位置分量的簡單線性加法和旋轉分量的四元數乘法。然后我們更新濾波器協方差:

注意，為了數值穩定性，可以改用Joseph形式。然后我們處理ESKF重置，這進一步更新：

其中是一個DEFINED為以下形式的雅可比矩陣：

其中是狀態誤差的角分量。

使用單圖像基于PnP姿態估計作為測量，我們遵循標準方法，校正，，，，其中狀態變量的后驗估計是通過添加先驗估計和對殘差(即先驗估計與當前測量之間的差異)的增益加權來獲得的。在我們的實驗結果中，此設置稱為基線EKF。

C. 動態車輛約束姿態濾波

現在我們考慮運動模型，即AV的運動將被限制在一個恒定的速度和方向上。每當檢測到約束時(如后續章節所述)，對于給定的測量姿態，我們可以期望隨后的測量姿態位于當前測量姿態的位置加上距離向量()。因此，我們可以根據實際測量與預期測量之間的距離調整對下一個測量的信任程度。這是通過根據實際測量與預期測量之間的軸位移(僅限平移分量)應用徑向基函數(RBF)核來動態調整測量噪聲方差實現的。我們也可以估計很可能接近; 然而，如果EKF發生漂移，這可能導致未來測量被不充分信任的情況。也可能會漂移，但漂移幅度會更小，因為速度是位置的導數。

在實踐中，我們觀察到單個姿態測量的偏差對濾波姿態估計是有害的。為了緩解這一問題，我們決定使用徑向基函數（RBF）核來懲罰預期姿勢測量值和實際姿勢測量值之間的巨大偏差。我們還觀察到，使用非線性RBF核比使用歐氏距離度量提供了更精確的定位。

現在我們通過將RBF核應用于實際測量與預測測量之間的軸位移(僅轉換分量)，定義我們的動態測量方差：

其中是SfM坐標框架的三個平移軸。，，表示內核的帶寬。如果自動駕駛車輛不受約束，我們仍然可以期望運動是可預測的，因為自動駕駛車輛的速度和航向不會立即改變。當約束檢測為真時，我們通過因子減小 和(始終受路面約束)來減小RBF誤差，這反過來增加動態測量方差，定義如下：

當自動駕駛車輛受到約束時，更小的值導致RBF的帶寬更小，這又導致預期測量中的偏差導致的值更小。的值更小然后導致更大，這意味著濾波器將通過新的測量進行調整的幅度更小。直觀地，當約束檢測處于活動狀態時，濾波器只會考慮與自動駕駛車輛當前運動模式一致的新測量。

D. 車輛檢測

我們的約束算法首先需要在每幅圖像中以像素級掩碼的形式檢測車輛。我們使用高密度語義分割網絡PanopticDeeplab-v3來獲得車輛標簽像素。為了將感知噪聲（導致假陽性檢測）的影響與所提方法的定位性能相分離，我們將語義分割輸出與物體檢測器網絡YOLO-P相結合，以模擬AV的高精度車輛檢測系統。利用YOLO-P，我們提取圖像中所有被分類為車輛的物體的邊界框。然后，我們將Panoptic-Deeplab的分割結果（轎車、卡車和公共汽車類別）與YOLO-P為每輛檢測到的車輛提取的邊界框區域進行掩碼相乘。這樣得到的每像素分割結果只考慮車輛本身，而不考慮背景像素。我們還檢查了每個像素車輛掩碼的大小，并刪除了像素面積小于0.04%的掩碼--這是為了刪除非常小的檢測結果，因為這些檢測結果的大小不足以準確計算約束檢測。

E. 約束檢測

在這最后一小節中,我們描述了一種根據對環境中動態車輛的檢測和分析來計算自動駕駛車輛是否處于受限狀態的方法。我們的目標是識別場景中任何動態車輛在兩幀連續圖像之間是否靜止,因為在圖像平面中看似靜止的動態車輛與自動駕駛車輛之間的相對位置大致保持不變。此外,我們可以合理地假設在這段時間內車輛的速度和航向也保持恒定。這使我們能夠設計一個條件算法,檢測動態車輛在圖像平面中的位置是否固定,并根據檢測結果調整非完全受限運動模型;如果沒有檢測到受限,姿態濾波器保持原樣運行。盡管這個受限定義非常簡單,但其優勢在于幾乎不需要采用其他方法中的復雜檢測和目標姿態估計與跟蹤機制:我們能夠在真實世界的自動駕駛相關實驗中評估其可行性。

V. 結論

在本文中，我們介紹了一種新穎、輕量級的方法，將從動態車輛檢測中獲得的有限姿態約束信息納入定位系統。通過檢測和跟蹤動態車輛，我們可以利用動態車輛相對于自主車輛的相對位置，為PnP-RANSAC的姿態估計添加幀到幀的運動約束。我們的研究表明，與現有的PnP+IMU融合方法相比，添加這些約束后的定位性能幾乎在所有相關指標上都得到了改善，特別是在操作關鍵的最壞情況下的定位性能方面。

該領域的進一步工作潛力巨大，主要是圍繞使用動態車輛作為定位輔助工具這一概念。未來的工作將研究是否可以將這些約束條件納入因子圖SLAM算法（如GTSAM ），從而實現對測量和約束條件的最大后驗估計。未來的工作還包括為我們的系統添加視覺里程測量，以應對無法獲得IMU數據的情況。最后，我們希望將這項工作擴展到極端視覺外觀條件（夜間/雨天）、突然的速度變化以及機器人、無人機和自主系統圍繞移動物體（從服務機器人到無人機）運行的動態領域。