一種新型車(chē)輛組合定位算法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種新型車(chē)輛組合定位算法，采用聯(lián)邦卡爾曼濾波數(shù)據(jù)融合技術(shù)，實(shí)現(xiàn)路側(cè)設(shè)備定位(RSU)、衛(wèi)星定位(GNSS)、航位推算(DR)三個(gè)定位子系統(tǒng)的信息融合，即采用線(xiàn)性卡爾曼濾波器作為RSU定位子系統(tǒng)的局部濾波器，記為L(zhǎng)F1，對(duì)應(yīng)的信息分配系數(shù)是β1；GNSS定位子系統(tǒng)也采用線(xiàn)性卡爾曼濾波器作為局部濾波器，記為L(zhǎng)F2，對(duì)應(yīng)的信息分配系數(shù)為β2；DR定位子系統(tǒng)采用擴(kuò)展卡爾曼濾波器作為局部濾波器，記為L(zhǎng)F3，對(duì)應(yīng)的信息分配系數(shù)為β3，主濾波器做信息融合；利用殘差χ2檢測(cè)和冗余硬件檢測(cè)構(gòu)建了兩級(jí)故障檢測(cè)方法，能同時(shí)檢測(cè)出子系統(tǒng)的硬故障和軟故障；同時(shí)根據(jù)定位子系統(tǒng)的定位精度因子來(lái)動(dòng)態(tài)調(diào)整組合系統(tǒng)的信息分配系數(shù)，使系統(tǒng)能夠快速適應(yīng)環(huán)境的變化，提高車(chē)輛定位的精度與可靠性。
【專(zhuān)利說(shuō)明】
一種新型車(chē)輛組合定位算法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及車(chē)輛定位的研究領(lǐng)域，特別涉及一種新型車(chē)輛組合定位算法。
【背景技術(shù)】
[0002]衛(wèi)星定位具有高精度、低成本、易用性等特點(diǎn)，只要能接收到衛(wèi)星信號(hào)的地方，衛(wèi) 星定位接收機(jī)都可以解算出車(chē)輛坐標(biāo)，而且誤差不會(huì)累積。但是在城市復(fù)雜環(huán)境中，由于衛(wèi) 星信號(hào)會(huì)被建筑物遮擋，衛(wèi)星定位精度下降。衛(wèi)星定位系統(tǒng)單獨(dú)使用時(shí)，系統(tǒng)定位可靠性較 低。航位推算(DR)定位系統(tǒng)是常用的車(chē)輛自主定位方法，只要給定車(chē)輛初始位置，就可以利 用車(chē)輛的行駛速度和行進(jìn)方向來(lái)推算出車(chē)輛的當(dāng)前位置，在短時(shí)間內(nèi)能提供高精度的定位 服務(wù)。如果DR定位系統(tǒng)的初始位置無(wú)法得到周期性的更新或校準(zhǔn)，由于其誤差累積特性，定 位可靠性下降。RSU定位方法隨著車(chē)聯(lián)自組織網(wǎng)絡(luò)(VANET)的發(fā)展得到了應(yīng)用，利用車(chē)輛與 RSU的通信，可以不依賴(lài)衛(wèi)星的情況下為車(chē)輛提供定位，在密集的建筑群中部署RSU可以實(shí) 現(xiàn)車(chē)輛定位。定位精度與車(chē)輛所在位置的RSU部署密集程度和無(wú)線(xiàn)信號(hào)是否被遮擋有關(guān)，車(chē) 輛接收到的RSU的定位信號(hào)越多且遮擋越少，系統(tǒng)定位精度越高。在城市中心高樓密集區(qū)域 或者地下停車(chē)場(chǎng)部署RSU可以有效地解決衛(wèi)星定位失效區(qū)域車(chē)輛定位問(wèn)題。以上三種系統(tǒng) 各有優(yōu)劣，面對(duì)復(fù)雜的城市環(huán)境，單一系統(tǒng)都無(wú)法滿(mǎn)足現(xiàn)在車(chē)聯(lián)網(wǎng)應(yīng)用的定位需求，利用信 息融合技術(shù)提高定位系統(tǒng)的可靠性和精度，將是解決車(chē)輛定位的可行方法。
[0003]如果這三個(gè)系統(tǒng)采用集中式卡爾曼濾波來(lái)實(shí)現(xiàn)信息融合，將會(huì)產(chǎn)生兩個(gè)問(wèn)題:第 一、計(jì)算負(fù)擔(dān)重。集中式濾波器對(duì)處理器的計(jì)算能力要求較高，車(chē)載嵌入式處理器無(wú)法滿(mǎn)足 計(jì)算的實(shí)時(shí)性要求;第二、容錯(cuò)性能差，不利于故障診斷。GNSS定位系統(tǒng)在有衛(wèi)星信號(hào)的情 況下實(shí)現(xiàn)定位，而基于RSU的定位系統(tǒng)則剛好相反，在衛(wèi)星定位失效區(qū)域部署RSU為車(chē)輛提 供定位服務(wù)。當(dāng)任一子系統(tǒng)發(fā)生故障，如果不能快速地檢測(cè)出來(lái)，組合定位系統(tǒng)就會(huì)被故障 子系統(tǒng)污染，使輸出的位置信息不可靠。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的主要目的在于克服現(xiàn)有技術(shù)的缺點(diǎn)與不足，提供一種新型車(chē)輛組合定位 算法，采用聯(lián)邦卡爾曼濾波(FKF)組合定位算法，實(shí)現(xiàn)車(chē)輛的高精度定位。
[0005] 為了達(dá)到上述目的，本發(fā)明采用以下技術(shù)方案：
[0006] 本發(fā)明一種新型車(chē)輛組合定位算法，采用聯(lián)邦卡爾曼濾波算法，實(shí)現(xiàn)R S U定位、 GNSS定位、DR定位三個(gè)定位子系統(tǒng)的信息融合，即采用線(xiàn)性卡爾曼濾波器作為RSU定位子系 統(tǒng)的局部濾波器，記為L(zhǎng)F1，對(duì)應(yīng)的信息分配系數(shù)是i3 1;GNSS定位子系統(tǒng)也采用線(xiàn)性卡爾曼 濾波器作為局部濾波器，記為L(zhǎng)F2,對(duì)應(yīng)的信息分配系數(shù)為i32;DR定位系統(tǒng)采用擴(kuò)展卡爾曼 濾波作為局部濾波器，記為L(zhǎng)F3，對(duì)應(yīng)的信息分配系數(shù)為β 3，主濾波器做信息融合，具體包括 下述步驟：
[0007] S1、根據(jù)汽車(chē)運(yùn)動(dòng)學(xué)建立系統(tǒng)狀態(tài)方程與觀(guān)測(cè)方程，所述觀(guān)測(cè)方程包括RSU觀(guān)測(cè)方 程、GNSS觀(guān)測(cè)方程以及DR觀(guān)測(cè)方程；
[0008] S2、根據(jù)LF1、LF2和LF3對(duì)三個(gè)子系統(tǒng)進(jìn)行時(shí)間更新和量測(cè)更新；
[0009] S3、在信息融合過(guò)程中進(jìn)行故障檢測(cè)，利用兩級(jí)故障檢測(cè)技術(shù)，檢測(cè)出故障子系統(tǒng) 并實(shí)現(xiàn)隔離；
[0010] S4、信息融合與信息分配，系統(tǒng)在信息分配過(guò)程中，根據(jù)定位子系統(tǒng)的定位精度因 子來(lái)動(dòng)態(tài)調(diào)整各子系統(tǒng)的信息分配系數(shù)，實(shí)現(xiàn)故障子系統(tǒng)的隔離與無(wú)故障子系統(tǒng)的重構(gòu)。
[0011] 作為優(yōu)選的技術(shù)方案，在步驟S4中，所述兩級(jí)故障檢測(cè)的方法:首先各子系統(tǒng)利用 殘差X2檢測(cè)法去檢測(cè)出現(xiàn)硬故障的子系統(tǒng)，然后利用冗余硬件檢測(cè)法去檢測(cè)系統(tǒng)軟故障， 另外，為了能校驗(yàn)車(chē)載定位系統(tǒng)本身的精確度，還提出了基于車(chē)輛與路側(cè)設(shè)備通信V2R的位 置參照檢測(cè)法，讓系統(tǒng)可以及時(shí)發(fā)現(xiàn)因車(chē)載設(shè)備自身問(wèn)題而導(dǎo)致定位不準(zhǔn)確的原因。
[0012] 作為優(yōu)選的技術(shù)方案，所述殘差X2檢測(cè)法具體如下：
[0013] 由觀(guān)測(cè)值的預(yù)測(cè)值之^與實(shí)測(cè)值Zk之差構(gòu)造檢驗(yàn)統(tǒng)計(jì)量的方法稱(chēng)為殘差X2檢驗(yàn) 法，殘差定義為：
[0014] r(k) = Zt -Zkfk_x = 'Zk-HkXkjk
[0015] 當(dāng)被檢測(cè)系統(tǒng)無(wú)故障時(shí)，殘差r(k)是均值為零的高斯白噪聲，當(dāng)被檢測(cè)系統(tǒng)發(fā)生 故障時(shí)，r(k)的期望是不為零的值，根據(jù)這個(gè)特性，通過(guò)檢測(cè)殘差r(k)均值就可以確定被檢 測(cè)系統(tǒng)是否發(fā)生了故障，由公式=-之^ = ? -可知，殘差向量r(k)也可以 被認(rèn)為均值為零的高斯白噪聲過(guò)程，其協(xié)方差S(k)為：
[0016] S⑷= 丨",?
[0017] 構(gòu)造以下故障檢測(cè)函數(shù)：
[0018] D(k)=rT(k)S_1(k)r(k)
[0019] 由殘差序列的統(tǒng)計(jì)特性知，D(k)服從自由度為η的X2分布，η為狀態(tài)向量X的維數(shù)， 如果設(shè)定故障檢測(cè)閥值T D，那么可以根據(jù)下述公式檢測(cè)系統(tǒng)故障，閥值TD決定了故障檢測(cè)的 性能：
[0021] 作為優(yōu)選的技術(shù)方案，所述冗余硬件檢測(cè)法具體為：
[0022] 采用低成本冗余硬件檢測(cè)，通過(guò)增加加速度計(jì)對(duì)比各子系統(tǒng)所測(cè)量的車(chē)輛速度的 一致性來(lái)實(shí)現(xiàn)故障的檢測(cè)與隔離。5、根據(jù)權(quán)利要求2所述的新型車(chē)輛組合定位算法，其特征 在于，所述位置參照檢測(cè)法具體為：
[0023]傳統(tǒng)車(chē)載定位通常接收衛(wèi)星信號(hào)定位或自主定位，是一個(gè)孤立的系統(tǒng)，V2R通信的 發(fā)展，在某些特定區(qū)域，如停車(chē)場(chǎng)，車(chē)輛可以通過(guò)后臺(tái)數(shù)據(jù)庫(kù)查詢(xún)到自己所處的精確位置坐 標(biāo)，利用所查詢(xún)到的坐標(biāo)信息與實(shí)時(shí)定位的坐標(biāo)相對(duì)比，以此來(lái)判斷車(chē)輛定位系統(tǒng)的準(zhǔn)確 性。
[0024] 作為優(yōu)選的技術(shù)方案，在步驟S4中，自適應(yīng)的信息分配系數(shù)調(diào)整方法以各子系統(tǒng) 的定位精度因子為主要參考，動(dòng)態(tài)調(diào)整信息分配系數(shù)，提高聯(lián)邦濾波器的性能，實(shí)現(xiàn)組合導(dǎo) 航。
[0025] 作為優(yōu)選的技術(shù)方案，三個(gè)子系統(tǒng)在設(shè)定初始參數(shù)后，經(jīng)過(guò)時(shí)間與量測(cè)更新后，得 到了進(jìn)一步預(yù)測(cè)值，根據(jù)殘差X2檢驗(yàn)算法對(duì)各子系統(tǒng)進(jìn)行第一級(jí)硬故障檢測(cè)，剩余無(wú)故障 子系統(tǒng)再使用冗余硬件故障檢測(cè)法，對(duì)比找出有故障的系統(tǒng)進(jìn)行故障隔離，對(duì)于余下的無(wú) 故障子系統(tǒng)，聯(lián)邦卡爾曼濾波組合算法根據(jù)子系統(tǒng)定位精度因子動(dòng)態(tài)調(diào)整信息系分配數(shù)， 具體如下：
[0026] (1)有故障子系統(tǒng)的信息分配：
[0027] 有故障子系統(tǒng)的情況一般有以下三種：1)極端情況是3個(gè)子系統(tǒng)都出現(xiàn)故障，這種 情況下沒(méi)有位置信息輸出；2)當(dāng)2個(gè)子系統(tǒng)出現(xiàn)故障時(shí)，把剩下的子系統(tǒng)的融合結(jié)果作為系 統(tǒng)輸出；3)只有1個(gè)子系統(tǒng)有故障，根據(jù)不同的故障子系統(tǒng)的定位精度分別進(jìn)行討論；
[0028]第一、基于RSU的定位子系統(tǒng)出現(xiàn)故障，= 0，在開(kāi)闊區(qū)域，一般沒(méi)有部署或少量 部署無(wú)法提供定位功能，那么基于RSU定位子系統(tǒng)將會(huì)出現(xiàn)故障，系統(tǒng)只剩下GNSS和DR子系 統(tǒng)，構(gòu)成了 GNSS/DR的組合定位，信息分配系數(shù)將按照GNSS接收機(jī)給定的Η)0Ρ參數(shù)來(lái)確定， ro〇P通常用來(lái)衡量定位精度的好壞，取值范圍從0.5到99.9，系數(shù)如下述公式所示：
[0031]第二、GNSS出現(xiàn)故障，β2 = 0,當(dāng)車(chē)輛進(jìn)入到隧道或者在高樓密集的城市中心區(qū)域， 衛(wèi)星信號(hào)被遮擋，GNSS將會(huì)出現(xiàn)故障，一般情況下，在這些特殊區(qū)域部署RSU基站，為車(chē)輛提 供定位服務(wù)，此時(shí)RSU/DR工作，信息分配系數(shù)將按照車(chē)載DSRC設(shè)備計(jì)算的的定位精度因子 D0P的值來(lái)確定，信息分配系數(shù)如下述公式所示：
[0033] β3=1-βι
[0034] 第三、DR出現(xiàn)故障時(shí)，根據(jù)冗余硬件實(shí)現(xiàn)偽DR系統(tǒng)功能，1)車(chē)載里程儀的速度測(cè)量 出現(xiàn)異常，取車(chē)速計(jì)的速度值代替；2)陀螺儀的角速度的值出現(xiàn)故障，將采用GNSS或RSU位 置信息推算出來(lái)的航向角代替，采用偽DR可以短時(shí)間解決DR子系統(tǒng)出現(xiàn)故障的問(wèn)題，如果 DR子系統(tǒng)出現(xiàn)硬件故障，可以采用位置參照檢測(cè)法檢測(cè)，及早發(fā)現(xiàn)問(wèn)題，更換相應(yīng)電路模 塊。
[0035] 作為優(yōu)選的技術(shù)方案，當(dāng)無(wú)故障子系統(tǒng)的信息分配時(shí)：
[0036]不存在無(wú)故障子系統(tǒng)時(shí)，基于RSU的定位系統(tǒng)，GNSS和DR三個(gè)子系統(tǒng)都正常工作， 設(shè)計(jì)基于RSU的定位系統(tǒng)的出發(fā)點(diǎn)是為了解決衛(wèi)星定位信號(hào)被遮擋區(qū)域的車(chē)輛定位問(wèn)題， GNSS不能正常工作的區(qū)域才部署RSU，但是在GNSS正常工作區(qū)域和RSU定位系統(tǒng)工作區(qū)域之 間，存在GNSS和RSU定位系統(tǒng)兩個(gè)系統(tǒng)都能正常工作的區(qū)域，因此，信息分配系數(shù)根據(jù)子系 統(tǒng)定位的質(zhì)量，表現(xiàn)為GNSS的roop和RSU的定位精度因子，來(lái)調(diào)整&的值，一般情況下，如果 GNSS的PD0P小于等于2或RSU的D0P大于等于0.9時(shí)，將以這兩個(gè)系統(tǒng)的定位信息為主，分情 形討論；
[0037] 1)當(dāng)GNSS的 PD0P〈 = 2且RSU的 D0P〈0 · 9時(shí)，& = 0 · 99，βι = β3 =(卜&) /2;
[0038] 2)當(dāng)GNSS的 PD0P>2 且 RSU 的 D0P> = 0 · 9時(shí)，βι = D0P，β2 = fo = α _βι) /2;
[0041] 系統(tǒng)根據(jù)不同的情況可以得到合理的信息分配系數(shù)，組合定位系統(tǒng)自適應(yīng)子系統(tǒng) 定位精度的變化。
[0042] 本發(fā)明與現(xiàn)有技術(shù)相比，具有如下優(yōu)點(diǎn)和有益效果：
[0043]本發(fā)明利用聯(lián)邦卡爾曼濾波算法實(shí)現(xiàn)了基于RSU的定位子系統(tǒng)，GNSS定位子系統(tǒng) 和DR子系統(tǒng)的信息融合;利用殘差X2檢測(cè)法和冗余硬件檢測(cè)法構(gòu)建了兩級(jí)故障檢測(cè)方法， 能同時(shí)檢測(cè)出系統(tǒng)的硬故障和軟故障;根據(jù)子系統(tǒng)的定位質(zhì)量因子來(lái)動(dòng)態(tài)調(diào)整組合系統(tǒng)的 信息分配系數(shù)，使系統(tǒng)能夠適應(yīng)環(huán)境的變化。與其他組合定位系統(tǒng)具有如下優(yōu)勢(shì)：
[0044] (1)環(huán)境適應(yīng)能力較強(qiáng)，可靠性高。國(guó)內(nèi)大城市越來(lái)越繁華，越來(lái)越密集，衛(wèi)星定位 在這樣的環(huán)境下無(wú)法工作，組合定位算法可以適應(yīng)，加上可靠的故障檢測(cè)和自適應(yīng)的信息 分配系數(shù)調(diào)整方法，系統(tǒng)可靠性更高。
[0045] (2)具有車(chē)-車(chē)、車(chē)-路通信能力。在停車(chē)場(chǎng)、隧道內(nèi)也可以通信，彌補(bǔ)了3G/4G網(wǎng)絡(luò) 的缺陷。
【附圖說(shuō)明】
[0046]圖1是多模衛(wèi)星定位系統(tǒng)定位原理圖；
[0047]圖2是RSU定位原理與坐標(biāo)轉(zhuǎn)換；
[0048]圖3是RSU協(xié)同定位示意圖；
[0049]圖4是RSU/GNSS/DRS組合定位系統(tǒng)框架圖；
[0050]圖5是位置參照檢測(cè)示意圖；
[0051 ]圖6是多級(jí)故障檢測(cè)與信息分配流程圖；
[0052]圖7是測(cè)試場(chǎng)景示意圖；
[0053] 圖8(a)-圖8(b)是GNSS定位結(jié)果；
[0054]圖9(a)-圖9(b)是基于RSU的子系統(tǒng)定位結(jié)果；
[0055] 圖10(a)-圖10(b)是基于RSU/GNSS/DR組合定位結(jié)果與對(duì)比圖。
【具體實(shí)施方式】
[0056]下面結(jié)合實(shí)施例及附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)的描述，但本發(fā)明的實(shí)施方式不限 于此。
[0057] 實(shí)施例
[0058]本發(fā)明采用聯(lián)邦卡爾曼濾波(FKF)組合定位算法，實(shí)現(xiàn)車(chē)輛的高精度定位。首先介 紹三個(gè)子系統(tǒng)的定位原理;然后詳細(xì)討論RSU/GNSS/DR組合定位算法和兩級(jí)故障檢測(cè)方法， 并提出了自適應(yīng)信息分配系數(shù)動(dòng)態(tài)調(diào)整方法;最后利用跑車(chē)實(shí)驗(yàn)驗(yàn)證了所提出的組合定位 算法的可行性。
[0059] 1、車(chē)輛組合定位系統(tǒng)概述
[0060] GPS是建成最早也是應(yīng)用最廣泛的全球定位系統(tǒng)。BDS是我國(guó)自行建設(shè)的衛(wèi)星系 統(tǒng)，隨著新一代的導(dǎo)航衛(wèi)星的發(fā)射，標(biāo)志著B(niǎo)DS邁入了全球部署階段，GLONASS雖然建設(shè)早， 但是由于俄羅斯的財(cái)力，逐漸被BDS所超越。Galileo是最精確的衛(wèi)星定位系統(tǒng)，在設(shè)計(jì)初就 考慮與GPS的兼容問(wèn)題，但是進(jìn)展最慢。單一系統(tǒng)已經(jīng)不能滿(mǎn)足高精度的定位需求，支持多 個(gè)衛(wèi)星定位系統(tǒng)的多模衛(wèi)星接收機(jī)由于其高性?xún)r(jià)比逐漸成為GNSS的主流。多模GNSS定位精 度比單一衛(wèi)星定位系統(tǒng)有較大的提高，但是由于衛(wèi)星信號(hào)容易被遮擋，DR系統(tǒng)仍然是其重 要的輔助部分。有了 DR系統(tǒng)的輔助，GNSS/DR組合定位可以解決短時(shí)間內(nèi)衛(wèi)星信號(hào)被遮擋環(huán) 境下的定位，如果GNSS長(zhǎng)時(shí)間無(wú)法獲取自身位置而導(dǎo)致DR系統(tǒng)無(wú)法重置其初始位置，由于 誤差累積會(huì)導(dǎo)致系統(tǒng)總體定位精度大幅下降，如在香港的部分街區(qū)，系統(tǒng)長(zhǎng)時(shí)間捕捉不到 足夠的衛(wèi)星信號(hào)?；赗SU的地面定位系統(tǒng)在類(lèi)似這樣的環(huán)境下將可以為GNSS/DR系統(tǒng)提供 很好的補(bǔ)充，因此RSU/GNSS/DR組合定位應(yīng)運(yùn)而生。
[0061 ] 1.UGNSS 定位原理
[0062]隨著中國(guó)北斗系統(tǒng)逐步投入使用，BDS與GPS組合定位成為研究熱點(diǎn)。采用支持BDS 和GPS導(dǎo)航系統(tǒng)一體化S0C芯片，多系統(tǒng)的融合不是對(duì)單模定位結(jié)果做選擇，而是同時(shí)利用 多個(gè)系統(tǒng)的衛(wèi)星播發(fā)的信息，提取相關(guān)原始測(cè)量值，并將原始測(cè)量單元做數(shù)據(jù)融合從而解 算出定位位置，如圖1所示。因此多模工作模式不是定位結(jié)果的數(shù)據(jù)融合，而是多衛(wèi)星定位 系統(tǒng)的原始測(cè)量單元的數(shù)據(jù)融合過(guò)程?？偠灾嗄Ｐl(wèi)星定位是衛(wèi)星接收機(jī)發(fā)展的趨勢(shì)。 [0063] 1.2、航位推算原理
[0064]航位推算(DR)系統(tǒng)是車(chē)輛定位導(dǎo)航中采用的一種比較經(jīng)典的完全自主的導(dǎo)航系 統(tǒng)，在二維的空間上給車(chē)輛進(jìn)行定位。假定汽車(chē)的初始點(diǎn)(xq，y〇)和初始航向角θ〇，通過(guò)車(chē)載 里程儀、角速率陀螺實(shí)時(shí)測(cè)量車(chē)輛的行駛距離和航向角的變化，推算出車(chē)輛的位置。
[0065] 具體算法如下：
[0066] (U) ν, = vn + 4sm^
[0067] 車(chē)輛在tk時(shí)刻的位置同樣可以表示為：
[0069]其中，（XQ，yQ)是車(chē)輛的初始位置，di和0汾別是車(chē)輛從ti時(shí)刻到t i+1時(shí)刻的位移矢 量的長(zhǎng)度和絕對(duì)航向。假定采樣周期是恒定的(如1秒1次），那么公式(1.3)可以表示為：
[0071]從以上討論可知，航位推算是一個(gè)基于初始位置逐步累加的過(guò)程，需要實(shí)時(shí)得到 一段時(shí)間內(nèi)的距離變化量CU和航向角0,，還必須確定初始位置。因此，不同時(shí)刻的測(cè)量誤差 和計(jì)算誤差都會(huì)累積起來(lái)，需要周期性地重置初始位置?？傊?，單一的DR系統(tǒng)不適合用于長(zhǎng) 時(shí)間的定位。
[0072] 1.3、基于RSU的定位原理與RSU自定位
[0073]利用時(shí)間間隔測(cè)量模塊測(cè)量無(wú)線(xiàn)信號(hào)從車(chē)輛到RSU的飛行時(shí)間，再乘以無(wú)線(xiàn)電波 的傳播速度，可以得到車(chē)輛與RSU之間的距離。
[0075] 當(dāng)η彡3時(shí)，利用EKF就可以解算出車(chē)輛的坐標(biāo)，然后完成坐標(biāo)變換，如圖2所示。
[0076] 根據(jù)算法要求RSU的坐標(biāo)需提前確定，所以要解決RSU自身的定位問(wèn)題。如果RSU坐 標(biāo)不準(zhǔn)確，就無(wú)法為車(chē)輛提供位置參考。因此，部署RSU時(shí)，在RSU中存儲(chǔ)其自身的位置信息 (如經(jīng)煒度(:《α:))，利用經(jīng)煒度推算出UTM坐標(biāo)化九2〇1^，他11^)。1?1]設(shè)備帶有6吧3定位模塊 可以自定位，可以解決開(kāi)闊區(qū)域的RSU自定位問(wèn)題。如果部署在城市中心區(qū)或地下停車(chē)場(chǎng) 時(shí)，RSU的自身位置需要在部署時(shí)利用協(xié)同定位方法確定自身位置然后存儲(chǔ)在RSU中，如圖3 所示。
[0077]在地下停車(chē)場(chǎng)附近可以接收到GNS S信號(hào)的地方部署RSU5、RSU6、RSU7，然后利用 0SS-TWR測(cè)距算法和擴(kuò)展卡爾曼濾波算法，可以解算出RSU3的坐標(biāo)，最后再依次解算出其他 3個(gè)RSU的坐標(biāo)，經(jīng)過(guò)多次迭代，RSUh的坐標(biāo)將會(huì)更精確。
[0078] 1 · 4、FKF輔助的RSU/GNSS/DR組合定位算法
[0079] 面對(duì)復(fù)雜的城市環(huán)境，需要多種定位技術(shù)融合才能實(shí)現(xiàn)高精度、高可靠的車(chē)輛定 位。集中式卡爾曼濾波組合算法由于計(jì)算量大、故障分離難等問(wèn)題，在多維度的信息融合中 略有不足。本發(fā)明采用聯(lián)邦卡爾曼濾波算法，實(shí)現(xiàn)基于RSU的定位、衛(wèi)星定位、航位推算三者 的信息融合，為車(chē)輛提供準(zhǔn)確的車(chē)輛位置服務(wù)。基于RSU的定位系統(tǒng)采用線(xiàn)性卡爾曼濾波器 作為局部濾波器，記為L(zhǎng)F1，對(duì)應(yīng)的信息分配系數(shù)是i3 1;GNSS定位系統(tǒng)也采用線(xiàn)性卡爾曼濾 波器作為局部濾波器，記為L(zhǎng)F2,對(duì)應(yīng)的信息分配系數(shù)為fe;DR定位系統(tǒng)采用擴(kuò)展卡爾曼濾 波作為局部濾波器器，記為L(zhǎng)F3，對(duì)應(yīng)的信息分配系數(shù)為β 3;主濾波器做信息融合，所設(shè)計(jì)的 RSU/GNSS/DR組合定位系統(tǒng)框架如圖4所示。
[0080] 2.、系統(tǒng)狀態(tài)方程與觀(guān)測(cè)方程
[0081] 采用"當(dāng)前"統(tǒng)計(jì)模型來(lái)描述車(chē)輛加速度的統(tǒng)計(jì)分布，當(dāng)車(chē)輛正以某一加速度機(jī)動(dòng) 時(shí)，它在下一瞬時(shí)的加速度取值范圍是有限的，而且只能在當(dāng)前加速度的鄰域內(nèi)。根據(jù)上述 討論可知
[0083]其中，ai(t)為機(jī)動(dòng)加速度，其方差為σ2，Ι·(?)為機(jī)動(dòng)加速度"當(dāng)前"均值，其中a在每 一采樣周期內(nèi)為常數(shù)。a(t)為均值為零的有色加速度噪聲，w(t)是均值為零的高斯白噪聲， τ為機(jī)動(dòng)加速度變化率的相關(guān)時(shí)間常數(shù)。
[0084]取組合定位整體狀態(tài)變量為X = [ Xe，Ve，ae，Xn，Vn，an]T，其中Xe，Χη分別是車(chē)輛東向 和北向的位置分量(單位:m)，V(3，Vn分別是車(chē)輛東向和北向的速度分量(單位:!11/ 8)，~，&11分 別是車(chē)輛東向和北向的加速度分量(單位:m/s2)。系統(tǒng)的狀態(tài)方程寫(xiě)成矩陣形式為：
[0088] '，'是均值為0,方差分別為σ^，<的高斯白噪聲&分別是車(chē)輛東向 和北向機(jī)動(dòng)加速度變化率的相關(guān)時(shí)間常數(shù);％，瓦分別為車(chē)輛東向和北向機(jī)動(dòng)加速度分量 的"當(dāng)前"均值。
[0089] 設(shè)采樣周期為T(mén)，將系統(tǒng)連續(xù)的狀態(tài)方程離散化，得到離散的系統(tǒng)狀態(tài)方程為
[0090] X(k) = ?(k/k-l)X(k/k-l)+U(k)+ff(k) (1.8)
[0091 ]其中，X(k) = [xe(k)，ve(k)，ae(k)，xn(k)，v n(k)，an(k) ]τ [0092] Φ (k/k-1) =diag[ 0e(k/k~l), ?n(k/k-l)] (1.9)
[0097]其中，
[0101] (l)RSU觀(guān)測(cè)方程
[0102] 對(duì)于基于RSU的定位子系統(tǒng)，取系統(tǒng)狀態(tài)變量X1 = X，系統(tǒng)狀態(tài)方程與總體系統(tǒng)狀 態(tài)方程相同。其觀(guān)測(cè)量為子系統(tǒng)定位輸出的車(chē)輛東向和北向位置坐標(biāo)分量e〇bs，n〇b s (單位： m)，觀(guān)測(cè)方程可表示為：
[0103] Zi(k)=Hi(k)Xi(k)+Vi(k) (1.11)
[0104] 其中，
[0108] we(k)和wn(k)是均值為0,方差分別為af , 的高斯白噪聲序列，表示在RSU定位 設(shè)備在東向和北向的位置測(cè)量噪聲，量測(cè)噪聲協(xié)方差矩陣Λ(幻= , )。
[0109] (2)GNSS 觀(guān)測(cè)方程
[0110] 對(duì)于GNSS定位子系統(tǒng)與RSU定位系統(tǒng)類(lèi)似，取系統(tǒng)狀態(tài)變量X2 = X，與總體系統(tǒng)方 程相同。其觀(guān)測(cè)量為GNSS接收機(jī)輸出的車(chē)輛東向和北向位置坐標(biāo)分量6。^，11^，觀(guān)測(cè)方程可 表示為：
[0111] Z2(k)=H2(k)X2(k)+V2(k) (1.12)
[0112] 其中，
[0116] We3(kWPwn(k)是衛(wèi)星定位接收機(jī)在東向和北向的噪聲誤差，其均值為0,方差分別 為#，σ Μ2的高斯白噪聲序列，量測(cè)噪聲協(xié)方差矩陣：<)。
[0117] (3)DR觀(guān)測(cè)方程
[0118] 航位推算系統(tǒng)的狀態(tài)變量為X3 = X，子系統(tǒng)狀態(tài)方程與總體系統(tǒng)狀態(tài)方程相同，其 觀(guān)測(cè)量取角速度陀螺儀的輸出ω和里程表輸出的車(chē)輛在采用周期Τ時(shí)間內(nèi)行進(jìn)的距離s，即 系統(tǒng)的觀(guān)測(cè)方程可表示為：
[0119] Z3(t)=h3(t，X3(t))+V 3(t) (1.13)
[0120] 其中，
[0124] 假定標(biāo)定系數(shù)價(jià)為1，εω為陀螺的漂移，近似為(0, <)的高斯白噪聲，es為里程表輸 出量s的觀(guān)測(cè)噪聲，近似為(0, of)的高斯白噪聲，量測(cè)噪聲協(xié)方差矩陣：/?α)=&軌σΚ)。
[0125] 將觀(guān)測(cè)方程離散化，得到系統(tǒng)離散的觀(guān)測(cè)方程為
[0126] Z3(k)=h3[k，X3(k)]+V 3(k) (1.14)
[0127] 局部濾波器采用擴(kuò)展卡爾曼濾波，將h3[X3(k)]在Μ -I)附近展開(kāi)為泰勒級(jí)數(shù)， 忽略二次以上的高次項(xiàng)，可得觀(guān)測(cè)方程
[0137] 3.子系統(tǒng)的時(shí)間更新與量測(cè)更新
[0138] LF1和LF2子系統(tǒng)時(shí)間更新與量測(cè)更新方程
[0139] XAk / k -1) = Φ(Α: /k -\)Χ^ -1) + U{k -1) (1.16)
[0144] 狀態(tài)轉(zhuǎn)移矩陣:把加速度的一步預(yù)測(cè)看做"當(dāng)前"加速度的均值，即
[0145] 元⑷二次(人"人·-ft A⑷二太(人"々-I)
[0146] 公式(1.20)可以簡(jiǎn)化為
[0147] (k / = Φ(^ Ik-\)X\{k-1) (1.21)
[0148] 其中，Φ (k/k-1) =diag[ ΦΘ(Τ)，Φη(Τ)]，即
[0150]系統(tǒng)噪聲協(xié)方差矩陣:Q(k_l)為系統(tǒng)噪聲協(xié)方差矩陣Q的離散化矩陣
[0153]由于采用了擴(kuò)展卡爾曼濾波作為L(zhǎng)F3的局部濾波器，其推導(dǎo)方程與LF1和LF2略有 不同，如公式（1.22)所示，其中，系統(tǒng)噪聲協(xié)方差矩陣Q(k-l)和狀態(tài)轉(zhuǎn)移矩陣?(k/k-l)與 LF1和LF2相同。
[0155] 4、全局信息融合與信息分配
[0156] 全局信息融合：
[0158] 信息分配：
[0160] 其中，&+β2+β3 = 1。
[0161] 5、兩級(jí)故障檢測(cè)與信息分配新方法
[0162] 車(chē)聯(lián)網(wǎng)應(yīng)用對(duì)定位的容錯(cuò)性和可靠性提出了更高的要求，故障檢測(cè)與故障隔離是 提高系統(tǒng)可靠性的有效途徑。殘差X2檢驗(yàn)法被認(rèn)為是比較可靠的檢測(cè)方法，在檢測(cè)系統(tǒng)硬 故障具有靈敏度高，可靠性好的特點(diǎn)，但是對(duì)軟故障檢測(cè)的有效性下降。
[0163] 超大集成電路技術(shù)的進(jìn)步和成本的下降，為組合定位系統(tǒng)的冗余硬件容錯(cuò)設(shè)計(jì)提 供了可能性，本系統(tǒng)增加一個(gè)加速度計(jì)AD)(L345測(cè)量車(chē)輛速度，對(duì)系統(tǒng)的故障進(jìn)行檢測(cè)。殘 差X 2故障檢測(cè)和冗余硬件故障檢測(cè)為組合定位算法構(gòu)建了一個(gè)兩級(jí)故障檢測(cè)新方法。
[0164] 另外，現(xiàn)有的車(chē)載定位設(shè)備暫時(shí)還沒(méi)有方法去檢測(cè)和校正自身設(shè)備的定位準(zhǔn)確 性，基于V2R通信，本發(fā)明提出了一個(gè)操作簡(jiǎn)單的基于位置參照的檢測(cè)方法，用于校驗(yàn)車(chē)載 設(shè)備GNSS定位精度和基于RSU的定位精度，有利于及早發(fā)現(xiàn)車(chē)載定位設(shè)備本身的問(wèn)題。
[0165] 組合定位系統(tǒng)利用兩級(jí)故障檢測(cè)技術(shù)，可以把系統(tǒng)軟硬故障都檢測(cè)出來(lái)。系統(tǒng)在 信息融合時(shí)，合理調(diào)整各子系統(tǒng)的信息分配系數(shù)，實(shí)現(xiàn)故障子系統(tǒng)的隔離與無(wú)故障子系統(tǒng) 的重構(gòu)。本發(fā)明提出了一種新的自適應(yīng)系數(shù)調(diào)整方法，構(gòu)建具有高容錯(cuò)性的聯(lián)邦組合定位 算法。
[0166] 5.1、兩級(jí)故障檢測(cè)方法
[0167] 本系統(tǒng)采用兩級(jí)故障檢測(cè)方法，首先各子系統(tǒng)利用殘差X2檢測(cè)法去檢測(cè)出現(xiàn)硬故 障的子系統(tǒng)，然后利用冗余硬件檢測(cè)法去檢測(cè)系統(tǒng)軟故障。另外，為了能校驗(yàn)車(chē)載定位系統(tǒng) 本身的精確度，本發(fā)明提出了基于V2R通信的位置參照檢測(cè)法，讓系統(tǒng)可以及時(shí)發(fā)現(xiàn)因車(chē)載 設(shè)備自身問(wèn)題而導(dǎo)致定位不準(zhǔn)確的原因。
[0168] (1)殘差義2檢測(cè)法
[0169] 由觀(guān)測(cè)值的預(yù)測(cè)值與實(shí)測(cè)值Zk之差構(gòu)造檢驗(yàn)統(tǒng)計(jì)量的方法稱(chēng)為殘差X2檢驗(yàn) 法。殘差定義為：
[0170] r{k) = Z, -tklk^ = Zk (1.25)
[0171] 當(dāng)被檢測(cè)系統(tǒng)無(wú)故障時(shí)，殘差r(k)是均值為零的高斯白噪聲，當(dāng)被檢測(cè)系統(tǒng)發(fā)生 故障時(shí)，r(k)的期望是不為零的值。根據(jù)這個(gè)特性，通過(guò)檢測(cè)殘差r(k)均值就可以確定被檢 測(cè)系統(tǒng)是否發(fā)生了故障。由公式（1.25)可知，殘差向量r(k)也可以被認(rèn)為均值為零的高斯 白噪聲過(guò)程，其協(xié)方差S(k)為：
[0172] S(k)^HkPk.k ,//； +Rk ( 1.26 )
[0173] 構(gòu)造以下故障檢測(cè)函數(shù)：
[0174] D(k)=rT(k)S-Hk)r(k) (1.27)
[0175] 由殘差序列的統(tǒng)計(jì)特性知，D(k)服從自由度為η(狀態(tài)向量X的維數(shù)）的x2分布，如 果設(shè)定故障檢測(cè)閥值Td，那么可以根據(jù)公式（1.32)檢測(cè)系統(tǒng)故障，閥值T D決定了故障檢測(cè)的 性能。
[0177] (2)冗余硬件檢測(cè)法
[0178] 采用低成本冗余硬件檢測(cè)，通過(guò)增加ADXL345 (市場(chǎng)價(jià)格7元人民幣，對(duì)總體成本影 響很小)對(duì)比各子系統(tǒng)所測(cè)量的車(chē)輛速度的一致性來(lái)實(shí)現(xiàn)故障的檢測(cè)與隔離。本系統(tǒng)中里 程儀、加速度計(jì)、基于RSU的定位系統(tǒng)、衛(wèi)星導(dǎo)航這四個(gè)系統(tǒng)可以測(cè)量一個(gè)周期內(nèi)的車(chē)輛速 度，按照少數(shù)服從多數(shù)的原則來(lái)實(shí)現(xiàn)檢測(cè)與隔離。冗余硬件檢測(cè)法可以有效解決軟故障檢 測(cè)問(wèn)題。
[0179] (3)位置參照檢測(cè)法
[0180]傳統(tǒng)車(chē)載定位通常接收衛(wèi)星信號(hào)定位或自主定位，是一個(gè)孤立的系統(tǒng)。V2R通信的 發(fā)展，在某些特定區(qū)域，車(chē)輛可以通過(guò)后臺(tái)查詢(xún)到自己所處的位置，如圖5所示。
[0181 ]在停車(chē)位，車(chē)輛通過(guò)與RSU (咪表)設(shè)備通信自動(dòng)完成停車(chē)交費(fèi)，在這個(gè)過(guò)程中，RSU 可以向車(chē)輛發(fā)送本車(chē)位的精確位置信息，車(chē)輛利用這個(gè)信息與車(chē)載定位系統(tǒng)提供的位置信 息的一致性來(lái)判斷車(chē)載定位系統(tǒng)是否準(zhǔn)確。新帶來(lái)的問(wèn)題就是現(xiàn)有的停車(chē)位位置不可知。 事實(shí)上，隨著I〇V的不斷推進(jìn)，全市的停車(chē)位會(huì)納入城市智能停車(chē)場(chǎng)管理系統(tǒng)進(jìn)行統(tǒng)一管理 和查詢(xún)，任何一個(gè)停車(chē)位都有精確的位置信息，并且清楚顯示在GIS地圖上。
[0182] 綜上所述，由于車(chē)載里程儀、加速度計(jì)等檢測(cè)設(shè)備性能下降或外部環(huán)境如溫濕度 的影響，子系統(tǒng)的可能發(fā)生故障，這些故障既有突變的硬故障也有緩慢變化的軟故障。殘差 X 2檢驗(yàn)法由于計(jì)算相對(duì)簡(jiǎn)單，對(duì)子系統(tǒng)硬故障的檢驗(yàn)非常有效，因此，在組合導(dǎo)航系統(tǒng)中應(yīng) 用廣泛?；谟布哂嗟臋z測(cè)方法，可以有效地檢測(cè)出系統(tǒng)的軟故障，但是會(huì)增加系統(tǒng)的成 本。采用成本低的硬件進(jìn)行部分冗余，可以兼顧成本和性能，提高系統(tǒng)可靠性。利用位置信 息已知的停車(chē)位給車(chē)輛定位系統(tǒng)提供檢測(cè)參照，是車(chē)聯(lián)網(wǎng)發(fā)展到現(xiàn)在的附加功能，它可以 檢測(cè)出定位系統(tǒng)中存在的一些利用自身無(wú)法確定的故障，相對(duì)前面兩種來(lái)說(shuō)，檢測(cè)結(jié)果更 客觀(guān)。檢測(cè)結(jié)果雖然無(wú)法直接用于組合定位算法中，如參與信息分配，但是可以用于定位設(shè) 備維護(hù)，或給DR子系統(tǒng)設(shè)置初始位置等。
[0183] 5.2、自適應(yīng)的信息分配系數(shù)調(diào)整方法
[0184] 最早Carlson提出的固定比例的信息分配原則，考慮到復(fù)雜城市環(huán)境中各個(gè)子濾 波器性能和位置估計(jì)準(zhǔn)確度在不斷地變化，因此引入動(dòng)態(tài)自適應(yīng)信息分配的方法，以各子 系統(tǒng)的定位精度為主要參考，動(dòng)態(tài)調(diào)整信息分配系數(shù)隊(duì)，改變聯(lián)邦濾波器的性能，實(shí)現(xiàn)組合 導(dǎo)航，主要工作流程如圖6所示。
[0185] 三個(gè)子系統(tǒng)在設(shè)定初始參數(shù)后，經(jīng)過(guò)時(shí)間與量測(cè)更新后，得到了進(jìn)一步預(yù)測(cè)值，根 據(jù)殘差X2檢驗(yàn)算法對(duì)各子系統(tǒng)進(jìn)行第一級(jí)硬故障檢測(cè)。剩余無(wú)故障子系統(tǒng)再使用冗余硬件 故障檢測(cè)法，對(duì)比找出有故障的系統(tǒng)進(jìn)行故障隔離。對(duì)于余下的無(wú)故障子系統(tǒng)，聯(lián)邦卡爾曼 濾波組合算法根據(jù)子系統(tǒng)定位精度因子動(dòng)態(tài)調(diào)整信息系分配數(shù)，具體如下：
[0186] (1)有故障子系統(tǒng)的信息分配
[0187] 有故障子系統(tǒng)的情況一般有以下三種：1)極端情況是3個(gè)子系統(tǒng)都出現(xiàn)故障，這種 情況下沒(méi)有位置信息輸出；2)當(dāng)2個(gè)子系統(tǒng)出現(xiàn)故障時(shí)，把剩下的子系統(tǒng)的融合結(jié)果作為系 統(tǒng)輸出；3)只有1個(gè)子系統(tǒng)有故障，根據(jù)不同的故障子系統(tǒng)的定位精度分別進(jìn)行討論。
[0188] 第一、基于RSU的定位子系統(tǒng)出現(xiàn)故障(fo = 0)。在開(kāi)闊區(qū)域，一般沒(méi)有部署或少量 部署無(wú)法提供定位功能，那么基于RSU定位子系統(tǒng)將會(huì)出現(xiàn)故障。系統(tǒng)只剩下GNSS和DR子系 統(tǒng)，構(gòu)成了 GNSS/DR的組合定位，信息分配系數(shù)將按照GNSS接收機(jī)給定的Η)ΟΡ參數(shù)來(lái)確定， ro〇P通常用來(lái)衡量定位精度的好壞，取值范圍從0.5到99.9，系數(shù)如公式（1.29)所示。
[0190] β3=1-β2
[0191] 第二、GNSS出現(xiàn)故障（β2 = 0)。當(dāng)車(chē)輛進(jìn)入到隧道或者在高樓密集的城市中心區(qū) 域，衛(wèi)星信號(hào)被遮擋，GNSS將會(huì)出現(xiàn)故障。一般情況下，在這些特殊區(qū)域部署RSU基站，為車(chē) 輛提供定位服務(wù)，此時(shí)RSU/DR工作。信息分配系數(shù)將按照車(chē)載DSRC設(shè)備計(jì)算的的定位精度 因子(D0P)的值來(lái)確定，信息分配系數(shù)如公式(1.30)所示。
[0193] β3=1-βι
[0194] 第三、DR出現(xiàn)故障時(shí)，根據(jù)冗余硬件實(shí)現(xiàn)偽DR系統(tǒng)功能。1)車(chē)載里程儀的速度測(cè)量 出現(xiàn)異常，取車(chē)速計(jì)的速度值代替；2)陀螺儀的角速度的值出現(xiàn)故障，將采用GNSS或RSU位 置信息推算出來(lái)的航向角代替，采用偽DR可以短時(shí)間解決DR子系統(tǒng)出現(xiàn)故障的問(wèn)題。如果 DR子系統(tǒng)出現(xiàn)硬件故障，可以采用位置參照檢測(cè)法檢測(cè)，及早發(fā)現(xiàn)問(wèn)題，更換相應(yīng)電路模 塊。
[0195] (2)無(wú)故障子系統(tǒng)的信息分配
[0196] 不存在無(wú)故障子系統(tǒng)時(shí)，基于RSU的定位系統(tǒng)，GNSS和DR三個(gè)子系統(tǒng)都正常工作。 設(shè)計(jì)基于RSU的定位系統(tǒng)的出發(fā)點(diǎn)是為了解決衛(wèi)星定位信號(hào)被遮擋區(qū)域的車(chē)輛定位問(wèn)題， GNSS不能正常工作的區(qū)域才部署RSU，但是在GNSS正常工作區(qū)域和RSU定位系統(tǒng)工作區(qū)域之 間，存在GNSS和RSU定位系統(tǒng)兩個(gè)系統(tǒng)都能正常工作的區(qū)域。因此，信息分配系數(shù)根據(jù)子系 統(tǒng)定位的質(zhì)量，表現(xiàn)為GNSS的roop和RSU的定位精度因子，來(lái)調(diào)整&的值。一般情況下，如果 GNSS的PD0P小于等于2或RSU的D0P大于等于0.9時(shí)，將以這兩個(gè)系統(tǒng)的定位信息為主，分情 形討論。
[0197] 1)當(dāng)GNSS的 PD0P〈 = 2且RSU的 D0P〈0 · 9時(shí)，& = 0 · 99，βι = β3 =(卜&) /2;
[0198] 2)當(dāng)GNSS的 PD0P>2 且 RSU 的 D0P> = 0 · 9時(shí)，βι = D0P，β2 = fo = α _βι) /2;
[0199] 3)其他情況，爲(wèi)=/</2,爲(wèi)二戽/2
[0201] 綜上所述，系統(tǒng)根據(jù)不同的情況可以得到合理的信息分配系數(shù)，組合定位系統(tǒng)自 適應(yīng)子系統(tǒng)定位精度的變化，提高了系統(tǒng)的可靠性。
[0202] 6、實(shí)驗(yàn)與結(jié)果分析
[0203] 車(chē)聯(lián)網(wǎng)基于位置服務(wù)的應(yīng)用很多，除了傳統(tǒng)的車(chē)輛導(dǎo)航服務(wù)外，還出現(xiàn)了很多新 的應(yīng)用：市區(qū)車(chē)位預(yù)約、自動(dòng)尋位泊車(chē)、廣告推送、特種車(chē)輛實(shí)時(shí)監(jiān)控。以特種車(chē)輛實(shí)時(shí)監(jiān)控 為例，傳統(tǒng)方法只依靠GPS接收機(jī)來(lái)獲取車(chē)輛位置，然后通過(guò)3G/4G網(wǎng)絡(luò)把車(chē)輛的位置(包含 車(chē)輛狀態(tài)信息)發(fā)送到監(jiān)控中心，監(jiān)控端通過(guò)GIS系統(tǒng)輔助可以對(duì)車(chē)輛進(jìn)行實(shí)時(shí)跟蹤及軌跡 回放。但是弊端是進(jìn)入城市中心區(qū)域或稍長(zhǎng)的隧道，就無(wú)法對(duì)車(chē)輛進(jìn)行定位?；赗SU/ GNSS/DR組合定位算法的定位解決方案，可以在城市中不同的環(huán)境中為車(chē)輛提供定位服務(wù)， 還可以通過(guò)V2R通信把車(chē)輛當(dāng)前的狀態(tài)實(shí)時(shí)傳送到監(jiān)控端。
[0204]如圖7所示，區(qū)域1沒(méi)有部署RSU，在區(qū)域1中主要測(cè)試多模GNSS子系統(tǒng)在開(kāi)闊區(qū)域 的定位精度，在區(qū)域2部署了 7個(gè)RSU，測(cè)試在GNSS失效的情況下，RSU/DR定位的效果，區(qū)域3 也沒(méi)有部署RSU，測(cè)試在樓群之間組合定位RSU子系統(tǒng)出現(xiàn)故障時(shí)GNSS/DR的定位效果。 [0205] 6.1、車(chē)輛定位及結(jié)果分析
[0206]根據(jù)預(yù)設(shè)路線(xiàn)，按照GNSS單一系統(tǒng)定位測(cè)試、基于RSU的定位系統(tǒng)的定位測(cè)試， RSU/GNSS/DR組合系統(tǒng)的定位測(cè)試的次序，分別進(jìn)行跑車(chē)實(shí)驗(yàn)，跑車(chē)測(cè)試流程如下：
[0207]設(shè)備初始化化：包括設(shè)置多模GNSS接收機(jī)串口號(hào)（C0M6)與波特率（4800 )，設(shè)置 GNSS數(shù)據(jù)存儲(chǔ)目錄，設(shè)置基于RSU定位子系統(tǒng)車(chē)載DSRC接收機(jī)信道(CH182)與數(shù)據(jù)存儲(chǔ)目 錄，設(shè)置DR子系統(tǒng)的初始位置。
[0208]時(shí)間校準(zhǔn):校準(zhǔn)GNSS與DSRC接收機(jī)的時(shí)間基準(zhǔn)，使兩者時(shí)間保持同步；
[0209]定位數(shù)據(jù)獲取:被測(cè)試車(chē)輛以20公里每小時(shí)的速度按土黃色軌跡勻速行駛，按次 序進(jìn)行車(chē)輛定位測(cè)試，獲取定位數(shù)據(jù)。
[0210]坐標(biāo)變換:GNSS輸出是經(jīng)煒度坐標(biāo)格式，DSRC定位數(shù)據(jù)是UTM坐標(biāo)格式。由于子系 統(tǒng)的坐標(biāo)標(biāo)準(zhǔn)不同，為了更好的解算出車(chē)輛坐標(biāo)，需要實(shí)現(xiàn)坐標(biāo)變換。
[0211]信息融合:使用信息融合算法，利用三個(gè)定位子系統(tǒng)的定位輸出，解算出車(chē)輛坐 標(biāo)。
[0212] 軌跡顯示:把定位系統(tǒng)輸出的數(shù)據(jù)生成KML文件，然后在Google Earth顯示出車(chē)輛 行駛軌跡。
[0213] 根據(jù)以上流程進(jìn)行跑車(chē)實(shí)驗(yàn)，測(cè)試結(jié)果如下：
[0214] (l)GNSS定位結(jié)果與分析
[0215] 采用多模衛(wèi)星定位芯片XN647-8，可以同時(shí)支持BDS和GPS，在開(kāi)闊區(qū)域可以捕捉到 8-9顆定位衛(wèi)星，根據(jù)定位模塊的輸出數(shù)據(jù)可知，PD0P在開(kāi)闊區(qū)域基本保持在2左右，定位精 度相對(duì)較高。根據(jù)輸出數(shù)據(jù)統(tǒng)計(jì)分析，在區(qū)域1，GNSS定位精度（以圓徑向誤差(2DRMS)來(lái)衡 量)達(dá)到在2米，如圖8(a)-圖8(b)所示。
[0216]當(dāng)車(chē)輛進(jìn)入到區(qū)域2時(shí)，由于濾波算法的作用，開(kāi)始十米左右定位還比較正常，但 是進(jìn)入隧道之后，定位精度驟然下降，從點(diǎn)A到點(diǎn)B這個(gè)區(qū)間，沒(méi)有定位結(jié)果輸出，衛(wèi)星信號(hào) 被完全遮擋。
[0217]當(dāng)車(chē)輛進(jìn)入到區(qū)域3后，由于樓群的影響，從點(diǎn)C到點(diǎn)D，GNSS接收機(jī)雖然有車(chē)輛位 置信息輸出，但是定位精度不高。GNSS接收機(jī)輸出的H)0D基本上都大于5，定位精度下降。 [0218] (2)基于RSU的定位結(jié)果與分析
[0219] 區(qū)域2部署了 7個(gè)RSU，為了不影響區(qū)域1和區(qū)域3個(gè)定位測(cè)試，在測(cè)試過(guò)程中調(diào)整了 RSU的發(fā)射功率，使其信號(hào)作用范圍限定在區(qū)域2。車(chē)輛從點(diǎn)E行駛到點(diǎn)F，定位精度(2DRMS) 達(dá)到2.5米。其他區(qū)域由于沒(méi)有信號(hào)，因此無(wú)法定位。測(cè)試結(jié)果如圖9(a)-圖9(b)所示。
[0220] (3)RSU/GNSS/DR定位結(jié)果與分析，如圖10(a)-圖10(b)所示：
[0221] 從圖10(b)可以知道組合定位在區(qū)域1與GNSS定位大致相同，差異較小。但是進(jìn)入 到區(qū)域2之后，GNSS子系統(tǒng)被判定為故障子系統(tǒng)，從組合定位系統(tǒng)隔離，在RSU/DR作用下，取 得了較好的定位結(jié)果。對(duì)比圖9(b)，在區(qū)域2中，有DR子系統(tǒng)參與定位后，定位精度有一定的 提高，波動(dòng)幅度減小。在區(qū)域3，從點(diǎn)C到點(diǎn)D超出了 RSU通信網(wǎng)絡(luò)的覆蓋范圍，RSU子系統(tǒng)被判 定為故障子系統(tǒng)，GNSS/DR作用下，由于DR的初始位置比較精確，前半段定位精度較高，但由 于DR系統(tǒng)的誤差累積，后半段被故障檢測(cè)算法判定為故障系統(tǒng)且被隔離，在這種情況下，只 有GNSS子系統(tǒng)正常工作。
[0222] 從以上定位結(jié)果可以看出，組合定位系統(tǒng)達(dá)到了設(shè)計(jì)的目的，滿(mǎn)足了車(chē)輛在復(fù)雜 城市環(huán)境下的定位需求，定位精度已經(jīng)達(dá)到道路級(jí)的精度要求。
[0223] 上述實(shí)施例為本發(fā)明較佳的實(shí)施方式，但本發(fā)明的實(shí)施方式并不受上述實(shí)施例的 限制，其他的任何未背離本發(fā)明的精神實(shí)質(zhì)與原理下所作的改變、修飾、替代、組合、簡(jiǎn)化， 均應(yīng)為等效的置換方式，都包含在本發(fā)明的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種新型車(chē)輛組合定位算法，其特征在于，采用聯(lián)邦卡爾曼濾波算法，實(shí)現(xiàn)RSU定位、 GNSS定位、DR定位三個(gè)定位子系統(tǒng)的信息融合，即采用線(xiàn)性卡爾曼濾波器作為RSU定位子系 統(tǒng)的局部濾波器，記為L(zhǎng)F1，對(duì)應(yīng)的信息分配系數(shù)是i3 1;GNSS定位子系統(tǒng)也采用線(xiàn)性卡爾曼 濾波器作為局部濾波器，記為L(zhǎng)F2,對(duì)應(yīng)的信息分配系數(shù)為fe;DR定位系統(tǒng)采用擴(kuò)展卡爾曼 濾波作為局部濾波器，記為L(zhǎng)F3，對(duì)應(yīng)的信息分配系數(shù)為β 3，主濾波器做信息融合，具體包括 下述步驟： 51、 根據(jù)汽車(chē)運(yùn)動(dòng)學(xué)建立系統(tǒng)狀態(tài)方程與觀(guān)測(cè)方程，所述觀(guān)測(cè)方程包括RSU觀(guān)測(cè)方程、 GNSS觀(guān)測(cè)方程以及DR觀(guān)測(cè)方程； 52、 根據(jù)LF1、LF2和LF3對(duì)三個(gè)子系統(tǒng)進(jìn)行時(shí)間更新和量測(cè)更新； 53、 在信息融合過(guò)程中進(jìn)行故障檢測(cè)，利用兩級(jí)故障檢測(cè)技術(shù)，檢測(cè)出故障子系統(tǒng)并實(shí) 現(xiàn)隔離； 54、 信息融合與信息分配，系統(tǒng)在信息分配過(guò)程中，根據(jù)定位子系統(tǒng)的定位精度因子來(lái) 動(dòng)態(tài)調(diào)整各子系統(tǒng)的信息分配系數(shù)，實(shí)現(xiàn)故障子系統(tǒng)的隔離與無(wú)故障子系統(tǒng)的重構(gòu)。2. 根據(jù)權(quán)利要求1所述的新型車(chē)輛組合定位算法，其特征在于，在步驟S4中，所述兩級(jí) 故障檢測(cè)的方法:首先各子系統(tǒng)利用殘差X 2檢測(cè)法去檢測(cè)出現(xiàn)硬故障的子系統(tǒng)，然后利用 冗余硬件檢測(cè)法去檢測(cè)系統(tǒng)軟故障，另外，為了能校驗(yàn)車(chē)載定位系統(tǒng)本身的精確度，還提出 了基于車(chē)輛與路側(cè)設(shè)備通信V2R的位置參照檢測(cè)法，讓系統(tǒng)可以及時(shí)發(fā)現(xiàn)因車(chē)載設(shè)備自身 問(wèn)題而導(dǎo)致定位不準(zhǔn)確的原因。3. 根據(jù)權(quán)利要求2所述的新型車(chē)輛組合定位算法，其特征在于，所述殘差X2檢測(cè)法具體 如下： 由觀(guān)測(cè)值的預(yù)測(cè)值與實(shí)測(cè)值Zk之差構(gòu)造檢驗(yàn)統(tǒng)計(jì)量的方法稱(chēng)為殘差X2檢驗(yàn)法，殘 差定義為：當(dāng)被檢測(cè)系統(tǒng)無(wú)故障時(shí)，殘差r(k)是均值為零的高斯白噪聲，當(dāng)被檢測(cè)系統(tǒng)發(fā)生故障 時(shí)，r(k)的期望是不為零的值，根據(jù)這個(gè)特性，通過(guò)檢測(cè)殘差r(k)均值就可以確定被檢測(cè)系 統(tǒng)是否發(fā)生了故障，由公式4々）=Ζ& -= ZA 可知，殘差向量r(k)也可以被認(rèn) 為均值為零的高斯白噪聲過(guò)程，其協(xié)方差S(k)為：= 構(gòu)造以下故障檢測(cè)函數(shù)： D(k)=rT(k)S_1(k)r(k) 由殘差序列的統(tǒng)計(jì)特性知，D(k)服從自由度為η的X2分布，η為狀態(tài)向量X的維數(shù)，如果設(shè) 定故障檢測(cè)閥值TD，那么可以根據(jù)下述公式檢測(cè)系統(tǒng)故障，閥值TD決定了故障檢測(cè)的性能：4. 根據(jù)權(quán)利要求2所述的新型車(chē)輛組合定位算法，其特征在于，所述冗余硬件檢測(cè)法具 體為： 采用低成本冗余硬件檢測(cè)，通過(guò)增加加速度計(jì)對(duì)比各子系統(tǒng)所測(cè)量的車(chē)輛速度的一致 性來(lái)實(shí)現(xiàn)故障的檢測(cè)與隔離。5. 根據(jù)權(quán)利要求2所述的新型車(chē)輛組合定位算法，其特征在于，所述位置參照檢測(cè)法具 體為： 傳統(tǒng)車(chē)載定位通常接收衛(wèi)星信號(hào)定位或自主定位，是一個(gè)孤立的系統(tǒng)，V2R通信的發(fā) 展，在某些特定區(qū)域，如停車(chē)場(chǎng)，車(chē)輛可以通過(guò)后臺(tái)數(shù)據(jù)庫(kù)查詢(xún)到自己所處的精確位置坐 標(biāo)，利用所查詢(xún)到的坐標(biāo)信息與實(shí)時(shí)定位的坐標(biāo)相對(duì)比，以此來(lái)判斷車(chē)輛定位系統(tǒng)的準(zhǔn)確 性。6. 根據(jù)權(quán)利要求1所述的新型車(chē)輛組合定位算法，其特征在于，在步驟S4中，自適應(yīng)的 信息分配系數(shù)調(diào)整方法以各子系統(tǒng)的定位精度因子為主要參考，動(dòng)態(tài)調(diào)整信息分配系數(shù)， 提高聯(lián)邦濾波器的性能，實(shí)現(xiàn)組合導(dǎo)航。7. 根據(jù)權(quán)利要求1所述的新型車(chē)輛組合定位算法，其特征在于，三個(gè)子系統(tǒng)在設(shè)定初始 參數(shù)后，經(jīng)過(guò)時(shí)間與量測(cè)更新后，得到了進(jìn)一步預(yù)測(cè)值，根據(jù)殘差X 2檢驗(yàn)算法對(duì)各子系統(tǒng)進(jìn) 行第一級(jí)硬故障檢測(cè)，剩余無(wú)故障子系統(tǒng)再使用冗余硬件故障檢測(cè)法，對(duì)比找出有故障的 系統(tǒng)進(jìn)行故障隔離，對(duì)于余下的無(wú)故障子系統(tǒng)，聯(lián)邦卡爾曼濾波組合算法根據(jù)子系統(tǒng)定位 精度因子動(dòng)態(tài)調(diào)整信息系分配數(shù)，具體如下： (1)有故障子系統(tǒng)的信息分配： 有故障子系統(tǒng)的情況一般有以下三種：1)極端情況是3個(gè)子系統(tǒng)都出現(xiàn)故障，這種情況 下沒(méi)有位置信息輸出；2)當(dāng)2個(gè)子系統(tǒng)出現(xiàn)故障時(shí)，把剩下的子系統(tǒng)的融合結(jié)果作為系統(tǒng)輸 出；3)只有1個(gè)子系統(tǒng)有故障，根據(jù)不同的故障子系統(tǒng)的定位精度分別進(jìn)行討論； 第一、基于RSU的定位子系統(tǒng)出現(xiàn)故障，fo = 0,在開(kāi)闊區(qū)域，一般沒(méi)有部署或少量部署無(wú) 法提供定位功能，那么基于RSU定位子系統(tǒng)將會(huì)出現(xiàn)故障，系統(tǒng)只剩下GNSS和DR子系統(tǒng)，構(gòu) 成了 GNSS/DR的組合定位，信息分配系數(shù)將按照GNSS接收機(jī)給定的H)OP參數(shù)來(lái)確定，PDOP通 常用來(lái)衡量定位精度的好壞，取值范圍從〇. 5到99.9，系數(shù)如下述公式所示：第二、GNSS出現(xiàn)故障，β2 = 0,當(dāng)車(chē)輛進(jìn)入到隧道或者在高樓密集的城市中心區(qū)域，衛(wèi)星 信號(hào)被遮擋，GNSS將會(huì)出現(xiàn)故障，一般情況下，在這些特殊區(qū)域部署RSU基站，為車(chē)輛提供定 位服務(wù)，此時(shí)RSU/DR工作，信息分配系數(shù)將按照車(chē)載DSRC設(shè)備計(jì)算的的定位精度因子D0P的 值來(lái)確定，信息分配系數(shù)如下述公式所示：第三、DR出現(xiàn)故障時(shí)，根據(jù)冗余硬件實(shí)現(xiàn)偽DR系統(tǒng)功能，1)車(chē)載里程儀的速度測(cè)量出現(xiàn) 異常，取車(chē)速計(jì)的速度值代替;2)陀螺儀的角速度的值出現(xiàn)故障，將采用GNSS或RSU位置信 息推算出來(lái)的航向角代替，采用偽DR可以短時(shí)間解決DR子系統(tǒng)出現(xiàn)故障的問(wèn)題，如果DR子 系統(tǒng)出現(xiàn)硬件故障，可以采用位置參照檢測(cè)法檢測(cè)，及早發(fā)現(xiàn)問(wèn)題，更換相應(yīng)電路模塊。8.根據(jù)權(quán)利要求1所述的新型車(chē)輛組合定位算法，其特征在于，當(dāng)無(wú)故障子系統(tǒng)的信息 分配時(shí)： 不存在無(wú)故障子系統(tǒng)時(shí)，基于RSU的定位系統(tǒng)，GNSS和DR三個(gè)子系統(tǒng)都正常工作，設(shè)計(jì) 基于RSU的定位系統(tǒng)的出發(fā)點(diǎn)是為了解決衛(wèi)星定位信號(hào)被遮擋區(qū)域的車(chē)輛定位問(wèn)題，GNSS 不能正常工作的區(qū)域才部署RSU，但是在GNSS正常工作區(qū)域和RSU定位系統(tǒng)工作區(qū)域之間， 存在GNSS和RSU定位系統(tǒng)兩個(gè)系統(tǒng)都能正常工作的區(qū)域，因此，信息分配系數(shù)根據(jù)子系統(tǒng)定 位的質(zhì)量，表現(xiàn)為GNSS的H)OP和RSU的定位精度因子，來(lái)調(diào)整隊(duì)的值，一般情況下，如果GNSS 的PD0P小于等于2或RSU的D0P大于等于0.9時(shí)，將以這兩個(gè)系統(tǒng)的定位信息為主，分情形討 論； 1) 當(dāng)GNSS的PD0P〈 = 2且RSU的D0P〈0 · 9時(shí)，β2 = 0 · 99，βι = β3 = (1_&) /2; 2) 當(dāng)GNSS的PD0P>2 且 RSU的D0P> = 0 · 9時(shí)，βι = DOP，β2 = fo = (1_βι) /2; 3) 其他情況，々=片 / 2. /J2 = /J; / 2 ,β3 = 1-^-022; 其中：系統(tǒng)根據(jù)不同的情況可以得到合理的信息分配系數(shù)，組合定位系統(tǒng)自適應(yīng)子系統(tǒng)定位 精度的變化。
【文檔編號(hào)】G01S19/47GK105866812SQ201610178772
【公開(kāi)日】2016年8月17日
【申請(qǐng)日】2016年3月24日
【發(fā)明人】劉建圻, 閆荷花, 鄒才鳳, 張嚴(yán)林
【申請(qǐng)人】廣東機(jī)電職業(yè)技術(shù)學(xué)院