I. 摘要

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

II. 引言

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

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

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

III. 相關(guān)工作

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

A. 6自由度視覺定位

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

B. 定位和語義場景理解

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

C. 定位和目標跟蹤

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

D. 基于約束的相機姿態(tài)估計

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

IV. 方法

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

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

A. SfM視覺定位過程

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

B. 使用IMU的EKF姿態(tài)優(yōu)化-基線系統(tǒng)

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

EKF的預測更新如下：

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

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

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

協(xié)方差和卡爾曼增益更新如下：

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

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

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

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

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

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

其中是狀態(tài)誤差的角分量。

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

C. 動態(tài)車輛約束姿態(tài)濾波

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

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

現(xiàn)在我們通過將RBF核應(yīng)用于實際測量與預測測量之間的軸位移(僅轉(zhuǎn)換分量)，定義我們的動態(tài)測量方差：

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

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

D. 車輛檢測

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

E. 約束檢測

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

V. 結(jié)論

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

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

審核編輯：劉清