[发明专利]一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法

说明书

本发明公开一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法，该方法包括以下步骤：(1)将初始时刻的载体系b和导航系n凝固为载体惯性系i_b和导航惯性系i_n，则t时刻的姿态矩阵可分解为由陀螺仪和加速度计测量值计算i_b系和i_n系下的速度v₁(t)和v₂(t)；(2)根据四元数相关性质，定义变量X＝[q^T b_a^T u]^T，并构建非线性函数F(X)＝0；(3)v₁(t)和v₂(t)，计算F(X)对应的Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数；(4)利用牛顿法迭代，解出对应的姿态角四元数q与加速度计常值误差。

技术领域

本发明属于导航技术领域，尤其涉及一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法。

背景技术

制导炮弹是指在传统炮弹基础上加装制导系统和可供驱动的弹翼或尾舱等空气动力装置，其上一般装配有GPS/INS组合。炮弹的姿态估计是后续精确打击的关键，但在制导弹药从平台发射过程中，由于环境恶劣，在高过载冲击环境下陀螺仪、加速度计等弹上导航系统组件是无法正常上电工作的，导航系统的初始化需要发射后在空中自主完成。且空中易受风力等气象环境影响，弹体的姿态估算也是当前的难点技术，提供仪表的常值误差估计则对进一步提升对准精度有重要意义。

发明内容

发明目的：针对以上问题，本发明提出一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法，该方法的目的在于仅利用陀螺仪，加速度计和GPS提供的信息，通过牛顿迭代法解出最优的炮弹姿态估计和加速度计常值误差。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于牛顿迭代法的炮弹姿态和加速度计常值误差方法，该方法包括以下步骤：

(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系i_b系相对于载体系b的炮弹姿态矩阵导航系n相对于导航惯性系i_n的炮弹姿态矩阵

(2)由陀螺仪和加速度计测量值计算i_b系和i_n系下的炮弹速度v₁(t)和v₂(t)；

(3)定义变量X＝[q^T b_a^T u]^T，并构建非线性函数F(X)＝0，其中，q为i_n系至i_b系的变化四元数，b_a＝[0 Ba^T]^T，Ba为炮弹上三轴加速度计的常值误差，u为待定系数；

(4)根据v₁(t)和v₂(t)，计算F对应的Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数；

(5)利用牛顿法迭代法并根据计算对应的姿态角四元数q与加速度计常值误差，为t时刻n系相对b系的姿态矩阵，为i_n系相对于i_b系的姿态矩阵。

其中，步骤(1)中，炮弹姿态矩阵和计算方法如下：

设初始时刻t₀时，b系与i_b系间姿态矩阵为I为3×3的单位矩阵；

记t时刻陀螺仪的输出值为即t时刻的b系相对于i_b系的角速度在b系上的投影值，由此跟踪b系相对于i_b系的变化：