本發(fā)明涉及計(jì)算機(jī)和自動(dòng)化技術(shù)，特別是智能車輛定位技術(shù)領(lǐng)域。

背景技術(shù)：

智能車輛的準(zhǔn)確定位是智能車輛輔助駕駛/自動(dòng)駕駛研究中的重要問題。

中國專利申請：車輛gps與電子地圖動(dòng)態(tài)定位方法及車輛智能管理系統(tǒng)(申請?zhí)枺?01210210213.8)公開了一種車輛gps與電子地圖動(dòng)態(tài)定位方法，該方法僅采用gps單一傳感器，沒有采用融合定位方法。中國專利申請：車輛定位的實(shí)現(xiàn)方法(申請?zhí)枺?00910302287.2)公開了一種將gps和dr相結(jié)合，與電子地圖匹配定位的方法，該方法沒有引入慣性導(dǎo)航及全景地圖圖像匹配。中國專利申請：一種基于路面圖像特征匹配的車輛定位方法(申請?zhí)枺?01310406668.1)公開了一種對連續(xù)兩幀路面圖像進(jìn)行特征匹配定位的方法，該方法要求路面有足夠多的特征點(diǎn)(路標(biāo))，并且沒有考慮幀幀匹配會帶來累計(jì)誤差的問題，也沒有與其他傳感器進(jìn)行融合。中國專利申請：一種基于車載全景影像與街景地圖匹配的視覺定位方法(申請?zhí)枺?01510094595.6)本發(fā)明公開了一種采用車載全景實(shí)時(shí)環(huán)境圖像與街景地圖匹配的定位方法，該方法僅適用于gps失效的時(shí)候作為替補(bǔ)，沒有考慮將兩者進(jìn)行融合以提高精度。

本發(fā)明針對目前的智能車輛定位系統(tǒng)中，使用單一傳感器時(shí)定位精度及可靠性不高的問題，提出一種引入全景地圖的智能車輛融合定位系統(tǒng)及方法，首先將智能車輛車載攝像機(jī)采集的實(shí)時(shí)環(huán)境圖像與全景地圖進(jìn)行匹配得到圖像定位結(jié)果，再將圖像定位結(jié)果與衛(wèi)星定位結(jié)果、慣性導(dǎo)航定位結(jié)果通過聯(lián)邦濾波器進(jìn)行融合濾波來提高智能車輛定位精度及可靠性。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明旨在解決以上現(xiàn)有技術(shù)的問題，提出了一種提高定位精度及可靠性的方法。本發(fā)明的技術(shù)方案如下：

一種引入全景地圖的智能車輛融合定位系統(tǒng)，其包括數(shù)據(jù)采集模塊、道路篩選模塊、圖像匹配模塊和融合定位模塊，其中：

所述數(shù)據(jù)采集模塊，通過車載衛(wèi)星定位設(shè)備、慣性導(dǎo)航設(shè)備和攝像機(jī)分別采集智能車輛當(dāng)前時(shí)刻的衛(wèi)星定位數(shù)據(jù)、慣性導(dǎo)航定位數(shù)據(jù)和環(huán)境圖像；

所述道路篩選模塊用于結(jié)合智能車輛狀態(tài)估計(jì)(在系統(tǒng)初始化時(shí)為衛(wèi)星定位數(shù)據(jù))和adas(advanceddriverassistancesystems,高級駕駛員輔助系統(tǒng))地圖，篩選并識別出智能車輛當(dāng)前時(shí)刻所在道路；

所述圖像匹配模塊用于將智能車輛數(shù)據(jù)采集系統(tǒng)的車載攝像機(jī)獲得的當(dāng)前環(huán)境圖像與當(dāng)前時(shí)刻所在道路的全景地圖圖像進(jìn)行匹配，獲得圖像定位結(jié)果；

所述融合定位模塊以慣性導(dǎo)航系統(tǒng)為參考系統(tǒng)，慣性導(dǎo)航/衛(wèi)星定位和慣性導(dǎo)航/攝像機(jī)為兩個(gè)子系統(tǒng)，用衛(wèi)星定位數(shù)據(jù)、慣性導(dǎo)航定位數(shù)據(jù)和圖像定位結(jié)果構(gòu)造聯(lián)邦濾波器進(jìn)行融合濾波，得到智能車輛的精確定位結(jié)果。

進(jìn)一步的，一種基于所述系統(tǒng)的引入全景地圖的智能車輛融合定位方法，其包括以下步驟：

(1)數(shù)據(jù)采集：數(shù)據(jù)采集模塊在智能車輛行駛的過程中，實(shí)時(shí)采集當(dāng)前時(shí)刻k的衛(wèi)星定位數(shù)據(jù)、慣性導(dǎo)航定位數(shù)據(jù)以及環(huán)境圖像；

(2)當(dāng)前時(shí)刻所在道路篩選：道路篩選模塊根據(jù)上一時(shí)刻智能車輛狀態(tài)估計(jì)進(jìn)行一步預(yù)測，利用一步預(yù)測結(jié)果中的位置信息(在系統(tǒng)初始化時(shí)為步驟(1)所述的衛(wèi)星定位數(shù)據(jù))，通過adas地圖接口，篩選并識別出智能車輛當(dāng)前時(shí)刻所在道路的編號；

(3)實(shí)時(shí)環(huán)境圖像與全景地圖匹配：圖像匹配模塊以步驟(2)獲得智能車輛位置信息以及當(dāng)前時(shí)刻所在道路編號為約束，構(gòu)造約束條件，將實(shí)時(shí)環(huán)境圖像與滿足約束條件的全景地圖圖像進(jìn)行匹配，得到智能車輛的圖像定位結(jié)果；

(4)融合濾波：融合定位模塊利用聯(lián)邦濾波器融合估計(jì)由步驟(1)獲得的衛(wèi)星定位數(shù)據(jù)、慣性導(dǎo)航定位數(shù)據(jù)和由步驟(3)獲得的圖像定位結(jié)果，得到智能車輛的精確定位結(jié)果。

進(jìn)一步的，所述圖像匹配模塊將智能車輛當(dāng)前的實(shí)時(shí)環(huán)境圖像與全景地圖圖像進(jìn)行匹配的步驟包括：

(1)對智能車輛數(shù)據(jù)采集模塊的車載攝像機(jī)獲得的實(shí)時(shí)環(huán)境圖像進(jìn)行預(yù)處理，根據(jù)攝像機(jī)內(nèi)部參數(shù)去除圖像畸變，得到修正的實(shí)時(shí)環(huán)境圖像c；

(2)以智能車輛狀態(tài)的一步預(yù)測中的位置信息(在系統(tǒng)初始化時(shí)為步驟2.1所述的衛(wèi)星定位數(shù)據(jù))為中心、給定參數(shù)r為半徑，構(gòu)造搜索區(qū)間，從全景地圖中檢索出該搜索區(qū)間內(nèi)并且屬于當(dāng)前時(shí)刻所在道路的n幀全景地圖圖像作為參考圖像集合，記為c1,c2,...,cn，對應(yīng)的地理位置為pi(xi,yi)，i＝1,2,...,n；

(3)利用圖像匹配算法對由步驟(1)預(yù)處理后的實(shí)時(shí)環(huán)境圖像與步驟(2)中獲得的參考圖像集合中的n幀參考圖像逐一進(jìn)行匹配，計(jì)算出實(shí)時(shí)環(huán)境圖像與每幀參考圖像之間的相似度γi(c,ci)，i＝1,2,...,n；

(4)以步驟(3)所述的n個(gè)相似度γi(c,ci)為權(quán)值，利用加權(quán)最小二乘算法對n幀參考圖像所對應(yīng)的地理位置pi(xi,yi)進(jìn)行融合，得到智能車輛位置的圖像定位結(jié)果。

進(jìn)一步的，所述融合定位模塊的融合濾波步驟包括：

(1)以慣性導(dǎo)航定位數(shù)據(jù)和衛(wèi)星定位數(shù)據(jù)作為慣性導(dǎo)航/衛(wèi)星定位子濾波器(記為lf1)的量測值，通過kalman濾波得到局部估計(jì)及其協(xié)方差陣p1；

(2)以慣性導(dǎo)航定位數(shù)據(jù)和圖像匹配模塊獲得的圖像定位結(jié)果作為慣性導(dǎo)航/攝像機(jī)子濾波器(記為lf2)的量測值，通過kalman濾波得到局部估計(jì)及其協(xié)方差陣p2；

(3)將步驟(1)和步驟(2)中各子濾波器lf1及l(fā)f2的局部估計(jì)及其協(xié)方差陣送入到聯(lián)邦濾波器，與主濾波器的估計(jì)及其協(xié)方差陣pm一起進(jìn)行融合，得到全局最優(yōu)估計(jì)及其協(xié)方差陣pf；

(4)根據(jù)信息分配原理確定信息分配系數(shù)，以步驟(3)獲得的全局估計(jì)協(xié)方差陣pf以及信息分配系數(shù)重置各子濾波器的估計(jì)及主濾波器的誤差協(xié)方差。

本發(fā)明的優(yōu)點(diǎn)及有益效果如下：

本發(fā)明提出一種引入全景地圖的智能車輛融合定位系統(tǒng)及方法。通過引入全景地圖，利用圖像匹配技術(shù)進(jìn)行圖像定位，再結(jié)合衛(wèi)星定位與慣性導(dǎo)航定位進(jìn)行融合濾波，提高智能車輛定位精度與可靠性。利用adas地圖對當(dāng)前時(shí)刻智能車輛所在道路進(jìn)行篩選與識別，可避免多層及交叉道路環(huán)境下難以有效選擇全景圖像的問題。

附圖說明

圖1.本發(fā)明提供優(yōu)選實(shí)施例引入全景地圖的智能車輛融合定位系統(tǒng)總體架構(gòu)；

圖2.本發(fā)明引入全景地圖的智能車輛融合定位方法流程圖；

圖3.本發(fā)明圖像匹配模塊流程圖；

圖4.本發(fā)明融合定位模塊聯(lián)邦濾波器結(jié)構(gòu)。

具體實(shí)施方式

下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、詳細(xì)地描述。所描述的實(shí)施例僅僅是本發(fā)明的一部分實(shí)施例。

本發(fā)明解決上述技術(shù)問題的技術(shù)方案是：

本發(fā)明提出的引入全景地圖的智能車輛融合定位方法，利用圖像匹配技術(shù)，將智能車輛實(shí)時(shí)環(huán)境圖像與全景地圖匹配，獲取車輛的位置信息，并與衛(wèi)星定位技術(shù)、慣性導(dǎo)航定位技術(shù)相結(jié)合，以提高定位精度及可靠性，為實(shí)現(xiàn)智能車輛輔助駕駛/自主駕駛奠定基礎(chǔ)。

以下結(jié)合附圖和具體實(shí)例對本發(fā)明的具體實(shí)施方式進(jìn)行描述。

如圖1所示為本發(fā)明提出的一種引入全景地圖的智能車輛融合定位系統(tǒng)的總體架構(gòu)。該系統(tǒng)由數(shù)據(jù)采集模塊、道路篩選模塊、圖像匹配模塊和融合定位模塊組成。其中：數(shù)據(jù)采集模塊通過車載衛(wèi)星定位設(shè)備、慣性導(dǎo)航設(shè)備和攝像機(jī)分別采集智能車輛當(dāng)前時(shí)刻的衛(wèi)星定位數(shù)據(jù)、慣性導(dǎo)航定位數(shù)據(jù)和環(huán)境圖像；道路篩選模塊用于結(jié)合智能車輛狀態(tài)估計(jì)(在系統(tǒng)初始化時(shí)為衛(wèi)星定位數(shù)據(jù))和adas地圖，篩選并識別出智能車輛當(dāng)前時(shí)刻所在道路；圖像匹配模塊用于將智能車輛車載攝像機(jī)獲得的當(dāng)前環(huán)境圖像與當(dāng)前時(shí)刻所在道路的全景地圖圖像進(jìn)行匹配，獲得圖像定位結(jié)果；融合定位模塊以慣性導(dǎo)航系統(tǒng)為參考系統(tǒng)，慣性導(dǎo)航/衛(wèi)星定位和慣性導(dǎo)航/攝像機(jī)為兩個(gè)子系統(tǒng)，用衛(wèi)星定位數(shù)據(jù)、慣性導(dǎo)航定位數(shù)據(jù)和圖像定位結(jié)果構(gòu)造聯(lián)邦濾波器進(jìn)行融合濾波，得到智能車輛的精確定位結(jié)果

如圖2所示為本發(fā)明提出的一種引入全景地圖的智能車輛融合定位方法流程，該方法包括以下步驟：

(1)數(shù)據(jù)采集：數(shù)據(jù)采集模塊在智能車輛行駛的過程中，實(shí)時(shí)采集當(dāng)前時(shí)刻k的衛(wèi)星定位數(shù)據(jù)z1、慣性導(dǎo)航定位數(shù)據(jù)x1以及環(huán)境圖像；

(2)當(dāng)前時(shí)刻所在道路篩選：道路篩選模塊根據(jù)上一時(shí)刻智能車輛狀態(tài)估計(jì)進(jìn)行一步預(yù)測，利用一步預(yù)測結(jié)果中的位置信息(在系統(tǒng)初始化時(shí)為步驟(1)所述的衛(wèi)星定位數(shù)據(jù))，通過adas地圖接口，篩選并識別出智能車輛當(dāng)前時(shí)刻所在道路的編號；

(3)實(shí)時(shí)環(huán)境圖像與全景地圖匹配：圖像匹配模塊以步驟(2)獲得智能車輛位置信息以及當(dāng)前時(shí)刻所在道路編號為約束，構(gòu)造約束條件，將實(shí)時(shí)環(huán)境圖像與滿足約束條件的全景地圖圖像進(jìn)行匹配，得到智能車輛的圖像定位結(jié)果z2；

(4)融合濾波：融合定位模塊利用聯(lián)邦濾波器融合估計(jì)由步驟(1)獲得的衛(wèi)星定位數(shù)據(jù)、慣性導(dǎo)航定位數(shù)據(jù)和由步驟(3)獲得的圖像定位結(jié)果，得到智能車輛的精確定位結(jié)果。

如圖3所示為本發(fā)明提出的一種引入全景地圖的智能車輛融合定位系統(tǒng)的圖像匹配模塊流程，其步驟包括：

(1)對智能車輛數(shù)據(jù)采集模塊的車載攝像機(jī)獲得的實(shí)時(shí)環(huán)境圖像進(jìn)行預(yù)處理，根據(jù)攝像機(jī)內(nèi)部參數(shù)去除圖像畸變，得到修正的實(shí)時(shí)環(huán)境圖像c；

(2)以智能車輛狀態(tài)的一步預(yù)測中的位置信息(在系統(tǒng)初始化時(shí)為步驟2.1所述的衛(wèi)星定位數(shù)據(jù))為中心、給定參數(shù)r為半徑，構(gòu)造搜索區(qū)間，從全景地圖中檢索出該搜索區(qū)間內(nèi)并且屬于當(dāng)前時(shí)刻所在道路的n幀全景地圖圖像作為參考圖像集合，記為c1,c2,...,cn，對應(yīng)的地理位置為pi(xi,yi)，i＝1,2,...,n；

(3)利用圖像匹配算法對由步驟(1)預(yù)處理后的實(shí)時(shí)環(huán)境圖像與步驟(2)中獲得的參考圖像集合中的n幀參考圖像逐一進(jìn)行匹配，計(jì)算出實(shí)時(shí)環(huán)境圖像與每幀參考圖像之間的相似度γi(c,ci)，i＝1,2,...,n。匹配過程的具體實(shí)施步驟如下：

a.提取全景地圖圖像的sift(scale-invariantfeaturetransform，尺度不變特征變換)特征；

b.利用k-means算法對步驟a所提取的大量的sift特征進(jìn)行聚類處理，構(gòu)建視覺詞匯；

c.構(gòu)建實(shí)時(shí)環(huán)境圖像c和n幀參考圖像的詞匯分布頻率直方圖，分別記為a和bi，i＝1,2,...,n；

d.按照式(1)，利用余弦相似性準(zhǔn)則分別計(jì)算實(shí)時(shí)環(huán)境圖像c與參考圖像ci的相似度：

其中，aj表示實(shí)時(shí)環(huán)境圖像c的詞匯分布頻率直方圖中第j個(gè)視覺詞匯的統(tǒng)計(jì)數(shù)量；bij表示參考圖像ci的詞匯分布頻率直方圖中第j個(gè)視覺詞匯的統(tǒng)計(jì)數(shù)量，m為視覺詞典中詞匯的總數(shù)量。

(4)以步驟(3)所述的n個(gè)相似度γi(c,ci)為權(quán)值，按照式(2)利用加權(quán)最小二乘算法對n幀參考圖像所對應(yīng)的地理位置pi(xi,yi)進(jìn)行融合，得到智能車輛位置的圖像定位結(jié)果z2：

本發(fā)明提出的一種引入全景地圖的智能車輛融合定位系統(tǒng)的融合定位模塊結(jié)構(gòu)，融合定位模塊采用聯(lián)邦濾波器結(jié)構(gòu)，其中參考系統(tǒng)采用慣性導(dǎo)航定位系統(tǒng)，其輸出x1直接送入主濾波器進(jìn)行濾波；利用慣性導(dǎo)航/衛(wèi)星定位系統(tǒng)構(gòu)成子濾波器lf1，慣性導(dǎo)航/攝像機(jī)構(gòu)成子濾波器lf2。

融合濾波方法包括以下步驟：

(1)以慣性導(dǎo)航定位數(shù)據(jù)和衛(wèi)星定位數(shù)據(jù)作為慣性導(dǎo)航/衛(wèi)星定位子濾波器lf1的量測值，通過kalman濾波得到局部估計(jì)及其協(xié)方差陣p1(k)；

(2)以慣性導(dǎo)航定位數(shù)據(jù)和圖像匹配模塊獲得的圖像定位結(jié)果作為慣性導(dǎo)航/攝像機(jī)子濾波器lf2的量測值，通過kalman濾波得到局部估計(jì)及其協(xié)方差陣p2(k)；

(3)將步驟(1)和步驟(2)中各子濾波器lf1及l(fā)f2的局部估計(jì)及其協(xié)方差陣送入到聯(lián)邦濾波器，與主濾波器的估計(jì)及其協(xié)方差陣pm(k)一起進(jìn)行融合，得到全局最優(yōu)估計(jì)及其協(xié)方差陣pf(k)：

(4)根據(jù)信息分配原理確定信息分配系數(shù)，以步驟(3)獲得的全局估計(jì)協(xié)方差陣pf(k)以及信息分配系數(shù)重置各子濾波器的估計(jì)及主濾波器的誤差協(xié)方差：

其中i＝1,2，qi(k)是lfi的過程噪聲協(xié)方差陣，q(k)是系統(tǒng)的過程噪聲協(xié)方差陣，βi是信息分配系數(shù)，滿足：

以上這些實(shí)施例應(yīng)理解為僅用于說明本發(fā)明而不用于限制本發(fā)明的保護(hù)范圍。在閱讀了本發(fā)明的記載的內(nèi)容之后，技術(shù)人員可以對本發(fā)明作各種改動(dòng)或修改，這些等效變化和修飾同樣落入本發(fā)明權(quán)利要求所限定的范圍。