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

摘要

本发明涉及飞行器或者弹体空间姿态的测量方法，具体为一种基于磁测滚转角速率信息的高速旋转弹姿态估计方法，采用三轴地磁传感器、两轴陀螺仪和卫星接收机组成弹载姿态传感系统和采用磁测辅助的旋转弹姿态快速估计方法，能够实现初始参数未知下的弹体姿态的空中在线自行快速获取。旋转弹姿态快速估计算法利用弹体速度信息（弹体速度信息由卫星接收机测量所得）来估算弹体偏航角和俯仰角姿态，并利用其作为快速估计算法的观测信息。旋转弹姿态快速估计方法在完成弹体姿态估计的同时实现了弹载三轴地磁传感器和二轴陀螺仪传感器的误差参数的在线估计与补偿。本发明设计合理，能够快速在线估计出旋转弹的飞行姿态，具有很好的推广应用价值。

主权项

1.一种基于磁测滚转角速率信息的高速旋转弹姿态估计方法，其特征在于：包括如下步骤：(1)、弹体三维姿态组合测量方案旋转弹弹载姿态传感器由三轴地磁传感器(2)、两轴陀螺仪(3)和卫星接收机(4)组成；所述三轴地磁传感器(2)捷联安装于旋转弹弹体(1)，三轴地磁传感器(2)的各敏感轴Sx、Sy和Sz与弹体坐标系OXbYbZb方向完全一致，用于测量弹体内的地磁场矢量信息；其中，弹体坐标系OXbYbZb和运动载体固连，坐标原点位于弹体的质心，Xb轴沿弹轴方向向前；Yb轴沿弹体纵轴方向向上；Zb轴与Xb、Yb轴构成右手坐标系；三轴地磁传感器的测量输出关系为：上式中，为弹体坐标系内的三轴地磁传感器测量输出分量；是导航参考坐标系下的地磁场三分量，由于常规弹药射程内变化很小，其在弹道射程内是常值量；为姿态变换矩阵；若选取地面发射坐标系作为导航参考坐标系，则三轴地磁传感器的测量输出为：所述两轴陀螺仪捷联安装弹体坐标系的Yb轴和Zb轴；所述卫星接收机(4)用来测量弹体的速度信息，利用测量所得速度信息的来估算弹体偏航角和俯仰角姿态，为弹体姿态组合测量系统提供观测信息；利用速度信息的估算弹体偏航角和俯仰角姿态公式为：(2)、基于地磁信息的弹体滚转角速率滤波估计方法I、基于地磁传感器信息的滚转角测量方法建立弹轴坐标系OXYZ，其中，弹轴坐标系和运动载体固连，坐标原点位于弹体的质心，Xz轴沿弹轴方向向前；Yz轴垂直于弹轴向上为正；Zb轴与Xz、Yz轴构成右手坐标系；在弹体径向平面YOZ，其中Y、Z分别为弹轴坐标系的Y和Z轴；Y^b、Z^b表示为弹体坐标系的Y和Z轴，而为捷联安装于弹体Y和Z轴上的三轴地磁传感器实际测量输出；H_YOZ为地磁场强度在弹体径向平面内YOZ内的投影分量，它与OZ轴之间存在夹角，定义为地磁基准角，用α来表示；根据当处于第二象限投影关系可知，捷联于弹体的磁传感器的测量所得地磁场分量的大小即为H_YOZ在Y^b和Z^b轴上的投影值，因此，在任意外弹道时刻t，弹体滚转角γ(t)按如下公式(4)计算得到：上式β(t)为HYOZ与Zb间夹角；同理，整理出全象限范围内滚转角的计算公式为：由于弹道各点磁基准角α(t)是时变的，但当采样率较高时前后采样点时刻的地磁基准角相同，即认为α(t)≈α(t‑Δt)，因此弹体滚转角变化率通过下式计算得到：上式(6)中，为旋转弹的理想滚转角速率；表示为测量计算误差；因此，利用两个正交安装的地磁传感器的测量输出值，综合式(5)和式(6)估算出弹体滚转角和角度变化率的大小；II、基于地磁信息的弹体滚转角速率观测模型由欧拉方程推出弹体滚转角速率ω_x与滚转角姿态变化率之间的关系为：将式(7)代入式(6)，进一步整理得到弹体滚转角速率的观测方程为：若选取滚转角变化率作为观测量，通过滤波方法估算出弹体滚转轴向的角速率用于替代X轴陀螺仪；由于旋转弹体高速旋转时，且偏航变化率很小，若令则观测方程式(8)简写为：因此，若以式(6)计算所得滚转角速率作为观测值，通过kalman滤波估计方法估算出弹体X轴向角速率用于替代X轴陀螺仪；(3)、旋转弹飞行姿态空中快速滤波估计方法I、旋转弹三维姿态组合式滤波器结构旋转弹三维姿态滤波器采用“组合式”快速滤波结构，旋转弹三维姿态空中快速滤波估计器包括地磁传感器在线补偿滤波器、弹体角速率滤波器和弹体姿态滤波估计器；其中，地磁传感器在线补偿滤波器实现地磁传感器的在线补偿；弹体角速率滤波器实现弹体角速率估计与补偿；弹体姿态滤波估计器完成旋转弹姿态估计；II、弹体角速率估计滤波方法若以式(6)计算所得滚转角速率作为观测值，通过滤波方法估算出弹体X轴向弹体角速率；根据上述式(9)所示的弹体X轴向角速率观测方程，并结合捷联Y和Z轴陀螺传感器测量误差模型，得到弹体三轴向角速率测量误差模型为：上式中，分别为Y、Z轴向角速率测量值；而为角速率估计值；s_Gy,s_Gz分别为Y和Z轴陀螺的灵敏度系数；b_i(i＝x,y,z)各轴向陀螺常值零偏误差；k_yx、k_yz和k_zx、k_yz分别为Y、Z轴陀螺的轴间的交叉耦合系数；n_i(i＝x,y,z)各轴向陀螺测量噪声；选取陀螺传感器误差参数bx,by,bz,sgy,sgx,szz,kyz,kzx,kzy共9个参数作为系统的状态变量Xg＝[bx,kyz,sgy,sgx,by,kzx,kzy,szz,bz]T，并假设陀螺传感器各误差参数是常值，因此，系统的状态方程表示为：Xg(k)＝Φ(k,k‑1)Xg(k‑1)+wk‑1……………………………………..……(13)上式Φ(k,k‑1)＝I9×9，w(k‑1)为系统零均值高斯白噪声；选取弹体滚转姿态变化率Y和Z陀螺测量输出共同构成量测变量：则弹体角速率测量误差模型(12)即为系统的量测方程，简写为：Zg(t)＝h[Xg(t),t]+v(t)………………………………………..…….(14)式中v(t)是系统的量测噪声，设为高斯白噪声，E[v(t)]＝0，E[v(t),vT(τ)]＝R(t)δ(t‑τ)；并对量测方程(14)进行线性与离散化处理：Zg(k)＝Hg(k)Xg(k)+uk+vk……………………………..……………(15)上式中：因此，由状态方程(13)和观测方程组(15)成系统滤波方程，采用离散kalman滤波方程进行滤波状态估计，滤波方程如下：时间更新：量测更新：其中，Kg(k)表示滤波增益阵；Hk为量测矩阵；Rk为量测噪声噪声阵；Qk‑1为系统噪声阵；Pg(k,k‑1)为前一时刻系统估计方差阵；Pg(k)为当前时刻系统方差阵，通过上述滤波步骤完成对陀螺误差参数测最优估计；III、旋转弹三维姿态估计滤波算法根据捷联惯性导航理论，旋转弹体姿态动力学方程由欧拉方程进行描述：上式中，为经过滤波估计后的弹体角速率，ψ,θ,γ分别为弹体的偏航角、俯仰角和滚转角；选取旋转弹三维姿态角ψ,θ,γ作为系统的状态变量Xa＝[ψ,θ,γ]T；并以旋转弹体姿态欧拉方程(18)作为系统状态方程，则其简写为如下形式：上式中w(t)是系统过程噪声，假设为零均值高斯白色噪声，并满足E[w(t)]＝0，E[w(t),wT(τ)]＝Q(t)δ(t‑τ)；捷联弹体的三轴地磁传感器测量输出和由卫星接收机测量所得到弹体速度估算所得的弹体偏航角和俯仰角(ψ_m,θ_m)作为系统的观测信息，则系统的观测变量：由于三轴地磁传感器捷联安装于旋转弹弹体坐标系，则地磁传感器的测量输出为：上式中，为弹体坐标系内的理想测量输出矢量；是导航参考坐标系下的地磁场矢量，在弹道射程内是常值变量；为姿态变换矩阵；因此，综合上述磁传感器的测量输出模型式(2)和偏航角及俯仰角的估计公式(3)，共同构成了系统的观测方程：上式中，ci,si(i＝ψ,θ,γ)是三角函数cosi和sini的简写形式；v(t)是系统的量测噪声，假设为零均值高斯白噪声，并满足E[v(t)]＝0，E[v(t),vT(τ)]＝R(t)δ(t‑τ)；由状态方程(18)和观测方程(21)共同构成初始姿态角未知下旋转弹姿态估计的滤波模型，利用滤波方法估计出弹体三维姿态参数，但由于状态方程和观测方程都是非线性连续系统，其简写为如下一般形式：上式中f(·)和h(·)是关于自变量Xa(t)的非线性函数，w(t)和v(t)分别是系统过程噪声和量测噪声；根据所建立的滤波模型，首先按上述方法进行滤波模型的离散化及线性处理，因此在给定滤波初值P₀情况下，基于EKF的弹体姿态滤波算法包括如下时间和量测更新过程：给定初始滤波参数P₀：时间更新：量测更新：Pa(k)＝(I‑Ka(k)Hk)Pa(k,k‑1)(I‑Ka(k)Hk)T+Ka(k)Rk(Ka(k))T………………(30)上式中Φk,k‑1为状态转移矩阵，Hk为量测矩阵，分别是状态方程中f(·)和量测方程中h(·)的雅可比矩阵；Ka(k)表示滤波增益阵；Rk为量测噪声阵；Qk‑1为系统噪声阵；Pa(k,k‑1)为前一时刻系统估计方差阵；Pa(k)为当前时刻系统方差阵；因此，通过上述的滤波初值选取、状态预测与量测更新三个滤波算法步骤，最终实现在初始参数未知状态下的弹体三维飞行姿态的空中快速估计。