基于矢量地圖的自主代客泊車定位算法研究

張?zhí)炱?曹容川

（中國第一汽車股份有限公司研發(fā)總院，長春 130013）

1 前言

自主代客泊車（Automated Valet Parking，AVP）技術(shù)近年來受到廣大學(xué)者和研究人員的關(guān)注。由于AVP 系統(tǒng)工作場景大多為停車場內(nèi)部，導(dǎo)致全球定位系統(tǒng)（Global Positoning System，GPS）無法正常工作，因此，穩(wěn)定的、高精度的定位模塊是其能夠正常運行的關(guān)鍵。使用慣性測量單元（Intertial Measurement Unit，IMU）和輪速數(shù)據(jù)能夠?qū)ψ詣玉{駛車輛的慣性數(shù)據(jù)進(jìn)行測量進(jìn)而推算車輛的位置以及姿態(tài)信息。雖然通過上述傳感器的測量值能夠在短期內(nèi)得到精準(zhǔn)的位姿估計結(jié)果，但是由于缺少絕對的觀測數(shù)據(jù)，使系統(tǒng)在長時間運行時其定位結(jié)果會因累積誤差的存在而產(chǎn)生“漂移”現(xiàn)象。因此，使用額外的傳感器實現(xiàn)對AVP 系統(tǒng)中定位數(shù)據(jù)的觀測是非常必要的。傳感器采集的實時數(shù)據(jù)與先驗地圖的匹配是解決該問題的有效手段，根據(jù)所采用傳感器的不同，現(xiàn)階段的研究方法可以分為基于激光雷達(dá)[1]和基于視覺2類。

通過發(fā)射激光束對目標(biāo)進(jìn)行探測，激光雷達(dá)能夠?qū)崿F(xiàn)厘米級測距精度[2-5]。雖然基于激光雷達(dá)的匹配定位算法[6-9]能夠?qū)崿F(xiàn)高精度的位姿輸出，然而在應(yīng)用時仍然存在問題：點云地圖數(shù)據(jù)量較大，不利于車載存儲；激光雷達(dá)的成本較高，在現(xiàn)階段的量產(chǎn)車型中無法應(yīng)用。因此，該類方法主要在科研中開展。

近年來，基于車載攝像頭的視覺定位算法在AVP 定位系統(tǒng)中的應(yīng)用得到研究人員的廣泛關(guān)注，例如基于視覺的實時定位與建圖（Simultaneously Localization and Mapping，SLAM）技術(shù)。在傳統(tǒng)的SLAM 技術(shù)中，自動駕駛車輛能夠在迭代更新局部特征地圖的同時實現(xiàn)其自身定位，且定位結(jié)果精準(zhǔn)度較高，其中具有代表性的方法有VINS-MONO[10]、ORB-SLAM[11-13]等。然而，因其使用視覺特征點的方式表達(dá)周圍環(huán)境，該方法的魯棒性不足且難以解決，這是由于視覺傳感器的工作原理導(dǎo)致其對光照等因素的不穩(wěn)定性所造成的。

為解決視覺定位系統(tǒng)不穩(wěn)定的問題，本文提出一種基于矢量地圖的AVP 場景定位算法，將視覺語義特征與矢量地圖進(jìn)行匹配，并融合IMU、輪速計等傳感器的信號構(gòu)建一種緊耦合的多傳感融合定位算法。為減少外部因素對視覺傳感器的影響，使用魯棒性更強的語義檢測結(jié)果作為特征對自動駕駛車輛周圍的環(huán)境進(jìn)行表達(dá)，增強定位的穩(wěn)定性；使用矢量化表示的地圖進(jìn)行匹配，以極大程度地減少地圖對存儲資源的占用，同時實現(xiàn)穩(wěn)定的元素匹配以及更快速的定位結(jié)果輸出。最后，使用真實場景的數(shù)據(jù)對提出的方法進(jìn)行驗證并與現(xiàn)階段主流算法進(jìn)行對比，驗證方法的穩(wěn)定性和可行性。

2 視覺定位技術(shù)概述

自主代客泊車技術(shù)中的高精定位模塊是核心模塊之一?？紤]到實際應(yīng)用中的成本問題，現(xiàn)階段各大汽車廠商以及相關(guān)研究人員在該方向的研究中主要采用以視覺傳感器為基礎(chǔ)的定位方案?；谝曈X的定位方案按照對周圍環(huán)境描述方式的不同可以分為基于傳統(tǒng)特征的定位方法和基于語義信息的定位方法。

傳統(tǒng)的視覺特征由關(guān)鍵點和描述子2個部分組成，其中關(guān)鍵點主要用于確認(rèn)特征所在的位置信息，描述子的功能是對其進(jìn)行編碼，從而實現(xiàn)對不同關(guān)鍵點的區(qū)分?；趥鹘y(tǒng)視覺特征點的思想，R.MUR-ARTA 等人提出了ORB-SLAM 系列算法[11-13]，以O(shè)RB 特征為基礎(chǔ)實現(xiàn)快速、準(zhǔn)確的SLAM 算法。該方法采用魯棒性更強的關(guān)鍵幀和三維點的選擇機制實現(xiàn)穩(wěn)定的視覺特征匹配。M.SONS 等人[14]定義并使用特征描述子實現(xiàn)了基于環(huán)視攝像頭的實時匹配定位算法的開發(fā)。為實現(xiàn)更高效率的實時定位與建圖，J.ENGEL 等人[15]提出直接稀疏里程計（Direct Sparse Odometry，DSO）方法，基于直接法和稀疏法的視覺里程計能夠?qū)崿F(xiàn)傳感器位姿的快速解算。HENRI 等人提出一種基于事件的視覺測距算法[16]，該算法不受運動模糊的影響，并且在具有挑戰(zhàn)性的高動態(tài)范圍以及強烈的照明變化條件下都能很好地運行。此外，基于雙目相機和能夠采集深度信息的RGB-D 相機的SLAM 技術(shù)在理論與應(yīng)用方面均取得了顯著進(jìn)展。

視覺攝像頭特定的工作原理導(dǎo)致其在工作過程中會受到光線等因素的影響，進(jìn)而影響傳統(tǒng)方法的特征匹配精度。此外，由于停車場內(nèi)環(huán)境不斷變化，上述方法建立的特征地圖無法在長時間內(nèi)保持魯棒性，進(jìn)而影響最終的定位精度。

基于語義特征與矢量地圖數(shù)據(jù)進(jìn)行匹配的定位方法能夠有效解決上述問題。首先，停車場內(nèi)構(gòu)建矢量地圖的語義特征能夠保持長時間的一致性。其次，通過將實時采集的圖像數(shù)據(jù)進(jìn)行語義特征的提取能夠避免環(huán)境變化對特征元素的影響。例如，RANGANATHAN 等人[17]提出一種基于路面標(biāo)志的匹配定位算法，該方法使用周圍易于檢測的標(biāo)志對車輛所處的環(huán)境進(jìn)行表達(dá)。SCHREIBER 等人[18]僅使用路沿石與地面的標(biāo)線作為特征，實現(xiàn)了穩(wěn)定的定位結(jié)果。FABIAN 提出一種模塊化方法[19]，將不同檢測算法的檢測數(shù)據(jù)與地圖中的元素相關(guān)聯(lián)，然后使用無跡卡爾曼濾波器融合得到絕對姿態(tài)。QIN 等人[20]提出一種SLAM方法，使用停車位作為主要特征實現(xiàn)實時定位、建圖和地圖匹配定位。

3 定位模塊

本文提出的定位模塊基于誤差狀態(tài)卡爾曼濾波器[21]實現(xiàn)，算法如圖1 所示，主要包括基于視覺數(shù)據(jù)與矢量地圖的匹配定位子模塊和多傳感器融合定位子模塊。

圖1 定位算法

3.1 視覺匹配定位

本文使用的視覺語義特征通過深度學(xué)習(xí)網(wǎng)絡(luò)實現(xiàn)，檢測的語義元素包括停車位、箭頭、道路標(biāo)線、斑馬線共4 個類別。雖然停車場內(nèi)部的語義種類更多，但上述4 種類別是停車場內(nèi)最常見的標(biāo)志，能夠?qū)崿F(xiàn)對自動駕駛車輛周圍環(huán)境的有效表達(dá)，同時，上述特征的選取也有利于深度學(xué)習(xí)網(wǎng)絡(luò)的收斂，提升特征提取的精度。視覺匹配定位問題可以描述為如下形式：給定車輛附近的矢量地圖以及根據(jù)實時采集到的圖像數(shù)據(jù)提取的語義特征，計算車輛當(dāng)前時刻在地圖坐標(biāo)系下的位置和姿態(tài)，數(shù)學(xué)上可以描述為如下優(yōu)化問題：

式中，h為誤差函數(shù)；P為語義特征元素的集合；M為矢量地圖中元素的集合；T為待優(yōu)化的變量；SE(3)為特殊歐氏群。

本文使用三維空間下變換矩陣的形式描述車輛的位姿。

3.1.1 語義特征匹配

語義特征與矢量地圖中元素的匹配是構(gòu)建誤差函數(shù)h的重要步驟和前提。為實現(xiàn)語義特征匹配，首先需要解決檢測數(shù)據(jù)與地圖數(shù)據(jù)表達(dá)形式不一致的問題。深度學(xué)習(xí)網(wǎng)絡(luò)提取的語義特征的可視化結(jié)果如圖2 所示，主要以結(jié)構(gòu)化數(shù)據(jù)中像素點的集合形式表示，矢量地圖中的數(shù)據(jù)則以關(guān)鍵點的形式存在。語義特征匹配的目的是實現(xiàn)上述2種數(shù)據(jù)元素的關(guān)聯(lián)。

圖2 語義特征可視化

本文采用最近鄰搜索的方式實現(xiàn)待匹配數(shù)據(jù)的選取，其中目標(biāo)數(shù)據(jù)由提取的語義特征數(shù)據(jù)采樣得到，搜索空間由局部地圖內(nèi)的元素構(gòu)成。不同類型的特征使用的采樣方法不同。其中車位和標(biāo)線特征數(shù)據(jù)的提取通過均勻采樣的方式實現(xiàn)，同時保留數(shù)據(jù)的角點（位于語義特征數(shù)據(jù)中折線位置的點和端點）信息。為實現(xiàn)箭頭和斑馬線的語義特征數(shù)據(jù)與地圖數(shù)據(jù)一致，在對其數(shù)據(jù)采樣之前額外執(zhí)行了輪廓的提取，且在輪廓數(shù)據(jù)上實現(xiàn)后續(xù)的采樣。為提升上述方式中搜索過程的執(zhí)行效率，采用KDTree存儲地圖元素，該方法能夠?qū)r間復(fù)雜度由O(n2)提升至O(nlog(n))。

3.1.2 目標(biāo)函數(shù)構(gòu)建

構(gòu)建目標(biāo)函數(shù)的過程即是構(gòu)建函數(shù)h的過程，也是根據(jù)上一節(jié)中匹配到的元素計算誤差約束的過程。本文針對角點和一般點提出2種不同的誤差計算方法，其中角點的約束根據(jù)點與點之間的歐式距離計算：

式中，Pc、Mc分別為特征數(shù)據(jù)和地圖元素中角點的集合。

在計算一般點的約束時，采用點到直線的歐式距離構(gòu)建誤差，其中直線通過地圖元素中最近的2個角點確定：

式中，pj∈Pg；mj,mk∈Mg。

由此可推算出函數(shù)h的形式為：

3.2 融合定位

本文采用誤差狀態(tài)卡爾曼濾波器實現(xiàn)視覺匹配定位、IMU 以及輪速計的數(shù)據(jù)融合，融合模塊中描述系統(tǒng)狀態(tài)、輸入輸出的符號及其物理意義如表1、表2 所示。本文使用四元數(shù)表示車輛的姿態(tài)，使用IMU 執(zhí)行模型的預(yù)測部分。其中系統(tǒng)狀態(tài)定義為積分狀態(tài)和誤差狀態(tài)的組合，預(yù)測階段通過運動學(xué)模型對積分狀態(tài)和誤差狀態(tài)分別進(jìn)行推算，更新階段僅在誤差狀態(tài)下進(jìn)行，即通過觀測值修正系統(tǒng)的誤差，并將誤差嵌入積分狀態(tài)以實現(xiàn)最優(yōu)的狀態(tài)估計。

表1 系統(tǒng)狀態(tài)符號及定義

表2 系統(tǒng)測量及噪聲

3.2.1 系統(tǒng)模型

根據(jù)運動學(xué)方程，系統(tǒng)的真實狀態(tài)可以為：

根據(jù)表1分別推導(dǎo)積分狀態(tài)和誤差狀態(tài)的系統(tǒng)模型，結(jié)果為：

對比式（6）和式（5）可以看出，積分狀態(tài)的系統(tǒng)模型為真實狀態(tài)的系統(tǒng)模型去除誤差項的形式。誤差狀態(tài)的模型根據(jù)表1 中的組合方式近似得到，由于系統(tǒng)的誤差通常處于較小的量級，因此可以根據(jù)一階泰勒展開得到的線性化形式進(jìn)行建模，同時該模型具有較小的線性化誤差，具體形式見式（7）。

3.2.2 狀態(tài)預(yù)測

在本文提出的基于誤差卡爾曼濾波的融合框架中，系統(tǒng)狀態(tài)的預(yù)測基于IMU 的測量實現(xiàn)。其中積分狀態(tài)根據(jù)式（6）中的運動方程推算。為方便書寫，本文使用向量化的形式描述誤差狀態(tài)：

同時，該狀態(tài)滿足高斯分布假設(shè)：

增值稅由于稅基廣闊、征管手段高效的特點，已經(jīng)成為我國最大的稅種，2017年我國稅收收入144360億元，其中，國內(nèi)增值稅收入達(dá)到56378億元，占比接近40%。在試點工作不斷推進(jìn)的過程中，結(jié)合征管實踐經(jīng)驗逐步完善相關(guān)法規(guī)顯得愈發(fā)重要，本文旨在為完善增值稅會計，促進(jìn)增值稅會計準(zhǔn)則的制定提供參考建議。

式中，δx?為誤差狀態(tài)的預(yù)測值。

根據(jù)式（7），可以推導(dǎo)出誤差狀態(tài)的推算服從：

其中，變換矩陣Fx和Fi為式（7）中稀疏的矩陣化表達(dá)，即

式中，Δt為積分時間。

式中，I為單位矩陣。

um和i分別為IMU的讀數(shù)和系統(tǒng)中的擾動項：

式中，vi、θi分別為速度、角度的噪聲；ai、ωi分別為加速度、角速度高斯白噪聲中的方差。

根據(jù)式（10）～式（16）可得誤差狀態(tài)的參數(shù)服從：

3.2.3 融合定位

本質(zhì)上，融合定位的過程就是使用語義匹配定位的結(jié)果和輪速數(shù)據(jù)更新誤差狀態(tài)的過程，且該過程服從卡爾曼濾波的更新規(guī)則，可以描述為:

式中，K為卡爾曼增益；V為觀測值的協(xié)方差矩陣；H為函數(shù)h1關(guān)于δx的雅可比矩陣；h1為函數(shù)h的簡化形式；y為優(yōu)化后的特征匹配位姿帶入式（4）得到的誤差值；δx?t為[pt vt qt Rt abt ωbt gbt]的預(yù)測值。

值得注意的是，由于更新過程針對誤差狀態(tài)，因此矩陣H為觀測誤差對誤差狀態(tài)的雅克比矩陣，可以通過鏈?zhǔn)椒▌t計算：

4 試驗驗證

為驗證本文提出方法的可行性以及在實際自主代客泊車場景下的性能，分別在仿真環(huán)境下和真實場景下測試，并將測試結(jié)果與目前主流方法做了對比。其中試驗平臺為Nuvo-6108GC 工控機，搭載英特爾至強E3v5 CPU、英偉達(dá)GTX1080 GPU；操作系統(tǒng)為Ubuntu16.04，搭載ROS-kinetic子系統(tǒng)；攝像頭采用MV-CA013-21UM ?？倒I(yè)攝像頭，系統(tǒng)輸出圖像大小為640×480。

為了能夠量化方法的性能，使用平均旋轉(zhuǎn)角度誤差和平均位移誤差作為評價算法性能的指標(biāo)，計算方法為：

式中，Θ為模型輸出結(jié)果的集合；T?i為i時刻模型的輸出位姿；Ti為i時刻的真實位姿。

本文方法與主流方法的平均誤差對比結(jié)果如表3 所示。對比結(jié)果表明，本文提出的方法的平均旋轉(zhuǎn)角度誤差和平均位移誤差均處于較小的量級，證明了方法的有效性。本文方法與AVP-SLAM 算法均使用視覺語義特征，其中AVP-SLAM 僅使用停車位的語義信息表達(dá)周圍環(huán)境，本文使用的特征更豐富，試驗結(jié)果也表明本文提出的語義特征能夠?qū)崿F(xiàn)更好的定位效果。ORB-SLAM3 和VINS-MONO是基于傳統(tǒng)視覺特征點的方法，該類算法能夠獲得較高的結(jié)果精度，但是長期魯棒性較弱。本文提出的方法能夠與ORB-SLAM3保持相似的誤差，實現(xiàn)了高精度的定位結(jié)果。與ICP 及其改進(jìn)算法進(jìn)行對比可以看出，本文提出的方法優(yōu)于傳統(tǒng)的基于最鄰近搜索的方法。

表3 算法指標(biāo)對比

同時，算法執(zhí)行效率的優(yōu)劣是其能否在實際環(huán)境中應(yīng)用的前提，因此本文也針對模型的平均計算時間進(jìn)行對比試驗。

算法運算時間的試驗結(jié)果如圖3 所示。值得注意的是，該部分試驗內(nèi)容中算法的計算時間是指視覺匹配定位的時間。理論上，特征數(shù)據(jù)的表達(dá)方式越復(fù)雜，其算法的執(zhí)行效率就越差，因此基于特征點的方法能夠?qū)崿F(xiàn)高效的計算。同時，本文使用的語義特征（AVP-SLAM）是最簡潔的特征描述方式，因此理論上比基于關(guān)鍵點和描述子的方法（AVPSLAM、ORB-SLAM3）具備更快的計算速度。試驗結(jié)果驗證了前文論述的內(nèi)容，其中基于語義特征的算法能夠?qū)崿F(xiàn)快速的結(jié)果輸出，節(jié)省了計算資源。

圖3 計算時間對比

5 結(jié)束語

本文提出了一種基于矢量地圖的多傳感器融合定位方法，并應(yīng)用于自主代客泊車場景，實現(xiàn)高精定位功能。同時將提出的方法基于真實數(shù)據(jù)進(jìn)行測試，與主流算法的結(jié)果進(jìn)行對比分析可知，該算法在結(jié)果精度和執(zhí)行效率上均處于領(lǐng)先水平，能夠?qū)崿F(xiàn)快速穩(wěn)定的實時定位功能。然而，由于停車場一般屬于封閉室內(nèi)場景，驗證階段真值數(shù)據(jù)的獲取比較復(fù)雜，未來，該方法仍需要在更復(fù)雜的場景中進(jìn)行驗證，并根據(jù)測試結(jié)果提升算法的性能。