[发明专利]一种基于航向角准确修正的惯性导航室内定位方法

权利要求书

1.一种基于航向角准确修正的惯性导航室内定位方法，包括以下步骤：

A、初始位置与初始航向角的确定：在建筑物入口通过GPS进行初始位置确定，并分别进行两点位置坐标数据采集，获取初始位置以及初始航向角；

B、对采集的三轴和加速度进行去重力、低通滤波等数据预处理；

C、步频检测：在三窗式滑动窗口中采用峰峰值探测法进行检测；首先在三窗式滑动窗口中对和加速度进行波峰波谷检测，并分别存放在于队列中；然后分别依次获取一个波峰、波谷并用横向时间阈值和纵向峰峰值幅值进行有效步频检测；

D、步长估计：采用Weinberg等人提出的非线性的模型：

式中，k为跟行人身高、步频等参数有关的常数，通过训练来获得；A_max、A_min分别是波峰、波谷队列中存储的每一步的加速度的最大值和最小值；

E、航向角估计：使用的九轴惯性导航器件，能够直接实时输出运载体坐标系下的三轴矢量角速度和四元数q并更新；首先将陀螺仪数据进行坐标系转换得到导航坐标系下的角速度然后在步骤A得到的初始方位角ψ₀的基础上，进行角速度积分运算，得到当前航向在下一个周期中需要更新ψ₀＝ψ；为载体坐标系到导航坐标系下的姿态旋转矩阵；

F、航向角修正：室内航向标定为16个主方向，主方向间隔Δ＝22.5°；计算前一时刻航向角ψ_i-1相对于当前主方向的偏移角以及偏移方向；接着计算偏移方向值E_i，通过判断E_i的正负来决定反馈变量I_i；通过增强算法得到实时修正后的航向角ψ_i＝ψ′_i+I_i；

G、航向角的进一步修正：在所得实时修正后的航向角的基础上，通过对行走的路线进行直行和非直行两种路径类型判别，在直线行走下再区分沿着建筑主方向行走和非建筑主方向行走路径，然后分别使用不同的反馈系数i_c，以降低系统对修正航向角的反馈变量的I_i的敏感性，达到航向角准确修正目的；

H、位置推算：通过步频探测、步长估计得到每一步步长d，航向角估计值ψ′_i以及修正得到航向角ψ后，推算得到当前三维位置坐标(X_k,Y_k,Z_k)：

2.根据权利要求1所述的基于航向角准确修正的惯性导航室内定位方法，其特征在于，步骤C中，三窗式滑动窗口为一存储和加速度等数据的队列，将其均等分成三段即三个窗口，选择中间一个窗口数据进行峰值特征提取，而左右两个窗口只参与滤波及数据保存；所述峰峰值探测法是利用横向时间阈值和纵向峰峰值幅值进行加速度波形的波峰值和波谷值的识别、提取；行人常规行走时，每步大概在50～60个采样点之间，横向时间阈值典型值为55；跑的情况下，每步大概在10～40个采样点之间，横向时间阈值典型值为25；纵向峰峰值幅值即波峰值和波谷的绝对值差值大于2.3。

3.根据权利要求1所述的基于航向角准确修正的惯性导航室内定位方法，其特征在于，所述步骤D中，步长估计模型参数k值采用离线测试获取，在测试程序中设定初始k值，行人定位设备穿戴于腰间，在规定距离进行行走测试，测量值和实际值进行对比后不断修正k值大小，取测量最接近真实值的k值，根据不同的行进速度训练不同的k的值；基于峰峰值大小区分走、跑步两种状态下的k值，公式如下：

式中，k₁为在行走状态下的k值，取0.4332；k₂为跑步状态下的k值，取0.4792；vpp为和加速度的峰峰值，d为步长。