[发明专利]基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法

摘要

本发明针对旋转弹发射高过载、高自旋转和高动态的“三高”限制了现有成熟的弹载姿态测量系统无法直接移植应用于旋转弹的问题。采用了基于三轴地磁传感器、两个单轴陀螺仪和卫星接收机组成旋转弹飞行姿态测量方案，针对该测量方案提出了一种适用于旋转弹飞行姿态高精度滤波估计方法，包括如下步骤：（1）、旋转弹飞行姿态组合测量，包括1.1、弹载传感器及安装方式，1.2、弹体飞行姿态高精度组合滤波结构；（2）、旋转弹高精度误差模型，包括2.1、旋转弹姿态误差模型，2.2、旋转弹速度误差模型，2.3、位置误差方程；（3）、弹体姿态高阶非线性滤波模型；（4）、弹体姿态高精度实时滤波算法，进行滤波更新过程。

主权项

1.一种基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法，其特征在于：包括如下步骤：(1)、旋转弹飞行姿态组合测量1.1、弹载传感器及安装方式弹载传感器采用三轴地磁传感器、两个单轴陀螺仪和卫星接收机，其中，三轴地磁传感器用于测量弹体内的地磁场矢量信息，其各敏感轴方向Mx、My和Mz与弹体坐标系OXbYbZb各轴方向完全一致；两个单轴陀螺仪Gy和Gz分别捷联安装于Yb和Zb轴上，陀螺仪用于测量弹体Y轴和Z轴角速率信息，X轴向角速率信息由地磁传感器测量信息滤波估算得到；卫星接收机用于测量弹体速度和位置信息，接收机天线环形安装于弹体表面；1.2、弹体飞行姿态高精度组合滤波结构组合滤波器由滚转角速率估计滤波器和弹体飞行姿态高精度滤波器两个滤波器组成，分别用于完成弹体滚转速率的估计和弹体飞行姿态参数的高精度估计；(2)、旋转弹高精度误差模型2.1、旋转弹姿态误差模型根据捷联惯性导航理论，弹体坐标系到导航坐标系的姿态四元数的微分方程表示为：上式中，为四元数矩阵，是反对阵矩阵形式：其中，为弹体坐标系相对于惯性坐标系的角速度在弹体坐标系上的投影分量即为导航坐标系相对与惯性坐标系的角速度在导航坐标系上的投影分量和分别表示为：由于角速率分量是通过传感器测量所得，其测量值必然存在测量误差其表示为：捷联解算所得到的角速度也存在解算误差，其表示为：把上式(3)和(4)代入姿态四元数微分方程(1)，得到实际解算时的姿态四元数更微分方程为：因此，把式(5)减去式(1)得到如下形式：令则得到姿态四元数误差方程为：上式(7)中最后两项表达式满足如下关系：其中，和分别定义为：上式和中，满足如下关系：把上式关系代入(7)，得姿态四元数误差方程式：2.2、旋转弹速度误差模型由比力方程推导出弹体速度误差方程为：上式中，为姿态变换矩阵的计算误差，由于是关于四元数误差的非线性函数，所以也是关于的非线性函数；为加速度计在导航系下的等效零偏，满足δK_A和δA分别为加速度计的刻度系数误差矩阵和安装误差矩阵；Vⁿ为导航系下速度分量；δVⁿ为速度误差；地球自转角速度；地球自转角速度误差；为导航坐标系相对地球的角速；为导航坐标系相对地球的角速误差；f^b为加速度计比力；若加速度计事前经校准后，则不考虑δKA和δA，则弹体速度误差方程为：根据上述式(11)所述转换关系，得弹体姿态变换矩阵有：因此，姿态变换矩阵的计算误差为：如前所述，代入上式(16)得到又由于则简化为：对是非线性的，其中的计算公式为：由上述的推导关系可知，速度误差方程体现出是一个非线性的误差方程，其中项即为其非线性部分，若假设速度误差方程(14)中非线性部分为f₁(x,t)：在对非线性的速度误差方程进行线性化处理时，f₁(x,t)对的雅克比矩阵公式为：上式δu_i(i＝1,2,3,4)表示的第i行，其中，和分别定义为：综合方程(14)和(18)为适用于大误差角状态下的弹体速度误差模型；2.3、位置误差方程弹体位置误差方程为：式(24)中，VE、VN、VU分别为弹体沿东、北、天方向的速度；RM和RN分别为弹体所在点子午圈和卯酉圈的曲率半径；L为弹体所在点纬度，λ为弹体所在点经度，h为弹体所在点的高度；δVE,δVN,δVU为旋转弹速度误差参数，δh，δλ和δL为位置误差参数；(3)、弹体姿态高阶非线性滤波模型选取旋转弹姿态四元数误差δQ、速度误差δv和位置误差δp以及陀螺漂移ε^b、加速度计零偏作为系统的状态变量其表示为选取上述推导所得弹体飞行姿态误差方程(12)、速度误差方程(14)和位置误差方程(24)共同构成了滤波器状态方程组，将其简化为如下一般形式：上式中f[X(t),t]是关于自变量X(t)的非线性函数，系统过程白噪声w(t)满足E[w(t)]＝0和E[w(t),wT(τ)]＝Q(t)δ(t‑τ)；选取卫星导航系统测量输出的位置和速度与惯导相应解算的位置和速度相减结果作为组合滤波系统的观测变量Z(t)，其表示为：上式中LGPS,λGPS,hGPS和vi,GPS(i＝N,E,U)分别表示为卫星导航系统测量输出的位置和速度信息，下标为INS变量表示为惯导解算的位置和速度信息；量测噪声v(t)满足E[v(t)]＝0和E[v(t),vT(τ)]＝R(t)δ(t‑τ)；综合上述状态方程(25)和量测方程(26)共同构成适用于大误差角状态下的弹体姿态高阶非线性滤波模型；(4)、弹体姿态高精度实时滤波算法基于修正系数的SRUKF滤波算法流程如下：4.1、滤波初始化：对于任一时段UKF滤波算法计算一个循环的具体步骤：4.2、对于给定的和P_k‑1(k＝0,2,...n)，首先计算状态一步预测及一步预报误差协方差阵P_k,k‑1，包括sigma点计算及其传播过程：4.2.1、计算sigma点即为：4.2.2、计算通过状态方程f(·)传播的sigma点，即为：上式中qr{·}表示对其矩阵进行矩阵的正交三角分解，而表为其QR分解后的返回值R；公式S′＝cholupdate{S,u,±v}是等价于SS^T＝P，S′＝chol{P±vuu^T}；4.3、利用UT变换，求sigma点和P_k,k‑1通过量测方程的传播，包括如下两更新计算过程：4.3.1、首先，计算sigma点和P_k,k‑1通过量测方程h(·)的对X_k的传播：4.3.2、然后，计算输出的一步提前预测值：4.4、最后，利用得到的新的量测值，进行滤波更新过程：