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

張?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)定的、高精度的定位模塊是其能夠正常運(yùn)行的關(guān)鍵。使用慣性測量單元（Intertial Measurement Unit，IMU）和輪速數(shù)據(jù)能夠?qū)ψ詣?dòng)駕駛車輛的慣性數(shù)據(jù)進(jìn)行測量進(jìn)而推算車輛的位置以及姿態(tài)信息。雖然通過上述傳感器的測量值能夠在短期內(nèi)得到精準(zhǔn)的位姿估計(jì)結(jié)果，但是由于缺少絕對的觀測數(shù)據(jù)，使系統(tǒng)在長時(shí)間運(yùn)行時(shí)其定位結(jié)果會(huì)因累積誤差的存在而產(chǎn)生“漂移”現(xiàn)象。因此，使用額外的傳感器實(shí)現(xiàn)對AVP 系統(tǒng)中定位數(shù)據(jù)的觀測是非常必要的。傳感器采集的實(shí)時(shí)數(shù)據(jù)與先驗(yàn)地圖的匹配是解決該問題的有效手段，根據(jù)所采用傳感器的不同，現(xiàn)階段的研究方法可以分為基于激光雷達(dá)[1]和基于視覺2類。

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

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

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

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

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

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

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

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

3 定位模塊

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

圖1 定位算法

3.1 視覺匹配定位

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

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

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

3.1.1 語義特征匹配

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

圖2 語義特征可視化

本文采用最近鄰搜索的方式實(shí)現(xiàn)待匹配數(shù)據(jù)的選取，其中目標(biāo)數(shù)據(jù)由提取的語義特征數(shù)據(jù)采樣得到，搜索空間由局部地圖內(nèi)的元素構(gòu)成。不同類型的特征使用的采樣方法不同。其中車位和標(biāo)線特征數(shù)據(jù)的提取通過均勻采樣的方式實(shí)現(xiàn)，同時(shí)保留數(shù)據(jù)的角點(diǎn)（位于語義特征數(shù)據(jù)中折線位置的點(diǎn)和端點(diǎn)）信息。為實(shí)現(xiàn)箭頭和斑馬線的語義特征數(shù)據(jù)與地圖數(shù)據(jù)一致，在對其數(shù)據(jù)采樣之前額外執(zhí)行了輪廓的提取，且在輪廓數(shù)據(jù)上實(shí)現(xiàn)后續(xù)的采樣。為提升上述方式中搜索過程的執(zhí)行效率，采用KDTree存儲(chǔ)地圖元素，該方法能夠?qū)r(shí)間復(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é)中匹配到的元素計(jì)算誤差約束的過程。本文針對角點(diǎn)和一般點(diǎn)提出2種不同的誤差計(jì)算方法，其中角點(diǎn)的約束根據(jù)點(diǎn)與點(diǎn)之間的歐式距離計(jì)算：

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

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

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

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

3.2 融合定位

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

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

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

3.2.1 系統(tǒng)模型

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

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

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

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

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

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

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

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

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

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

式中，Δt為積分時(shí)間。

式中，I為單位矩陣。

um和i分別為IMU的讀數(shù)和系統(tǒng)中的擾動(dòng)項(xià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計(jì)算：

4 試驗(yàn)驗(yàn)證

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

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

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

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

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

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

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

圖3 計(jì)算時(shí)間對比

5 結(jié)束語

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