一种基于牛顿迭代法的炮弹姿态估计方法技术

本发明专利技术公开一种基于牛顿迭代法的炮弹姿态估计方法，该方法包括以下步骤：(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵

A New Method of Artillery Attitude Estimation Based on Newton Iteration

The invention discloses a method for estimating artillery shell attitude based on Newton iteration method, which includes the following steps: (1) calculating the artillery shell attitude matrix of carrier inertial system IB system relative to carrier system B at T-Time according to the velocity and position of the projectile provided by gyroscope and GPS

【技术实现步骤摘要】
一种基于牛顿迭代法的炮弹姿态估计方法
本专利技术属于导航
，尤其涉及一种基于牛顿迭代法的炮弹姿态估计方法。
技术介绍
制导炮弹是指在传统炮弹基础上加装制导系统和可供驱动的弹翼或尾舱等空气动力装置，以提高炮弹对目标打击精度的一种低成本、小型化的精确制导武器。GPS/INS组合制导弹药从平台发射过程中，通常会承受高过载、高转速的恶劣情况，在此高过载冲击环境下陀螺仪、加速度计等弹上导航系统组件是无法正常上电工作的，所有弹上设备必须在经受此冲击出管后上电工作，导航系统的初始化需要发射后在空中自主完成。且空中易受风力等气象环境影响，弹体的姿态估算是后续导航系统工作的前提，也是当前的难点技术。弹体姿态探测的常用方法，主要包括采用地磁传感器、GPS、惯性系统以及组合的姿态探测方法，根据载体绕质心运动的方程,利用陀螺测量角速度信息对姿态角进行估计等。然而上述方法在实际使用时需要引入地磁传感器，增大了成本，且一般的组合导航估计算法在高动态复杂的环境中效果不佳。利用陀螺测量角速度信息对滚转角进行估计时将陀螺测量值近似为载体相对于导航系运动的角速度信息,在高速飞行下,这种近似将带来很大的计算误差，甚至不能满足粗估计的要求。
技术实现思路
专利技术目的：针对以上现有技术存在的问题，本专利技术提出一种基于牛顿迭代法的炮弹姿态估计方法，该方法目的在仅利用陀螺仪，加速度计和GPS提供的信息，通过牛顿迭代法解出炮弹最优的姿态估计。技术方案：为实现本专利技术的目的，本专利技术所采用的技术方案是：一种基于牛顿迭代法的炮弹姿态估计方法，该方法包括以下步骤：(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵导航系n相对于导航惯性系in的炮弹姿态矩阵(2)由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)；(3)定义变量X＝[qTu]T，并构建非线性函数F(X)＝0；其中，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数；(4)通过函数F(X)一阶偏导数和二阶偏导数，求Jacobian矩阵和Hessian矩阵；(5)利用牛顿法迭代解出四元数q，根据计算出姿态角。其中，步骤(1)中，炮弹姿态矩阵和计算方法如下：设初始时刻t0时，b系相对于ib系的炮弹姿态矩阵为I为3×3的单位矩阵；记t时刻陀螺仪的输出值为即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：其中，为矩阵的变化率，“×”表示三维矢量对应的叉乘矩阵变换，设其中a，b，c分别表示炮弹沿三轴的旋转角速度，则采用毕卡法解式(1)微分方程可得到式(2)：记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；为tk-1时刻陀螺仪输出，dt为采样周期，最终t＝tk，k＝1,2,3,...,m，由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度可计算如下：其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由计算出其中，为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；为t＝tk-1时的值，dt为采样周期，t＝tk，k＝12,3,...,m。其中，步骤(2)中，炮弹速度v1(t)和v2(t)计算方法如下：其中，fb(t)为t时刻炮弹上加速度计的输出，表示炮弹三轴加速度，通过姿态矩阵将fb(t)投影到ib系中，得到其中，Vn(t)为t时刻的n系炮弹速度，Vn(0)为起始时刻的n系炮弹速度，gn＝[00g]T，g为重力加速度，ωie为地球自转角速度。其中，步骤(3)中，定义变量X＝[qTu]T，并构建非线性函数F(X)＝0，具体方法如下：记四元数q＝[sηT]T，q*＝[s-ηT]T，s为q的标量部分，η为q的矢量部分，q*表示q的共轭四元数，定义四元数q的变换矩阵如下：其中，I为3×3单位矩阵，η×为η对应的叉乘矩阵；将v1(t)和v2(t)扩充成零标量四元数，即定义V1(t)＝[0v1(t)T]T，V2(t)＝[0v2(t)T]T，定义W＝M(V2(t))q-M'(V1(t))q＝(M(V2(t))-M'(V1(t)))q，且对q进行模值约束qqT＝1，引入拉格朗日乘子式，构造函数：F(X)＝∑WTW-u(qTq-1)(7)令X＝[qTu]T。其中，步骤(4)中，通过Jacobian矩阵和Hessian矩阵，求函数F(X)一阶偏导数和二阶偏导数，具体方法如下：其中，V＝M(V2(t))-M'(V1(t)),I4×4为四阶单位矩阵；则Jacobian矩阵J和Hessian矩阵H可记为：其中，步骤(4)中，利用牛顿法迭代，解出四元数q，具体方法如下：起始时，取X0＝[10000]T，令k＝0,2,3,...,m-1，每次迭代时计算J与H；Xk+1＝Xk-H-1J(9)由式(9)可不断递推Xk，直到所有数据全部解算完毕，从最终得到的Xk中取前4个元素组成q，即为in系至ib系的变化四元数，记q＝[q0q1q2q3]T,q0,q1,q2,q3为q的四个元素。其中，步骤(5)中，计算方法如下：计算由下式得到其中，为t时刻n系相对b系的姿态矩阵；为t时刻ib系相对b系的姿态矩阵、为t时刻n系相对in系的姿态矩阵。其中，步骤(5)中，t时刻炮弹姿态角计算方法如下：矩阵为3阶方阵，其中各元素记为：则t时刻炮弹的姿态角由下式解出：φ，θ，γ分别是炮弹的航向角，纵摇角，横滚角。有益效果：与现有技术相比，本专利技术的技术方案具有以下有益技术效果：(1)估算弹体在空中的姿态时，只需要IMU和GPS提供的信息，无需多余传感器；(2)引入牛顿迭代算法进行寻优计算，速度快且精度高；(3)仿真结果表明本方案在高动态的飞行环境下效果良好。附图说明图1为本专利技术姿态角误差估计误差图。具体实施方式下面对本专利技术技术方案进行详细说明，但是本专利技术的保护范围不局限于所述实施例。实施例：本专利技术适用于炮弹飞行估计。首先定义如下坐标系：导航系n：原点为炮弹所在位置处，Y轴指向地理北向，X轴指向地理东向，Z轴垂直于大地水平面指向上。载体系b：原点为弹体质心，Y轴沿弹体前进方向向前，X轴指向右，Z轴指向上。导航惯性系in：初始时刻的导航系n凝固在惯性空间所得，不随时间变化。载体惯性系ib：初始时刻的载体系b凝固在惯性空间所得，不随时间变化。定义如上坐标系后，t时刻的n系相对于b系的姿态矩阵可分解为其中为t时刻ib系相对于b系的姿态矩阵；为t时刻n系相对于in系的姿态矩阵；为in系相对于ib系的姿态矩阵,为3阶方阵。由炮弹上陀螺仪和加速度计测量值计算炮弹速度分别在ib系和in系下的值v1(t)和v2(t)，根据四元数相关性质，定义状态变量X＝[qTu]T，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数。构建非线性函数，通过Jacobian矩阵和Hessian矩阵，求函数一阶偏导数和二阶偏导数，利用牛顿法迭代，解出四元数q，进而计算出炮弹姿态角。下面结合附图对本专利技术实施方法做更详细地描述：1、由本文档来自技高网...

【技术保护点】
1.一种基于牛顿迭代法的炮弹姿态估计方法，其特征在于，该方法包括以下步骤：(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵

【技术特征摘要】
1.一种基于牛顿迭代法的炮弹姿态估计方法，其特征在于，该方法包括以下步骤：(1)根据陀螺仪和GPS提供的炮弹速度和位置，计算t时刻载体惯性系ib系相对于载体系b的炮弹姿态矩阵导航系n相对于导航惯性系in的炮弹姿态矩阵(2)由陀螺仪和加速度计测量值计算ib系和in系下的炮弹速度v1(t)和v2(t)；(3)定义变量X＝[qTu]T，并构建非线性函数F(X)＝0；其中，q是4维列向量，代表in系至ib系的变化四元数，u为待定系数；(4)通过函数F(X)一阶偏导数和二阶偏导数，求Jacobian矩阵和Hessian矩阵；(5)利用牛顿法迭代解出四元数q，根据计算出姿态角。2.根据权利要求1所述的一种基于牛顿迭代法的炮弹姿态估计方法，其特征在于，步骤(1)中，炮弹姿态矩阵和计算方法如下：设初始时刻t0时，b系相对于ib系的炮弹姿态矩阵为I为3×3的单位矩阵；记t时刻陀螺仪的输出值为即t时刻的b系相对于ib系的角速度在b系上的投影值，由此跟踪b系相对于ib系的变化：其中，为矩阵的变化率，“×”表示三维矢量对应的叉乘矩阵变换，设其中a，b，c分别表示炮弹沿三轴的旋转角速度，则采用毕卡法解式(1)微分方程可得到式(2)：记待解算数据时长为T，将时间段0～T以采样周期dt为间隔划分为多个时刻点t0,t1,t2...tm，k＝0,1,2,...,m，则式(2)中为tk时刻的b系相对于tk-1时刻的b系的姿态矩阵；为tk-1时刻陀螺仪输出，dt为采样周期，最终由GPS输出的炮弹位置信息纬度L，东向、北向、天向速度分别为VE,VN,VU，则n系相对in系的角速度可计算如下：其中，RN为地球子午圈曲率半径，ωie为地球自转角速度，RE为地球卯酉圈半径，根据公式(2)的计算方法，由计算出其中，为tk时刻的n系相对于tk-1时刻的n系的姿态矩阵；为t＝tk-1时的值，dt为采样周期，3.根据权利要求2所述的一种基于牛顿迭代法的炮弹姿态估计方法，其特征在于，步骤(2)中，炮弹速度v1(t)和v2(t)计算方法如下：其中，fb(t)为t时刻炮弹上加速度计的输出，表示炮弹三轴加速度，通过姿态矩阵将fb(t)投影到ib系中，得到其中，Vn(t)为t时刻的n系炮弹速度，Vn(0)为起始时刻的n系炮弹速度，gn＝[00g]T，...

【专利技术属性】
技术研发人员：刘锡祥，杨文强，许广富，郭小乐，马晓爽，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32