[发明专利]AHRS高精度姿态数据计算方法

说明书

技术领域

本发明涉及一种导航领域的姿态数据计算方法，具体地说，是涉及一种AHRS高精度姿态数据计算方法。

背景技术

众所周知，惯性导航主要涉及两方面的内容：定位和定姿，定位可通过全球卫星定位系统（如GPS、北斗等）完成；定姿主要将加速度计输出的初始角度信息和陀螺仪输出的三轴角速度信息通过积分计算得到当前的姿态数据（主要包括方向角、俯仰角和翻滚角）。由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求。

发明内容

本发明的目的在于提供一种AHRS高精度姿态数据计算方法，主要解决现有技术中存在的由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求问题。

为了实现上述目的，本发明采用的技术方案如下：

AHRS高精度姿态数据计算方法，包括以下步骤：

（1）将AHRS中的传感器和归一化坐标系转换矩阵进行初始化处理；

（2）读取AHRS中加速度计和磁力计的数据，计算载体的姿态数据和此时的坐标系转换矩阵R₁；

（3）读取AHRS中陀螺仪的数据，根据上一时刻的归一化坐标系转换矩阵计算该时刻的坐标系转换矩阵R₂；

（4）根据坐标系转换矩阵R₁和坐标系转换矩阵R₂确定该时刻最终的坐标系转换矩阵，并对最终的坐标系转换矩阵进行归一化，得到该时刻的归一化坐标系转换矩阵；

（5）根据步骤（4）中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据；

（6）返回步骤（2），进行下一时刻的姿态数据计算。

所述步骤（1）具体包括：

（1a）配置传感器的各项参数；

（1b）根据此时加速度计和磁力计的输出数据初始化归一化坐标系转换矩阵R_normalized。

所述步骤（1b）包括以下步骤：

（1b1）从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z；从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z；

（1b2）根据公式计算俯仰角θ；根据公式计算翻滚角φ；根据公式