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

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

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

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

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

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

1 本文方法

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

1.1 環(huán)視特征地圖構(gòu)建模型

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

P={p1,p2,…,pn}

(1)

式中，P表示環(huán)視特征地圖數(shù)據(jù)庫；pi表示環(huán)視特征地圖的第i個節(jié)點；n為環(huán)視特征地圖的節(jié)點數(shù)。

圖1 環(huán)視特征地圖示意Fig.1 Illustration of visual feature map from around view

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

(2)

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

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

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

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

1.2.1 基于二階MM-PF的節(jié)點級定位模型構(gòu)建

(3)

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

圖3 基于二階MM-PF的智能車環(huán)視地圖匹配與定位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的粒子狀態(tài)轉(zhuǎn)移模型

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

(6)

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

xk=2xj-xi+v(t)

(7)

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

圖4 狀態(tài)轉(zhuǎn)移及粒子權(quán)重分布建模Fig.4 State transition and particle weight distribution modeling

1.2.1.3 基于關(guān)鍵粒子的權(quán)重更新與重采樣

(8)

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

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

(9)

(10)

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

(11)

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

1.2.1.4 最優(yōu)地圖節(jié)點估計

根據(jù)當前狀態(tài)獲得的粒子權(quán)重，對粒子進行重采樣。則當前車輛對應的地圖節(jié)點為重采樣后粒子數(shù)量最多的地圖節(jié)點，即

(12)

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

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

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

(13)

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

(14)

2 試驗及結(jié)果分析

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

圖7 環(huán)視圖生成過程Fig.7 Generating procedure of around view image

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

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

2.1 工業(yè)園停車場場景測試

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

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

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

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

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

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

圖8 測試路段Fig.8 Testing routes

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

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

圖11 定位結(jié)果Fig.11 Localization results

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

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

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

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

圖12 商場停車場環(huán)境Fig.12 Scenarios of mall parking lots

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

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

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

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

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

3 結(jié) 論

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

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

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

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