面向智能車的地下停車場環視特征地圖構建與定位

周 哲，胡釗政，李 娜，肖漢彪，伍錦祥

1. 武漢理工大學智能交通系統研究中心，湖北 武漢 430063; 2. 武漢理工大學重慶研究院，重慶 401120; 3. 武漢理工大學自動化學院，湖北 武漢 430070

獲取高精度室內定位信息是實現智能車全域自主行駛最關鍵的技術之一，也是實現車路協同的基礎，受到工業界和學術界的廣泛關注。目前，基于導航衛星的定位技術是室外環境中獲取智能車位置信息的主流方法(如GPS、北斗)，但在室內環境中存在信號盲區等影響，難以滿足智能車對定位精度的需求[1]。2020年，中國科協公布的十大工程技術難題，其中就包含“無人車如何實現在衛星不可用條件下的高精度智能導航”問題[2]。

為了實現在室內環境中的高精度定位，研究者提出多種定位方案，包括慣性導航系統(inertial navigation system, INS)[3]、無線傳感網絡[4]、激光雷達傳感器[5]和視覺傳感器[6]等。然而，隨著運行時間的增加，INS產生嚴重的累積誤差，不宜用于較大場景的定位。無線傳感網絡的定位原理與GPS較為相似，能夠實現無累積誤差定位，但存在不穩定性和使用成本等矛盾。激光傳感器因使用成本高昂，使其難以大規模推廣應用。相比之下，視覺傳感器能夠提供豐富的場景信息，而且價格低廉，因而，基于視覺的定位技術成為近幾年的研究熱點[7]。

近年來，研究者提出多種基于視覺的定位方法。其中，VSLAM(vision simultaneous localization and mapping)能夠在未知的環境中實現精確的車輛定位[8]。然而，隨著定位場景的增大，后端優化復雜度增加，且系統受累積誤差的影響增大[9]。為了提高定位精度，降低計算負擔，滿足高級輔助駕駛的需求，高精度地圖被認為是L3級及以上高級輔助駕駛的關鍵基礎設施[10]。從而將定位過程分為視覺地圖構建與基于地圖的定位兩個階段[6,11]。定位階段首先是要找到視覺地圖中與當前圖像最相似的地圖節點，而后利用匹配的特征點計算當前車輛與地圖之間的位姿關系完成定位。其中，基于稀疏特征(SIFT[12]、ORB[13])的匹配是最常用的地圖匹配方法。文獻[14]利用SIFT特征點匹配，獲取最佳匹配，并利用PnP(perspective-n-point)解算在坐標系中的位置。文獻[15]提出多視角的智能車定位方案，前視圖像用來獲取最佳的地圖匹配節點，俯視圖像計算當前車輛與地圖之間的位姿關系，獲取車輛在全局坐標系下的位置信息。文獻[16]利用詞袋模型(bag-of-word，BoW)通過層次化的匹配方案快速實現車輛的定位。文獻[17]運用ICP算法計算路面標志與地圖子圖之間的位置關系，獲取車輛在全局坐標系下的位置。文獻[18]提出一種多視角、多尺度的智能車定位方法，將視覺地圖的定位結果與視覺里程計的定位結果相融合，實現在地下停車場環境中0.45 m的平均定位誤差。文獻[19]利用環視圖像獲取地下停車場的語義信息并構建語義地圖，采用圖優化方法獲得穩定的定位結果。文獻[20]在seqSLAM序列匹配的基礎上，采用“由粗到精”的匹配策略，并利用粒子濾波算法提出多分辨率的定位方法。但該序列匹配方法對車輛的行駛速度限制苛刻，且計算復雜度高，難以在移動平臺上布設。

針對上述文獻中的不足之處，本文在環視特征地圖構建的基礎上，提出基于二階馬爾可夫模型(Markov model，MM)的粒子濾波(particle filter，PF)算法(MM-PF)，實現穩定、準確的地圖匹配，并利用局部特征點匹配計算當前車輛的全局位置，完成智能車在地下停車場的高精度定位，其新穎之處在于：①利用粒子(地圖節點)之間的拓撲關系，構建基于二階MM的粒子狀態轉移概率模型，將車輛運動約束與視覺信息相融合，提高地圖匹配的準確性和穩定性。②為了提高算法的收斂效率，根據粒子的拓撲關系選取關鍵粒子，以降低計算復雜度。③提取場景的ORB全局特征，利用漢明距離構建當前圖像與各粒子之間的觀測概率模型，以降低計算負擔。

1 本文方法

本文首先利用環視圖像構建待定位場景的環視特征地圖。在定位階段，利用本文提出的二階MM-PF算法獲取與當前圖像最相近的地圖節點，完成節點級定位。而后在節點級定位的基礎上，利用環視圖像局部特征點匹配，求解當前車輛與環視特征地圖之間的相對位姿，進而求解當前車輛在全局坐標系中的位置，完成度量級定位。具體包括環視特征地圖構建和基于二階MM-PF的智能車定位方法。

1.1 環視特征地圖構建模型

特征地圖是利用離散的節點對待定位場景進行表征，通過預先獲取道路場景的視覺特征、結構等信息完成對場景的表征。本文利用文獻[6,11]中的地圖制作方法，并采用環視圖像構建待定位場景的視覺地圖，并稱之為環視特征地圖。通過獲取環視圖像特征、路面結構信息與位姿等組成地圖節點，遍歷待定位場景完成環視特征地圖構建，地圖結構如圖1所示。在定位階段，利用本文提出的二階MM-PF算法實現車輛在地下停車場環境中的高精度定位。于是，可將環視特征地圖定義為

P={p1,p2,…,pn}

(1)

式中，P表示環視特征地圖數據庫；pi表示環視特征地圖的第i個節點；n為環視特征地圖的節點數。

圖1 環視特征地圖示意Fig.1 Illustration of visual feature map from around view

本文利用現有的車載環視系統作為唯一的環境感知傳感器。一方面，可以覆蓋車輛周圍360°的場景，大幅提高車輛的感知范圍；另一方面，不需要增加額外的硬件設備即可實現車輛在GPS盲區環境中的高精度定位。由于魚眼相機獲取的圖像存在較大的畸變，當距離車輛較遠時，圖像存在較大的失真，故感興趣區域選擇為車輛周圍2～3 m的范圍。為了避免前、后車輛及對向車道車輛的影響，本文根據相鄰節點之間的距離，選取車輛右側的區域為感興趣區域，并提取該區域視覺特征，包括局部ORB特征[13]和全局ORB特征[21]。結構信息是指圖像坐標系和全局坐標系之間的變換關系，可由式(2)表示，位姿信息存儲車輛在全局坐標系中的位置[xmym]T(前軸中心的全局坐標)和姿態θ，可以將環視圖像中的車輛位置映射為全局坐標

(2)

式中，[umvm]T為制圖時車輛前軸中心的圖像坐標；車輛位姿信息由高精度慣導獲得，尺度因子λ通過系統標定得到；Rw和tw為要求的圖像坐標系與全局坐標系之間的映射關系。

1.2 基于二階MM-PF的智能車定位方法

針對室內環境中智能車無法有效利用GPS信號進行車輛定位，本文提出一種基于二階MM-PF的地下停車場定位方法。該方法在環視地圖構建的基礎上，通過獲取的環視圖像，實現智能車高精度定位，算法流程如圖2所示。首先，利用環視地圖節點之間的拓撲關系，提出基于二階MM的粒子狀態轉移模型。然后，提取當前圖像的ORB全局特征，并與環視特征地圖節點中存儲的ORB全局特征進行比較，通過構建基于高斯分布的觀測概率模型，計算各粒子的權重分布，根據粒子權重重采樣，獲得最佳節點級定位。最后，利用局部視覺特征匹配，計算當前車輛與地圖中車輛的相對位姿，進而求得當前車輛在全局坐標系中的位置。

圖2 基于二階MM-PF的定位流程Fig.2 Visual map localization based on second-order MM-PF from around view

1.2.1 基于二階MM-PF的節點級定位模型構建

(3)

為了解決最優匹配問題，本文提出基于二階MM-PF的環視特征地圖定位算法來實現地圖最優節點匹配，如圖3所示。在本文中，利用粒子在t-1、t時刻的狀態對t+1時刻的狀態進行預測，構建粒子狀態轉移模型。然后，提取當前圖像的ORB全局特征與特征地圖節點的ORB全局特征進行比較，并構建基于高斯分布的粒子權重分布模型，通過重要性采樣將拓撲結構信息與視覺信息進行融合，以提高車輛的定位精度和穩定性。主要包括粒子初始化、粒子狀態轉移模型、基于關鍵粒子的權重更新與重采樣和最優地圖節點估計4個步驟。

圖3 基于二階MM-PF的智能車環視地圖匹配與定位Fig.3 Map matching and localization of intelligent vehicle based on second-order MM-PF

1.2.1.1 粒子初始化

(4)

(5)

1.2.1.2 基于二階MM的粒子狀態轉移模型

在智能車定位中，控制量多為速度或加速度，而這些量的獲取需要額外增加傳感器，增加系統的使用成本。然而，在正常情況下，車輛的通行區域多為結構化的場景，在運動過程中，車輛狀態具有一定的連續性。即下一時刻車輛的可到達區域與歷史狀態存在一定的關聯關系。因此，為了適應不同的運動狀態，并兼顧模型的復雜度，提出基于二階MM的粒子狀態轉移模型，即t時刻粒子在空間中的分布情況取決于t-1和t-2兩個時刻車輛的狀態，如圖4所示。假設車輛在短時間內速度不變，則有

(6)

式中，xj和xi分別為t-1和t-2時刻的狀態；xk為t時刻車輛狀態，則有

xk=2xj-xi+v(t)

(7)

式中，v(t)為狀態轉移過程噪聲，其服從0均值高斯分布。狀態轉移利用歷史狀態提供的速度約束，確定粒子的狀態轉移關系。粒子分布如圖4所示，×表示粒子與粒子分布情況。

圖4 狀態轉移及粒子權重分布建模Fig.4 State transition and particle weight distribution modeling

1.2.1.3 基于關鍵粒子的權重更新與重采樣

(8)

圖5 全局ORB特征描述符Fig.5 Descriptor of ORB holistic feature

為了描述隨著漢明距離的增加，觀測概率逐漸變小這一現象，本文通過構建0均值高斯模型以構建觀測圖像與地圖節點之間的相似度模型，并計算各粒子權重

(9)

(10)

然而，隨著迭代次數的增加，幾乎所有粒子的權重都趨于0，粒子權重集中在少數粒子上，造成粒子枯竭，使PF退化，無法準確描述定位過程的不確定性。為了避免粒子枯竭造成PF性能惡化，在迭代若干次后，需要對粒子進行重分布。重分布可以增加粒子的多樣性，更易適應復雜環境的狀態估計。一種判斷PF是否發生粒子枯竭的方法是比較PF的有效采樣數

(11)

當Neff<2n/3時，需對PF進行重采樣，n為粒子總數。

1.2.1.4 最優地圖節點估計

根據當前狀態獲得的粒子權重，對粒子進行重采樣。則當前車輛對應的地圖節點為重采樣后粒子數量最多的地圖節點，即

(12)

式中，i表示第i個粒子；size(pi)表示重采樣后粒子pi出現的次數。與傳統粒子濾波算法相比，二階MM-PF模型大大拓展了算法的適用性，簡化了模型的復雜度，提高了定位的穩定性和精度。

1.2.2 基于局部特征匹配的度量級定位

在1.2.1節獲得節點級定位后，利用局部ORB特征匹配，計算當前車輛與地圖參考位置的相對位姿，得到當前車輛在全局坐標系中的位置。為了剔除錯誤的特征點匹配，本文利用RANSAC算法剔除錯誤的匹配特征點[22]，匹配效果如圖6所示。當前圖像和特征地圖節點中存儲圖像之間的單應性可以由式(13)求得

(13)

式中，[umsvms1]T為環視特征地圖節點中存儲的特征點；[ucsvcs1]T為當前獲取的環視圖像的特征點；Hcm為當前圖像與特征地圖節點圖像之間的單應矩陣。則當前車輛的全局坐標[xgyg]T可由圖像坐標系與全局坐標系之間的映射關系Rw和tw求得，為

(14)

2 試驗及結果分析

為了驗證本文算法在實際場景中的性能，分別采集兩個不同試驗場景的數據進行驗證。在試驗之前，需要對車載廣角魚眼相機獲取的原始圖像(圖7第1行)進行標定、畸變矯正(圖7第2行)、視角變換(圖7第3行)和拼接等處理。最后生成圖像大小為416×416像素，如圖7所示，具體過程不再贅述，見文獻[23]。

圖7 環視圖生成過程Fig.7 Generating procedure of around view image

試驗分別在兩個地下停車場景環境中下進行，第1個場景為上海某汽車公司地下停車場，該停車場為單位自用，動態目標較少，場景面積約為10 000 m2。第2個場景為武漢某商場地下停車場，場景面積約為8000 m2，場景中存在車輛和行人等交通參與者，交通場景較為復雜，容易被動態目標遮擋，試驗路段如圖8所示。在室內環境中，車輛行駛速度有嚴格限制，試驗過程中，車輛平均行駛速度均不超過20 km/h，所選地下停車場具有普適性。

環視特征地圖采用文獻[6,11]中的構建方法，首先，將待定位場景離散為一序列節點，采集各節點對應的路面結構信息、視覺特征和位姿信息等數據。視覺特征包括全局ORB特征和局部ORB特征。全局ORB特征的提取過程如1.2.1.3節所述，局部ORB特征為常規ORB特征。試驗中，各攝像頭采集圖像的分辨率均為1280×960像素。試驗在相同計算機硬件環境下運行，其配置為Intel Core i7-7700HQ 2.8 GHz CPU，8 GB RAM。為了比較不同算法之間的差異，選取基于BoW模型的定位算法[16]和基于PF多分辨率的定位算法[20]作為對比，進行試驗。

2.1 工業園停車場場景測試

定位試驗在已構建環視特征地圖的地下停車場中進行，測試場景如圖9所示，該場景環視特征地圖節點數為164個，節點平均間隔為2 m。因地下停車場等結構化的場景有固定入口，首先，根據場景結構對系統位置初始化，并用環視特征地圖中的節點信息初始化粒子。然后，根據車輛歷史信息預測車輛下一時刻的狀態。同時獲取車輛當前時刻的圖像信息，提取圖像的ORB全局特征，利用式(8)計算當前圖像的ORB全局特征與各粒子之間的漢明距離，并利用觀測概率模型計算粒子權重分布。最后，根據粒子權重對粒子進行重采樣，粒子數量最多的節點作為當前的定位結果。

試驗結果的評價方法采用閉環檢測中常用的衡量標準[24]，即當車輛在相鄰節點之間時，通常認為最近的節點和次近的節點都是正確的匹配結果，即當誤差幀等于0和等于1時，均認為是正確的匹配。試驗中，誤差幀是指試驗獲取的地圖節點與真值之間的節點差。此外，為了更好地呈現試驗結果，當節點誤差大于等于5幀時，均按5幀處理。節點級定位的結果如圖10所示。由圖10可知，節點級定位誤差集中在0幀和1幀，相較于對比算法，具有較好的穩定性和精度。

本文選擇經典的BoW定位算法和多分辨率PF定位方法作為對比算法，其節點級定位的均值分別為0.13、0.39和0.37幀，誤差均方差分別為0.65、1.25和1.18幀，成功匹配的概率分別為94.5%、82.9%和90.1%(表1)。從節點級定位的結果來看，仍存在誤匹配的現象，主要是因為在地下停車場環境中，為了限制車輛的行駛速度而布設有減速帶，當車輛通過減速帶時，會引起車輛顛簸使視角變化較大，進而導致誤匹配。然而匹配誤差均小于3幀，這也說明了本文提出的二階MM-PF的匹配方法能夠將車輛的運動信息和視覺信息相融合，提高地圖匹配的成功率。

在獲得與當前圖像最佳匹配的地圖節點后，利1.2.2節介紹的圖像特征點匹配方法，計算當前車輛與地圖中參考坐標的相對位置，通過式(14)求得當前車輛在全局坐標系下的位置，試驗結果如圖11所示。 定位軌跡中由方框標注的定位波動較大，其原因是減速帶或門檻的存在導致定位誤差較大，也進一步說明了減速帶或門檻導致節點級定位誤差較大。

表1 不同算法在工業園地下停車場場景的節點級定位對比

統計結果見表2，定位平均誤差為0.36 m，誤差均方差為0.25 m。此外，與對比算法的程序耗時26.0 ms和98.3 ms相比，本文研究的平均耗時為15.6 ms，大幅提升了計算效率，使得本文算法具有較好的實時性。較高的計算效率歸因于利用全局圖像描述符來計算發射概率。一方面可以直接提取圖像中心的描述符，省去特征點提取過程，一副圖像由一個ORB描述符來描述，大幅簡化匹配耗時。另一方面利用運動約束縮小地圖匹配范圍，并通過選取關鍵粒子減少重采樣過程的計算耗時。由表2可知，定位誤差小于1 m的概率為97.9%，試驗結果也驗證了本文算法具有較好的定位性能。

圖8 測試路段Fig.8 Testing routes

圖9 工業園停車場測試場景Fig.9 Scenarios of test route in parking lots in industrial park

圖10 工業園地下停車場節點級定位誤差概率分布Fig.10 Error distribution of node-level localization in underground parking lots in industrial park

圖11 定位結果Fig.11 Localization results

2.2 商場地下停車場場景測試

為了驗證本文算法對不同場景的適應性，另外采集武漢某商場地下停車場數據進行驗證，測試場景如圖12所示。該測試場景環視特征地圖節點數為135個，節點平均間隔為2 m。該地下停車場環境中存在移動的車輛和行人。

在商場地下停車場環境中，測試結果的誤差概率分布如圖13所示。由圖13可以看出，此停車場環境下的地圖匹配結果與2.1節工業園停車場測試結果具有相似的效果，地圖匹配成功的概率為94.1%。該場景下，本文算法與對比算法的性能比較見表3，其中本文算法的平均節點級定位誤差為0.10幀，誤差均方差為0.60幀，試驗匹配準確度和耗時均優于對比算法。由上述數據可知，本文提出的二階MM-PF定位方法，在不同場景中具有穩定的地圖匹配結果。

表2 不同算法在工業園地下停車場場景的度量級定位對比

圖12 商場停車場環境Fig.12 Scenarios of mall parking lots

表3 不同算法在商場地下停車場的節點級定位對比

圖13 商場地下停車場節點級定位誤差概率分布Fig.13 Error distribution of node-level localization in underground parking lots shopping malls

在該場景中，車輛在全局坐標系中的定位結果如圖14所示，其平均定位誤差為0.38 m，誤差均方差為0.29 m，其定位誤差小于1 m的概率為95.4%，本文算法與對比算法的定位結果見表4。由圖14可知，動態目標的存在惡化了系統的定位性能，然而，在車輛運動的約束下，其定位誤差不會出現較大的波動。說明本文算法對存在動態目標的場景也具有較好的定位精度和穩定性。這也進一步說明了本文算法在不同場景中的有效性。

圖14 商場地下停車場定位結果Fig.14 The results of localization in the underground parking of mall

表4 不同算法在商場地下停車場的度量級定位對比

3 結 論

針對地下停車場環境中GPS定位精度較低，本文利用環視圖像構建地下停車場場景的環視特征地圖，并提出一種基于二階馬爾可夫模型與粒子濾波算法相結合的節點級定位方法。在定位過程中，該方法僅利用車輛自身配備的環視系統即可實現車輛的高精度定位，大幅降低了車輛的定位成本，并在不同的場景中對所提算法進行驗證，可以得出以下結論：

(1) 本文算法在不同場景中的平均定位誤差不超過0.38 m，定位誤差均方差小于0.29 m，定位誤差在1 m以內的概率不低于95.4%，試驗結果表明，利用所提出的二階MM-PF定位方法可有效地提高定位精度。

(2) 通過獲取關鍵粒子，平均定位耗時在16 ms以內，均優于對比算法，說明本文算法具有較高的定位效率，能夠滿足智能車實時定位的需求。

(3) 在兩個不同的地下停車場環境中，對所提出的算法進行驗證，能夠獲得相近的定位結果，證明本文算法僅利用車輛自身搭載的環視系統即可實現穩健的定位效果。