[发明专利]基于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为滤波间隔；

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

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

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

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

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

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

具体地说，所述步骤（2）具体包括以下步骤：

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