[发明专利]一种基于航迹预测的机动扩展目标轴向姿态实时估计方法

摘要

本发明提供一种基于航迹预测的机动扩展目标轴向姿态实时估计方法，根据光电跟踪系统对目标的角位置数据与测距数据，实现根据目标的航迹预测实时对目标的轴向姿态进行估计，得到目标轴线在图像探测器中的俯仰角与偏航角，从而能够为目标的动力学分析、跟踪态势的决策、以及为图像处理所需的方向性基准提供依据。本发明只利用仪器本身的角位置信息和目标的距离信息，不依赖图像处理，不受目标外形调整影响，实时性好，精度高，适应性强。

主权项

一种基于航迹预测的机动扩展目标轴向姿态实时估计方法，其特征在于：实现步骤如下：(1)保证仪器跟踪上目标；(2)建立视场中跟踪点A，虚拟参考点B，观测点O之间的空间关系；观测点O与参考点B的连线与为参考点的视轴，过A点且垂直与连线的的平面成为虚拟视平面，连线与虚拟视平面的交点为B1，即为AB在虚拟视平面上的投影；(3)计算跟踪点A(x₀,y₀,z₀)在直角坐标系中的位置：$\left\{\begin{array}{c}{y}_0=r\cos ({\mathrm{\β}}_0+\mathrm{\Δ}{E}_{\mathrm{tv}})\sin ({\mathrm{\α}}_0+\frac{{\mathrm{\ΔA}}_{\mathrm{tv}}}{\cos {\mathrm{\β}}_0})\\ x_0=r\cos ({\mathrm{\β}}_0+{\mathrm{\ΔE}}_{\mathrm{tv}})\cos ({\mathrm{\α}}_0+\frac{{\mathrm{\ΔA}}_{\mathrm{tv}}}{{\cos \mathrm{\β}}_0})\\ z_0=r\sin ({\mathrm{\β}}_0+{\mathrm{\ΔE}}_{\mathrm{tv}})\end{array}\right.---\left(1\right)$其中：r：目标距离，α₀：跟踪点仪器方位角，β₀：跟踪点仪器俯仰角，ΔA_tv：跟踪点电视X方向脱靶量，ΔE_tv：跟踪点电视Y方向脱靶量；(4)使用Kalman滤波在有噪声数据(x₀,y₀,z₀)序列中求准确的目标飞行速度矢量：Kalman滤波的模型采用CA模型即常加速度模型，在X,Y,Z三个方向上加速度的波动作为零均值白噪声处理，则目标在X方向上的运动在状态空间中描述为：$\left\{\begin{array}{c}{X}_{\left(k\right)}={\mathrm{\Φ}}_{(k,k-1)}{X}_{(k-1)}+{\mathrm{\Γ}}_{(k,k-1)}{W}_{(k-1)},X_{\left(k\right)}{|}_{k=0}=X_{\left(0\right)}\\ Y_{\left(k\right)}=C_{\left(k\right)}{X}_{\left(k\right)}+V_{\left(k\right)}\end{array}\right.---\left(2\right)$取状态转移矩阵${\mathrm{\Φ}}_{(k,k-1)}=\left[\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& T\\ 0& 0& 1\end{array}\right],$观测矩阵C_(k)＝[1 0 0]，系统过程噪声输入矩阵Γ_(k,k‑1)，V_(k)为观测噪声序列，其中T为滤波迭代周期，滤波参数取：模型方差q＝0.01，观测方差r＝1.0；Kalman滤波的迭代频率为50Hz，经过迭代，即滤得跟踪点在X方向上的当帧速度同理，能够滤得跟踪点在Y方向和Z方向上的速度从而求得跟踪点当帧准确的速度矢量$V=({\stackrel{\·}{x}}_0,{\stackrel{\·}{y}}_0,{\stackrel{\·}{z}}_0);$(5)计算虚拟参考点B(x₁,y₁,z₁)的座标：跟踪点座标与其速度矢量确定后，能够求得参考点B的座标为：$x_1=x_0-l\frac{{\stackrel{\·}{x}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}$$y_1=y_0-l\frac{{\stackrel{\·}{y}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}---\left(3\right)$$z_1=z_0-l\frac{{\stackrel{\·}{z}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}$或$x_1=x_0+l\frac{{\stackrel{\·}{x}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}$$y_1=y_0+l\frac{{\stackrel{\·}{y}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}---\left(4\right)$$z_1=z_0+l\frac{{\stackrel{\·}{z}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}$其中式(3)适用于虚拟参考点在跟踪点航迹偏后，式(4)适用于虚拟参考点在跟踪点航迹偏前；其极坐标为：${\mathrm{\α}}_1=\left\{\begin{array}{c}\arctan \left(\frac{y_1}{x_1}\right),x_1>0\\ \arctan \left(\frac{y_1}{x_1}\right)+\mathrm{\π},x_1\<0\end{array}\right.$${\mathrm{\β}}_1=\arctan \left(\frac{z_1}{\sqrt{{x_1}^2+{y_1}^2}}\right)---\left(5\right)$$r_1=\sqrt{{x_1}^2+{y_1}^2+{z_1}^2}$(6)计算虚拟参考点在假想视平面上的投影点B₁(x₂,y₂,z₂)的座标：投影点是OB连线与过A的以OA连线为法线的平面的交点，通过解下列方程组求得B₁(x₂,y₂,z₂)座标：$\left\{\begin{array}{c}\frac{X-x_1}{x_1}=\frac{Y-y_1}{y_1}\\ \frac{X-x_1}{x_1}=\frac{Z-z_1}{z_1}\\ x_0(X-x_0)+y_0(Y-y_0)+z_0(Z-z_0)=0\end{array}\right.---\left(6\right)$解之得：X＝x₁(x₀²+y₀²+z₀²)/(x₀x₁+y₀y₁+z₀z₁)＝x₂Y＝y₁(x₀²+y₀²+z₀²)/(x₀x₁+y₀y₁+z₀z₁)＝y₂      (7)Z＝z₁(x₀²+y₀²+z₀²)/(x₀x₁+y₀y₁+z₀z₁)＝z₂该点极坐标为：${\mathrm{\α}}_2=\left\{\begin{array}{c}\arctan \left(\frac{y_2}{x_2}\right),x_2>0\\ \arctan \left(\frac{y_2}{x_2}\right)+\mathrm{\π},x_2\<0\end{array}\right.$${\mathrm{\β}}_2=\arctan \left(\frac{z_2}{\sqrt{{x_2}^2+{y_2}^2}}\right)---\left(8\right)$$r_2=\sqrt{{x_2}^2+{y_2}^2+{z_2}^2}$(7)计算视场内的水平线对应的AC连线的方向矢量：$\stackrel{\→}{\mathrm{AC}}=\left|\begin{array}{ccc}i& j& k\\ 0& 0& 1\\ x_0& y_0& z_0\end{array}\right|=-y_{0}i+x_{0}j---\left(9\right)$(8)求矢量与矢量之间的夹角，即俯仰角：设矢量则：(9)求偏航角：根据以上结果得：$\mathrm{\γ}=\left\{\begin{array}{c}\arcsin \left(\frac{\sqrt{{x_3}^2+{y_3}^2+{z_3}^2}}{l}\right),r_1\<r_0\\ \mathrm{\π}-\arcsin \left(\frac{\sqrt{{x_3}^2+{y_3}^2+{z_3}^2}}{l}\right),r_1\≥r_0\end{array}\right.---\left(11\right).$