基于AHRS的精对准算法

摘要

本发明公开了一种基于AHRS的精对准算法，主要解决了现有技术中存在的精对准技术收敛较慢、误差估计精度不高，不能满足人们需求的问题。该基于AHRS的精对准算法采用两个方向上的加速计的输出和陀螺仪三个方向上的输出做观测值，并采用卡尔曼滤波求姿态角的误差，进而进行精对准。通过上述方案，本发明达到了收敛较快，精度较高的目的，具有很高的实用价值和推广价值。

主权项

基于AHRS的精对准算法，其特征在于，包括以下步骤：(1)采集当前的纬度信息和重力加速度信息；(2)根据AHRS中加速度计和陀螺仪的输出测得当前的姿态转移矩阵C_bp；(3)测算出地球相对于惯性坐标系的旋转角速度w_ie；(4)确定当前X轴向的角度误差、Y轴向的角度误差、Z轴向的角度误差、陀螺仪在X轴向的常值偏移、陀螺仪在Y轴向的常值偏移、陀螺仪在Z轴向的常值偏移、加速度计在X轴向的常值偏移和加速度计在Y轴向的常值偏移的被估计值；(5)根据步骤(4)中被估计值前后两个时刻的关系得出连续卡尔曼滤波下的状态转移矩阵A，并根据公式F＝eye(8)+A×t；将其转换为离散卡尔曼滤波下的状态转移矩阵，其中，t为滤波间隔，eye(8)为维度为8的单位矩阵；(6)根据当前噪声和观测值前后两个时刻间的关系求出噪声驱动矩阵G，根据被估计值和观测值之间的关系求出观测矩阵H，根据陀螺仪的随机误差确定AHRS中的系统噪声方差矩阵，并根据加速度计的随机误差确定观测噪声方差矩阵；(7)将被估计值的初始滤波值设置为零；(8)开始卡尔曼滤波，每次取两个方向上的加速度计的输出和陀螺仪的三个方向上的输出作为观测值；(9)根据求得的滤波结果和方差更新被估计值和观测噪声方差的数据，并再次取观测值进行卡尔曼滤波；(10)返回步骤(8)，直至两次滤波结果小于用户设定的阈值；(11)使用求得的X轴向、Y轴向和Z轴向上的姿态角的误差对姿态转移矩阵C_bp进行纠正。