[發(fā)明專利]一種基于航向角準(zhǔn)確修正的慣性導(dǎo)航室內(nèi)定位方法

在審

• 申請?zhí)枺?/td>: 202110247534.4
• 申請日: 2021-03-06
• 公開（公告）號: CN113029148A
• 公開（公告）日: 2021-06-25
• 發(fā)明（設(shè)計）人: 鄧平;朱飛翔;趙榮鑫;王浩祥;吳明輝;張劍
• 申請（專利權(quán)）人: 西南交通大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G01S19/47
• 代理公司: 成都盈信專利代理事務(wù)所(普通合伙) 51245
• 代理人: 張澎

1.一種基于航向角準(zhǔn)確修正的慣性導(dǎo)航室內(nèi)定位方法，包括以下步驟：

A、初始位置與初始航向角的確定：在建筑物入口通過GPS進行初始位置確定，并分別進行兩點位置坐標(biāo)數(shù)據(jù)采集，獲取初始位置以及初始航向角；

B、對采集的三軸和加速度進行去重力、低通濾波等數(shù)據(jù)預(yù)處理；

C、步頻檢測：在三窗式滑動窗口中采用峰峰值探測法進行檢測；首先在三窗式滑動窗口中對和加速度進行波峰波谷檢測，并分別存放在于隊列中；然后分別依次獲取一個波峰、波谷并用橫向時間閾值和縱向峰峰值幅值進行有效步頻檢測；

D、步長估計：采用Weinberg等人提出的非線性的模型：

式中，k為跟行人身高、步頻等參數(shù)有關(guān)的常數(shù)，通過訓(xùn)練來獲得；A_max、A_min分別是波峰、波谷隊列中存儲的每一步的加速度的最大值和最小值；

E、航向角估計：使用的九軸慣性導(dǎo)航器件，能夠直接實時輸出運載體坐標(biāo)系下的三軸矢量角速度和四元數(shù)q并更新；首先將陀螺儀數(shù)據(jù)進行坐標(biāo)系轉(zhuǎn)換得到導(dǎo)航坐標(biāo)系下的角速度然后在步驟A得到的初始方位角ψ₀的基礎(chǔ)上，進行角速度積分運算，得到當(dāng)前航向在下一個周期中需要更新ψ₀＝ψ；為載體坐標(biāo)系到導(dǎo)航坐標(biāo)系下的姿態(tài)旋轉(zhuǎn)矩陣；

F、航向角修正：室內(nèi)航向標(biāo)定為16個主方向，主方向間隔Δ＝22.5°；計算前一時刻航向角ψ_i-1相對于當(dāng)前主方向的偏移角以及偏移方向；接著計算偏移方向值E_i，通過判斷E_i的正負(fù)來決定反饋變量I_i；通過增強算法得到實時修正后的航向角ψ_i＝ψ′_i+I_i；

G、航向角的進一步修正：在所得實時修正后的航向角的基礎(chǔ)上，通過對行走的路線進行直行和非直行兩種路徑類型判別，在直線行走下再區(qū)分沿著建筑主方向行走和非建筑主方向行走路徑，然后分別使用不同的反饋系數(shù)i_c，以降低系統(tǒng)對修正航向角的反饋變量的I_i的敏感性，達到航向角準(zhǔn)確修正目的；

H、位置推算：通過步頻探測、步長估計得到每一步步長d，航向角估計值ψ′_i以及修正得到航向角ψ后，推算得到當(dāng)前三維位置坐標(biāo)(X_k,Y_k,Z_k)：

2.根據(jù)權(quán)利要求1所述的基于航向角準(zhǔn)確修正的慣性導(dǎo)航室內(nèi)定位方法，其特征在于，步驟C中，三窗式滑動窗口為一存儲和加速度等數(shù)據(jù)的隊列，將其均等分成三段即三個窗口，選擇中間一個窗口數(shù)據(jù)進行峰值特征提取，而左右兩個窗口只參與濾波及數(shù)據(jù)保存；所述峰峰值探測法是利用橫向時間閾值和縱向峰峰值幅值進行加速度波形的波峰值和波谷值的識別、提?。恍腥顺Ｒ?guī)行走時，每步大概在50～60個采樣點之間，橫向時間閾值典型值為55；跑的情況下，每步大概在10～40個采樣點之間，橫向時間閾值典型值為25；縱向峰峰值幅值即波峰值和波谷的絕對值差值大于2.3。

3.根據(jù)權(quán)利要求1所述的基于航向角準(zhǔn)確修正的慣性導(dǎo)航室內(nèi)定位方法，其特征在于，所述步驟D中，步長估計模型參數(shù)k值采用離線測試獲取，在測試程序中設(shè)定初始k值，行人定位設(shè)備穿戴于腰間，在規(guī)定距離進行行走測試，測量值和實際值進行對比后不斷修正k值大小，取測量最接近真實值的k值，根據(jù)不同的行進速度訓(xùn)練不同的k的值；基于峰峰值大小區(qū)分走、跑步兩種狀態(tài)下的k值，公式如下：

式中，k₁為在行走狀態(tài)下的k值，取0.4332；k₂為跑步狀態(tài)下的k值，取0.4792；vpp為和加速度的峰峰值，d為步長。