CN105182326A - 一种利用方位信息的目标跟踪快速方法及装置 - Google Patents

Abstract

本发明公开了一种利用方位信息进行距离函数线性转换的目标跟踪快速方法。所述方法通过分析并构建距离函数的泰勒级数展开式的误差函数，利用目标的方位信息得到距离函数的无误差线性展开式。通过得到的无误差线性展开式，选取合适的状态向量和参考向量，利用卡尔曼滤波算法，实现了对飞行器的快速精确跟踪。与此领域广泛使用的扩展卡尔曼滤波算法相比较，此方法有效地降低了运算复杂度，有利于满足目标跟踪的实时性要求，同时也能达到与之相当的跟踪性能。

Description

一种利用方位信息的目标跟踪快速方法及装置

技术领域

本发明属于飞行器目标跟踪领域，涉及一种利用方位信息进行目标跟踪的快速方法，具体涉及一种利用卡尔曼滤波算法的飞行器目标跟踪。

背景技术

在飞行器目标跟踪中，传统的跟踪方法是基于方位信息和距离信息的扩展卡尔曼滤波算法，扩展的卡尔曼滤波算法要对非线性系统状态方程和观测方程进行线性化近似后再采用卡尔曼滤波,线性化的过程不可避免产生处理误差,当非线性系统非线性较强时,这种非线性近似误差可能会造成滤波器发散，同时由于扩展卡尔曼滤波需要求雅克比行列式，算法复杂度较高，计算量大，实时性在某些情况下不能满足需求。

发明内容

为解决上述技术问题，本发明所采取的技术方案是利用卡尔曼滤波算法实现对飞行器的跟踪。

本发明是通过分析并构建距离函数的泰勒级数展开式的误差函数，利用目标的方位信息得到距离函数的无误差线性展开式；通过得到的无误差线性展开式，选取合适的状态向量和参考向量，利用卡尔曼滤波算法，实现了对飞行器的快速跟踪。

飞行器按给定的方向和速度运动，测量飞行器的距离和方位，应用卡尔曼滤波器来跟踪飞行器的位置和速度。在飞行器动力学模型中，假定恒定速度，只受到由风、轻微的速度修正等产生的扰动，把这些扰动看作噪声输入，所以在n时刻，飞行器x和y方向的速度分量分别为

v_x[n]＝v_x[n-1]+u_x[n]

v_y[n]＝v_y[n-1]+u_y[n]

(1)

根据运动方程，飞行器在n时刻的位置为

r_x[n]＝r_x[n-1]+v_x[n-1]·Δt

r_y[n]＝r_y[n-1]+v_y[n-1]·Δt

(2)

其中Δt是样本之间的时间间隔。这种离散化运动方程模型是对真实连续行为的近似。

设飞行器n时刻所在的位置为(r_x[n]，r_y[n])，用T表示飞行器。此外，假设位于有一任意参考点S的位置为(x_o，y_o)，T和S所在位置的坐标差用(Δr_x[n],Δr_y[n])表示，

令

$f\left(r_x\[n\],r_y\[n\]\right)=\sqrt{r_x{\[n\]}^2+r_y{\[n\]}^2}=r\[n\],---\left(3\right)$

其中，r_x[n]＝x_o+Δr_x[n],r_y[n]＝y_o+Δr_y[n]。

对(1)式在r_x[n]＝x_o,r_y[n]＝y_o处进行泰勒级数展开，忽略高阶项，得到关于[r_x[n],r_y[n]]^T的近似线性展开式：

$\begin{array}{c}\tilde{f}\left(r_x\[n\]=x_o+{\mathrm{\Δr}}_x\[n\],r_y\[n\]=y_o+{\mathrm{\Δr}}_y\[n\]\right)\\ =f\left(x_o,y_o\right)+({\mathrm{\Δr}}_x\[n\]\frac{\∂}{\∂x_o}+{\mathrm{\Δr}}_x\[n\]\frac{\∂}{\∂y_o})f\left(x_o,y_o\right)\\ =\sqrt{{x_o}^2+{y_o}^2}+\frac{x_o*{\mathrm{\Δr}}_x}{\sqrt{{x_o}^2+{y_o}^2}}+\frac{y_o*{\mathrm{\Δr}}_y}{\sqrt{{x_o}^2+{y_o}^2}}\end{array}---\left(4\right)$

令上式可以表示为

$\begin{array}{c}\tilde{f}\left(r_x\[n\],r_y\[n\]\right)\\ =r_o+\frac{x_o*{\mathrm{\Δr}}_x}{r_o}+\frac{y_o*{\mathrm{\Δr}}_y}{r_o}\end{array}---\left(5\right)$

那么其误差函数为：

$\begin{array}{c}\mathrm{\Δ}e=r\[n\]-\tilde{f}\left(r_x\[n\],r_y\[n\]\right)\\ =\[1-\left(\frac{{r_o}^2}{r\[n\]*r_o}+\frac{x_o*{\mathrm{\Δr}}_x\[n\]}{r\[n\]*r_o}+\frac{y_o*{\mathrm{\Δr}}_y\[n\]}{r\[n\]*r_o}\right)\]*r\[n\]\end{array}---\left(6\right)$

由余弦定理，

$cos\mathrm{\β}\[n\]=\frac{{r_o}^2+r{\[n\]}^2-r_{st}}{2*r_o*r\[n\]}---\left(7\right)$

其中β[n]代表n时刻∠TOS的角度，r_st表示S与T的距离，将(7)式带入(6)式可以得到

Δe＝(1-2cosβ[n]+cosβ[n])*r[n](8)

＝(1-cosβ[n])*r[n]

所以，将(8)式与(6)式相比较可得到距离的无误差线性方程为：

$\begin{array}{c}f\left(r_x\[n\],r_y\[n\]\right)\\ =\left(r_o+\frac{x_o*{\mathrm{\Δr}}_x}{r_o}+\frac{y_o*{\mathrm{\Δr}}_y}{r_o}\right)*\frac{1}{cos\mathrm{\β}\[n\]}\end{array}---\left(9\right)$

选取状态向量为s[n]＝[Δr_x[n],Δr_y[n],v_x[n],v_y[n]]^T，其中v_x[n],v_y[n]分别代表飞行器在x轴和y轴的飞行速度。状态转移矩阵为：

$A=\left[\begin{array}{cccc}1& 0& \mathrm{\Δ}& 0\\ 0& 1& 0& \mathrm{\Δ}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]---\left(10\right)$

满足状态方程

s[n]＝A□s[n-1]+u[n](11)

其中，驱动噪声矢量u[n]□N(0,Q)，假定风、速度修正等在任何方向以同样的幅度出现，那么给u_x[n]和u_y[n]赋予相同的方差(用表示)，且假定它们独立，那么驱动矢量噪声协方差矩阵为

$Q=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& {\mathrm{\σ}}_u^2& 0\\ 0& 0& 0& {\mathrm{\σ}}_u^2\end{array}\right]---\left(12\right)$

测量为x[n]＝r[n]*cosβ[n]-r_o。测量矩阵为：

$H=\left[\begin{array}{cccc}\frac{x_o}{r_o}& \frac{y_o}{r_o}& 0& 0\end{array}\right]---\left(13\right)$

满足测量方程

x[n]＝H□s[n]+w[n](14)

其中观测噪声w[n]□N(0,σ_x ²)。

假设初始状态矢量s[-1]□N(μ_s,C_s)，初始化可以通过下列卡尔曼滤波方程实现飞行器的目标跟踪：

预测：

$\hat{s}\[n|n-1\]=A\hat{s}\[n-1|n-1\]---\left(15\right)$

最小预测MSE矩阵：

M[n|n-1]＝AM[n-1|n-1]A^T+Q(16)

卡尔曼增益矩阵：

K[n]＝M[n|n-1]Η^T[n](σ_x ²+H[n]M[n|n-1]H^T[n])^-1(17)

修正：

$\hat{s}\[n|n\]=\hat{s}\[n|n-1\]+K\[n\]\left(x\[n\]-H\[n\]\hat{s}\[n|n-1\]\right)---\left(18\right)$

最小MSE矩阵：

M[n|n]＝(I-K[n]H[n])M[n|n-1](19)

本申请中的利用方位信息的目标跟踪快速方法的装置包括命令控制器、测向单元、测距单元、微处理器以及显示装置，其特征在于，命令控制器分别与测向单元和测距单元相连接，测向单元和测距单元与微处理器相连。

优选地，本申请中的命令控制器为MCU；

优选地，本申请中的测向单元为雷达测向装置，得到目标的方位信息；

优选地，本申请中的测距单元为雷达测距装置，目标到观测站的距离；

优选地，本申请中的微处理器为DSP，运行本申请中的计算方法得到本申请的跟踪结果；

本申请中的利用方位信息进行飞行器目标跟踪的快速方法及装置，与此领域广泛使用的扩展卡尔曼滤波算法相比较，此方法降低了运算复杂度，有利于满足目标跟踪的实时性要求，同时也能达到与之相当的跟踪性能。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1是本发明中表示各参数的示意图。

图2是算法对飞行器某条航迹进行跟踪仿真图。

图3是利用方位信息进行飞行器目标跟踪的装置示意图。

具体实施方式

下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

对飞行器的航迹进行仿真追踪，首先假设飞行器的真实航迹为一条直线，坐标为：

r_x[n]＝10-0.2n

ry[n]＝-5+0.2n，n＝0，1，...100，

根据运动方程，飞行器在n时刻的位置为

r_x[n]＝r_x[n-1]+v_x[n-1]·Δt

r_y[n]＝r_y[n-1]+v_y[n-1]·Δt

为方便，假定Δt＝1，由(2)式，假定这条航迹的真实速度为v_x＝-0.2，v_y＝0.2。驱动噪声的方差为起始状态为

$s\[-1\]=\left[\begin{array}{c}10\\ -5\\ -0.2\\ 0.2\end{array}\right]$

假定测量噪声的方差是其中β的单位是弧度。

仿真引入扩展卡尔曼滤波作为对比算法，由于扩展卡尔曼滤波需要求雅克比行列式，运算复杂度高于所述方法。表1和表2分别是两种算法的复杂度比较和运行时间比较。由表1可以计算，非线性转换算法比EKF算法减少了26％的计算量。表2的计算机仿真运行时间也验证了算法量的分析结果。

从仿真图2看到传统的扩展卡尔曼滤波和本文所述非线性转换卡尔曼滤波算法都能达到很好的跟踪效果，几乎与真实航迹重合。

表1算法复杂度比较

算法	单次滤波需要的乘法次数
		EKF	371
LKF	273

表2算法运行时间比较

其中，EKF代表扩展卡尔曼滤波算法，LKF代表本申请中的非线性转换卡尔曼滤波算法。

表2运算时间比较

算法	500次平均运行时间(s)
		EKF	14.5
LKF	11.5

其中，EKF代表扩展卡尔曼滤波算法，LKF代表本申请中的非线性转换卡尔曼滤波算法。

所述方法通过分析并构建距离函数的泰勒级数展开式的误差函数，利用目标的方位信息得到距离函数的无误差线性展开式。通过得到的无误差线性展开式，选取合适的状态向量和参考向量，利用卡尔曼滤波算法，实现了对飞行器的快速精确跟踪。与此领域广泛使用的扩展卡尔曼滤波算法相比较，此方法降低了运算复杂度，有利于满足目标跟踪的实时性要求，同时也能达到与之相当的跟踪性能。

本申请中的利用方位信息进行目标跟踪的装置，包括命令控制器、测向单元、测距单元、微处理器以及显示单元，其特征在于，命令控制器分别与测向单元和测距单元相连接，测向单元和测距单元与微处理器相连，显示单元与微处理器相连。

观测站需要配备具有测向和测距功能的雷达装置，以得到目标的方位信息和目标到观测站的距离。其中测向单元可采用单脉冲测向装置，测距单元采用测距雷达。

单脉冲测向装置是雷达对抗中常用的测向装置，目前最常用的高精度瞬时测向技术是干涉仪测向。测向可供选择的天线有螺旋天线、对数周期天线等等，其中平面螺旋天线等特别适用于集成应用场合，这类天线有单/多臂、单/多模之分，多模螺旋天线避免了干涉仪测向的弱点，保留了其优点，因此本装置可考虑采用多模螺旋天线，其方位分辨率较高，可以提高跟踪的准确性。测向装置例如THALES公司VHF/UHF频段车载测向系统(双信道相关干涉仪测向)。其天线系统由内外两个五元均匀圆阵组成，分别覆盖20MHz-700MHz及700MHz-3GHz频段。

测距雷达可选用LFMCW(linearfrequency-modulatedcontinuouswave)，这种雷达可以对距离远、高速运动的目标进行检测。雷达测距系统由收发天线、射频收发前端、信息处理模块及控制装置组成。性能指标主要包括最大探测距离、测距精度、最小探测距离、方位精度等信息。可以根据实际情况选用或设计雷达系统，例如对于小型无人机的距离覆盖范围一般在几十米到几千米范围内，对方位和距离分辨率都有较高要求，这就对雷达信号波形的选用增加了很多限制条件。最小距离限制脉冲宽度，分辨率要求信号带宽足够大。因此要选用峰值功率高的发射机。如美国联邦航空局20世纪80年代装备的第3代机场面目标监视雷达，其频段选择Ku波段行波管发射机，峰值发射功率为3kW，距离分辨率为6m，采用40ns的脉冲信号。

以上所述仅为本申请的较佳实施例，并不用以限制本发明，凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (9)

1.一种利用方位信息进行飞行器目标跟踪的方法，其特征在于，该方法是利用卡尔曼滤波算法实现对飞行器的跟踪。

2.根据权利要求1所述的利用方位信息进行飞行器目标跟踪的方法，其特征在于，所述方法通过分析并构建距离函数的泰勒级数展开式的误差函数，利用目标的方位信息得到距离函数的无误差线性展开式；通过得到的无误差线性展开式，选取合适的状态向量和参考向量，利用线性卡尔曼滤波算法，实现了对飞行器的快速跟踪。

3.根据权利要求2所述的利用方位信息进行飞行器目标跟踪的方法，其特征在于，所述距离函数的无误差线性展开式包括以下步骤：

S31获取飞行器动力模型中飞行器的速度和具体位置；

v_x[n]＝v_x[n-1]+u_x[n]

v_y[n]＝v_y[n-1]+u_y[n]

S32根据S31步骤中的速度分量和具体位置，得到飞行器与原点间的距离；

其中，r_x[n]＝x₀+Δr_x[n]，r_y[n]＝y₀+Δr_y[n]；

S33选取任一点S(x₀，y₀)，将S2步骤中的飞行器与原点间的距离，在(x₀，y₀)进行泰勒线性展开，忽略高阶项，得到关于[r_x[n],r_y[n]]^T的近似线性展开式：

S34根据S33步骤的泰勒近似线性展开式，得到误差函数：

S35将余弦定理带入上式中，得到误差函数的另外一种形式，

Δe＝(1-cosβ[n])*r[n]

其中β[n]代表n时刻∠TOS的角度，r_st表示S与T之间的距离；

S36根据上述步骤S34和S35得到距离无误差线性方程：

。

4.根据权利要求2所述的利用方位信息进行目标飞行器目标跟踪的方法，其特征在于，所述的利用卡尔曼滤波实现对飞行器目标跟踪包括以下步骤：

S41选取s[n]＝[Δr_x[n],Δr_y[n],v_x[n],v_y[n]]^T状态向量，状态转移矩阵：

并且满足状态方程

s[n]＝A·s[n-1]+u[n]

其中，u[n]为驱动噪声矢量，并且满足u[n]□N(0,Q)

那么，驱动矢量噪声协方差矩阵为：

其中，σ_u ²为u_x[n]，u_y[n]的方差；

S42测量向量x[n]＝r[n]*cosβ[n]-r₀，测量矩阵为：

满足测量方程

其中，观测噪声w[n]□N(0,σ_x ²)

S43根据上述步骤S42可以预测得到：

最小预测MSE矩阵：

M[n|n-1]＝AM[n-1|n-1]A^T+Q

卡尔曼增益矩阵：

K[n]＝M[n|n-1]H^T[n](σ²+H[n]M[n|n-1]H^T[n])^-1

修正之后的矩阵为：

那么最小的MSE矩阵：

M[n|n]＝(I-K[n]H[n])M[n|n-1]。

5.一种利用方位信息的目标快速跟踪的装置，包括命令控制器、测向单元、测距单元、微处理器以及显示单元，其特征在于，命令控制器分别与测向单元和测距单元相连接，测向单元和测距单元与微处理器相连，显示单元与微处理器相连。

6.根据权利要求5所述的目标跟踪装置，其特征在于，所述命令控制器是MCU。

7.根据权利要求5所述的目标跟踪装置，其特征在于，所述的测向单元是雷达测向装置。

8.根据权利要求5所述的目标跟踪装置，其特征在于，所述的测距单元是雷达测距装置。

9.根据权利要求5所述的目标跟踪装置，其特征在于，所述的微处理器是DSP。