面向智能車的地下停車場環(huán)視特征地圖構(gòu)建與定位

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

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

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

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

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

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

1 本文方法

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

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

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

P={p1,p2,…,pn}

(1)

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

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

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

(2)

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

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

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

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

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

(3)

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

圖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)化的場景，在運(yùn)動(dòng)過程中，車輛狀態(tài)具有一定的連續(xù)性。即下一時(shí)刻車輛的可到達(dá)區(qū)域與歷史狀態(tài)存在一定的關(guān)聯(lián)關(guān)系。因此，為了適應(yīng)不同的運(yùn)動(dòng)狀態(tài)，并兼顧模型的復(fù)雜度，提出基于二階MM的粒子狀態(tài)轉(zhuǎn)移模型，即t時(shí)刻粒子在空間中的分布情況取決于t-1和t-2兩個(gè)時(shí)刻車輛的狀態(tài)，如圖4所示。假設(shè)車輛在短時(shí)間內(nèi)速度不變，則有

(6)

式中，xj和xi分別為t-1和t-2時(shí)刻的狀態(tài)；xk為t時(shí)刻車輛狀態(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é)點(diǎn)之間的相似度模型，并計(jì)算各粒子權(quán)重

(9)

(10)

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

(11)

當(dāng)Neff<2n/3時(shí)，需對(duì)PF進(jìn)行重采樣，n為粒子總數(shù)。

1.2.1.4 最優(yōu)地圖節(jié)點(diǎn)估計(jì)

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

(12)

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

1.2.2 基于局部特征匹配的度量級(jí)定位

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

(13)

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

(14)

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

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

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

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

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

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

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

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

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

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

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

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

圖8 測試路段Fig.8 Testing routes

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

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

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

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

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

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

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

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

表3 不同算法在商場地下停車場的節(jié)點(diǎn)級(jí)定位對(duì)比

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

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

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

表4 不同算法在商場地下停車場的度量級(jí)定位對(duì)比

3 結(jié) 論

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

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

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

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