一種基于雙imu的雙模式室內(nèi)個人導(dǎo)航系統(tǒng)及方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于復(fù)雜環(huán)境下組合定位技術(shù)領(lǐng)域，尤其設(shè)及一種基于雙IMU的雙模式室 內(nèi)個人導(dǎo)航系統(tǒng)及方法。
【背景技術(shù)】
[0002] 近年來，行人導(dǎo)航任edestrian化vigation,PN)作為導(dǎo)航技術(shù)應(yīng)用的新興領(lǐng)域， 正越來越受到各國學(xué)者的重視，并逐漸成為該領(lǐng)域的研究熱點。然而在隧道、大型倉庫、地 下停車場等室內(nèi)環(huán)境下，外界無線電信號微弱、電磁干擾強烈等因素都會對目標行人導(dǎo)航 信息獲取的準確性、實時性及魯棒性有很大影響。如何將室內(nèi)環(huán)境下獲取的有限信息進行 有效的融合W消除室內(nèi)復(fù)雜環(huán)境影響，保證行人導(dǎo)航精度的持續(xù)穩(wěn)定，具有重要的科學(xué)理 論意義和實際應(yīng)用價值。
[0003] 在現(xiàn)有的定位方式中，全球衛(wèi)星導(dǎo)航系統(tǒng)（Global化vigationSatellite System,GNS巧是最為常用的一種方式。雖然GNSS能夠通過精度持續(xù)穩(wěn)定的位置信息，但是 其易受電磁干擾、遮擋等外界環(huán)境影響的缺點限制了其應(yīng)用范圍，特別是在室內(nèi)、地下巷道 等一些密閉的、環(huán)境復(fù)雜的場景，GNSS信號被嚴重遮擋，無法進行有效的工作。近年來，WSN W其低成本、低功耗和低系統(tǒng)復(fù)雜度的特點在短距離局部定位領(lǐng)域表現(xiàn)出很大的潛力。學(xué) 者們提出將基于WSN的目標跟蹤應(yīng)用于GNSS失效環(huán)境下的行人導(dǎo)航。運種方式雖然能夠 實現(xiàn)室內(nèi)定位，但是由于室內(nèi)環(huán)境復(fù)雜多變，WSN信號十分容易受到干擾而導(dǎo)致定位精度下 降甚至失鎖；與此同時，目前的導(dǎo)航定位精度為米級，不能保證對室內(nèi)行人高精度的導(dǎo)航需 求；除此之外，由于WSN采用的通信技術(shù)通常為短距離無線通信技術(shù)，因此若想完成大范圍 的室內(nèi)目標跟蹤定位，需要大量的網(wǎng)絡(luò)節(jié)點共同完成，運必將引入網(wǎng)絡(luò)組織結(jié)構(gòu)優(yōu)化設(shè)計、 多節(jié)點多簇網(wǎng)絡(luò)協(xié)同通信等一系列問題。因此現(xiàn)階段基于WSN的目標跟蹤在室內(nèi)導(dǎo)航領(lǐng)域 仍舊面臨很多挑戰(zhàn)。為了克服上述兩種導(dǎo)航方法需要參考節(jié)點并容易產(chǎn)生失鎖的缺點，學(xué) 者們提出將航姿參考系統(tǒng)（IMU)應(yīng)用于小區(qū)域目標跟蹤領(lǐng)域。IMU具有全自主、運動信息全 面、短時、高精度的優(yōu)點，雖然可W實現(xiàn)自主導(dǎo)航，但誤差隨時間積累，長航時運行條件下將 導(dǎo)致導(dǎo)航精度嚴重下降。

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

[0004] 本發(fā)明的目的就是解決上述問題，提供一種基于雙IMU的雙模式室內(nèi)個人導(dǎo)航系 統(tǒng)及方法，該系統(tǒng)及方法將行人運行分為靜止和運動兩個狀態(tài)，分別采用兩個卡爾曼濾波 器對行人運行軌跡誤差進行解算，通過雙模式導(dǎo)航算法，能夠在整個行走環(huán)節(jié)中實現(xiàn)對IMU 解算的誤差漂移進行限制，從而提高IMU的解算精度。
[0005] 為實現(xiàn)上述目的，本發(fā)明采用下述技術(shù)方案，包括：
[0006] 一種基于雙IMU的雙模式室內(nèi)個人導(dǎo)航系統(tǒng)，包括：參考系統(tǒng)和行人導(dǎo)航系統(tǒng)；
[0007] 所述參考系統(tǒng)包括：碼盤和固定于碼盤的IMU;
[000引所述行人導(dǎo)航系統(tǒng)包括：足部IMU、肩部電子羅盤和控制器；足部IMU與肩部電子 羅盤連接，參考系統(tǒng)、足部IMU和肩部電子羅盤分別與控制器連接；
[0009] 肩部電子羅盤將其測量得到的行人航向信息直接輸入到足部IMU，足部IMU通過 自身攜帶的加速度計和巧螺儀W及肩部電子羅盤提供的行人航向信息對行人的軌跡進行 解算；通過足部IMU所采集到的數(shù)據(jù)判斷行人所處運動狀態(tài)，根據(jù)運動狀態(tài)的不同分別進 行解算誤差的預(yù)估；
[0010] 利用固定在肩部的IMU測量得到的航向信息對足部IMU解算的導(dǎo)航信息進行誤差 預(yù)估。
[0011] 一種基于雙IMU的雙模式室內(nèi)個人導(dǎo)航方法，包括W下步驟：
[0012] (1)將兩個IMU分別安裝于行人的足部和肩上；
[0013] (2)足部IMU和肩上的IMU通過其自身攜帶的磁力計和加速度計測量得到的數(shù)據(jù) 解算得到兩個IMU各自的姿態(tài)轉(zhuǎn)移矩陣，在此基礎(chǔ)上，利用計算得到的姿態(tài)轉(zhuǎn)移矩陣與加 速度計得到的信息，得到導(dǎo)航坐標系下的加速度信息，通過對加速度信息的積分，得到速度 信息，進一步，通過對速度的積分，得到位置信息；
[0014] (3)將人的行走分為靜止和運動兩個狀態(tài)；通過足部IMU自身攜帶的3個加速度 和3個巧螺儀傳感器所采集到的數(shù)據(jù)進行計算，判斷行人處于哪個狀態(tài)；
[0015] (4)如果行人處于靜止狀態(tài)，通過第一卡爾曼濾波器對當前時刻足部IMU的解算 誤差進行預(yù)估，將足部IMU解算的導(dǎo)航信息與第一卡爾曼濾波器計算的當前時刻最優(yōu)誤差 預(yù)估做差，最終得到當前時刻最優(yōu)的導(dǎo)航信息預(yù)估；導(dǎo)航信息包括行人的位置、速度和姿態(tài) 的信息，其中，姿態(tài)信息包括：橫搖、縱搖和航向角信息。
[0016] (5)如果行人處于運動狀態(tài)，通過第二卡爾曼濾波器對當前時刻足部IMU的解算 誤差進行預(yù)估，將足部IMU解算的航向角信息與第二卡爾曼濾波器輸出的最優(yōu)航向角誤差 預(yù)估做差，得到當前時刻最優(yōu)的航向角預(yù)估，在此基礎(chǔ)上，重新計算姿態(tài)轉(zhuǎn)移矩陣，利用重 新計算的姿態(tài)轉(zhuǎn)移矩陣和加速度信息，最終得到當前時刻最優(yōu)的導(dǎo)航信息預(yù)估。
[0017] 所述步驟（4)中判斷行人處于哪個狀態(tài)的具體方法為：
[0018] 通過足部IMU自身攜帶的3個加速度傳感器采集到的數(shù)據(jù)，得到當前時刻行人的 加速度模值，通過加速度模值的大小判斷鞋子的運動狀態(tài)：
[0020] 其中，加速度模值：\\Aee\\ = +〇^+￡il,曰^、曰^、a,分別為在載體坐標系下的x、 y和z=個方向的加速度值。
[0021] 行人處于靜止狀態(tài)時，行人速度和角速度均為零，足部IMU解算出的導(dǎo)航信息即 為此時速度和角速度的誤差預(yù)估；
[002引將固定在足部的IMU解算的航向角信息與固定在肩部的IMU測量得到的航向角信 息做差，差值作為航向角的誤差預(yù)估。
[0023] 行人處于靜止狀態(tài)時，將足部IMU解算的行人速度誤差、角速度誤差和航向角誤 差作為第一卡爾曼濾波器的觀測量，第一卡爾曼濾波器的狀態(tài)方程具體為： CN105115507A 說明書 3/6 頁
[00巧]其中
為IMU測量得到的導(dǎo)航坐標系下的 東向、北向和天向S個方向的加速度誤差；S'嫁為IMU測量得到的導(dǎo)航坐標系下的東向、北 向和天向S個方向的速度誤差；5Pk"為AHRS測量得到的導(dǎo)航坐標系下的東向、北向和天向 S個方向的位置誤差；(K為IMU測量得到的導(dǎo)航坐標系下的橫搖、縱搖和航向角誤差；vt 為載體坐標系下的東向、北向和天向=個方向的加速度誤差；誠為載體坐標系下的x，y和Z S個方向的角速度誤差；為狀態(tài)噪聲，G為載體坐標系到導(dǎo)航坐標系的姿態(tài)轉(zhuǎn)移矩陣， T為采樣周期；I為單位矩陣。
[0026] 行人處于靜止狀態(tài)時，第一卡爾曼濾波器的觀測方程具體為：
[002引其中，擁|*為當前時刻測量到的加1]解算的導(dǎo)航坐標系下的東向、北向和天向立個 方向的速度，由于鞋子處于靜止狀態(tài)，因此當前時刻的理論值為零，故足部IMU測量得到的 速度均為足部IMU的解算誤差；SYaw= 為固定在足部的IMU測量得 到的航向角與固定在肩部的IMU測量的到的航向角之差；ru為濾波器的觀測噪聲；為IMU測量得到的導(dǎo)航坐標系下的東向、北向和天向S個方向的速度誤差；(K為I