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

【發(fā)明內容】

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