[发明专利]一种结合行人运动场景约束的方位角校准算法

说明书

本发明公开了一种结合行人运动场景约束的方位角校准算法，在捷联式惯导系统中，利用陀螺仪测量载体各轴旋转运动的瞬时角度增量(角速度)，采用积分的方式计算方位信息，随着时间的增长，方位角误差会呈现发散的状态，使得系统的长期精度差，基于地磁和重力属性的电子罗盘，不存在误差累积的问题，但是当存在干扰磁场时，磁强计的数据反而会使方位角产生较大偏差，则引入基于运动场景约束的方位角校准算法。它采用一些已知的规则运动模式，如结合行人在地面上直线行走模式、上下楼梯模式等将其作为约束条件，以纠正方位角误差，本发明提出的方位角校准算法避免了由于陀螺漂移和外界磁场干扰带来的方位角偏差的问题，进一步提高了导航定位精度。

技术领域

本发明设计的是一种方位角校准算法，尤其是涉及一种结合行人运动场景约束的自主方位角校准算法。

背景技术

捷联惯性导航系统可以实现完全自主导航，不需要外部设施。近年来，基于低成本捷联惯导系统，小型MEMS惯性传感器在一些军事和民用导航定位中已经受到了越来越多的关注，特别是卫星导航不可用的情况下，如室内、水下或地下。对于捷联式惯性导航定位系统来说，载体方位角的解算主要依靠陀螺仪完成，而陀螺仪具有积分漂移特性，当方位角偏差较大时，该偏差将成为位置误差的主要来源。因此，根据MEMS陀螺仪精度的现状，偏差随时间累积上升很快，如果不采用必要的方位角误差定期校正措施，很难支持长时间的准确导航定位。

为了提高MEMS捷联惯导系统的方位角估计精度，往往引入磁强计作为辅助进行方位角的校准，磁强计是最常用的辅助方位角校准装置，由于磁强计也属于自主型元件，因此引入磁强计的惯性导航定位系统能保持其自主性不受影响。但是当存在干扰磁场时，磁强计的数据反而会使方位角产生较大偏差，使得系统的长期精度差。故现有的方位角校准算法在存在磁场干扰的环境下不能保证方位角长时间高精度的准确输出。

发明内容

本发明的目的在于克服现有方位角校准算法的不足，提供一种结合行人运动场景约束的方位角校准算法。

本发明采用的技术方案为：

一种结合行人运动场景约束的方位角校准算法，包括以下步骤：

(1)根据传感器输出的行人在直线行走模式或行人在楼梯行走模式中的测量数据，通过捷联惯导算法得到行人在直线行走模式或行人在楼梯行走模式中每一时刻行人的速度、位置解算结果和方位解算结果；并根据陀螺仪测量的数据进行四阶龙格库塔算法得到的四元数；所述传感器输出的测量数据包括加速度计测量的数据和陀螺仪测量的数据；

(2)根据位置解算结果、零速检测和零速修正技术，得到相邻两个零速区间上的竖直方向位移差；根据方位角解算结果、零速检测和零速修正技术，得到相邻两个零速区间上的航向角偏差；

(3)根据相邻两个零速区间上的竖直方向位移差和航向角偏差以及四元数建立卡尔曼滤波器，进行行人在直线行走模式或行人在楼梯行走模式中航向角的更新和校准。

其中，步骤(3)中建立的卡尔曼滤波器具体为：

式中，X(K)和X(k-1)分别为卡尔曼滤波算法中k时刻和k-1时刻的状态量，Z(K)为卡尔曼滤波算法中k时刻的观测量；Φ(k,k-1)为卡尔曼滤波算法中的过程转移矩阵，Γ(k-1)为卡尔曼滤波器的外加矩阵，W(k-1)为陀螺噪声矩阵，H(k)为状态变化过程中的调整系数，V(k)为观测噪声；

其中，k-1时刻的状态量和观测量分别表示为：

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

Z(k-1)＝[ΔSz；Δyaw]