I. 摘要

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

II. 引言

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

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

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

III. 相關(guān)工作

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

A. 6自由度視覺(jué)定位

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

B. 定位和語(yǔ)義場(chǎng)景理解

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

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

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

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

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

IV. 方法

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

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

A. SfM視覺(jué)定位過(guò)程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

D. 車(chē)輛檢測(cè)

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

E. 約束檢測(cè)

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

V. 結(jié)論

在本文中，我們介紹了一種新穎、輕量級(jí)的方法，將從動(dòng)態(tài)車(chē)輛檢測(cè)中獲得的有限姿態(tài)約束信息納入定位系統(tǒng)。通過(guò)檢測(cè)和跟蹤動(dòng)態(tài)車(chē)輛，我們可以利用動(dòng)態(tài)車(chē)輛相對(duì)于自主車(chē)輛的相對(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)車(chē)輛作為定位輔助工具這一概念。未來(lái)的工作將研究是否可以將這些約束條件納入因子圖SLAM算法（如GTSAM ），從而實(shí)現(xiàn)對(duì)測(cè)量和約束條件的最大后驗(yàn)估計(jì)。未來(lái)的工作還包括為我們的系統(tǒng)添加視覺(jué)里程測(cè)量，以應(yīng)對(duì)無(wú)法獲得IMU數(shù)據(jù)的情況。最后，我們希望將這項(xiàng)工作擴(kuò)展到極端視覺(jué)外觀條件（夜間/雨天）、突然的速度變化以及機(jī)器人、無(wú)人機(jī)和自主系統(tǒng)圍繞移動(dòng)物體（從服務(wù)機(jī)器人到無(wú)人機(jī)）運(yùn)行的動(dòng)態(tài)領(lǐng)域。