[发明专利]一种基于地磁信息的高旋弹丸姿态估计方法

权利要求书

1.一种基于地磁信息的高旋弹丸姿态估计方法，其特征在于包括以下步骤：

步骤1：计算弹丸理论姿态角；

通过将磁场投影到旋转弹丸动力学模型所在的测量坐标系来获得数据，所述动力学模型如下式：

I为弹丸惯性张量，在弹丸测量坐标系的x、y、z三轴上分量分别为0.16、1.8、1.8kgm²，M为弹丸力矩，()_b表示在弹体坐标系的投影，为一个3×3的反对称矩阵；

力矩方程表示为：

a＝10^-4Ns²/m²为转矩系数，b＝10^-4Ns²/m为横向阻尼系数，c＝5×10^-6Ns²/m为滚动阻尼系数，d＝10^-6Ns²/m为玛格努斯力矩系数，Δ为总攻角，V为速度向量；

运动方程表示为：

Q为旋转四元数；

根据公式(1)-(3)计算弹丸理论姿态角偏航角ψ、俯仰角θ以及滚转角φ，分别表示为：

弹体的初始角速度在测量坐标系x、y、z三轴上分量分别为350πRad/s、0Rad/s、0Rad/s，弹体的初始偏航角和俯仰角均为0Rad，弹体在惯性坐标系中的初始角速度在x、y、z三轴上分量分别为1000m/s、0m/s、0m/s；

步骤2：将三轴地磁传感器(1)、A/D转换芯片(2)、FPGA芯片(3)和黑匣子(4)安装在待测弹丸上；

步骤3：对待使用的所述三轴地磁传感器(1)进行标定；

步骤4：发射待测弹丸，所述三轴地磁传感器(1)收集数据；

步骤5：所述三轴地磁传感器(1)收集的数据通过A/D转换芯片(2)，进入FPGA芯片(3)中；

步骤6：对所述三轴地磁传感器(1)进行在线校正，通过卡尔曼滤波器消除干扰磁场，将分析结果存入黑匣子(4)中，具体包括：

步骤6.1：确定卡尔曼滤波器的状态方程和测量方程：

所述三轴地磁传感器(1)的实际测量值由下式表示；

(H_m)_m＝(H_t)_m+(H_r)_m； (5)

其中H_m为所述三轴地磁传感器(1)的实际测量值，H_t为地磁场的真实值，H_r为干扰磁场，符号()_m表示在测量坐标系的投影，在测量坐标系和弹体坐标系重合的情况下，上式(5)转化为：

(H_m)_m＝T_bi(H_t)_i+(H_r)_b； (6)

符号()_b表示在弹体坐标系的投影，符号()_i表示在惯性坐标系的投影，T_bi为从惯性坐标系到弹体坐标系的传递矩阵；

方程(6)两边对时间微分，在干扰磁场短时间内不发生巨大变化的情况下，其时间微分为0，得到以下方程：

为一个3×3的反对称矩阵，有三个独立的非零分量，对传递矩阵T_bi分解重组得：

T₂T₁(H_t)_i即为地磁场在弹体坐标系中的投影，弹体主轴固定的情况下，弹体在径向上的角速度为零，所以矩阵第一行与第一列均为0，则矩阵第一个分量为0，第二和第三个分量为调和函数，因此(H_m)_m在横向上的真实值表示为两个互补的正弦信号，横向上的真实值的离散状态方程如下：

x₁和x₂是测量坐标系中径向上的所述三轴地磁传感器(1)的两个输出，x₃为滚转角速度，j和ω₀分别为弹体运动的振幅和相位，得到下面两式：

式(10)和(11)分别为推导到第k步及第k+1步得到的方程，t_k+1＝t_k+Δt，Δt为采样间隔，将式(11)右边的三角函数展开，得到；

用常数和比例的乘积代表x₃，得到；

常数ω＝350π，是ω的比例；

短时间内弹体的滚转角速度和干扰磁场的强度保持不变，因此第k步和第k+1步中x₃、x₄和x₅的值分别相等，得到：

x₄和x₅是干扰磁场在径向上的两个分量；

式(13)、(14)即为卡尔曼滤波器的状态方程F(x)；

y₁和y₂为接收到的测量坐标系中径向上的两个输出，v₁和v₂为测量坐标系中径向上的两个测量噪声，式(15)即为卡尔曼滤波器的测量方程H(x)；

步骤6.2：对所述三轴地磁传感器(1)进行在线校正，通过卡尔曼滤波器消除干扰磁场，包括以下步骤：

步骤6.2.1：确定性采样点更新，确定性采样点更新中无损变换的参数分别为：α＝10^-3，μ＝9×10^-3，β＝2，κ＝-2；

步骤6.2.2：时间更新，设定状态变量矩阵x₀＝[x₁，x₂，x₃，x₄，x₅]^T＝[1，0，0，0，0]^T；状态变量的初始协方差矩阵P₀＝diag[1，1，1，1，1]；过程噪声初始协方差矩阵P_v为diag[5×10^-4，5×10^-4，2×10^-3，5×10^-3，5×10^-3]；测量噪声初始协方差矩阵P_w为diag[2.5×10^-4，2.5×10^-4]；

i为当前时刻，从0逐渐更新至2n，n为状态变量数量，取值为5，χ_i为i时刻的确定性采样点，f()为将上一时刻i-1的状态映射到当前i时刻状态的非线性函数，为过程相对于前一时刻i的状态变量的先验估计；

为状态变量的均值，w_m,i为平均加权系数；

步骤6.2.3：测量更新；

为过程相对于前一时刻i的观测变量的先验估计，h()为反映了观测变量和状态变量间关系的非线性函数；

为观测变量的均值；

P_yy为观测变量的协方差矩阵的先验估计，P_w为测量噪声协方差矩阵；

步骤6.2.4：衰落因子计算；

y_k为k时刻的观测变量值，v为k时刻的误差因子；

C_k为k时刻的新息矩阵，λ为控制新息矩阵更新权重的权重系数；

δ为k时刻的衰落因子，δ₀为δ的初始值；

步骤6.2.5：卡尔曼滤波器数据更新；

P_v为过程噪声初始协方差矩阵，P_xx为状态变量的协方差矩阵的先验估计，w_c,i为方差加权系数；

P_xy为状态变量与观测变量的联合协方差矩阵的先验估计；

P_yy为观测变量的协方差矩阵的先验估计；

B为卡尔曼增益；

x_k为k时刻的状态变量；

P_k＝P_xx-BP_yyB^T； (30)

P_k为k时刻的状态变量的协方差矩阵；

此时即可求出k时刻的各状态变量x₁至x₅，从而能够得到径向上消除干扰后的两个信号分量(x₁-x₄)和(x₂-x₅)及此时的滚转角速度x₃；

步骤7：以步骤6最后求出的消除干扰后的信号分量为基础，通过三正交比法确定弹丸姿态角。