[发明专利]姿态测量方法及终端设备

摘要

本发明适用于导航测姿技术领域，提供了一种姿态测量方法及终端设备，所述方法包括：依据陀螺、加速度计以及磁强计或者外部航向信息初始化卡尔曼滤波的状态量；构建卡尔曼滤波的状态方程，对载体的姿态信息和传感器误差进行预测；构建量测方程，利用加速度计输出对水平姿态横滚角和俯仰角进行量测更新，并且根据载体的运动状态自适应调整量测方差矩阵；构建航向角的量测方程并且利用磁强计或者外部航向信息对AHRS系统的航向进行量测更新；根据加速度计的输出判断载体的运动状态，判断陀螺零偏误差的反馈策略，利用上述估计的陀螺零偏误差反馈修正陀螺的原始输出，可以解决现有AHRS系统无法为载体姿态解算提供准确可靠信息的问题。

主权项

1.一种姿态测量方法，其特征在于，包括：在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿。