本發(fā)明涉及無線定位技術(shù)領(lǐng)域，特別是指一種智能交通系統(tǒng)中多車輛協(xié)作定位方法。

背景技術(shù)：

近年來，隨著車輛變得更加智能和自動(dòng)化，在智能交通系統(tǒng)中，各種安全相關(guān)的應(yīng)用，如交通狀況的實(shí)時(shí)估計(jì)，碰撞警告系統(tǒng)，車道偏離警告系統(tǒng)等，這些都是為了提高駕駛的效率和安全性，從而減少了車輛碰撞事故。而這些安全應(yīng)用程序主要依賴于本地交通網(wǎng)絡(luò)提供的車輛位置信息。車輛導(dǎo)航技術(shù)包括全球定位系統(tǒng)(gps)、全球衛(wèi)星導(dǎo)航系統(tǒng)(glonass)，伽利略和beidou系統(tǒng)(bds)，它們可以為車輛用戶提供位置信息。gps是車輛定位中最常用的定位設(shè)備之一。然而，眾所周知，gps信號(hào)受到不同來源的噪音和退化以及在復(fù)雜的環(huán)境下信號(hào)的暫時(shí)丟失，而且gps衛(wèi)星可見度估計(jì)不足，這使得gps不能在所有情況下提供精確的位置信息。我們用于汽車應(yīng)用的低成本gps接收機(jī)導(dǎo)航系統(tǒng)遭受著低精度和頻繁的信號(hào)中斷問題。通常情況下，gps標(biāo)稱的精度約為10m，這對(duì)于車輛主動(dòng)安全系統(tǒng)來說誤差太大。

提高自我定位準(zhǔn)確性的最常用方法之一是使用其他嵌入式信息源，結(jié)合導(dǎo)航數(shù)據(jù)，通過數(shù)據(jù)融合，獲取更準(zhǔn)確的位置估計(jì)。目前常用的提高gps性能的技術(shù)有基于卡爾曼濾波(kalmanfiltering)的方法?；诳柭鼮V波的方法主要思想是通過濾波減小gps偽距誤差，但這種方法并沒有結(jié)合周邊車輛的位置信息，在智能交通系統(tǒng)中所提供的定位精度不高。

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

本發(fā)明要解決的技術(shù)問題是提供一種智能交通系統(tǒng)中多車輛協(xié)作定位方法，以解決現(xiàn)有技術(shù)所存在的定位精度低的問題。

為解決上述技術(shù)問題，本發(fā)明實(shí)施例提供一種智能交通系統(tǒng)中多車輛協(xié)作定位方法，包括：

獲取系統(tǒng)中目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值，所述相鄰車輛為與所述目標(biāo)車輛相鄰的車輛；

獲取所述目標(biāo)車輛在當(dāng)前時(shí)刻的運(yùn)動(dòng)觀測(cè)值；

根據(jù)獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，計(jì)算所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值；

獲取所述相鄰車輛的數(shù)目，并結(jié)合獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，以及計(jì)算得到的所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值，構(gòu)造系統(tǒng)的運(yùn)動(dòng)方程；

根據(jù)獲取的目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值，計(jì)算所述目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，并根據(jù)相鄰車輛的數(shù)目，構(gòu)造系統(tǒng)的觀測(cè)方程；

將系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值。

進(jìn)一步地，所述獲取系統(tǒng)中目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值包括：

獲取k時(shí)刻目標(biāo)車輛x0的位置觀測(cè)值x0k＝[x0ky0kθ0k]^t；

其中，k時(shí)刻表示當(dāng)前時(shí)刻，t表示轉(zhuǎn)置，x0k表示k時(shí)刻目標(biāo)車輛x0在x軸坐標(biāo)，y0k表示k時(shí)刻目標(biāo)車輛x0在y軸坐標(biāo)，θ0k表示k時(shí)刻目標(biāo)車輛x0運(yùn)動(dòng)方向與x軸所形成的夾角；

獲取k時(shí)刻相鄰車輛xj的位置觀測(cè)值xjk＝[xjkyjkθjk]^t，(j＝1,2…n)；

其中，n表示相鄰車輛的數(shù)目，xjk表示k時(shí)刻相鄰車輛xj在x軸坐標(biāo)，yjk表示k時(shí)刻相鄰車輛xj在y軸坐標(biāo)，θjk表示k時(shí)刻相鄰車輛xj運(yùn)動(dòng)方向與x軸所形成的夾角。

進(jìn)一步地，所述獲取所述目標(biāo)車輛在當(dāng)前時(shí)刻的運(yùn)動(dòng)觀測(cè)值包括：

獲取k時(shí)刻目標(biāo)車輛x0的運(yùn)動(dòng)觀測(cè)值u0k＝[v0ka0kφ0k]^t；

其中，v0k表示k時(shí)刻目標(biāo)車輛x0的速度，a0k表示k時(shí)刻目標(biāo)車輛x0的加速度，φ0k表示k時(shí)刻目標(biāo)車輛x0的轉(zhuǎn)向角。

進(jìn)一步地，所述根據(jù)獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，計(jì)算所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值包括：

根據(jù)目標(biāo)車輛x0在k時(shí)刻的位置觀測(cè)值x0k和運(yùn)動(dòng)觀測(cè)值u0k以及目標(biāo)車輛x0的運(yùn)動(dòng)模型，計(jì)算目標(biāo)車輛x0在k+1時(shí)刻的位置測(cè)量值x0(k+1)＝f(x0k,u0k)，其中，f(x0k,u0k)表示目標(biāo)車輛x0的運(yùn)動(dòng)模型的離散運(yùn)動(dòng)方程；

根據(jù)f(x0k,u0k)，得到f(x0k,u0k)關(guān)于位置觀測(cè)值x0k的雅可比矩陣為：

根據(jù)f(x0k,u0k)，得到f(x0k,u0k)關(guān)于運(yùn)動(dòng)觀測(cè)值u0k的雅可比矩陣bu0k為：

進(jìn)一步地，所述獲取所述相鄰車輛的數(shù)目，并結(jié)合獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，以及計(jì)算得到的所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值，構(gòu)造系統(tǒng)的運(yùn)動(dòng)方程包括：

獲取所述相鄰車輛的數(shù)目n；

根據(jù)獲取的相鄰車輛的數(shù)目n，結(jié)合獲取的目標(biāo)車輛在k時(shí)刻的位置觀測(cè)值x0k和運(yùn)動(dòng)觀測(cè)值u0k，構(gòu)造k時(shí)刻包含所述目標(biāo)車輛和相鄰車輛的整個(gè)系統(tǒng)的系統(tǒng)狀態(tài)系統(tǒng)輸入則系統(tǒng)的運(yùn)動(dòng)方程為xk+1＝f(xk,uk)；其中，t表示矩陣轉(zhuǎn)置；

根據(jù)雅可比矩陣得到系統(tǒng)的運(yùn)動(dòng)方程f(xk,uk)關(guān)于系統(tǒng)狀態(tài)xk的雅可比矩陣ak為：

根據(jù)雅可比矩陣得到系統(tǒng)的運(yùn)動(dòng)方程f(xk,uk)關(guān)于系統(tǒng)輸入uk的雅可比矩陣bk為：

進(jìn)一步地，所述根據(jù)獲取的目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值，計(jì)算所述目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，并根據(jù)相鄰車輛的數(shù)目，構(gòu)造系統(tǒng)的觀測(cè)方程包括：

根據(jù)獲取的k時(shí)刻目標(biāo)車輛x0的位置觀測(cè)值x0k和相鄰車輛xj的位置觀測(cè)值xjk，計(jì)算k時(shí)刻目標(biāo)車輛x0和相鄰車輛xj之間的相對(duì)位置信息zjk；

根據(jù)計(jì)算得到的k時(shí)刻目標(biāo)車輛x0和相鄰車輛xj之間的相對(duì)位置信息zjk，構(gòu)造系統(tǒng)的觀測(cè)方程zk＝[z1kz2k…zjk…z(n-1)kznk]^t,(j＝1,2,…n)，其中，n表示相鄰車輛的數(shù)目；其中，所述zjk表示為：

其中，hj(·)表示相對(duì)位置信息的計(jì)算公式，djk為目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)距離，為目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角；

根據(jù)zjk＝hj(x0k,xjk)，目標(biāo)車輛x0相對(duì)于zjk＝hj(x0k,xjk)的雅可比矩陣hjk為：

根據(jù)雅可比矩陣hjk，得到觀測(cè)方程zk的雅可比矩陣hk為：

進(jìn)一步地，所述將系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值包括：

將系統(tǒng)狀態(tài)xk的雅可比矩陣ak、系統(tǒng)輸入uk的雅可比矩陣bk、觀測(cè)方程zk的雅可比矩陣hk以及系統(tǒng)的運(yùn)動(dòng)方程xk+1和觀測(cè)方程zk代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛x0的位置估計(jì)值

進(jìn)一步地，所述將系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值包括：

s1、根據(jù)系統(tǒng)狀態(tài)xk的雅可比矩陣ak和系統(tǒng)輸入uk的雅可比矩陣bk，計(jì)算k+1時(shí)刻系統(tǒng)狀態(tài)協(xié)方差的預(yù)測(cè)值其中，pk為k時(shí)刻系統(tǒng)狀態(tài)協(xié)方差，q為位置觀測(cè)值誤差協(xié)方差，t為矩陣轉(zhuǎn)置，ts為采樣周期；

s2、根據(jù)觀測(cè)方程zk的雅可比矩陣hk和計(jì)算得到的k+1時(shí)刻系統(tǒng)狀態(tài)協(xié)方差的預(yù)測(cè)值計(jì)算k+1時(shí)刻卡爾曼濾波增益其中，r為觀測(cè)方程zk的誤差協(xié)方差；

s3、根據(jù)計(jì)算得到的k+1時(shí)刻卡爾曼濾波增益kk+1，計(jì)算k+1時(shí)刻系統(tǒng)狀態(tài)協(xié)方差其中，i為單位矩陣；

s4、根據(jù)系統(tǒng)狀態(tài)xk的雅可比矩陣ak和系統(tǒng)輸入uk的雅可比矩陣bk，計(jì)算k+1時(shí)刻系統(tǒng)狀態(tài)預(yù)測(cè)值和系統(tǒng)觀測(cè)方程預(yù)測(cè)值其中，

所述和分別表示為：

其中，n表示相鄰車輛的數(shù)目；

其中，hj(·)表示相對(duì)位置信息的計(jì)算公式；

s5、根據(jù)得到的系統(tǒng)觀測(cè)方程預(yù)測(cè)值判斷目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角預(yù)測(cè)值是否滿足預(yù)設(shè)的判斷公式，若滿足，則確定k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值若不滿足，則確定k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值為：

其中，zk+1為k+1時(shí)刻系統(tǒng)觀測(cè)方程的觀測(cè)值；

s6、根據(jù)k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值計(jì)算k+1時(shí)刻目標(biāo)車輛位置估計(jì)值為：

進(jìn)一步地，所述預(yù)設(shè)的判斷公式表示為：

其中，θthresh為預(yù)設(shè)的閾值。

進(jìn)一步地，所述

其中，為目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角的方差。

本發(fā)明的上述技術(shù)方案的有益效果如下：

上述方案中，通過獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，計(jì)算所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值；獲取所述相鄰車輛的數(shù)目，并結(jié)合獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，以及計(jì)算得到的所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值，構(gòu)造系統(tǒng)的運(yùn)動(dòng)方程；根據(jù)獲取的目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值，計(jì)算所述目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，并根據(jù)相鄰車輛的數(shù)目，構(gòu)造系統(tǒng)的觀測(cè)方程；將系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值；這樣，通過將目標(biāo)車輛的位置觀測(cè)值與相鄰車輛的位置觀測(cè)值相結(jié)合，計(jì)算目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，基于計(jì)算得到的目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，利用擴(kuò)展卡爾曼濾波進(jìn)行濾波后，估計(jì)出目標(biāo)車輛的位置，實(shí)現(xiàn)協(xié)作定位，能夠減少位置誤差，從而提高車輛的定位精度。

附圖說明

圖1為本發(fā)明實(shí)施例提供的智能交通系統(tǒng)中多車輛協(xié)作定位方法的流程示意圖；

圖2為本發(fā)明實(shí)施例提供的智能交通系統(tǒng)多車輛系統(tǒng)模型示意圖；

圖3為本發(fā)明實(shí)施例測(cè)量的位置誤差與gps測(cè)量的位置誤差對(duì)比示意圖。

具體實(shí)施方式

為使本發(fā)明要解決的技術(shù)問題、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖及具體實(shí)施例進(jìn)行詳細(xì)描述。

本發(fā)明針對(duì)現(xiàn)有的定位精度低的問題，提供一種智能交通系統(tǒng)中多車輛協(xié)作定位方法。

如圖1所示，本發(fā)明實(shí)施例提供的智能交通系統(tǒng)中多車輛協(xié)作定位方法，包括：

s101，獲取系統(tǒng)中目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值，所述相鄰車輛為與所述目標(biāo)車輛相鄰的車輛；

s102，獲取所述目標(biāo)車輛在當(dāng)前時(shí)刻的運(yùn)動(dòng)觀測(cè)值；

s103，根據(jù)獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，計(jì)算所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值；

s104，獲取所述相鄰車輛的數(shù)目，并結(jié)合獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，以及計(jì)算得到的所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值，構(gòu)造系統(tǒng)的運(yùn)動(dòng)方程；

s105，根據(jù)獲取的目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值，計(jì)算所述目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，并根據(jù)相鄰車輛的數(shù)目，構(gòu)造系統(tǒng)的觀測(cè)方程；

s106，將系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值。

本發(fā)明實(shí)施例所述的智能交通系統(tǒng)中多車輛協(xié)作定位方法，通過獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，計(jì)算所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值；獲取所述相鄰車輛的數(shù)目，并結(jié)合獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，以及計(jì)算得到的所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值，構(gòu)造系統(tǒng)的運(yùn)動(dòng)方程；根據(jù)獲取的目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值，計(jì)算所述目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，并根據(jù)相鄰車輛的數(shù)目，構(gòu)造系統(tǒng)的觀測(cè)方程；將系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值；這樣，通過將目標(biāo)車輛的位置觀測(cè)值與相鄰車輛的位置觀測(cè)值相結(jié)合，計(jì)算目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，基于計(jì)算得到的目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，利用擴(kuò)展卡爾曼濾波進(jìn)行濾波后，估計(jì)出目標(biāo)車輛的位置，實(shí)現(xiàn)協(xié)作定位，能夠減少位置誤差，從而提高車輛的定位精度。

本發(fā)明實(shí)施例所述的智能交通系統(tǒng)中多車輛協(xié)作定位方法，在低信噪比環(huán)境下，測(cè)量結(jié)果比gps測(cè)量結(jié)果更加準(zhǔn)確，且擴(kuò)展卡爾曼濾波運(yùn)算速度快，有助于提高車輛的定位精度。且在實(shí)際應(yīng)用中，可以根據(jù)實(shí)際情況自適應(yīng)地選擇相鄰車輛個(gè)數(shù)。在智能交通系統(tǒng)中，目標(biāo)車輛周圍的相鄰車輛是不斷變化的，本發(fā)明實(shí)施例所述的智能交通系統(tǒng)中多車輛協(xié)作定位方法可自主選擇目標(biāo)車輛周圍的相鄰車輛的個(gè)數(shù)。

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述獲取系統(tǒng)中目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值包括：

獲取k時(shí)刻目標(biāo)車輛x0的位置觀測(cè)值x0k＝[x0ky0kθ0k]^t；

其中，k時(shí)刻表示當(dāng)前時(shí)刻，t表示轉(zhuǎn)置，x0k表示k時(shí)刻目標(biāo)車輛x0在x軸坐標(biāo)，y0k表示k時(shí)刻目標(biāo)車輛x0在y軸坐標(biāo)，θ0k表示k時(shí)刻目標(biāo)車輛x0運(yùn)動(dòng)方向與x軸所形成的夾角；

獲取k時(shí)刻相鄰車輛xj的位置觀測(cè)值xjk＝[xjkyjkθjk]^t，(j＝1,2…n)；

其中，n表示相鄰車輛的數(shù)目，xjk表示k時(shí)刻相鄰車輛xj在x軸坐標(biāo)，yjk表示k時(shí)刻相鄰車輛xj在y軸坐標(biāo)，θjk表示k時(shí)刻相鄰車輛xj運(yùn)動(dòng)方向與x軸所形成的夾角。

本實(shí)施例中，目標(biāo)車輛x0可以通過車載導(dǎo)航(例如，gps設(shè)備或北斗設(shè)備)獲取k時(shí)刻目標(biāo)車輛x0的位置觀測(cè)值x0k＝[x0ky0kθ0k]^t；同時(shí)，目標(biāo)車輛x0可以通過短程距離通信(dedicatedshort-rangecommunication，dsrc)獲取k時(shí)刻相鄰車輛xj的位置觀測(cè)值xjk＝[xjkyjkθjk]^t，(j＝1,2…n)，開支小。

本實(shí)施例中，θk表示k時(shí)刻目標(biāo)車輛x0運(yùn)動(dòng)方向與x軸所形成的夾角，即k時(shí)刻目標(biāo)車輛x0的方位角；θjk表示k時(shí)刻相鄰車輛xj運(yùn)動(dòng)方向與x軸所形成的夾角，即k時(shí)刻相鄰車輛xj的方位角。

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述獲取所述目標(biāo)車輛在當(dāng)前時(shí)刻的運(yùn)動(dòng)觀測(cè)值包括：

獲取k時(shí)刻目標(biāo)車輛x0的運(yùn)動(dòng)觀測(cè)值u0k＝[v0ka0kφ0k]^t；

其中，v0k表示k時(shí)刻目標(biāo)車輛x0的速度，a0k表示k時(shí)刻目標(biāo)車輛x0的加速度，φ0k表示k時(shí)刻目標(biāo)車輛x0的轉(zhuǎn)向角。

本實(shí)施例中，可以通過車載傳感器獲取k時(shí)刻目標(biāo)車輛x0的運(yùn)動(dòng)觀測(cè)值u0k＝[v0ka0kφ0k]^t，其中，所述車載傳感器可以包括但不限于：加速度傳感器，速度傳感器。

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述根據(jù)獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，計(jì)算所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值包括：

根據(jù)目標(biāo)車輛x0在k時(shí)刻的位置觀測(cè)值x0k和運(yùn)動(dòng)觀測(cè)值u0k以及目標(biāo)車輛x0的運(yùn)動(dòng)模型，計(jì)算目標(biāo)車輛x0在k+1時(shí)刻的位置測(cè)量值x0(k+1)＝f(x0k,u0k)，其中，f(x0k,u0k)表示目標(biāo)車輛x0的運(yùn)動(dòng)模型的離散運(yùn)動(dòng)方程；

根據(jù)f(x0k,u0k)，得到f(x0k,u0k)關(guān)于位置觀測(cè)值x0k的雅可比矩陣為：

根據(jù)f(x0k,u0k)，得到f(x0k,u0k)關(guān)于運(yùn)動(dòng)觀測(cè)值u0k的雅可比矩陣為：

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述獲取所述相鄰車輛的數(shù)目，并結(jié)合獲取的目標(biāo)車輛在當(dāng)前時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，以及計(jì)算得到的所述目標(biāo)車輛在下一時(shí)刻的位置測(cè)量值，構(gòu)造系統(tǒng)的運(yùn)動(dòng)方程包括：

獲取所述相鄰車輛的數(shù)目n；

根據(jù)獲取的相鄰車輛的數(shù)目n，結(jié)合獲取的目標(biāo)車輛在k時(shí)刻的位置觀測(cè)值x0k和運(yùn)動(dòng)觀測(cè)值u0k，構(gòu)造k時(shí)刻包含所述目標(biāo)車輛和相鄰車輛的整個(gè)系統(tǒng)的系統(tǒng)狀態(tài)系統(tǒng)輸入則系統(tǒng)的運(yùn)動(dòng)方程為xk+1＝f(xk,uk)；其中，t表示矩陣轉(zhuǎn)置；

根據(jù)雅可比矩陣得到系統(tǒng)的運(yùn)動(dòng)方程f(xk,uk)關(guān)于系統(tǒng)狀態(tài)xk的雅可比矩陣ak為：

根據(jù)雅可比矩陣得到系統(tǒng)的運(yùn)動(dòng)方程f(xk,uk)關(guān)于系統(tǒng)輸入uk的雅可比矩陣bk為：

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述根據(jù)獲取的目標(biāo)車輛和相鄰車輛在當(dāng)前時(shí)刻的位置觀測(cè)值，計(jì)算所述目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，并根據(jù)相鄰車輛的數(shù)目，構(gòu)造系統(tǒng)的觀測(cè)方程包括：

根據(jù)獲取的k時(shí)刻目標(biāo)車輛x0的位置觀測(cè)值x0k和相鄰車輛xj的位置觀測(cè)值xjk，計(jì)算k時(shí)刻目標(biāo)車輛x0和相鄰車輛xj之間的相對(duì)位置信息zjk；

根據(jù)計(jì)算得到的k時(shí)刻目標(biāo)車輛x0和相鄰車輛xj之間的相對(duì)位置信息zjk，構(gòu)造系統(tǒng)的觀測(cè)方程zk＝[z1kz2k…zjk…z(n-1)kznk]^t,(j＝1,2,…n)，其中，n表示相鄰車輛的數(shù)目；其中，所述zjk表示為：

其中，hj(·)表示相對(duì)位置信息的計(jì)算公式，djk為目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)距離，為目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角；

根據(jù)zjk＝hj(x0k,xjk)，目標(biāo)車輛x0相對(duì)于zjk＝hj(x0k,xjk)的雅可比矩陣hjk為：

根據(jù)雅可比矩陣hjk，得到觀測(cè)方程zk的雅可比矩陣hk為：

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述將系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值包括：

將系統(tǒng)狀態(tài)xk的雅可比矩陣ak、系統(tǒng)輸入uk的雅可比矩陣bk、觀測(cè)方程zk的雅可比矩陣hk以及系統(tǒng)的運(yùn)動(dòng)方程xk+1和觀測(cè)方程zk代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛x0的位置估計(jì)值

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述將系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值包括：

s1、根據(jù)系統(tǒng)狀態(tài)xk的雅可比矩陣ak和系統(tǒng)輸入uk的雅可比矩陣bk，計(jì)算k+1時(shí)刻系統(tǒng)狀態(tài)協(xié)方差的預(yù)測(cè)值其中，pk為k時(shí)刻系統(tǒng)狀態(tài)協(xié)方差，q為位置觀測(cè)值誤差協(xié)方差，t為矩陣轉(zhuǎn)置，ts為采樣周期；

s2、根據(jù)觀測(cè)方程zk的雅可比矩陣hk和計(jì)算得到的k+1時(shí)刻系統(tǒng)狀態(tài)協(xié)方差的預(yù)測(cè)值計(jì)算k+1時(shí)刻卡爾曼濾波增益其中，r為觀測(cè)方程zk的誤差協(xié)方差；

s3、根據(jù)計(jì)算得到的k+1時(shí)刻卡爾曼濾波增益kk+1，計(jì)算k+1時(shí)刻系統(tǒng)狀態(tài)協(xié)方差其中，i為單位矩陣；

s4、根據(jù)系統(tǒng)狀態(tài)xk的雅可比矩陣ak和系統(tǒng)輸入uk的雅可比矩陣bk，計(jì)算k+1時(shí)刻系統(tǒng)狀態(tài)預(yù)測(cè)值和系統(tǒng)觀測(cè)方程預(yù)測(cè)值其中，

所述和分別表示為：

其中，n表示相鄰車輛的數(shù)目；

其中，hj(·)表示相對(duì)位置信息的計(jì)算公式；

s5、根據(jù)得到的系統(tǒng)觀測(cè)方程預(yù)測(cè)值判斷目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角預(yù)測(cè)值是否滿足預(yù)設(shè)的判斷公式，若滿足，則確定k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值若不滿足，則確定k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值為：

其中，zk+1為k+1時(shí)刻系統(tǒng)觀測(cè)方程的觀測(cè)值；

s6、根據(jù)k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值計(jì)算k+1時(shí)刻目標(biāo)車輛位置估計(jì)值為：

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述預(yù)設(shè)的判斷公式表示為：

其中，θthresh為預(yù)設(shè)的閾值。

在s5中，可以通過預(yù)設(shè)的判斷公式判斷目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角預(yù)測(cè)值是否接近若接近則保留系統(tǒng)狀態(tài)預(yù)測(cè)值即k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值若不接近，則k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值為：

在前述智能交通系統(tǒng)中多車輛協(xié)作定位方法的具體實(shí)施方式中，進(jìn)一步地，所述

其中，為目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角的方差。

如圖2所示，以具體的例子對(duì)本實(shí)施例所述的智能交通系統(tǒng)中多車輛協(xié)作定位方法進(jìn)行詳細(xì)說明，并使用matlab仿真平臺(tái)，對(duì)本實(shí)施例所述的智能交通系統(tǒng)中多車輛協(xié)作定位方法的性能進(jìn)行仿真分析：

步驟1，如圖2所示，考慮到智能交通系統(tǒng)中一個(gè)目標(biāo)車輛x0和四個(gè)相鄰車輛x1,x2,x3,x4，行駛在道路上，車輛x0,x1,x2,x3,x4通過自身gps設(shè)備分別接收到k時(shí)刻各自的位置觀測(cè)值；

步驟2，目標(biāo)車輛x0通過車載傳感器獲取k時(shí)刻自身的運(yùn)動(dòng)觀測(cè)值u0k＝[v0ka0kφ0k]^t，設(shè)目標(biāo)車輛做的是直線運(yùn)動(dòng)，在采樣周期內(nèi)可以看作是勻加速直線運(yùn)動(dòng)，即a0k為定值；

步驟3，根據(jù)目標(biāo)車輛x0在k時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，得出目標(biāo)車輛在k+1時(shí)刻的位置觀測(cè)值和運(yùn)動(dòng)觀測(cè)值，即目標(biāo)車輛x0的運(yùn)動(dòng)模型；具體的，步驟3可以包括：

3.1)目標(biāo)車輛x0的運(yùn)動(dòng)模型的離散運(yùn)動(dòng)方程為：

其中，ts為采樣周期。

3.2)根據(jù)離散運(yùn)動(dòng)方程f(x0k,u0k)，可得f(x0k,u0k)關(guān)于x0k的雅可比矩陣為：

3.3)根據(jù)離散運(yùn)動(dòng)方程f(x0k,u0k)，可得f(x0k,u0k)關(guān)于u0k的雅可比矩陣為：

步驟4，通過步驟3構(gòu)造k時(shí)刻包含x0k與相鄰車輛x1k,x2k,x3k,x4k的整個(gè)系統(tǒng)的系統(tǒng)狀態(tài)為xk＝[x0kx0kx0kx0k]^t，系統(tǒng)輸入為uk＝[u0ku0ku0ku0k]^t，則系統(tǒng)的運(yùn)動(dòng)方程為xk+1＝f(xk,uk)；具體的，步驟4可以包括：

4.1)根據(jù)步驟3的雅可比矩陣可得系統(tǒng)的運(yùn)動(dòng)方程f(xk,uk)關(guān)于系統(tǒng)狀態(tài)xk的雅可比矩陣ak為：

4.2)根據(jù)步驟3的雅可比矩陣可得系統(tǒng)的運(yùn)動(dòng)方程f(xk,uk)關(guān)于系統(tǒng)輸入uk的雅可比矩陣bk為：

步驟5：計(jì)算得到目標(biāo)車輛與相鄰車輛之間的相對(duì)位置信息，構(gòu)造觀測(cè)方程zk＝[z1kz2kz3kz4k]^t，具體的，步驟5可以包括：

5.1)k時(shí)刻目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)位置信息zjk為：

其中，djk為目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)距離，為目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角；

5.2)目標(biāo)車輛x0相對(duì)于zjk＝hj(x0k,xjk)的雅可比矩陣hjk為：

5.3)根據(jù)雅可比矩陣hjk可得觀測(cè)方程zk的雅可比矩陣hk為：

步驟6，將步驟4得到的系統(tǒng)狀態(tài)xk的雅可比矩陣ak、系統(tǒng)輸入uk的雅可比矩陣bk和步驟5得到的觀測(cè)方程zk的雅可比矩陣hk以及系統(tǒng)的運(yùn)動(dòng)方程和觀測(cè)方程代入到擴(kuò)展卡爾曼濾波中，得到目標(biāo)車輛的位置估計(jì)值具體的，步驟6可以包括：

6.1)計(jì)算系統(tǒng)狀態(tài)協(xié)方差的預(yù)測(cè)值

其中，pk為k時(shí)刻系統(tǒng)狀態(tài)協(xié)方差，q為位置觀測(cè)值誤差協(xié)方差，ts為采樣周期；

6.2)根據(jù)系統(tǒng)狀態(tài)協(xié)方差的預(yù)測(cè)值計(jì)算k+1時(shí)刻卡爾曼濾波增益kk+1：

其中，r為觀測(cè)方程zk的誤差協(xié)方差，hk為觀測(cè)方程zk的雅可比矩陣。

6.3)根據(jù)k+1時(shí)刻卡爾曼濾波增益kk+1得到k+1時(shí)刻系統(tǒng)狀態(tài)協(xié)方差pk+1：

其中，i為單位矩陣，hk為觀測(cè)方程zk的雅可比矩陣。

6.4)根據(jù)系統(tǒng)狀態(tài)xk的雅可比矩陣ak和系統(tǒng)輸入uk的雅可比矩陣bk計(jì)算系統(tǒng)狀態(tài)預(yù)測(cè)值

其中，

和系統(tǒng)觀測(cè)方程預(yù)測(cè)值

其中，

6.5)計(jì)算k+1時(shí)刻系統(tǒng)狀態(tài)估計(jì)值根據(jù)6.4)得到的系統(tǒng)觀測(cè)方程的預(yù)測(cè)值判斷目標(biāo)車輛x0與相鄰車輛xj之間的相對(duì)方位角預(yù)測(cè)值是否接近若接近則保留系統(tǒng)狀態(tài)預(yù)測(cè)值即若不接近，則

其中，kk+1為k+1時(shí)刻卡爾曼濾波增益，zk+1為k+1時(shí)刻系統(tǒng)觀測(cè)方程的觀測(cè)值，為k+1時(shí)刻系統(tǒng)狀態(tài)預(yù)測(cè)值。

6.6)根據(jù)6.5)計(jì)算得到的可得k+1時(shí)刻目標(biāo)車輛位置估計(jì)值為：

本實(shí)施例中，如圖3所示，圖3描述了本發(fā)明實(shí)施例測(cè)量的位置誤差與gps測(cè)量的位置誤差對(duì)比示意圖，圖3中的橫坐標(biāo)是采樣時(shí)間，縱坐標(biāo)是位置誤差大小，該結(jié)果是在信噪比為3db下進(jìn)行1000次的同條件重復(fù)實(shí)驗(yàn)得出的。由圖3可知，本發(fā)明實(shí)施例提出的方法降低了位置誤差，并給出了更準(zhǔn)確的位置估計(jì)。這說明本發(fā)明實(shí)施例提出的協(xié)作定位方法是準(zhǔn)確并且有效的，同時(shí)對(duì)比結(jié)果也表明本發(fā)明采用的方法性能優(yōu)于gps測(cè)量的性能。

需要說明的是，在本文中，諸如第一和第二等之類的關(guān)系術(shù)語僅僅用來將一個(gè)實(shí)體或者操作與另一個(gè)實(shí)體或操作區(qū)分開來，而不一定要求或者暗示這些實(shí)體或操作之間存在任何這種實(shí)際的關(guān)系或者順序。

以上所述是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明所述原理的前提下，還可以做出若干改進(jìn)和潤飾，這些改進(jìn)和潤飾也應(yīng)視為本發(fā)明的保護(hù)范圍。