采用偽距和位置信息的室內(nèi)行人uwb/ins緊組合導(dǎo)航系統(tǒng)及方法
【專利摘要】本發(fā)明公開了一種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航系統(tǒng)及方法，包括：慣性導(dǎo)航器件INS、UWB定位標簽、UWB參考節(jié)點、數(shù)據(jù)處理系統(tǒng)；所述慣性導(dǎo)航器件INS和UWB定位標簽分別設(shè)置在行人的鞋子和身體上，所述UWB參考節(jié)點設(shè)置在任意位置，所述慣性導(dǎo)航器件INS和UWB定位標簽分別與數(shù)據(jù)處理系統(tǒng)連接。本發(fā)明有益效果：通過將參考節(jié)點的位置信息引入狀態(tài)變量中，并通過濾波器完成對參考節(jié)點位置信息的預(yù)估，減少了組合導(dǎo)航系統(tǒng)需要預(yù)先獲取參考節(jié)點位置信息以及組合導(dǎo)航系統(tǒng)的定位精度依賴于預(yù)先獲取到的參考節(jié)點位置信息精度的缺點。
【專利說明】
采用偽距和位置信息的室內(nèi)行人UWB/1 NS緊組合導(dǎo)航系統(tǒng)及 方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及復(fù)雜環(huán)境下組合定位技術(shù)領(lǐng)域，尤其涉及一種采用偽距信息和位置信 息的室內(nèi)行人UWB/INS緊組合導(dǎo)航系統(tǒng)及方法。
【背景技術(shù)】
[0002] 近年來，行人導(dǎo)航(Pedestrian Navigation，PN)作為導(dǎo)航技術(shù)應(yīng)用的新興領(lǐng)域， 正越來越受到各國學者的重視，并逐漸成為該領(lǐng)域的研究熱點。然而在隧道、大型倉庫、地 下停車場等室內(nèi)環(huán)境下，外界無線電信號微弱、電磁干擾強烈等因素都會對目標行人導(dǎo)航 信息獲取的準確性、實時性及魯棒性有很大影響。如何將室內(nèi)環(huán)境下獲取的有限信息進行 有效的融合以消除室內(nèi)復(fù)雜環(huán)境影響，保證行人導(dǎo)航精度的持續(xù)穩(wěn)定，具有重要的科學理 論意義和實際應(yīng)用價值。
[0003] 在現(xiàn)有的定位方式中，全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite SyStem，GNSS)是最為常用的一種方式。雖然GNSS能夠通過精度持續(xù)穩(wěn)定的位置信息，但是 其易受電磁干擾、遮擋等外界環(huán)境影響的缺點限制了其應(yīng)用范圍，特別是在室內(nèi)、地下巷道 等一些密閉的、環(huán)境復(fù)雜的場景，GNSS信號被嚴重遮擋，無法進行有效的工作。近年來，UWB (Ultra Wideband)以其在復(fù)雜環(huán)境下定位精度高的特點在短距離局部定位領(lǐng)域表現(xiàn)出很 大的潛力。學者們提出將基于UWB的目標跟蹤應(yīng)用于GNSS失效環(huán)境下的行人導(dǎo)航。這種方式 雖然能夠?qū)崿F(xiàn)室內(nèi)定位，但是由于室內(nèi)環(huán)境復(fù)雜多變，UWB信號十分容易受到干擾而導(dǎo)致定 位精度下降甚至失鎖；與此同時，由于UWB采用的通信技術(shù)通常為短距離無線通信技術(shù)，因 此若想完成大范圍的室內(nèi)目標跟蹤定位，需要大量的網(wǎng)絡(luò)節(jié)點共同完成，這必將引入網(wǎng)絡(luò) 組織結(jié)構(gòu)優(yōu)化設(shè)計、多節(jié)點多簇網(wǎng)絡(luò)協(xié)同通信等一系列問題。因此現(xiàn)階段基于UWB的目標跟 蹤在室內(nèi)導(dǎo)航領(lǐng)域仍舊面臨很多挑戰(zhàn)。
[0004] 在導(dǎo)航模型方面，目前在室內(nèi)行人組合導(dǎo)航領(lǐng)域應(yīng)用較多的為松組合導(dǎo)航模型。 該模型具有容易實現(xiàn)的優(yōu)點，但是需要指出的是，該模型的實現(xiàn)需要參與組合導(dǎo)航的多種 技術(shù)能夠獨立的完成導(dǎo)航定位。例如，需要UWB設(shè)備能夠提供行人的導(dǎo)航信息，這就要求目 標行人所處的環(huán)境必須能夠獲取至少3個參考節(jié)點信息，這大大的降低了組合導(dǎo)航模型的 應(yīng)用范圍，與此同時，參與導(dǎo)航的子技術(shù)獨立完成定位，也引入了新的誤差，不利于組合導(dǎo) 航技術(shù)精度的提高。為了克服這一問題，學者們提出將緊組合模型應(yīng)用于室內(nèi)行人導(dǎo)航領(lǐng) 域，緊組合模型直接將參與組合導(dǎo)航的子技術(shù)的原始傳感器數(shù)據(jù)應(yīng)用于最后的導(dǎo)航信息的 解算，減少了子技術(shù)自行解算引入新誤差的風險，提高了組合導(dǎo)航的精度，但是需要指出的 是，緊組合導(dǎo)航模型需要提前獲取參與組合的參考節(jié)點的精確位置信息，這限制了導(dǎo)航模 型的靈活性。

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

[0005] 本發(fā)明的目的就是為了解決上述難題，提供了一種采用偽距和位置信息的室內(nèi)行 人UWB/INS緊組合導(dǎo)航系統(tǒng)及方法，該系統(tǒng)及方法無需提前獲取參考節(jié)點的位置信息，而是 將參考節(jié)點的位置信息引入狀態(tài)變量中，并通過濾波器完成對參考節(jié)點位置信息的預(yù)估。
[0006] 為實現(xiàn)上述目的，本發(fā)明的具體方案如下：
[0007] 一種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航系統(tǒng)，包括：
[0008]慣性導(dǎo)航器件INS:用于測量行人的位置、速度和姿態(tài)導(dǎo)航信息；
[0009] UWB定位標簽：用于測量UWB定位標簽與UWB參考節(jié)點之間的距離信息，即偽距信 息；
[0010] UWB參考節(jié)點:預(yù)先放置在設(shè)定的位置，便于測量與定位標簽之間的距離；
[0011] 數(shù)據(jù)處理系統(tǒng):用于對采集到的傳感器數(shù)據(jù)進行數(shù)據(jù)融合；
[0012] 所述慣性導(dǎo)航器件INS和UWB定位標簽分別設(shè)置在行人的鞋子和身體上，所述UWB 參考節(jié)點設(shè)置在任意位置，所述慣性導(dǎo)航器件INS和UWB定位標簽分別與數(shù)據(jù)處理系統(tǒng)連 接。
[0013] 進一步地，所述數(shù)據(jù)處理系統(tǒng)包括迭代擴展卡爾曼濾波器，通過所述迭代擴展卡 爾曼濾波器對慣性導(dǎo)航器件INS在本地相對坐標系中獲取的目標行人的導(dǎo)航信息以及UWB 定位標簽在本地相對坐標系中獲取的偽距信息進行數(shù)據(jù)融合。
[0014] 一種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航方法，包括：
[0015] (1)以慣性導(dǎo)航器件INS的誤差向量和各UWB參考節(jié)點的東向和北向位置作為狀態(tài) 量，以慣性導(dǎo)航器件INS測量得到的目標行人的東向和北向位置信息的平方和與UWB定位標 簽測量得到的偽距的平方之差作為系統(tǒng)觀測量，構(gòu)建UWB/INS組合導(dǎo)航模型；
[0016] (2)利用所述UWB/INS組合導(dǎo)航模型，通過迭代擴展卡爾曼濾波器將慣性導(dǎo)航器件 INS在本地相對坐標系中獲取的目標行人的導(dǎo)航信息以及UWB定位標簽在本地相對坐標系 中獲取的偽距信息進行數(shù)據(jù)融合；
[0017] 最終得到當前時刻目標行人最優(yōu)的導(dǎo)航信息以及參考節(jié)點位置的最優(yōu)估計。
[0018]進一步地，作為狀態(tài)量的慣性導(dǎo)航器件INS的誤差向量以及各UWB參考節(jié)點的東向 和北向位置的初值根據(jù)實際需要自行設(shè)定。
[0019]進一步地，所述迭代擴展卡爾曼濾波器的狀態(tài)方程為：
h gpn.
[0021] 其中，^了 為15維INS誤差向量； Y /V(wA-+l)2Aixl
[0022] = [[if、Pf、],[/T2 f ]，…，[^f]]二為2M維參考節(jié)點坐標向量； M為參考節(jié)點數(shù)目； 0 am
[0023] 4^')= ~aDk 0 (4 ;嶙4]為慣性導(dǎo)航器件INS測量得到的導(dǎo)航坐標 _ClM ~^Ek 〇 系下目標行人的東向、北向和天向三個方向的加速度誤差；
[0024]巾k、巾k+1分別為k時刻和k+1時刻慣性導(dǎo)航器件INS測量得到的導(dǎo)航坐標系下目標 行人的橫搖、縱搖和航向角誤差yvr、分別為k時刻和k+i時刻ins測量得到的導(dǎo)航坐 標系下目標行人的東向、北向和天向三個方向的速度誤差;▽丨、分別為k時刻和k+1時刻 導(dǎo)航坐標系下目標行人的東向、北向和天向三個方向的加速度誤差;4、分別為k時刻 和k+i時刻導(dǎo)航坐標系下目標行人的東向、北向和天向三個方向的角速度誤差；、#/分 別為k時刻和k+1時刻導(dǎo)航坐標系下目標行人的東向、北向和天向三個方向的位置誤差；
[0025] ? k為狀態(tài)噪聲;T為采樣周期；I為單位矩陣;C!為從載體系到導(dǎo)航系的狀態(tài)轉(zhuǎn)移矩 陣。
[0026] 進一步地，所述迭代擴展卡爾曼濾波器的觀測方程為：
[0028] 其中，(巧,〇為k時亥lj INS解算的東向和北向位置;(咕叱:;)為k時亥lj INS解算的 東向和北向位置誤差;…，岣為k時刻UWB測量得到的未知節(jié)點到第i個參考節(jié)點之 間的偽距；= 1,2, 為第i個參考節(jié)點的坐標。
[0029] 本發(fā)明的有益效果：
[0030] 1、組合導(dǎo)航方法采用改進的INS/UWB緊組合模型，該模型以INS解算得到目標行人 的東向和北向位置信息的平方和與UWB定位標簽測量得到的偽距的平方之差作為系統(tǒng)觀測 量;在此基礎(chǔ)上，通過迭代擴展卡爾曼濾波將INS和UWB獲取的導(dǎo)航信息進行數(shù)據(jù)融合，最終 得到當前時刻最優(yōu)的導(dǎo)航信息和對參考節(jié)點位置信息的預(yù)估。
[0031] 2、現(xiàn)有的緊組合定位方法需要預(yù)先獲取參考節(jié)點的位置信息，在此基礎(chǔ)上再做定 位，那這就會出現(xiàn)一個問題，就是參考節(jié)點的位置信息的精度會對組合導(dǎo)航系統(tǒng)的精度有 一定的影響。本發(fā)明方法將參考節(jié)點的位置信息加入到狀態(tài)變量中，人工設(shè)定初值即可，不 需要提前獲取參考節(jié)點的位置信息就能夠直接進行預(yù)估。
[0032] 3、本發(fā)明通過將參考節(jié)點的位置信息引入狀態(tài)變量中，并通過濾波器完成對參考 節(jié)點位置信息的預(yù)估，減少了組合導(dǎo)航系統(tǒng)需要預(yù)先獲取參考節(jié)點位置信息以及組合導(dǎo)航 系統(tǒng)的定位精度依賴于預(yù)先獲取到的參考節(jié)點位置信息精度的缺點。
[0033] 4、可用于室內(nèi)環(huán)境下的智能鞋子的中高精度定位。
【附圖說明】
[0034]圖1為采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航系統(tǒng)示意圖；
[0035]圖2為采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航方法的示意圖。
【具體實施方式】：
[0036]下面結(jié)合附圖對本發(fā)明進行詳細說明：
[0037] 一種采用偽距信息和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航，如圖1所示，包括： 慣性導(dǎo)航器件INS、UWB定位標簽、UWB參考節(jié)點和數(shù)據(jù)處理系統(tǒng)；
[0038]慣性導(dǎo)航器件INS和UWB定位標簽分別設(shè)置在行人的鞋子和身體上，UWB參考節(jié)點 設(shè)置在任意位置，慣性導(dǎo)航器件INS和UWB定位標簽分別與數(shù)據(jù)處理系統(tǒng)連接。
[0039] 其中，慣性導(dǎo)航器件INS:用于測量行人的位置、速度和姿態(tài)等導(dǎo)航信息；
[0040] UWB定位標簽:用于測量UWB定位標簽與參考節(jié)點之間的距離信息，即偽距信息； [0041] UWB參考節(jié)點:預(yù)先放置在已知的位置，便于測量與定位標簽之間的距離；
[0042]數(shù)據(jù)處理系統(tǒng)：用于對采集到的傳感器數(shù)據(jù)進行數(shù)據(jù)融合。
[0043]數(shù)據(jù)處理系統(tǒng)包括迭代擴展卡爾曼濾波器，通過所述迭代擴展卡爾曼濾波器對慣 性導(dǎo)航器件INS在本地相對坐標系中獲取的目標行人的導(dǎo)航信息以及UWB定位標簽在本地 相對坐標系中獲取的偽距信息進行數(shù)據(jù)融合。
[0044] -種面向室內(nèi)行人的UWB/INS緊組合導(dǎo)航方法，如圖2所示，包括：
[0045] (1)以慣性導(dǎo)航器件INS的誤差向量和各UWB參考節(jié)點的東向和北向位置作為狀態(tài) 量，以慣性導(dǎo)航器件INS測量得到的目標行人的東向和北向位置信息的平方和與UWB定位標 簽測量得到的偽距的平方之差作為系統(tǒng)觀測量，構(gòu)建INS/UWB組合SLAM模型；
[0046] 作為狀態(tài)量的慣性導(dǎo)航器件INS的誤差向量以及各UWB參考節(jié)點的東向和北向位 置的初值根據(jù)實際需要自行設(shè)定，因此不需要預(yù)先獲知各UWB參考節(jié)點的位置信息，避免了 組合導(dǎo)航系統(tǒng)的定位精度依賴于預(yù)先獲取到的參考節(jié)點位置信息精度的問題。
[0047] (2)通過迭代擴展卡爾曼濾波器將慣性導(dǎo)航器件INS和UWB定位標簽在本地相對坐 標系中獲取的目標行人的導(dǎo)航信息進行數(shù)據(jù)融合;迭代擴展卡爾曼濾波器的輸出即得到當 前時刻目標行人最優(yōu)的導(dǎo)航信息以及參考節(jié)點位置的最優(yōu)估計。
[0048] 根據(jù)INS/UWB組合SLAM模型，得到的迭代擴展卡爾曼濾波器的狀態(tài)方程為：
-I -
[0050] 其中，為15維INS誤差向量； Y k^l
[0051] X,則=[[#' Pf:],.?.,[#? 戶 M為參考節(jié)點數(shù)目； 0 4 -a'k
[0052] ^(f；^ -a'pi 0 ; [4 4k 4]為慣性導(dǎo)航器件INS測量得到的導(dǎo)航坐標 _ a\k ~aa 0 系下的東向、北向和天向三個方向的加速度誤差；
[0053] (i>k、(i)k+1分別為k時刻和k+1時刻慣性導(dǎo)航器件INS測量得到的導(dǎo)航坐標系下的橫 搖、縱搖和航向角誤差;、（?￥二分別為k時刻和k+1時刻INS測量得到的導(dǎo)航坐標系下的 東向、北向和天向三個方向的速度誤差;g、VL,分別為k時刻和k+1時刻導(dǎo)航坐標系下的東 向、北向和天向三個方向的加速度誤差；4、. 分別為k時刻和k+1時刻導(dǎo)航坐標系下的 東向、北向和天向三個方向的角速度誤差;spr、分別為k時刻和k+i時刻導(dǎo)航坐標系下 的東向、北向和天向三個方向的位置誤差；
[0054] co k為狀態(tài)噪聲；T為采樣周期；I為單位矩陣；C:為從載體系到導(dǎo)航系的狀態(tài)轉(zhuǎn)移 矩陣。
[0055] 根據(jù)INS/UWB組合SLAM模型，得到的迭代擴展卡爾曼濾波器的觀測方程為：
[0057] 其中，(松,〇為k時亥ljINS解算的東向和北向位置;(叱';，機::)為k時亥ljINS解算的 東向和北向位置誤差；…，W為k時刻UWB測量得到的未知節(jié)點到第i個參考節(jié)點 之間的偽距;)(/ = 1,2,..為第i個參考節(jié)點的坐標。
[0058] 上述雖然結(jié)合附圖對本發(fā)明的【具體實施方式】進行了描述，但并非對本發(fā)明保護范 圍的限制，所屬領(lǐng)域技術(shù)人員應(yīng)該明白，在本發(fā)明的技術(shù)方案的基礎(chǔ)上，本領(lǐng)域技術(shù)人員不 需要付出創(chuàng)造性勞動即可做出的各種修改或變形仍在本發(fā)明的保護范圍以內(nèi)。
【主權(quán)項】
1. 一種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航系統(tǒng)，其特征是，包括： 慣性導(dǎo)航器件INS:用于測量行人的位置、速度和姿態(tài)導(dǎo)航信息； UWB定位標簽:用于測量UWB定位標簽與UWB參考節(jié)點之間的距離信息，即偽距信息； UWB參考節(jié)點:預(yù)先放置在設(shè)定的位置，便于測量與定位標簽之間的距離； 數(shù)據(jù)處理系統(tǒng):用于對采集到的傳感器數(shù)據(jù)進行數(shù)據(jù)融合； 所述慣性導(dǎo)航器件INS和UWB定位標簽分別設(shè)置在行人的鞋子和身體上，所述UWB參考 節(jié)點設(shè)置在任意位置，所述慣性導(dǎo)航器件INS和UWB定位標簽分別與數(shù)據(jù)處理系統(tǒng)連接。2. 如權(quán)利要求1所述的一種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航系統(tǒng)， 其特征是，所述數(shù)據(jù)處理系統(tǒng)包括迭代擴展卡爾曼濾波器，通過所述迭代擴展卡爾曼濾波 器對慣性導(dǎo)航器件INS在本地相對坐標系中獲取的目標行人的導(dǎo)航信息以及UWB定位標簽 在本地相對坐標系中獲取的偽距信息進行數(shù)據(jù)融合。3. -種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航方法，其特征是，包括： (1) 以慣性導(dǎo)航器件INS的誤差向量和各UWB參考節(jié)點的東向和北向位置作為狀態(tài)量， 以慣性導(dǎo)航器件INS測量得到的目標行人的東向和北向位置信息的平方和與UWB定位標簽 測量得到的偽距的平方之差作為系統(tǒng)觀測量，構(gòu)建UWB/INS組合導(dǎo)航模型； (2) 利用所述UWB/INS組合導(dǎo)航模型，通過迭代擴展卡爾曼濾波器將慣性導(dǎo)航器件INS 在本地相對坐標系中獲取的目標行人的導(dǎo)航信息以及UWB定位標簽在本地相對坐標系中獲 取的偽距信息進行數(shù)據(jù)融合； 最終得到當前時刻目標行人最優(yōu)的導(dǎo)航信息以及參考節(jié)點位置的最優(yōu)估計。4. 如權(quán)利要求3所述的一種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航方法， 其特征是，作為狀態(tài)量的慣性導(dǎo)航器件INS的誤差向量以及各UWB參考節(jié)點的東向和北向位 置的初值根據(jù)實際需要自行設(shè)定。5. 如權(quán)利要求3所述的一種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航方法， 其特征是，所述迭代擴展卡爾曼濾波器的狀態(tài)方程為：圳加丨=[[f Ακν]，.·.5[4~ir' 考節(jié)點數(shù)目； 為慣性導(dǎo)航器件INS測量得到的導(dǎo)航坐標系下目標行人的東向、北向和天向三個方向的加速度誤差； (K、(i)k+1分別為k時刻和k+Ι時刻慣性導(dǎo)航器件INS測量得到的導(dǎo)航坐標系下目標行人 的橫搖、縱搖和航向角誤差、?ΛΙ分別為k時刻和k+Ι時刻INS測量得到的導(dǎo)航坐標系 下目標行人的東向、北向和天向三個方向的速度誤差；▽〗、巧 +1分別為k時刻和k+Ι時刻導(dǎo)航 坐標系下目標行人的東向、北向和天向三個方向的加速度誤差;4分別為k時刻和k+ 1時刻導(dǎo)航坐標系下目標行人的東向、北向和天向三個方向的角速度誤差;#Γ、分別為k 時刻和k+1時刻導(dǎo)航坐標系下目標行人的東向、北向和天向三個方向的位置誤差； ω k為狀態(tài)噪聲;T為采樣周期；I為單位矩陣;C:為從載體系到導(dǎo)航系的狀態(tài)轉(zhuǎn)移矩陣。6.如權(quán)利要求3所述的一種采用偽距和位置信息的室內(nèi)行人UWB/INS緊組合導(dǎo)航方法， 其特征是，所述迭代擴展卡爾曼濾波器的觀測方程為：其中，為k時亥UINS解算的東向和北向位置;為k時亥UINS解算的東向 和北向位置誤差;為k時刻UWB測量得到的未知節(jié)點到第i個參考節(jié)點之間的 偽距；= 為第i個參考節(jié)點的坐標。
【文檔編號】G01C21/16GK105928518SQ201610231406
【公開日】2016年9月7日
【申請日】2016年4月14日
【發(fā)明人】徐元, 張勇, 程金, 趙欽君, 王濱, 王宜敏, 馬思源
【申請人】濟南大學