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

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

[0006] 本發(fā)明的目的就是解決上述問題，提供一種基于足部航姿參考和肩部電子羅盤的 個(gè)人導(dǎo)航系統(tǒng)及方法，該方法將肩部電子羅盤測(cè)量得到行人航向信息引入到足部AHRS的 導(dǎo)航信息解算過程中，提高足部AHRS的姿態(tài)轉(zhuǎn)移矩陣的計(jì)算精度。
[0007] 為實(shí)現(xiàn)上述目的，本發(fā)明采用下述技術(shù)方案，包括：
[0008] -種基于足部航姿參考和肩部電子羅盤的個(gè)人導(dǎo)航系統(tǒng)，包括：參考系統(tǒng)和行人 導(dǎo)航系統(tǒng)；
[0009] 所述參考系統(tǒng)包括：碼盤和固定于碼盤的AHRS;
[0010] 所述行人導(dǎo)航系統(tǒng)包括：足部AHRS、肩部電子羅盤和控制器；足部AHRS與肩部電 子羅盤連接，參考系統(tǒng)、足部AHRS和肩部電子羅盤分別于控制器連接；
[0011]肩部電子羅盤將其測(cè)量得到的行人航向信息直接輸入到足部AHRS，作為其解算姿 態(tài)轉(zhuǎn)移矩陣時(shí)所需的航向信息；足部AHRS通過自身攜帶的加速度計(jì)和陀螺儀以及肩部電 子羅盤提供的行人航向信息對(duì)行人的軌跡進(jìn)行解算。
[0012] 一種基于足部航姿參考和肩部電子羅盤的個(gè)人導(dǎo)航方法，包括以下步驟：
[0013] (1)將兩個(gè)低成本AHRS分別安裝于行人的鞋上和肩上，其中安裝于鞋上的AHRS主 要負(fù)責(zé)導(dǎo)航解算，肩上的AHRS作為電子羅盤主要負(fù)責(zé)行人航向測(cè)量；
[0014] (2)肩上的AHRS將其測(cè)量得到的行人航向信息直接輸入到鞋上的AHRS，作為其解 算姿態(tài)轉(zhuǎn)移矩陣時(shí)所需的航向信息；鞋上的AHRS通過自身攜帶的加速度計(jì)和陀螺儀以及 肩上的AHRS提供的行人航向信息對(duì)行人的軌跡進(jìn)行解算；
[0015] (3)將人的行走分為靜止和運(yùn)動(dòng)兩個(gè)狀態(tài)，對(duì)行人的當(dāng)前運(yùn)動(dòng)狀態(tài)進(jìn)行判斷；
[0016] (4) 一旦行人處于靜止?fàn)顟B(tài)，將鞋上的AHRS通過將自身攜帶的3個(gè)加速度和3個(gè) 陀螺儀傳感器所采集到的數(shù)據(jù)作為卡爾曼濾波器的觀測(cè)量，并對(duì)AHRS采集到的數(shù)據(jù)進(jìn)行 誤差補(bǔ)償，最終得到當(dāng)前時(shí)刻最優(yōu)的導(dǎo)航信息預(yù)估；
[0017] (5)將AHRS解算的速度和姿態(tài)信息與卡爾曼濾波器輸出的誤差預(yù)估分別作為人 工智能算法的培訓(xùn)輸入和目標(biāo)輸入，對(duì)二者的映射關(guān)系進(jìn)行構(gòu)建；
[0018] (6) -旦行人處于運(yùn)動(dòng)狀態(tài)，人工智能算法進(jìn)入預(yù)估階段；在這一階段，AHRS中輸 出的速度和姿態(tài)信息作為預(yù)估輸入傳送到人工智能算法中，通過構(gòu)建的映射關(guān)系，對(duì)卡爾 曼濾波器失鎖的觀測(cè)向量進(jìn)行預(yù)估，保證卡爾曼濾波器的正常工作，進(jìn)而保證整個(gè)導(dǎo)航系 統(tǒng)的無縫導(dǎo)航。
[0019] 所述步驟（3)中判斷行人的當(dāng)前運(yùn)動(dòng)狀態(tài)的方法為：
[0020] 鞋上的AHRS通過將自身攜帶的加速度和陀螺儀傳感器所采集到的數(shù)據(jù)計(jì)算當(dāng)前 時(shí)刻鞋子的加速度模值，通過加速度模值的大小判斷鞋子的運(yùn)動(dòng)狀態(tài)。
[0021] 通過加速度模值的大小判斷鞋子的運(yùn)動(dòng)狀態(tài)的具體方法為：
[0023] 其中，加速度模值
ax、ay、az分別為在載體坐標(biāo)系下的X、 y和z三個(gè)方向的加速度值。
[0024] 所述步驟（4)中一旦當(dāng)前時(shí)刻鞋子處于靜止?fàn)顟B(tài)，所采用的卡爾曼濾波器的狀態(tài) 方程具體為：
[0026] 其中，
為AHRS測(cè)量得到的導(dǎo)航坐標(biāo)系下的 東向、北向和天向三個(gè)方向的加速度誤差；SVn為AHRS測(cè)量得到的導(dǎo)航坐標(biāo)系下的東向、 北向和天向三個(gè)方向的速度誤差；巾為AHRS測(cè)量得到的導(dǎo)航坐標(biāo)系下的橫搖、縱搖和航向 角誤差；SP^^/AHRS測(cè)量得到的導(dǎo)航坐標(biāo)系下的東向、北向和天向三個(gè)方向的位置誤差； ▽"為導(dǎo)航坐標(biāo)系下的東向、北向和天向三個(gè)方向的加速度誤差；en為導(dǎo)航坐標(biāo)系下的東 向、北向和天向三個(gè)方向的角速度誤差；為狀態(tài)噪聲；T為采樣周期；I為單位矩陣。
[0027] 所述卡爾曼濾波器的觀測(cè)方程具體為：
[0029] 其中，為當(dāng)前時(shí)刻測(cè)量到的AHRS解算的導(dǎo)航坐標(biāo)系下的東向、北向和天向三 個(gè)方向的速度；S為當(dāng)前時(shí)刻測(cè)量到的AHRS解算的姿態(tài)誤差；nk為濾波器的觀測(cè)噪聲； SVn為AHRS測(cè)量得到的導(dǎo)航坐標(biāo)系下的東向、北向和天向三個(gè)方向的速度誤差；巾為AHRS 測(cè)量得到的導(dǎo)航坐標(biāo)系下的橫搖、縱搖和航向角誤差；SPnSAHRS測(cè)量得到的導(dǎo)航坐標(biāo)系 下的東向、北向和天向三個(gè)方向的位置誤差；▽"為導(dǎo)航坐標(biāo)系下的東向、北向和天向三個(gè) 方向的加速度誤差；e"為導(dǎo)航坐標(biāo)系下的東向、北向和天向三個(gè)方向的角速度誤差。
[0030] 鞋子處于靜止?fàn)顟B(tài)時(shí)，AHRS測(cè)量得到的速度均為AHRS的解算誤差；AHRS測(cè)量得到 的姿態(tài)角均為AHRS的解算誤差。
[0031] 本發(fā)明有益效果：
[0032] 1、該方法將肩部電子羅盤測(cè)量得到行人航向信息引入到足部AHRS的導(dǎo)航信息解 算過程中，以提高足部AHRS的姿態(tài)轉(zhuǎn)移矩陣的計(jì)算精度。
[0033] 2、將人的行走分為靜止和運(yùn)動(dòng)兩個(gè)狀態(tài)。在人處于靜止?fàn)顟B(tài)時(shí)，將鞋上的AHRS通 過將自身攜帶的3個(gè)加速度和3個(gè)陀螺儀傳感器所采集到的數(shù)據(jù)作為卡爾曼濾波器的觀測(cè) 量，并對(duì)AHRS采集到的數(shù)據(jù)進(jìn)行誤差補(bǔ)償，最終得到當(dāng)前時(shí)刻最優(yōu)的導(dǎo)航信息預(yù)估。
[0034] 人工智能算法在此時(shí)處于培訓(xùn)狀態(tài)，將AHRS解算的速度和姿態(tài)信息與卡爾曼濾 波器輸出的誤差預(yù)估分別作為人工智能算法的培訓(xùn)輸入和目標(biāo)輸入，對(duì)二者的映射關(guān)系進(jìn) 行構(gòu)建。一旦行人處于運(yùn)動(dòng)狀態(tài)，人工智能算法進(jìn)入預(yù)估階段；在這一階段，AHRS中輸出的 速度和姿態(tài)信息作為預(yù)估輸入傳送到人工智能算法中，通過構(gòu)建的映射關(guān)系，對(duì)卡爾曼濾 波器失鎖的觀測(cè)向量進(jìn)行預(yù)估，保證卡爾曼濾波器的正常工作，進(jìn)而保證整個(gè)導(dǎo)航系統(tǒng)的 無縫導(dǎo)航。
[0035] 3、可用于室內(nèi)環(huán)境下的智能鞋子的中高精度無縫定位和定向。
【附圖說明】
[0036] 圖1為用于一種基于足部航姿參考和肩部電子羅盤的個(gè)人導(dǎo)航系統(tǒng)示意圖；
[0037] 圖2為行人靜止情況下一種基于足部航姿參考和肩部電子羅盤的室內(nèi)個(gè)人導(dǎo)航 方法的示意圖；
[0038] 圖3為行人運(yùn)動(dòng)情況下一種基于足部航姿參考和肩部電子羅盤的室內(nèi)個(gè)人導(dǎo)航 方法的示意圖。
【具體實(shí)施方式】
[0039] 下面結(jié)合附圖與實(shí)施例對(duì)本發(fā)明做進(jìn)一步說明：
[0040] 如圖1所示，用于一種基于足部航姿參考和肩部電子羅盤的室內(nèi)個(gè)人導(dǎo)航方法的 系統(tǒng)，包括參考系統(tǒng)和行人導(dǎo)航系統(tǒng)兩個(gè)部分。其中行人導(dǎo)航系統(tǒng)包括一個(gè)足部AHRS、一個(gè) 肩部電子羅盤和一個(gè)計(jì)算機(jī)，參考系統(tǒng)包括一個(gè)碼盤和一個(gè)固定于碼盤的AHRS。
[0041] 足部AHRS與肩部電子羅盤連接，參考系統(tǒng)、足部AHRS和肩部電子羅盤分別于控制 器連接；
[0042]肩部電子羅盤將其測(cè)量得到的行人航向信息直接輸入到足部AHRS，作為其解算姿 態(tài)轉(zhuǎn)移矩陣時(shí)所需的航向信息