一種基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法
【專利摘要】本發(fā)明涉及一種基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法。該方法通過GNSS/RFID/IMU進(jìn)行組合定位。該方法能在GNSS信號失鎖時，通過搜索權(quán)重數(shù)據(jù)庫，對IMU和RFID的定位信息進(jìn)行加權(quán)自適應(yīng)卡爾曼濾波，輸出更加精準(zhǔn)的定位。本發(fā)明所述基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，利用擴(kuò)展卡爾曼濾波算法提高GNSS的定位精度；利用多傳感器聯(lián)合定位的算法改善了盲區(qū)的定位精度，克服了高層建筑遮擋帶來的影響；通過建立權(quán)值數(shù)據(jù)庫，降低了權(quán)重計算的難度，提高了運算速率。
【專利說明】
一種基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及一種基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，屬于多源 聯(lián)合定位的技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 多源信息融合的方法已被廣泛應(yīng)用在航天航空工程、航海安全、無人機(jī)導(dǎo)航與定 位等領(lǐng)域。單個傳感器在空間上僅僅能覆蓋環(huán)境中的某個特定區(qū)域，而且若單獨使用一個 傳感器，整個系統(tǒng)可能會因為單個傳感器故障，造成量測數(shù)據(jù)的丟失，從而導(dǎo)致整個系統(tǒng)癱 瘓或崩潰，影響定位精度，所以多傳感器聯(lián)合定位在提高定位精度方面發(fā)揮著越來越重要 的作用。
[0003] 現(xiàn)有的主要定位技術(shù)有以下幾種：（一）全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)，泛指所有的衛(wèi)星導(dǎo)航系統(tǒng)包括美國的全球定位系統(tǒng)(GPS)、俄羅 斯的格洛納斯衛(wèi)星系統(tǒng)（GLONASS )、歐盟的伽利略系統(tǒng)（Gal i 1 eo )、中國的北斗系統(tǒng) (COMPASS)，它能夠?qū)崟r地為軍民用戶提供三維位置、速度等信息，但是存在著定位盲區(qū)信 號缺失的問題。（二)無線射頻識別(RFID)是利用現(xiàn)有的無線網(wǎng)絡(luò)系統(tǒng)來跟蹤和定位特定場 合的RF標(biāo)識。接收機(jī)通過對接收到的信號進(jìn)行處理，從而定位該設(shè)備。RFID雖然有價格便 宜、短距離精度高等優(yōu)點，但是隨著定位距離的增加，定位精度會受到很大的影響。（三)慣 性導(dǎo)航單元（IMU)定位利用加速度計和陀螺儀來定位，不需要輔助信號基站，具有完全自 主、不受外界環(huán)境的影響等優(yōu)點，但是MU存在著定位誤差會隨著時間推移累加的缺點。
[0004] 國內(nèi)外許多學(xué)者對多源聯(lián)合定位技術(shù)有深入的研究。中國專利CN1908587公開了 一種GPS和DR車載組合定位系統(tǒng)及定位方法，該方法將GPS和DR的定位信息分別送到微控制 器內(nèi)的基于遺傳優(yōu)化算法的模糊聯(lián)合卡爾曼濾波器，經(jīng)過融合處理后輸出定位信息。中國 專利CN202631740U中研究了 RFID和GPS的車輛定位裝置，該裝置通過讀取電子標(biāo)簽中的準(zhǔn) 確信息，計算GPS誤差，實現(xiàn)車輛的準(zhǔn)確定位。中國專利CN101319902中研究了低成本的MU 和GPS的組合定位定向裝置。該裝置將GPS測得的信息和MU解算得到的信息通過信息融合 處理，得到載體最優(yōu)的導(dǎo)航信息。但是上述定位裝置和方法仍然無法解決或者避免各定位 技術(shù)本身的缺點和不足。
[0005] 卡爾曼濾波算法是導(dǎo)航定位系統(tǒng)中常用的算法?？柭鼮V波是由計算機(jī)實現(xiàn)的實 時遞推算法，它根據(jù)系統(tǒng)狀態(tài)方程和觀測方程估計出所有需要處理的信號。普通的卡爾曼 濾波是在系統(tǒng)線性的前提下討論的，而在實際中，系統(tǒng)總是存在不同程度的非線性，運用擴(kuò) 展卡爾曼濾波(EKF)可以有效解決非線性濾波的問題，實現(xiàn)更加精準(zhǔn)的定位。關(guān)于擴(kuò)展卡爾 曼算法的推導(dǎo)等具體內(nèi)容，參考付夢印、鄧志紅等人編寫的《Kalman濾波理論及其在導(dǎo)航系 統(tǒng)中的應(yīng)用》。

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

[0006] 針對現(xiàn)有技術(shù)的不足，本發(fā)明提供一種基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián) 合定位方法。
[0007] 發(fā)明概述：
[0008] 用GNSS/RFID/MU進(jìn)行組合定位。該方法能在GNSS信號失鎖時，通過搜索權(quán)重數(shù)據(jù) 庫，對IMU和RFID的定位信息進(jìn)行加權(quán)自適應(yīng)卡爾曼濾波，輸出更加精準(zhǔn)的定位。
[0009] 定位算法，主要包括兩個方面：1)GNSS信號接收良好時，分別對GNSS、RFID和頂U(kuò)的 測量信息進(jìn)行擴(kuò)展卡爾曼濾波，同時建立RFID和頂U(kuò)的權(quán)重數(shù)據(jù)庫；2)在GNSS信號缺失時， 首先把RFID和頂U(kuò)的測量信息放入擴(kuò)展卡爾曼濾波器，得到濾波信息，搜索權(quán)重數(shù)據(jù)庫，將 濾波后的信息與數(shù)據(jù)庫預(yù)存的GNSS濾波校正信息按照最小誤差準(zhǔn)則進(jìn)行配對，確定最優(yōu)的 RFID和頂U(kuò)的權(quán)值，最后對RFID和頂U(kuò)的數(shù)據(jù)進(jìn)行加權(quán)信息融合，輸出最優(yōu)的定位信息。
[0010] 術(shù)語說明：
[0011]閱讀器：閱讀器是RFID定位系統(tǒng)的組成部分，起到讀取標(biāo)簽的ID號或數(shù)據(jù)區(qū)內(nèi)容 的作用。
[0012] 參考點：參考點是GNSS定位系統(tǒng)的組成部分，通過測量參考點和移動物體的距離， 起到提高定位精度的作用。
[0013] 標(biāo)簽:標(biāo)簽是RFID定位系統(tǒng)的組成部分，起到標(biāo)識識別、信息采集的作用。
[0014] 接收機(jī):起到接收、跟蹤、變換和測量信號的作用，為目標(biāo)定位提供原始數(shù)據(jù)。
[0015] 本發(fā)明的技術(shù)方案為：
[0016] -種基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，包括以下步驟：
[0017] 1)接收機(jī)分別接收GNSS、RFID、頂啲信號，判斷GNSS信號是否受到干擾；
[0018] 2)如果GNSS信號未受到干擾，以定位目標(biāo)的坐標(biāo)X/ =[Af jff為狀態(tài)向量，以 =? Α 為觀測向量建立GNSS系統(tǒng)運動模型和GNSS觀測模型，通過擴(kuò)展卡爾曼濾波 器計算得到GNSS狀態(tài)最優(yōu)估計i/二[if if 其中η.....rn分別是GNSS測定的定位目 標(biāo)的坐標(biāo)到η個基站的距離;xk，yk分別為WGS-84坐標(biāo)下，k時刻，高斯投影平面的東向坐標(biāo)和 北向坐標(biāo);η彡2;
[0019] 3)以定位目標(biāo)的坐標(biāo)為狀態(tài)向量，以Zf二……《了為 RFID觀測向量，建立RFID系統(tǒng)運動模型和RFID觀測模型，通過擴(kuò)展卡爾曼濾波器計算得到 RFID狀態(tài)最優(yōu)估計允]、其中，#…心分別是通過RFID測量的定位目 標(biāo)攜帶的閱讀器到m個基站的距離;111多3;
[0020] 4)以定位目標(biāo)的坐標(biāo)X/=[x丨太f為狀態(tài)向量，以攻=[w V, 為IMU觀測向 量，建立頂U(kuò)系統(tǒng)運動模型和頂U(kuò)觀測模型，通過擴(kuò)展卡爾曼濾波器計算得到頂U(kuò)狀態(tài)最優(yōu)估 計矣4 :其中，w、vy、vj別為通過IMU測量的定位目標(biāo)的航向角、定位目標(biāo)X方向 的速度、定位目標(biāo)y方向的速度；
[0021] 5)根據(jù)之G:[if if ]Γ)'7]Γ確定RFID和MJ的權(quán) 重系數(shù)，并將各個時刻的GNSS狀態(tài)最優(yōu)估計、RFID和IMU的權(quán)重系數(shù)
[Λ- Λ蚴存入數(shù)據(jù)庫;其中之二[if jf]7是GNSS信號未受到干擾時，k時刻的全 局最優(yōu)狀態(tài)；
[0022] 6)當(dāng)GNSS信號受到干擾時，通過RFID和MU組合導(dǎo)航對定位目標(biāo)定位;將RFID和 MU的定位信息分別放入擴(kuò)展卡爾曼濾波器，得到RFID的最優(yōu)估計= [ff 和頂U(kuò)的 最優(yōu)估計名/=[天/ i'/y;
[0023] 7)分別在數(shù)據(jù)庫中搜索RFID的權(quán)重系數(shù)和MJ的權(quán)重系數(shù)；
[0024] 8)對RFID的最優(yōu)估計和MU的最優(yōu)估計根據(jù)誤差最小原則進(jìn)行加權(quán)信息融合，得 至1Ji時刻的全局狀態(tài)最優(yōu)估計龍,=[元又];·
[0025]優(yōu)選的，系統(tǒng)運動模型的公式：
[0026] Xk = f(Xk-i,k-l)+r (Xk-i,k-l)ffk-i
[0027]
; dk是k-1時刻到k刻定位目標(biāo)的位移，wk-i為 k-Ι時刻IMU測量的定位目標(biāo)的航向角；Γ (Xk-^k-Ι)是噪聲輸入函數(shù)，{Wk-U是零均值系統(tǒng) 白噪聲序列，E[WkWjT]=Q!Aj，Qk是系統(tǒng)噪聲序列的方差；
[0028]優(yōu)選的，GNSS觀測模型的公式：
[0029] Zf ^h(X^,k) + V^
[0030]
_;(xi，yi)、· · ·、（xn，yn)分別是參與定位的 η個基站的坐標(biāo)；貧=?…是零均值觀測白噪聲序列，其 中，是觀測噪聲序列的方差；
[0031 ] 丨和是不相關(guān)的白噪聲序列；ilF^^GNSS的零均值系統(tǒng)白噪聲；
[0032] 優(yōu)選的，擴(kuò)展卡爾曼濾波的步驟如下：
[0033] A、將系統(tǒng)運動模型和觀測模型線性化；
[0034] B、根據(jù)線性化的系統(tǒng)運動模型和線性化的觀測模型，結(jié)合系統(tǒng)噪聲協(xié)方差Qk和觀 測噪聲協(xié)方差Rk，給定k-Ι時刻的狀態(tài)最優(yōu)估計和協(xié)方差矩陣，根據(jù)擴(kuò)展卡爾曼濾波方程計 算得到k時刻的狀態(tài)最優(yōu)估計尤=丨4爲(wèi)f和協(xié)方差矩陣Pk。狀態(tài)最優(yōu)估計和協(xié)方差矩陣 使用的是遞歸的算法，所述"給定"即假設(shè)k-Ι時刻狀態(tài)最優(yōu)估計和協(xié)方差矩陣是已知的，通 過遞歸算法可確定下一時刻k時刻的狀態(tài)最優(yōu)估計和協(xié)方差矩陣。
[0035] 進(jìn)一步優(yōu)選的，步驟2)中通過擴(kuò)展卡爾曼濾波器計算得到GNSS狀態(tài)最優(yōu)估計的過 程如下：
[0036] GNSS系統(tǒng)運動模型線性化:將系統(tǒng)狀態(tài)方程按照泰勒公式圍繞濾波值展開：
[0040] GNSS狀態(tài)最優(yōu)估計If = [if ^和估計的協(xié)方差矩陣Pke的求解過程如下：

[0046] 進(jìn)一步優(yōu)選的，步驟3)中RFID狀態(tài)最優(yōu)估計i/ = [if 和估計的協(xié)方差矩 陣< 的計算過程如下：
[0047] RFID 系統(tǒng)運動模型：< =/(<_，-1) + Γβ(；τυ-1}汗A ;
[0048] RFID觀測模型：= /?Κ，幻 + ;
[0049]
，G?)、…、分別是m個標(biāo) 簽的坐標(biāo)；
[0050] k-Ι時刻RFID狀態(tài)最優(yōu)估計J"，RFID估計的協(xié)方差矩陣為;通 過擴(kuò)展卡爾曼濾波器計算得到k時刻RFID狀態(tài)最優(yōu)估計尤"估計的協(xié)方差矩 陣是
[0051] 進(jìn)一步優(yōu)選的，步驟4)中IMU狀態(tài)最優(yōu)估計穴了和估計的協(xié)方差矩陣 瑢的計算過程如下：
[0052] 頂U(kuò)系統(tǒng)運動模型：< =/(J -1) + Γ?χ。夕-1):%^ ;
[0053] 頂U(kuò)狀態(tài)觀測模型：Zf = A7?,/i) + F/ ;
[0054]
，Ζ,(=[η· /，ΔΤ是k時刻與k-1時刻的時間差；
[0055] k-1時亥ijlMU狀態(tài)最優(yōu)估計尤1]，IMU估計的協(xié)方差矩陣為通過 擴(kuò)展卡爾曼濾波器計算得到k時刻的mu狀態(tài)最優(yōu)估計文/ =μι允j'估計的協(xié)方差矩陣 Ρ卜
[0056] 優(yōu)選的，步驟5)中確定RFID的權(quán)重系數(shù)和頂U(kuò)的權(quán)重系數(shù)的方法如下：
[0059]優(yōu)選的，步驟7)中在數(shù)據(jù)庫中分別搜索RFID的權(quán)重系數(shù)和MU的權(quán)重系數(shù)的具體 方法如下：
[0060]
k，…搜索數(shù)據(jù)庫中的(#，f)，確定使Q最小的(if，j>f)，則其對應(yīng)的(h，K )為RFID的權(quán)重 系數(shù)和IMU的權(quán)重系數(shù)。
[0061]優(yōu)選的，步驟10)中確定全局狀態(tài)最優(yōu)估計1=[夂夂/的具體方法如下：
[0062]首先建立目標(biāo)誤差函數(shù)：
[0063] Z = wf(X0-If ) + w；(X -Χ/)Γ(^)-!(Χ -X；)
[0064] 使目標(biāo)誤差函數(shù)怎最小的為全局狀態(tài)最優(yōu)估計：
[0069]融合后的全局狀態(tài)最優(yōu)估計之=[i" .兔_]τ和誤差最優(yōu)估計P。分別為：

[0073] 全局狀態(tài)最優(yōu)估計是i時刻在GNSS沒有信號時，最終的經(jīng)過校正后輸出的定位信 息。
[0074] 優(yōu)選的，頂U(kuò)中的航向角根據(jù)陀螺儀測得。
[0075] 本發(fā)明的有益效果為：
[0076] 1.本發(fā)明所述基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，利用擴(kuò)展卡 爾曼濾波算法提高GNSS的定位精度;利用多傳感器聯(lián)合定位的算法改善了盲區(qū)的定位精 度，克服了高層建筑遮擋帶來的影響;通過建立權(quán)值數(shù)據(jù)庫，降低了權(quán)重計算的難度，提高 了運算速率；
[0077] 2 .本發(fā)明所述基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，用GNSS/ RFID/lMU進(jìn)行組合定位，在GNSS信號失鎖時，通過搜索權(quán)重數(shù)據(jù)庫，對頂U(kuò)和RFID的定位信 息進(jìn)行加權(quán)自適應(yīng)卡爾曼濾波，輸出更加精準(zhǔn)的定位。
【附圖說明】
[0078] 圖1為本發(fā)明所述方法的流程示意圖；
[0079] 圖2為GNSS信號未受到干擾時建立數(shù)據(jù)庫的流程示意圖；
[0080] 圖3為GNSS信號受到干擾時時，通過RFID和MU組合導(dǎo)航對定位目標(biāo)定位的流程示 意圖。
【具體實施方式】
[0081] 下面結(jié)合實施例和說明書附圖對本發(fā)明做進(jìn)一步說明，但不限于此。
[0082] 實施例1
[0083] 如圖1-3所示。
[0084] -種基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，包括以下步驟：
[0085] 1)接收機(jī)分別接收GNSS、RFID、頂U(kuò)的信號，判斷GNSS信號是否受到干擾；
[0086] 2)如果GNSS信號未受到干擾，以定位目標(biāo)的坐標(biāo)X/ ? jff為狀態(tài)向量，以 Q 為觀測向量建立GNSS系統(tǒng)運動模型和GNSS觀測模型，通過擴(kuò)展卡爾曼濾波 器計算得到GNSS狀態(tài)最優(yōu)估計i7/ = [if 其中ri、r2、r3分別是GNSS測定的定位目標(biāo) 的坐標(biāo)到三個基站的距離;Xk，yk分別為WGS-84坐標(biāo)下，k時刻，高斯投影平面的東向坐標(biāo)和 北向坐標(biāo)；
[0087] 3)以定位目標(biāo)的坐標(biāo)< ? 為狀態(tài)向量，以義二[< < < RFID觀測向量，建立RFID系統(tǒng)運動模型和RFID觀測模型，通過擴(kuò)展卡爾曼濾波器計算得到 RFID狀態(tài)最優(yōu)估計其中，rff、4"、<、<分別是通過RFID測量的定位 目標(biāo)攜帶的閱讀器到四個基站的距離；
[0088] 4)以定位目標(biāo)的坐標(biāo)允]7為狀態(tài)向量，以茍=[> v, vyf為mj觀測向 量，建立頂U(kuò)系統(tǒng)運動模型和頂U(kuò)觀測模型，通過擴(kuò)展卡爾曼濾波器計算得到頂U(kuò)狀態(tài)最優(yōu)估 計尤/ %];其中，w、Vy、vj別為通過IMU測量的定位目標(biāo)的航向角、定位目標(biāo)X方向 的速度、定位目標(biāo)y方向的速度；
[0089] 5)根據(jù)巧二[if 、戈允]'、：1/=[名只]7確定RFID和頂U(kuò)的權(quán) 重系數(shù)[wf ><]，并將各個時刻的GNS S狀態(tài)最優(yōu)估計、RF I D和IMU的權(quán)重系數(shù) C-i V,, < Η'丨]存入數(shù)據(jù)庫;其中爲(wèi)=[為，丸]^是GNSS信號未受到干擾時，k時刻的全局 最優(yōu)狀態(tài)；
[0090] 6)當(dāng)GNSS信號受到干擾時，通過RFID和MU組合導(dǎo)航對定位目標(biāo)定位；將RFID和 MU的定位信息分別放入擴(kuò)展卡爾曼濾波器，得到RFID的最優(yōu)估計i/ =[if .i'f]·'和頂U(kuò)的 最優(yōu)估計龍7=[< ff;
[0091] 7)分別在數(shù)據(jù)庫中搜索RFID的權(quán)重系數(shù)和頂U(kuò)的權(quán)重系數(shù)；
[0092] 8)對RFID的最優(yōu)估計和MU的最優(yōu)估計根據(jù)誤差最小原則進(jìn)行加權(quán)信息融合，得 至iji時刻的全局狀態(tài)最優(yōu)估計尤=[之九廠 [0093] 實施例2
[0094] 如實施例1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，GNSS系統(tǒng)運動模型的公式：
[0095] =f(X^,k-l) + rG(Xl1,k-i)Wti
[0096]
;dk是k-1時刻到k刻定位目標(biāo)的位移，wk-1為 k-Ι時刻IMU測量的定位目標(biāo)的航向角；r6'(ZU -1)是噪聲輸入函數(shù)，是零均值系統(tǒng) 白噪聲序列，((^__廣| = ，gf是系統(tǒng)噪聲序列的方差矩陣。
[0097] 實施例3
[0098] 如實施例1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，GNSS觀測模型的公式：
[0099] Zf = h(X^ ,1c)+ yf
[0100]
;(叉1，71)、（叉2,72)、（叉3,73)分另1|是參與定 位的3個基站的坐標(biāo);Zf = [q ，1*1、^、^分別是6~33測定的定位目標(biāo)的坐標(biāo)到三個 基站的距離；化6)是零均值觀測白噪聲序列，= ，其中，砣是觀測噪聲序列 的方差。
[0101] 實施例4
[0102] 如實施例1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，擴(kuò)展卡爾曼濾波的步驟如下：
[0103] A、將系統(tǒng)運動模型和觀測模型線性化；
[0104] B、根據(jù)線性化的系統(tǒng)運動模型和線性化的觀測模型，結(jié)合系統(tǒng)噪聲協(xié)方差Qk和觀 測噪聲協(xié)方差Rk，給定k-Ι時刻的狀態(tài)最優(yōu)估計和協(xié)方差矩陣，根據(jù)擴(kuò)展卡爾曼濾波方程計 算得到k時刻的狀態(tài)最優(yōu)估計Α Γ和協(xié)方差矩陣Pk。狀態(tài)最優(yōu)估計和協(xié)方差矩陣 使用的是遞歸的算法，所述"給定"即假設(shè)k-i時刻狀態(tài)最優(yōu)估計和協(xié)方差矩陣是已知的，通 過遞歸算法可確定下一時刻k時刻的狀態(tài)最優(yōu)估計和協(xié)方差矩陣。
[0105] 實施例5
[0106] 如實施例4所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，步驟2)中通過擴(kuò)展卡爾曼濾波器計算得到GNSS狀態(tài)最優(yōu)估計的過程如下：
[0107] GNSS系統(tǒng)運動模型線性化:將系統(tǒng)狀態(tài)方程按照泰勒公式圍繞濾波值展開：
[0111] GNSS狀態(tài)最優(yōu)估計If二[if 和估計的協(xié)方差矩陣Pke的求解過程如下：
[0117] 實施例6
[0118] 如實施例4所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，步驟3)中RFID狀態(tài)最優(yōu)估計為"=[紀(jì)允『和估計的協(xié)方差矩陣/f的計算過程如 下：
[0119] RFID系統(tǒng)運動模型：對=- ].) + 尸(XL, A： - 1)?么；
[0120] RFID觀測模型：Zf = M(Xf，幻 + F/ ;
[0121]
別是四個標(biāo)簽的坐標(biāo)；
[0122] k-Ι時刻RFID狀態(tài)最優(yōu)估計氧_/ i^J^RFID估計的協(xié)方差矩陣為通 過擴(kuò)展卡爾曼濾波器計算得到k時刻RFID狀態(tài)最優(yōu)估計>估計的協(xié)方差矩 陣是？1/:?
[0123] 實施例7
[0124] 如實施例4所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，步驟4)中mj狀態(tài)最優(yōu)估計之1 = [f(只]f和估計的協(xié)方差矩陣P/的計算過程如下：
[0125] MJ系統(tǒng)運動模型D + rK-W-lXi;
[0126] 頂U(kuò)狀態(tài)觀測模型：Z/ = V (Ji/，幻+ ;
[0127]
，..·ζ = [* t ，.. Δ T是k時刻與k~l時刻的時間差；
[0128] k-1時亥_U狀態(tài)最優(yōu)估計氧_/ = i了，MU估計的協(xié)方差矩陣為<1;通過 擴(kuò)展卡爾曼濾波器計算得到k時刻的IMU狀態(tài)最優(yōu)估計估計的協(xié)方差矩陣 T) / pk.。
[0129] 實施例8
[0130] 如實施例1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，步驟5)中確定RFID的權(quán)重系數(shù)和頂U(kuò)的權(quán)重系數(shù)的方法如下：
[0133] 實施例9
[0134] 如實施例4所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，步驟7)中在數(shù)據(jù)庫中分別搜索RFID的權(quán)重系數(shù)和頂U(kuò)的權(quán)重系數(shù)的具體方法如下：
[0135]
其中，i = l，2，··· k，…搜索數(shù)據(jù)庫中的(f，Λ )，確定使Q:最小的(.if，if)，則其對應(yīng)的為RFID的權(quán)重 系數(shù)和IMU的權(quán)重系數(shù)。
[0136] 實施例10
[0137] 如實施例4所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其區(qū)別 在于，步驟10)中確定全局狀態(tài)最優(yōu)估計之=[.(·"又]'的具體方法如下：
[0138] 首先建立目標(biāo)誤差函數(shù)：
[0139] Z^wf{Xo-Xf)\PlRr\X\-Xf) + wl(Xo-Xl)\P；y\Xo-XIi)
[0140] 使目標(biāo)誤差函數(shù)E最小的為全局狀態(tài)最優(yōu)估計：
[0145]融合后的全局狀態(tài)最優(yōu)估計尤=[毛和誤差最優(yōu)估計P。分別為：
[0149]全局狀態(tài)最優(yōu)估計是i時刻在GNSS沒有信號時，最終的經(jīng)過校正后輸出的定位信 息。
【主權(quán)項】
1. 一種基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特征在于，包括W下 步驟： 1) 接收機(jī)分別接收GNSS、RFID、IMU的信號，判斷GNSS信號是否受到干擾； 2) 如果GNSS信號未受到干擾，W定位目標(biāo)的坐標(biāo)義/=掉乂f為狀態(tài)向量，W 馬' =h A rgf為觀測向量建立GNSS系統(tǒng)運動模型和GNSS觀測模型，通過擴(kuò)展卡爾曼濾波 器計算得到GNSS狀態(tài)最優(yōu)估i-其中η.....。分別是GNSS測定的定位目 標(biāo)的坐標(biāo)到η個基站的距離;xk，yk分別為WGS-84坐標(biāo)下，k時刻，高斯投影平面的東向坐標(biāo)和 北向坐標(biāo);n>2; 3似定位目標(biāo)的坐標(biāo)種=妃yff為狀態(tài)向量，W巧二[壞……巧了為RFID觀 測向量，建立RFID系統(tǒng)運動模型和RFID觀測模型，通過擴(kuò)展卡爾曼濾波器計算得到RFID狀 態(tài)最優(yōu)估計：其中，郵……的分別是通過RFID測量的定位目標(biāo)攜帶的 閱讀器到m個基站的距離;3; 4. W定位目標(biāo)的坐標(biāo)蚊姥Γ為狀態(tài)向量，W巧=[w V, v/為IMU觀測向量， 建立IMU系統(tǒng)運動模型和IM的見測模型，通過擴(kuò)展卡爾曼濾波器計算得到IMU狀態(tài)最優(yōu)估計 義/ =詩J'i ;其中，w、Vy、Vx分別為通過IMU測量的定位目標(biāo)的航向角、定位目標(biāo)X方向的 速度、定位目標(biāo)y方向的速度； 5) 根據(jù)資定RFID和IMU的權(quán)重系 數(shù)[}皆，并將各個時刻的GNSS狀態(tài)最優(yōu)估計、RFID和IMU的權(quán)重系數(shù)憐Λ < 存入數(shù)據(jù)庫;其中韋二μ,,. .?·,,」'<是GNSS信號未受到干擾時，k時刻的全局最優(yōu)狀態(tài)； 6) 當(dāng)GNSS信號受到干擾時，通過RFID和IMU組合導(dǎo)航對定位目標(biāo)定位;將RFID和IMU的 定位信息分別放入擴(kuò)展卡爾曼濾波器，得到RFID的最優(yōu)估計矣* =[輝好f和IMU的最優(yōu) 估計斬=巧巧7'; 7) 分別在數(shù)據(jù)庫中捜索RFID的權(quán)重系數(shù)和IMU的權(quán)重系數(shù)； 8) 對RFID的最優(yōu)估計和IMU的最優(yōu)估計根據(jù)誤差最小原則進(jìn)行加權(quán)信息融合，得到i時 刻的全局狀態(tài)最優(yōu)估計X二氏捉Γ。2. 根據(jù)權(quán)利要求1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，系統(tǒng)運動模型的公式：其中dk是k-1時刻到k刻定位目標(biāo)的位移，wk-i為k-1 時刻IMU測量的定位目標(biāo)的航向角；Γ (Xk-i，k-l)是噪聲輸入函數(shù)，{Wk-i}是零均值系統(tǒng)白噪 聲序列，E[WkWjT] =QiAj，祐是系統(tǒng)噪聲序列的方差。3. 根據(jù)權(quán)利要求1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，GNSS觀測模型的公式：其中，；（xi，yi)、...、（xn，yn)分別是參與定位的η個 基站的坐標(biāo)；巧二是零均值觀測白噪聲序列，巧= ，其中， 雌是觀測噪聲序列的方差。4. 根據(jù)權(quán)利要求1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，擴(kuò)展卡爾曼濾波的步驟如下： A、 將系統(tǒng)運動模型和觀測模型線性化； B、 根據(jù)線性化的系統(tǒng)運動模型和線性化的觀測模型，結(jié)合系統(tǒng)噪聲協(xié)方差Qk和觀測噪 聲協(xié)方差化，給定k-1時刻的狀態(tài)最優(yōu)估計和協(xié)方差矩陣，根據(jù)擴(kuò)展卡爾曼濾波方程計算得 至化時刻的狀態(tài)最優(yōu)估計本;Λ Γ和協(xié)方差矩陣Pk。5. 根據(jù)權(quán)利要求4所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，步驟2)中通過擴(kuò)展卡爾曼濾波器計算得到GNSS狀態(tài)最優(yōu)估計的過程如下： GNSS系統(tǒng)運動模型線性化:將系統(tǒng)狀態(tài)方程按照泰勒公式圍繞濾波值展開：6. 根據(jù)權(quán)利要求4所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，步驟3)中RFID狀態(tài)最優(yōu)估計巧估計的協(xié)方差矩陣PkK的計算過程 如下：其中，咕，記)、...、(誠，姑)分別是m個標(biāo)簽的 坐標(biāo)； k-1時刻RFID狀態(tài)最優(yōu)估計，RFID估計的協(xié)方差矩陣為/f_i;通過擴(kuò) 展卡爾曼濾波器計算得到k時刻RFID狀態(tài)最優(yōu)估計=[背興]了 j古計的協(xié)方差矩陣是皆。7. 根據(jù)權(quán)利要求4所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，步驟4)中IMU狀態(tài)最優(yōu)估計i·/二[為興了和估計的協(xié)方差矩陣兮的計算過程如 下： IMU系統(tǒng)運動模型:為=/(為_1，A -1) +『乂解_1，灰- 1)略1; IMU狀態(tài)觀測模型：為=的，A) + F/ ; 其中AT是k時刻與k-1時刻的時間差； k-1時刻IMU狀態(tài)最優(yōu)估?IMU估計的協(xié)方差矩陣為逆1;通過擴(kuò)展 卡爾曼濾波器計算得到k時刻的IMU狀態(tài)最優(yōu)估計義/=[磚齊了，估計的協(xié)方差矩陣巧'。8. 根據(jù)權(quán)利要求1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，步驟5)中確定RFID的權(quán)重系數(shù)和IMU的權(quán)重系數(shù)的方法如下：9. 根據(jù)權(quán)利要求1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，步驟7)中在數(shù)據(jù)庫中分別捜索RFID的權(quán)重系數(shù)和IMU的權(quán)重系數(shù)的具體方法如下：其中，1 = 1，2，。，4，。-捜 索數(shù)據(jù)庫中的(護(hù)乂')，確定使巧最小的皆，於），則其對應(yīng)的巧RFID的權(quán)重系數(shù)和 IMU的權(quán)重系數(shù)。10.根據(jù)權(quán)利要求1所述的基于自適應(yīng)加權(quán)混合卡爾曼濾波的多源聯(lián)合定位方法，其特 征在于，步驟10)中確定全局狀態(tài)最優(yōu)估計束,,=口。丸1'的具體方法如下： 首先建立目標(biāo)誤差函數(shù)：
【文檔編號】G01S19/49GK106093994SQ201610372937
【公開日】2016年11月9日
【申請日】2016年5月31日
【發(fā)明人】熊海良, 唐娟, 馬丕明, 朱維紅, 杜正鋒
【申請人】山東大學(xué)