CN111796271B - 一种比例导引目的地约束下的目标跟踪方法及装置 - Google Patents
一种比例导引目的地约束下的目标跟踪方法及装置 Download PDFInfo
- Publication number
- CN111796271B CN111796271B CN202010669163.4A CN202010669163A CN111796271B CN 111796271 B CN111796271 B CN 111796271B CN 202010669163 A CN202010669163 A CN 202010669163A CN 111796271 B CN111796271 B CN 111796271B
- Authority
- CN
- China
- Prior art keywords
- state
- covariance
- proportional
- measurement
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 76
- 238000005259 measurement Methods 0.000 claims abstract description 98
- 238000001914 filtration Methods 0.000 claims abstract description 25
- 230000003190 augmentative effect Effects 0.000 claims description 45
- 238000004364 calculation method Methods 0.000 claims description 40
- 239000013598 vector Substances 0.000 claims description 36
- 239000011159 matrix material Substances 0.000 claims description 34
- 230000001133 acceleration Effects 0.000 claims description 19
- 230000008569 process Effects 0.000 claims description 17
- 238000005070 sampling Methods 0.000 claims description 16
- 230000007704 transition Effects 0.000 claims description 10
- 230000003416 augmentation Effects 0.000 claims description 8
- 238000006243 chemical reaction Methods 0.000 description 10
- 238000004088 simulation Methods 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000011423 initialization method Methods 0.000 description 3
- 230000002452 interceptive effect Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/66—Radar-tracking systems; Analogous systems
- G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
- G01S13/723—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar by using numerical data
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明涉及雷达目标跟踪技术领域，尤其涉及比例导引目的地约束下目标跟踪方法及装置。本发明的比例导引目的地约束下的目标跟踪方法为：首先进行初始化，然后依次计算状态一步预测、状态一步预测协方差、量测预测、量测预测协方差以及滤波增益；更新无约束状态估计和无约束状态估计协方差；再依次计算新的量测预测、新的量测预测协方差、伪量测和状态间的互协方差、新的滤波增益；最后更新约束状态估计和约束状态估计协方差。所述装置包括初始化模块、第一至第三计算模块、第一更新模块、第四至第六计算模块以及第二更新模块。
Description
技术领域
本发明涉及雷达目标跟踪技术领域，尤其涉及比例导引目的地约束下目标跟踪方法及装置。
背景技术
约束状态估计是在目标状态服从等式或不等式约束条件的情况下，对其进行状态估计的方法。在很多实际目标跟踪场景中，目标运动轨迹不完全由目标本身的运动速度决定，而是受其所处物理环境或是其自身运动特点影响或限制，并不是无约束的***。合理利用这些实际场景中蕴含的约束信息可以有效的提高估计性能和滤波精度。
在实际跟踪场景中存在这样一种约束条件，其来源于目标运动规律及其目的地信息，我们称之为目的地约束。举例来讲，在战术导弹目标跟踪问题中，导弹遵从导引头中预设的导引策略，按照一定规律向其打击目标运动。若导弹要打击的目标的位置和导引头导引策略都已知，可以将其作为先验信息引入跟踪***来提高估计精度。这类约束条件中只包含目标估计的部分信息，而传统的约束目标跟踪方法均是针对包含完整轨迹信息的约束条件的，无法直接用于处理这类目的地约束下的目标跟踪问题。
目前针对目的地约束目标跟踪方法已存在一些研究，例如，对直线和圆弧轨迹目的地约束建立约束模型，然而，这些目的地约束目标跟踪方法都假设目标轨迹具有固定的形状。对于轨迹形状不确定的情况，例如目标运动遵从比例导引律时，尚无有效方法来处理。
因此，针对以上不足，需要提供一种比例导引目的地约束下的目标跟踪方法及装置。
发明内容
本发明要解决的技术问题在于，现有目的地约束目标跟踪方法只适用于目标轨迹形状固定的情况，针对现有技术中的缺陷，提供一种比例导引目的地约束下的目标跟踪方法及装置。
为了解决上述技术问题，本发明提供了一种比例导引目的地约束下的目标跟踪方法，所述方法包括：
步骤S1、判断当前时刻k的值，若k＝1或k＝2，则进行滤波初始化，计算状态初值及初始协方差，然后执行步骤S2；若k＞2，则执行步骤S2；
步骤S2、利用增广状态方程计算状态一步预测及状态一步预测协方差，所述增广状态方程包含目标当前时刻和前一时刻的位置和速度状态分量、以及目的地位置、导航常数、以及偏差角；
步骤S3、利用雷达量测方程计算量测预测和量测预测协方差；
步骤S4、计算滤波增益；
步骤S5、更新无约束状态估计和无约束状态估计协方差；
步骤S6、将所述无约束估计作为新的一步预测，利用伪量测方程计算新的量测预测和新的量测预测协方差，所述伪量测由增广状态构造得到；
步骤S7、计算伪量测和状态间的互协方差；
步骤S8、计算新的滤波增益；
步骤S9、更新约束状态估计和约束状态估计协方差。
可选地，纯比例导引或真比例导引条件下，所述步骤S2中的增广状态方程为：
其中，为纯比例导引或真比例导引条件下k时刻目标的增广状态向量：
其中，
xk、yk和zk分别为k时刻导弹在三维笛卡尔坐标系中沿x、y和z方向位置分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，/>和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，/>和分别表示示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔；
为纯比例导引或真比例导引条件下k时刻的状态转移矩阵：
0m×n为m行n列的零矩阵；In表示n维单位矩阵；
可选地，广义比例导引条件下，所述步骤S2中的增广状态方程的k时刻目标的增广状态向量为：
其中，
xk、yk和zk分别为k时刻导弹在三维笛卡尔坐标系中沿x、y和z方向位置分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，和/>分别表示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔，ηk为k时刻偏差角的状态分量表达形式。
可选地，纯比例导引条件下，所述步骤S6中的伪量测为：
真比例导引条件下，所述步骤S6中的伪量测为：
其中，
可选地，广义比例导引条件下，所述步骤S6中的伪量测为：
其中，
本发明还提供了一种比例导引目的地约束下的目标跟踪装置，所述装置包括：
初始化模块，其配置成判断当前时刻k的值，若k＝1或k＝2，则进行滤波初始化，计算状态初值及初始协方差，然后启动第一计算模块；若k＞2，则直接启动第一计算模块；
第一计算模块，其配置成利用增广状态方程计算状态一步预测及状态一步预测协方差，所述增广状态方程包含目标当前时刻和前一时刻的位置和速度状态分量、以及目的地位置、导航常数、以及偏差角；
第二计算模块，其配置成利用雷达量测方程计算量测预测和量测预测协方差；
第三计算模块，其配置成计算滤波增益；
第一更新模块，其配置成更新无约束状态估计和无约束状态估计协方差；
第四计算模块，其配置成将所述无约束估计作为新的一步预测，利用伪量测方程计算新的量测预测和新的量测预测协方差，所述伪量测由增广状态构造得到；
第五计算模块，其配置成计算伪量测和状态间的互协方差；
第六计算模块，其配置成计算新的滤波增益；
第二更新模块，其配置成更新约束状态估计和约束状态估计协方差。
可选地，纯比例导引或真比例导引条件下，所述第一计算模块中的增广状态方程为：
其中，为纯比例导引或真比例导引条件下k时刻目标的增广状态向量：
其中，
xk、yk和zk分别为k时刻导弹在三维笛卡尔坐标系中沿x、y和z方向位置分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，和/>分别表示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔；
为纯比例导引或真比例导引条件下k时刻的状态转移矩阵：
0m×n为m行n列的零矩阵；In表示n维单位矩阵；
可选地，广义比例导引条件下，所述第一计算模块中的增广状态方程的k时刻目标的增广状态向量为：
其中，
xk、yk和zk分别为k时刻导弹在三维笛卡尔坐标系中沿x、y和z方向位置分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，和/>分别表示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔，ηk为k时刻偏差角的状态分量表达形式。
可选地，纯比例导引条件下，所述第四计算模块中的伪量测为：
真比例导引条件下，所述第四计算模块中的伪量测为：
其中，
可选地，广义比例导引条件下，所述第四计算模块中的伪量测为：
其中，
本发明的比例导引目的地约束下的目标跟踪方法及装置，具有以下有益效果：
提出了基于状态增广和参数增广的比例导引目的地约束建模方法。对于遵从某种确定比例导引律的目标，利用增广状态中当前时刻(k时刻)和前一时刻(k-1时刻)的位置和速度状态分量以及目的地位置和导航常数等信息，根据导引律中包含的约束关系得到比例导引目的地模型。
根据所提出的比例导引目的地约束模型构造伪量测，提出了相应的有效滤波方法。将伪量测引入到跟踪***量测模型中，从而将其中包含的与目标状态有关的约束先验信息引入跟踪***，增加滤波器可利用的信息量，进而达到提升估计精度的目的。
因此，本发明解决了比例导引律引导下目标轨迹形状不固定情况下的目的地约束跟踪问题。
附图说明
图1是本发明实施例一提供的比例导引目的地约束下的目标跟踪方法的示意性流程图；
图2是本发明实施例一的利用两种无约束状态估计方法(转换量测卡尔曼滤波和交互式多模型方法)和本发明的一种实施方法在纯比例导引情况下分别得到的位置均方根误差对比结果；
图3是本发明实施例一的三种方法在纯比例导引情况下的速度均方根误差对比结果；
图4是本发明实施例一的三种方法在真比例导引情况下的位置均方根误差对比结果；
图5是本发明实施例一的三种方法在真比例导引情况下的速度均方根误差对比结果；
图6是本发明实施例一的三种方法在广义比例导引情况下的位置均方根误差对比结果；
图7是本发明实施例一的三种方法在广义比例导引情况下的速度均方根误差对比结果；
图8是本发明实施例一的比例导引目的地约束下的目标跟踪装置的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例一
如图1所示，本发明实施例提供的一种比例导引目的地约束下的目标跟踪方法一般性地可以包括：
步骤S1、判断当前时刻即k时刻的值，若k＝1或k＝2，则进行滤波初始化，计算状态初值及初始协方差，然后执行步骤S2；若k＞2，则执行步骤S2；
步骤S2、利用增广状态方程计算状态一步预测及状态一步预测协方差，所述增广状态方程包含目标当前时刻和前一时刻的位置和速度状态分量、以及目的地位置、导航常数、以及偏差角；
步骤S3、利用雷达量测方程计算量测预测和量测预测协方差；
步骤S4、计算滤波增益；
步骤S5、更新无约束状态估计和无约束状态估计协方差；
步骤S6、将所述无约束估计作为新的一步预测，利用伪量测方程计算新的量测预测和新的量测预测协方差，所述伪量测由增广状态构造得到；
步骤S7、计算伪量测和状态间的互协方差；
步骤S8、计算新的滤波增益；
步骤S9、更新约束状态估计和约束状态估计协方差。
在笛卡尔坐标系下，目标运动可以建模为：
xk+1＝Φkxk+Γkvk
其中xk是状态向量，该状态向量包含k时刻目标在三维笛卡尔坐标系中沿x、y、z方向的位置分量xk、yk、zk，和速度分量以及加速度分量/>等；Φk是状态转移矩阵；vk是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(vk)＝Qk≥0；Γk是噪声分布矩阵。本实施例中运动模型采用与导弹真实运动较为接近的近匀加速(Nearly Constant Acceleration，NCA)模型，其状态转移矩阵和噪声分布矩阵分别为：
其中0m×n是m行n列的零矩阵，
此时对应状态向量为τ为雷达采样间隔。
跟踪过程中从观测雷达处获得目标位置量测信息，所述目标位置量测信息包括目标相对雷达坐标系原点的距离量测方位角量测/>和俯仰角/>至于雷达如何获得这些信息、以及本发明实施例的方法如何从雷达处获得这些信息，本领域技术人员可以采取各种方式来实现，无论采用哪种方法都在本发明的保护范围内。
上述步骤S2利用增广状态方程计算状态一步预测及状态一步预测协方差，那么，首先需要对状态进行增广，将上一时刻的状态增广到状态向量中，得到增广状态向量为：
增广状态向量中包含了总共两个连续时刻的状态，上标“a”代表增广。
增广状态对应的状态方程中的状态转移矩阵和噪声分布矩阵也需要做相应的增广，如下式所示：
其中I代表与Φk维数相同的单位矩阵。增广后的过程噪声协方差矩阵为：
本实施例利用增广状态向量针对三种有代表性的比例导引律：纯比例导引，真比例导引和广义比例导引分别给出了相应的无噪声目的地约束模型。
(1)纯比例导引：
其中rk＝[xd-xk，yd-yk，zd-zk]为k时刻导弹的视线向量，为k时刻导弹的速度方向向量，(xd，yd，zd)为目的地位置，N为导航常数。
(2)真比例导引：
(3)广义比例导引：
其中η为偏差角。
当目的地坐标、导航常数和偏差角信息全部或部分准确已知时，可以直接将真值代入到伪量测表达式中，得到相应形式的伪量测。
可以看到，在目的地约束模型中，除了状态向量，还包含了目的地位置、导航常数以及偏差角等信息。实际应用中，目的地位置可能是通过不完美的传感器获取的，因此会受到量测噪声影响。而导航常数以及偏差角信息可能无法提前获取。在这种情况下，本实施例将目的地坐标和未知参数增广到状态向量中，得到三种比例导引下的新的状态向量如下：
(1)对应纯比例导引和真比例导引的新状态向量为：
其中，和/>分别表示示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式；
(2)对应广义比例导引的新状态向量为
其中，ηk为k时刻偏差角的状态分量表达形式。
对应的增广状态方程可以写作：
其中，
所述步骤S6中的伪量测可以通过上述增广状态和约束模型进行构造，以描述约束关系，目的地信息受噪声影响且导航常数未知时纯比例导引律对应的伪量测为：
而根据真比例导引中对应的伪量测为：
对于同样条件下广义比例导引下的目的地约束对应的伪量测为：
其中
当目的地坐标、导航常数和偏差角信息全部或部分准确已知时，可以直接将真值代入到伪量测表达式中，得到相应形式的伪量测。
伪量测和雷达量测一起被引入到序贯处理框架中用来更新跟踪过程中的状态估计结果。在序贯处理框架中，通过在两步状态更新中先处理非线性度较低的雷达量测得到更准确的状态预测有利于提高估计精度。包含更强非线性的伪量测放在雷达量测之后处理。对于三维空间的雷达量测，其量测方程为
其中，是距离量测，/>是方位角量测，/>是俯仰角量测，/>和/>分别是对应距离、方位角和俯仰角量测的零均值高斯白噪声。
以对应纯比例导引的伪量测为例，其量测方程为：
其中，为伪量测对应的量测函数。
对于对应其他导引类型的伪量测，可以通过同样的方式写出量测方程。
由于伪量测与状态之间是非线性关系。因此在滤波过程中需要采用非线性滤波方法，常用的非线性滤波方法包括转换量测卡尔曼滤波方法、无迹卡尔曼滤波方法、扩展卡尔曼滤波方法、容积卡尔曼滤波方法以及粒子滤波方法等。作为本申请的优选实施例，采用容积卡尔曼滤波方法。
在滤波过程中，首先用转换量测滤波方法处理雷达量测，然后利用容积卡尔曼滤波器处理伪量测。以引入伪量测的滤波方法为例，其流程如下：
步骤S1、初始化阶段(k＝1，2)。增广状态向量的初始化分为几个部分来进行：首先，采用两点差分初始化方法利用前两个时刻的转换量测来计算位置和速度分量的初值；利用单点初始化方法来初始化加速度分量，该方法将加速度初值设置为零；根据最大加速度的大小将其初始方差设置为一个合适的数值；前一时刻的状态分量初值都设置为零，初始协方差也都为零；目的地分量利用带噪声的目的地信息初始化；导引常数和偏差角同样利用单点初始化方法来进行初始化。需要注意的是，k＝1、2都是初始化阶段，k＝1时无法单独完成初始化，需要等待k＝2时刻量测到来才能计算k＝2时刻状态初值和协方差。
步骤S2、状态预测阶段(k＞2)：
根据k-1时刻的约束状态估计(即滤波结果，也就是步骤S1得到的所有状态分量的初值)k时刻状态进行一步预测：
计算状态一步预测：
其中，是上一时刻的增广状态估计结果；
计算状态一步预测协方差：
其中是NCA模型对应的增广状态转移矩阵，/>是过程噪声协方差矩阵，为已知量。
步骤S3、利用雷达量测对应的转换量测进行状态更新(k＞2)：
计算量测预测：
其中，是转换量测对应的量测矩阵，其表达式如下：
计算量测预测协方差矩阵：
其中，为转换量测对应的噪声协方差矩阵，可以利用雷达量测和量测噪声方差利用无偏量测转换方法得到。
步骤S4、计算滤波增益：
步骤S5、更新无约束状态估计和无约束状态估计协方差：
计算无约束状态估计：
其中，表示k时刻的转换量测，可以利用雷达量测和量测噪声方差利用无偏量测转换方法得到；
计算无约束状态估计协方差：
步骤S6、利用伪量测进行状态更新(k＞2)：
对无约束状态估计协方差矩阵进行Cholesky分解：
计算基于选择的2n1个容积采样点/>
/>
所述容积采样点包含与状态向量相同的元素；
根据伪量测方程，计算后验容积采样点：
根据后验容积采样点，计算新的量测预测均值：
计算新的量测预测协方差矩阵：
步骤S7、计算伪量测和状态间的互协方差；
步骤S8、计算新的滤波增益：
步骤S9、更新约束状态估计和约束状态估计协方差：
计算约束状态估计：
计算约束状态估计协方差：
以上各式中，i＝1，2，…，2n1，n1＝2n+4表示增广状态向量的维数，且
其中ei表示只有第i个元素为1、其它元素都为0的维度为n1×1的向量。
然后，将步骤S9的输出作为步骤S2的输入，开始循环，直至用户输入终止跟踪指令或目标离开雷达监测区域后结束跟踪。
对于对应真比例导引律和广义比例导引律下不同情况的目的地约束滤波方法，其流程与上述流程基本一致，只需要将其中的状态方程和量测方程替换为相应的形式并将涉及到的矩阵和向量调整为相应的维度，这里不再赘述。
为了验证本申请实施例所提出的比例导引目的地约束下目标跟踪方法的有效性，在三个分别对应纯比例导引律、真比例导引律和广义比例导引律的场景中进行了仿真实验。在仿真中，假设导弹由确定的比例导引律引导向其目标所在的固定目的地运动。仿真目标的真实状态首先通过递推的方式在目标速度向量和视线所在的平面内生成，然后将二维平面内的目标真实状态转换到三维空间内。在生成数据的过程中，目标速度只受到指令加速度和随机过程噪声的影响。过程噪声为均值为零，标准差为1m/s2的高斯白噪声。假设导弹所受到的空气动力学阻力和重力影响都能利用导弹本身的自动驾驶仪来补偿，因此可以忽略。仿真得到的导弹状态服从比例导引率带来的目的地约束。在三个仿真场景中，导弹的起始位置为(x00，y00，z00)＝(5km，35km，0km)，起始速度为vM，0＝0.6km/s，关于参考线的初始角为φ0＝-11.75°，这里选择的参考线平行于速度向量所在平面和水平面之间的交线。仿真中目的地信息受量测噪声影响，真实的目的地坐标为(xd，yd，zd)＝(40km，0km，0km)，目的地噪声标准差为假设雷达在三维笛卡尔坐标系原点处获取导弹距离、方位角和俯仰角的量测。对应各分量的量测噪声标准差分别为σr＝1km，σa＝0.5°和σe＝0.5。采样间隔为τ＝1s。在仿真过程中，将无约束的采用近匀加速动态模型的转换量测卡尔曼滤波器和采用近匀速和近匀加速两种动态模型(即交互式多模型，InteractingMultiple Model，IMM)方法作为对比算法。并给出近似PCRLB(Posterior Cramer-RaoLower Bound)作为理论基准。每次仿真持续50s，共进行1000次蒙特卡洛实验。采用滤波方法得到位置和速度估计的RMSE(Root Mean Squared Error)来评估所提出算法性能。
图2和图3分别示出了纯比例导引情况下本申请实施所提出的方法与对比算法位置和速度滤波结果对应的均方根误差对比，可以看到由于引入了目的地约束，本申请实施例所提出的方法性能优于无约束的转换量测滤波方法和交互式多模型方法。显著的性能提升验证了所提出方法的有效性。还可以看到，本申请实施例所提出的方法的RMSE接近克拉美罗下限，这验证了本申请实施例所提出的方法的优越性能。
图4和图5分别示出了真比例导引情况下本申请实施所提出的与对比算法位置和速度滤波结果对应的均方根误差对比。图6和图7分别示出了广义比例导引情况下本申请实施所提出的方法与对比算法位置和速度滤波结果对应的均方根误差对比。可以看到两种情况下本申请实施例所提出的方法性能均优于无约束估计器。估计精度的提升证实了本申请实施例所提出的出目的地模型和状态估计方法同样可以在真比例导引和广义比例导引情况下有效提取和利用目的地信息。对比纯比例导引情况，可以看到所提出方法的收敛速度由于引入额外项导致非线性度增加而减慢了。
如图8所示，本实施例还提供了一种比例导引目的地约束下的目标跟踪装置，所述装置一般性地可包括：
初始化模块1，其配置成判断当前时刻k的值，若k＝1或k＝2，则进行滤波初始化，计算状态初值及初始协方差，然后执启动第一计算模块；若k＞2，则直接启动第一计算模块；
第一计算模块2，其配置成利用增广状态方程计算状态一步预测及状态一步预测协方差，所述增广状态方程包含目标当前时刻和前一时刻的位置和速度状态分量、以及目的地位置、导航常数、以及偏差角；
第二计算模块3，其配置成利用雷达量测方程计算量测预测和量测预测协方差；
第三计算模块4，其配置成计算滤波增益；
第一更新模块5，其配置成更新无约束状态估计和无约束状态估计协方差；
第四计算模块6，其配置成将所述无约束估计作为新的一步预测，利用伪量测方程计算新的量测预测和新的量测预测协方差，所述伪量测由增广状态构造得到；
第五计算模块7，其配置成计算伪量测和状态间的互协方差；
第六计算模块8，其配置成计算新的滤波增益；和
第二更新模块9，其配置成更新约束状态估计和约束状态估计协方差。
在纯比例导引或真比例导引条件下，所述第一计算模块中的增广状态方程为：
其中，为纯比例导引或真比例导引条件下k时刻目标的增广状态向量：
其中，
xk、yk和zk分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的位置分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，和/>分别表示示k时刻目的地的x坐标、Y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔；
为纯比例导引或真比例导引条件下k时刻的状态转移矩阵：
0m×n为m行n列的零矩阵；In表示n维单位矩阵；
vk为k时刻过程噪声向量：
vk＝[vx，vy，vz]T。
其中vx、vy、vz分别为x、y、z方向上的过程噪声。
在广义比例导引条件下，所述第一计算模块中的增广状态方程的k时刻目标的增广状态向量为：
其中，
xk、yk和zk分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的位置分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，和/>分别表示示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔，ηk为k时刻偏差角的状态分量表达形式。
在纯比例导引条件下，所述第四计算模块中的伪量测为：
真比例导引条件下，所述第四计算模块中的伪量测为：
其中，nk是导航常数N的状态分量表达形式，
在广义比例导引条件下，所述第四计算模块中的伪量测为：
其中，nk是导航常数N的状态分量表达形式，
当目的地坐标、导航常数和偏差角信息全部或部分准确已知时，可以直接将真值代入到伪量测表达式中，得到相应形式的伪量测。
所述比例导引目的地约束下的目标跟踪装置能够执行本实施例提供的比例导引目的地约束下的目标跟踪方法的步骤。
综上所述，本申请实施例所提出的方法为解决比例导引目的地约束下的目标跟踪问题提供了一条新的途径，通过合理利用比例导引目标运动轨迹对应的运动规律信息避免了信息浪费，提高了估计精度。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (2)
1.一种比例导引目的地约束下的目标跟踪方法，其特征在于，包括：
步骤S1、判断当前时刻k的值，若k＝1或k＝2，则进行滤波初始化，计算状态初值及初始协方差，然后执行步骤S2；若k＞2，则执行步骤S2；
步骤S2、利用增广状态方程计算状态一步预测及状态一步预测协方差，所述增广状态方程包含目标当前时刻和前一时刻的位置和速度状态分量、以及目的地位置、导航常数、以及偏差角；
步骤S3、利用雷达量测方程计算量测预测和量测预测协方差；
步骤S4、计算滤波增益；
步骤S5、更新无约束状态估计和无约束状态估计协方差；
步骤S6、将所述无约束状态估计作为新的一步预测，利用伪量测方程计算新的量测预测和新的量测预测协方差，所述伪量测由增广状态构造得到；
步骤S7、计算伪量测和状态间的互协方差；
步骤S8、计算新的滤波增益；
步骤S9、更新约束状态估计和约束状态估计协方差；
纯比例导引或真比例导引条件下，所述步骤S2中的增广状态方程为：
其中，为纯比例导引或真比例导引条件下k时刻目标的增广状态向量：
其中，
xk、yk和zk分别为k时刻导弹在三维笛卡尔坐标系中沿x、y和z方向位置分量，和分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，/>和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，/>和/>分别表示示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔；vk是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(vk)＝Qk≥0；
为纯比例导引或真比例导引条件下k时刻的状态转移矩阵：
0m×n为m行n列的零矩阵；In表示n维单位矩阵；
广义比例导引条件下，广义比例导引：
其中，η为偏差角，rk＝[xd-xk，yd-yk，zd-zk]为k时刻导弹的视线向量，(xd，yd，zd)为目的地位置，N为导航常数；
所述步骤S2中的增广状态方程的k时刻目标的增广状态向量为：
其中，
xk、yk和zk分别为k时刻导弹在三维笛卡尔坐标系中沿x、y和z方向位置分量，和分别表示k时刻目标在三维笛卡尔坐标系中沿x、Y和z方向的速度分量，/>和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，/>和/>分别表示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔，ηk为k时刻偏差角的状态分量表达形式；
纯比例导引条件下，所述步骤S6中的伪量测为：
真比例导引条件下，所述步骤S6中的伪量测为：
其中，
广义比例导引条件下，所述步骤S6中的伪量测为：
其中，
2.一种比例导引目的地约束下的目标跟踪装置，其特征在于，包括：
初始化模块，其配置成判断当前时刻k的值，若k＝1或k＝2，则进行滤波初始化，计算状态初值及初始协方差，然后启动第一计算模块；若k＞2，则直接启动第一计算模块；
第一计算模块，其配置成利用增广状态方程计算状态一步预测及状态一步预测协方差，所述增广状态方程包含目标当前时刻和前一时刻的位置和速度状态分量、以及目的地位置、导航常数、以及偏差角；
第二计算模块，其配置成利用雷达量测方程计算量测预测和量测预测协方差；
第三计算模块，其配置成计算滤波增益；
第一更新模块，其配置成更新无约束状态估计和无约束状态估计协方差；
第四计算模块，其配置成将所述无约束状态估计作为新的一步预测，利用伪量测方程计算新的量测预测和新的量测预测协方差，所述伪量测由增广状态构造得到；
第五计算模块，其配置成计算伪量测和状态间的互协方差；
第六计算模块，其配置成计算新的滤波增益；
第二更新模块，其配置成更新约束状态估计和约束状态估计协方差；
纯比例导引或真比例导引条件下，所述第一计算模块中的增广状态方程为：
其中，为纯比例导引或真比例导引条件下k时刻目标的增广状态向量：
其中，
xk、yk和zk分别为k时刻导弹在三维笛卡尔坐标系中沿x、y和z方向位置分量，和分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，/>和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，/>和/>分别表示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔；vk是过程噪声向量，假设过程噪声是零均值方差已知的高斯白噪声，其协方差矩阵为cov(vk)＝Qk≥0；
为纯比例导引或真比例导引条件下k时刻的状态转移矩阵：
0m×n为m行n列的零矩阵；In表示n维单位矩阵；
广义比例导引条件下，广义比例导引：
其中，η为偏差角，rk＝[xd-xk，yd-yk，zd-zk]为k时刻导弹的视线向量，(xd，yd，zd)为目的地位置，N为导航常数；
所述第一计算模块中的增广状态方程的k时刻目标的增广状态向量为：
其中，
xk、yk和zk分别为k时刻导弹在三维笛卡尔坐标系中沿x、y和z方向位置分量，和分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的速度分量，/>和/>分别表示k时刻目标在三维笛卡尔坐标系中沿x、y和z方向的加速度分量，/>和/>分别表示k时刻目的地的x坐标、y坐标和z坐标，nk是导航常数N的状态分量表达形式，τ为雷达采样间隔，ηk为k时刻偏差角的状态分量表达形式；
纯比例导引条件下，所述第四计算模块中的伪量测为：
真比例导引条件下，所述第四计算模块中的伪量测为：
其中，
广义比例导引条件下，所述第四计算模块中的伪量测为：
其中，
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010669163.4A CN111796271B (zh) | 2020-07-13 | 2020-07-13 | 一种比例导引目的地约束下的目标跟踪方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010669163.4A CN111796271B (zh) | 2020-07-13 | 2020-07-13 | 一种比例导引目的地约束下的目标跟踪方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111796271A CN111796271A (zh) | 2020-10-20 |
| CN111796271B true CN111796271B (zh) | 2024-02-23 |
Family
ID=72808384
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010669163.4A Active CN111796271B (zh) | 2020-07-13 | 2020-07-13 | 一种比例导引目的地约束下的目标跟踪方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111796271B (zh) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108279412A (zh) * | 2018-01-30 | 2018-07-13 | 哈尔滨工业大学 | 一种目的地约束下目标跟踪装置及方法 |
| CN108802721A (zh) * | 2018-08-22 | 2018-11-13 | 哈尔滨工业大学 | 一种任意直线约束下目标跟踪方法 |
| CN109001699A (zh) * | 2018-01-30 | 2018-12-14 | 哈尔滨工业大学 | 基于带噪声目的地信息约束的跟踪方法 |
| CN110208792A (zh) * | 2019-06-26 | 2019-09-06 | 哈尔滨工业大学 | 同时估计目标状态和轨迹参数的任意直线约束跟踪方法 |
-
2020
- 2020-07-13 CN CN202010669163.4A patent/CN111796271B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108279412A (zh) * | 2018-01-30 | 2018-07-13 | 哈尔滨工业大学 | 一种目的地约束下目标跟踪装置及方法 |
| CN109001699A (zh) * | 2018-01-30 | 2018-12-14 | 哈尔滨工业大学 | 基于带噪声目的地信息约束的跟踪方法 |
| CN108802721A (zh) * | 2018-08-22 | 2018-11-13 | 哈尔滨工业大学 | 一种任意直线约束下目标跟踪方法 |
| CN110208792A (zh) * | 2019-06-26 | 2019-09-06 | 哈尔滨工业大学 | 同时估计目标状态和轨迹参数的任意直线约束跟踪方法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于改进比例导引法的机器人动态避障算法;敖建华;;计算机测量与控制(04);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111796271A (zh) | 2020-10-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109211276B (zh) | 基于gpr与改进的srckf的sins初始对准方法 | |
| CN104567871B (zh) | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 | |
| CN110208792B (zh) | 同时估计目标状态和轨迹参数的任意直线约束跟踪方法 | |
| CN108592945B (zh) | 一种惯性/天文组合***误差的在线标定方法 | |
| CN104809326B (zh) | 一种异步传感器空间配准算法 | |
| Tahk et al. | Recursive time-to-go estimation for homing guidance missiles | |
| Wang et al. | Huber-based unscented filtering and its application to vision-based relative navigation | |
| CN107618678B (zh) | 卫星姿态角度偏差下的姿控信息联合估计方法 | |
| CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
| CN105785359B (zh) | 一种多约束机动目标跟踪方法 | |
| CN108802721B (zh) | 一种任意直线约束下目标跟踪方法 | |
| CN104182609B (zh) | 基于去相关的无偏转换量测的三维目标跟踪方法 | |
| CN108981696B (zh) | 一种sins任意失准角无奇异快速传递对准方法 | |
| CN103776453A (zh) | 一种多模型水下航行器组合导航滤波方法 | |
| CN109001699B (zh) | 基于带噪声目的地信息约束的跟踪方法 | |
| CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
| CN110209180B (zh) | 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 | |
| Psiaki | The blind tricyclist problem and a comparative study of nonlinear filters: A challenging benchmark for evaluating nonlinear estimation methods | |
| Zhao et al. | A filtering approach based on MMAE for a SINS/CNS integrated navigation system | |
| van Goor et al. | EqVIO: An equivariant filter for visual-inertial odometry | |
| CN110111356B (zh) | 运动旋转物体的转动轴方向与转动角速度解算方法 | |
| Fernandes et al. | GNSS/MEMS-INS integration for drone navigation using EKF on lie groups | |
| Liu et al. | On terrain-aided navigation for unmanned aerial vehicle using B-spline neural network and extended Kalman filter | |
| CN111796271B (zh) | 一种比例导引目的地约束下的目标跟踪方法及装置 | |
| CN110186483B (zh) | 提高惯性制导航天器落点精度的方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |