[发明专利]一种基于MEMS惯性导航的室内行人方位校准方法

权利要求书

1.一种基于MEMS惯性导航的室内行人方位校准方法，其特征在于，包括如下步骤：

步骤S1，对MEMS惯性传感器采集的数据进行地磁干扰评估，判断地磁场是否受到强烈干扰，如果是，则采用室内场景模式进行方位角校准，否则采用Kalman滤波和互补滤波相融合的算法进行方位角校准；

其中，对采集数据中的地磁信号进行预处理，根据磁强计三个轴上磁场强度的输出矢量和与标准地磁作对比，对磁强计各个轴上的矢量和与标准地磁进行差值计算，如果计算得到的偏差超出预设阈值范围，则判断地磁场受到强烈干扰，采用室内场景模式进行方位角校准；如果计算得到的偏差未超出预设阈值范围，则判断地磁场没有受到强烈干扰，采用Kalman滤波和互补滤波相融合的算法进行方位角校准；

根据下述公式判断地磁场是否受到强烈干扰，

其中，m(i)为磁强计输出的磁场强度，m_x，m_y，m_z表示三个轴的磁场强度，m₀表示本地标准地磁场强度，

|m(i)-m₀|＜0.1*m₀

当上式条件满足时，则判断地磁场没有受到强烈干扰，采用Kalman滤波和互补滤波相融合的算法进行方位角校准；而当上述条件不成立时，判断地磁场受到强烈干扰，采用室内场景模式进行方位角校准；

步骤S2，当判断采用Kalman滤波和互补滤波相融合的算法进行方位角校准时，执行如下步骤：

(1)进行第一次Kalman滤波，将加速度计测量值与重力场在载体坐标系下投影的差值和磁强计测量值与当地磁场强度在载体坐标系下投影的差值共同作为Kalman滤波算法中的观测值；将陀螺仪的测量值通过四阶-龙格库塔法进行解算得到不断更新的四元数将其作为Kalman滤波的状态值；

(2)利用互补滤波算法，由电子罗盘对陀螺仪校正之后得到的校正角速度通过四阶龙格库塔法得到校正后的四元数；

(3)根据第一次Kalman滤波得到的优化四元数和互补滤波算法得到的校正四元数，进行第二次Kalman滤波，通过Kalman滤波算法的不断更新，得到最优方位角；

步骤S3，当判断采用室内场景模式进行方位角校准，执行如下步骤：利用将地理坐标系下的重力与加速度计在载体坐标系下测量得到的加速度做向量积的运算来校正陀螺偏差，校正后的角速度值，通过四阶龙格库塔法可直接得出最优方位角。

2.如权利要求1所述的基于MEMS惯性导航的室内行人方位校准方法，其特征在于，在所述步骤S2中，对于步骤(1)中进行第一次Kalman滤波，

在第一次Kalman滤波算法中设状态估计量X₁(k)为

X₁(k)＝[q₀,q₁,q₂,q₃]^T

设第一次Kalman滤波器的观测量Z₁为

其中，a为加速度计测量值，a _s为重力场在载体坐标系下投影；m为磁强计测量值，m_s为当地磁场强度在载体坐标系下投影；为姿态矩阵参数，q₀,q₁,q₂,q₃为陀螺仪通过四阶龙格库塔法所得值；

将其按照所述的Kalman滤波算法可以得到第一次最优估计四元数q₀₀,q₁₁,q₂₂,q₃₃。