[发明专利]基于AHRS的精对准算法

权利要求书

1.基于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为滤波间隔；

（6）根据当前噪声和观测值前后两个时刻间的关系求出噪声驱动矩阵G，根据被估计值和观测值之间的关系求出观测矩阵H，根据陀螺仪的随机误差确定AHRS中的系统噪声方差矩阵，并根据加速度计的随机误差确定观测噪声方差矩阵；

（7）将被估计值的初始滤波值设置为零；

（8）开始卡尔曼滤波，每次取两个方向上的加速度计的输出和陀螺仪的三个方向上的输出作为观测值；

（9）根据求得的滤波结果更新观测噪声方差矩阵和被估计值，并再次取观测值进行卡尔曼滤波；

（10）返回步骤（8），直至两次滤波结果小于用户设定的阈值；

（11）使用求得的X轴向、Y轴向和Z轴向上的姿态角的误差对姿态转移矩阵C_bp进行纠正。

2.根据权利要求1所述的基于AHRS的精对准算法，其特征在于，所述步骤（2）具体包括以下步骤：

（2a）根据AHRS中加速度计和陀螺仪的输出得出重力加速度在地理坐标系下的投影和地球绕惯性坐标系自转角速度在地理坐标系下的投影

（2b）根据公式rt→=g→g→×w→ie(g→×w→ie)×g→=r1t→r2t→r3t→]]>构造坐标系下的三个向量和

（2c）根据方程式r1b→=Ctbr1t→r2b→=Ctbr2t→r3b→=Ctbr3t→]]>解得矩阵其中，t为时间间隔，将以平台坐标系表示；

（2d）根据公式计算出姿态转移矩阵C_bp。