I. 摘要

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

II. 引言

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

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

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

III. 相關(guān)工作

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

A. 6自由度視覺定位

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

B. 定位和語義場景理解

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

C. 定位和目標(biāo)跟蹤

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

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

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

IV. 方法

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

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

A. SfM視覺定位過程

我們提出的運(yùn)動(dòng)約束姿態(tài)濾波方法與單幅圖像6自由度姿態(tài)的確切來源無關(guān)，因此可以使用任何現(xiàn)有的單幅圖像視覺定位方法。我們選擇了使用局部特征的視覺定位過程HLoc 與R2D2 局部特征，因?yàn)槲覀冎匦率褂眠@些特征進(jìn)行運(yùn)動(dòng)約束檢測。HLoc使用PyCOLMAP 進(jìn)行SfM重建和PnP-RANSAC估計(jì)單幅圖像的3D姿態(tài)，我們?cè)陧樞驗(yàn)V波器中使用它，如下后文所述。

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

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

EKF的預(yù)測更新如下：

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

我們計(jì)算過程雅可比矩陣如下：

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

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

其中，和分別表示運(yùn)動(dòng)模型噪聲和測量模型的雅可白矩陣：

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

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

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

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

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

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

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

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

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

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

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

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

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

D. 車輛檢測

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

E. 約束檢測

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

V. 結(jié)論

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

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