[发明专利]一种室内惯性导航定位的航向漂移误差消除方法

摘要

一种室内惯性导航定位的航向漂移误差消除方法，基于惯性测量单元实现，该惯性测量单元包括有三轴加速度计、三轴磁力计和三轴陀螺仪，预先通过三轴加速度计和三轴磁力计来计算姿态四元数初值；在室内惯性导航定位过程中，先通过三轴陀螺仪读取角速度ω(ωx,ωy,ωz)来判断惯性测量单元是否处于运动状态，若是，则结合角速度ω(ωx,ωy,ωz)并通过四元数微分方程计算实时的姿态四元数Q(q0,q1,q2,q3)，若否，则保持当前姿态四元数不变；最后，通过实时的姿态四元数求解惯性测量单元的航向角和姿态角。本发明提出的航向漂移误差的消除方法，可以有效的解决由于静止时陀螺仪不工作导致的航向漂移误差，从而提高室内行人惯性导航定位系统的精度。

主权项

一种室内惯性导航定位的航向漂移误差消除方法，基于惯性测量单元实现，该惯性测量单元包括有三轴加速度计、三轴磁力计和三轴陀螺仪，其特征在于：预先通过三轴加速度计和三轴磁力计来计算姿态四元数初值；在室内惯性导航定位过程中，先通过三轴陀螺仪读取角速度ω(ωx,ωy,ωz)来判断惯性测量单元是否处于运动状态，若是，则结合角速度ω(ωx,ωy,ωz)并通过四元数微分方程计算实时的姿态四元数Q(q0,q1,q2,q3)，若否，则保持当前姿态四元数不变；最后，通过实时的姿态四元数求解惯性测量单元的航向角和姿态角；所述的姿态四元数初值通过如下步骤计算得到:首先，通过三轴加速度计确定初始姿态角，根据该初始姿态角计算坐标变换矩阵；其次，通过三轴磁力计的磁场强度结合坐标变换矩阵得到与当地水平面平行的坐标系的磁场强度，根据该坐标系的X轴和Y轴的磁场强度计算初始航向角；最后，通过欧拉角和四元数之间的转换公式，得到初始姿态四元数。