CN108871365B - 一种航向约束下的状态估计方法及*** - Google Patents

一种航向约束下的状态估计方法及*** Download PDF

Info

Publication number
CN108871365B
CN108871365B CN201810739573.4A CN201810739573A CN108871365B CN 108871365 B CN108871365 B CN 108871365B CN 201810739573 A CN201810739573 A CN 201810739573A CN 108871365 B CN108871365 B CN 108871365B
Authority
CN
China
Prior art keywords
covariance
time
state
constraint
augmented
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
Application number
CN201810739573.4A
Other languages
English (en)
Other versions
CN108871365A (zh
Inventor
周共健
张转花
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201810739573.4A priority Critical patent/CN108871365B/zh
Publication of CN108871365A publication Critical patent/CN108871365A/zh
Application granted granted Critical
Publication of CN108871365B publication Critical patent/CN108871365B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Feedback Control In General (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明提供了一种航向约束下的状态估计方法及***，其中方法包括：在已知航向的条件下，利用约束直线的截距增广状态向量和协方差矩阵，即增广原来的状态方程，并计算初始值；基于位置约束关系和相对应的速度约束关系构建伪量测，并利用该伪量测增广量测方程；基于增广后的状态方程和增广后的量测方程，及状态向量和协方差矩阵的初始值，利用扩展卡尔曼滤波得到目标位置的滤波结果。本发明解决了不完整约束条件下的状态估计问题，充分利用了先验已知的信息，避免了信息的浪费。

Description

一种航向约束下的状态估计方法及***
技术领域
本发明涉及目标跟踪技术领域，尤其涉及一种航向约束下的状态估计方法及***。
背景技术
在估计满足约束的状态时，存在约束信息不完整问题。例如，在多车道沿公路行驶的车辆，目标的航向被约束在直公路的方向，即隐含航向约束先验信息，然而，车辆的运行轨道先验未知。传统的滤波方法解决估计问题时，(1)不考虑约束信息，造成信息浪费；(2)要求约束信息已知，不能解决约束信息模糊或部分已知的约束估计问题。因此，提供一种合理利用不完整约束信息，提高约束估计精度的方法具有现实意义。
目前比较常见的约束目标跟踪方法包括：
(1)模型降阶法，例如根据状态变量的等式约束关系，减少***模型向量的维度。该算法的优点是实现简单，计算代价小，但缺点是状态变量的物理意义模糊。
(2)完美量测法，例如利用状态约束构造没有量测噪声的完美量测，增广原来的量测方程，优点是估计值总是满足约束，缺点是奇异的量测协方差矩阵引发数值模拟问题。
(3)估计后投影法，例如将标准的无约束估计映射到约束子空间，最小化无约束估计到约束子空间的距离，获得约束估计。由于不是最小化与真实值之间的距离，这种方法得到的解不是最优解。
(4)概率密度函数截断法，例如在不等式约束的条件下，截断由卡尔曼滤波方法获得的概率密度函数，利用被截断的概率密度函数求解被约束的状态估计。该方法也适用于等式约束问题。
(5)模型修正法，例如采用直接消除法和沿轨道运动的映射方法得到满足约束条件的修正模型。这类方法仍然无法解决不完整约束的状态估计问题。
总的来看，以上几种方法都是在约束已知的基础上研究约束状态估计问题，并不能解决不完整约束的估计问题。在已知行驶航向和轨迹形状的条件下，多车道上运行目标的轨道未知。因此，亟待提出一种在不完整约束情况下进行状态估计的方法。
发明内容
本发明要解决的技术问题在于，针对现有方法不能解决不完整约束的估计问题，提出一种航向约束下的状态估计方法及***，求解约束直线上的截距信息，确定目标状态的约束直线，并与优化的滤波技术相结合，提高约束状态估计精度。
为了解决上述技术问题，本发明第一方面，提供了一种航向约束下的状态估计方法，包括以下步骤：
S1、利用约束直线的截距增广状态向量和协方差矩阵，并计算初始值；
S2、基于位置约束关系和相对应的速度约束关系构建伪量测，并利用该伪量测增广量测方程；
S3、基于增广后的状态方程和增广后的量测方程，在状态向量和协方差矩阵初始值的条件下，利用扩展卡尔曼滤波得到目标位置的滤波结果。
在根据本发明所述的航向约束下的状态估计方法中，优选地，所述步骤S1中增广后的状态向量的初始值为：
Figure BDA0001722901470000021
式中，T是采样周期，μθ表示无偏因子，
Figure BDA0001722901470000031
和
Figure BDA0001722901470000032
分别表示第k时刻和k-1时刻的距离，
Figure BDA0001722901470000033
和
Figure BDA0001722901470000034
分别表示第k时刻和k-1时刻的方位角，α表示已知的航向，其中截距通过μθrksinθk-tanα·(μθrkcosθk)表示；
步骤S1中增广后的协方差矩阵的初始值为：
Figure BDA0001722901470000035
式中，
Figure BDA0001722901470000036
和
Figure BDA0001722901470000037
为块矩阵，Pk,b为截距bk的协方差，Pk,b＝Rk,yy+(tanα)2×Rk,xx-2tanα×Rk,xy，Rk,yy为y方向的量测噪声协方差，Rk,xx为x方向的量测噪声协方差，Rk,xy为交叉协方差。
在根据本发明所述的航向约束下的状态估计方法中，优选地，所述步骤S2中构建的伪量测为：
ξk＝yk-tanα·xk-b
Figure BDA0001722901470000038
式中，xk,
Figure BDA0001722901470000039
分别为x方向的位置和相对应的速度，yk,
Figure BDA00017229014700000310
分别为y方向的位置和相对应的速度，α表示已知的航向，b为未知的截距；
基于上述伪量测增广后的量测方程为：
Figure BDA00017229014700000311
式中，wk为量测噪声。
在根据本发明所述的航向约束下的状态估计方法中，优选地，所述步骤S3包括迭代执行的以下步骤：
(1)滤波的预测步骤：
通过以下公式计算第k时刻的状态向量预测值
Figure BDA00017229014700000312
和协方差预测值
Figure BDA00017229014700000313
Figure BDA0001722901470000041
Figure BDA0001722901470000042
式中，
Figure BDA0001722901470000043
为第k-1时刻增广后的状态向量的更新值，
Figure BDA0001722901470000044
为第k-1时刻增广后的状态转移矩阵，
Figure BDA0001722901470000045
为第k-1时刻增广后的协方差矩阵，
Figure BDA0001722901470000046
为第k-1时刻增广后的噪声增益矩阵，
Figure BDA0001722901470000047
为第k-1时刻增广后的过程噪声的协方差；
(2)滤波的更新步骤：
通过以下公式计算第k时刻增广后的状态向量
Figure BDA0001722901470000048
的更新值，以及第k时刻增广后的协方差
Figure BDA0001722901470000049
的更新值，令k＝k+1，转步骤(1)进行下一时刻的滤波：
Figure BDA00017229014700000410
Figure BDA00017229014700000411
式中，
Figure BDA00017229014700000412
为卡尔曼增益，并且
Figure BDA00017229014700000413
其中
Figure BDA00017229014700000414
为新息协方差，
Figure BDA00017229014700000415
表示新息，
Figure BDA00017229014700000416
为量测算子
Figure BDA00017229014700000417
为增广后的量测方程，
Figure BDA00017229014700000418
为增广后的量测噪声协方差；
Figure BDA00017229014700000419
为状态向量与量测之间的协方差
Figure BDA00017229014700000420
在根据本发明所述的航向约束下的状态估计方法中，优选地，所述步骤S3中基于CV模型表示增广后的状态转移矩阵
Figure BDA00017229014700000421
和噪声增益矩阵
Figure BDA00017229014700000422
Figure BDA00017229014700000423
本发明还提供了一种航向约束下的状态估计***，包括：
初始向量计算单元，用于利用约束直线的截距增广状态向量和协方差矩阵，计算初始值；
量测计算单元，用于基于位置约束关系和相对应的速度约束关系构建伪量测，并利用该伪量测增广量测方程；
卡尔曼滤波单元，用于基于增广后的状态方程和增广后的量测方程，在状态向量和协方差矩阵初始值的条件下，利用扩展卡尔曼滤波得到目标位置的滤波结果。
在根据本发明所述的航向约束下的状态估计***中，优选地，所述初始向量计算单元计算的增广后的状态向量的初始值为：
Figure BDA0001722901470000051
式中，T是采样周期，μθ表示无偏因子，
Figure BDA0001722901470000052
和
Figure BDA0001722901470000053
分别表示第k时刻和k-1时刻的距离，
Figure BDA0001722901470000054
和
Figure BDA0001722901470000055
分别表示第k时刻和k-1时刻的方位角α表示已知的航向，其中截距通过μθrksinθk-tanα·(μθrkcosθk)表示；
增广后的协方差矩阵的初始值为：
Figure BDA0001722901470000056
式中，
Figure BDA0001722901470000057
和
Figure BDA0001722901470000058
为块矩阵，Pk,b为截距的协方差，Pk,b＝Rk,yy+(tanα)2×Rk,xx-2tanα×Rk,xy，Rk,yy为y方向的量测噪声协方差，Rk,xx为x方向的量测噪声协方差，Rk,xy为交叉协方差。
在根据本发明所述的航向约束下的状态估计***中，优选地，所述量测计算单元构建的伪量测为：
ξk＝yk-tanα·xk-b
Figure BDA0001722901470000059
式中，xk,
Figure BDA00017229014700000510
分别为x方向的位置和相对应的速度，yk,
Figure BDA00017229014700000511
分别为y方向的位置和相对应的速度，α表示已知的航向，b为未知的截距；
基于上述伪量测增广后的量测方程为：
Figure BDA0001722901470000061
式中，wk为量测噪声
在根据本发明所述的航向约束下的状态估计***中，优选地，所述卡尔曼滤波单元迭代执行以下步骤：
(1)滤波的预测步骤：
通过以下公式计算第k时刻的状态向量预测值
Figure BDA0001722901470000062
和协方差预测值
Figure BDA0001722901470000063
Figure BDA0001722901470000064
Figure BDA0001722901470000065
式中，
Figure BDA0001722901470000066
为第k-1时刻增广后的状态向量的更新值，
Figure BDA0001722901470000067
为第k-1时刻增广后的状态转移矩阵，
Figure BDA0001722901470000068
为第k-1时刻增广后的协方差矩阵，
Figure BDA0001722901470000069
为第k-1时刻增广后的噪声增益矩阵，
Figure BDA00017229014700000610
为第k-1时刻增广后的过程噪声的协方差；
(2)滤波的更新步骤：
通过以下公式计算第k时刻增广后的状态向量
Figure BDA00017229014700000611
的更新值，以及第k时刻增广后的协方差
Figure BDA00017229014700000612
的更新值，令k＝k+1，转步骤(1)进行下一时刻的滤波：
Figure BDA00017229014700000613
Figure BDA00017229014700000614
式中，
Figure BDA00017229014700000615
为卡尔曼增益，并且
Figure BDA00017229014700000616
其中
Figure BDA00017229014700000617
为新息协方差，
Figure BDA0001722901470000071
表示新息，
Figure BDA0001722901470000072
为量测算子
Figure BDA0001722901470000073
为增广后的量测方程，
Figure BDA0001722901470000074
为增广后的量测噪声协方差；
Figure BDA0001722901470000075
为状态向量与量测之间的协方差
Figure BDA0001722901470000076
在根据本发明所述的航向约束下的状态估计***中，优选地，所述卡尔曼滤波单元基于CV模型表示增广后的状态转移矩阵
Figure BDA0001722901470000077
和噪声增益矩阵
Figure BDA0001722901470000078
Figure BDA0001722901470000079
实施本发明的航向约束下的状态估计方法及***，具有以下有益效果：本发明解决了不完整约束条件下的状态估计问题，充分利用了先验已知的信息，避免了信息的浪费；本发明尤其可以解决现实情况中存在的多车道轨迹估计问题，合理利用了先验已知的航向和轨迹形状信息，确定了车辆的行驶轨道，提高了目标状态的估计精度。
附图说明
图1为根据本发明优选实施例的航向约束下的状态估计方法的流程图；
图2为车辆在多车道公路行驶的示意图；
图3a和图3b分别为本发明的航向约束下的状态估计方法在第一种距离和方位角量测的协方差情况下的位置和速度的均方根误差；
图4a和图4b分别为本发明的航向约束下的状态估计方法在第二种距离和方位角量测的协方差情况下的位置和速度的均方根误差。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明不同于现有的方法(即直接利用滤波算法解决已知约束的估计问题)，而是首先用约束直线的截距增广原来的状态向量，利用状态分量和先验已知的航向及轨迹的形状求解被约束的直线。其次为了提高状态估计的性能，把航向约束整合到优化***，即根据包含目标状态额外信息的约束构建伪量测，增广原来的量测方程。然后利用扩展卡尔曼滤波(EKF)来处理二维状态变量和笛卡尔坐标量测之间存在的非线性关系，引入常速(CV)模型表征目标的动态特性。最后得到目标位置的滤波结果。
请参阅图1，为根据本发明优选实施例的航向约束下的状态估计方法的流程图。如图1所示，该实施例提供的航向约束下的状态估计方法包括以下步骤：
步骤S1：利用约束直线的截距增广状态向量和协方差矩阵，并计算初始值。
优选地，该步骤S1中增广后的状态向量的初始值为：
Figure BDA0001722901470000081
式中，T是采样周期，μθ表示无偏因子，
Figure BDA0001722901470000082
和
Figure BDA0001722901470000083
分别表示第k时刻和k-1时刻的距离，
Figure BDA0001722901470000084
分别表示第k时刻和k-1时刻的方位角α表示已知的航向，其中截距bk通过μθrksinθk-tanα·(μθrkcosθk)表示；
相应地，该步骤S1中增广后的协方差矩阵的初始值为：
Figure BDA0001722901470000085
式中，
Figure BDA0001722901470000091
和
Figure BDA0001722901470000092
为块矩阵，Pk,b为截距bk的协方差，Pk,b＝Rk,yy+(tanα)2×Rk,xx-2tanα×Rk,xy，Rk,yy为y方向的量测噪声协方差，Rk,xx为x方向的量测噪声协方差，Rk,xy为交叉协方差。
上述增广后的状态向量的初始值和增广后的协方差矩阵的初始值，均在k＝1和k＝2时刻进行计算。后续步骤S3在k＝3时刻开始滤波。
步骤S2：基于位置约束关系和相对应的速度约束关系构建伪量测，并利用该伪量测增广量测方程；
优选地，步骤S2中构建的伪量测为：
ξk＝yk-tanα·xk-b
Figure BDA0001722901470000093
式中，xk,
Figure BDA0001722901470000094
分别为x方向的位置和相对应的速度，yk,
Figure BDA0001722901470000095
分别为y方向的位置和相对应的速度，α表示已知的航向，b为未知的截距；
基于上述伪量测增广后的量测方程为：
Figure BDA0001722901470000096
式中，wk为量测噪声。
步骤S3：基于增广后的状态方程和增广后的量测方程，在状态向量和协方差矩阵初始值的条件下，利用扩展卡尔曼滤波得到目标位置的滤波结果。增广后的状态方程可以通过截距增广状态向量和协方差矩阵获得。
优选地，步骤S3包括迭代执行的以下步骤：
(1)滤波的预测步骤：
通过以下公式计算第k时刻的状态向量预测值
Figure BDA0001722901470000097
和协方差预测值
Figure BDA0001722901470000098
Figure BDA0001722901470000101
Figure BDA0001722901470000102
式中，
Figure BDA0001722901470000103
为第k-1时刻增广后的状态向量的更新值，
Figure BDA0001722901470000104
为第k-1时刻增广后的状态转移矩阵，
Figure BDA0001722901470000105
为第k-1时刻增广后的协方差矩阵，
Figure BDA0001722901470000106
为第k-1时刻增广后的噪声增益矩阵，
Figure BDA0001722901470000107
为第k-1时刻增广后的过程噪声的协方差；
(2)滤波的更新步骤：
通过以下公式计算第k时刻增广后的状态向量
Figure BDA0001722901470000108
的更新值，以及第k时刻增广后的协方差
Figure BDA0001722901470000109
的更新值，令k＝k+1，转步骤(1)进行下一时刻的滤波：
Figure BDA00017229014700001010
Figure BDA00017229014700001011
式中，
Figure BDA00017229014700001012
为卡尔曼增益，并且
Figure BDA00017229014700001013
其中
Figure BDA00017229014700001014
为新息协方差，
Figure BDA00017229014700001015
表示新息，
Figure BDA00017229014700001016
为量测算子
Figure BDA00017229014700001017
为增广后的量测方程，
Figure BDA00017229014700001018
为增广后的量测噪声协方差；
Figure BDA00017229014700001019
为状态向量与量测之间的协方差
Figure BDA00017229014700001020
上述步骤S3从k＝3时刻开始迭代直至待估计时刻。第k时刻增广后的状态向量
Figure BDA00017229014700001021
的更新值即可作为第k时刻目标位置的滤波结果。
上述虽然给出了截距的两种不同表达形式，即bk和b，但是由于航向不变的情况下，截距不会发生改变，即bk+1＝bk，因此该截距均为同一数值。此外，由于截距不变，使得增广后的状态方程中截距对应的噪声部分为0。
本发明并不限定上述步骤S1、步骤S2和步骤S3的具体运行顺序，只需要在当前迭代所需的数据获取或者计算完毕后即可以执行步骤S3的预测和更新步骤。
本发明可以用于对任何航向已知的行进目标的位置和速度进行预测。例如，对于在多车道沿公路行驶的车辆，目标的航向被约束在直公路的方向。因此，可以通过雷达采集车辆在二维极坐标中的距离和方位角，并基于这些采集的数据和***的动态方程，以及已知的航向α便可以通过上述方法更新状态向量，该状态向量中含有位置及速度信息。本发明也可以用于对航空器的轨迹进行预测。
下面对本发明的原理进行详细说明。
(1)笛卡尔坐标中的动态***是：
Xk+1＝FkXk+Γkvk
对于CV模型存在
Figure BDA0001722901470000111
其中，
Figure BDA0001722901470000112
是k时刻的状态向量，包含x方向和y方向的位置和相对应的速度。Fk是状态转移矩阵，Γk是噪声增益矩阵。vk表示具有零均值的高斯白噪声，vk～N(0,Qk)，Qk表示过程噪声的协方差。
在极坐标系中，包含距离和方位角的雷达量测方程是：
Figure BDA0001722901470000113
Figure BDA0001722901470000114
θk＝arctan(yk/xk),
Figure BDA0001722901470000115
Figure BDA0001722901470000116
Figure BDA0001722901470000117
和
Figure BDA0001722901470000118
分别表示量测的距离和方位角，rk和θk表示距离和方位角的真实量测，wk为量测噪声，
Figure BDA0001722901470000119
和
Figure BDA00017229014700001110
分别是距离和方位角量测的噪声，二者相互独立，服从零均值的高斯分布。wk～N(0,Rk)，Rk表示量测噪声的协方差。
(2)状态增广方法
在x-y平面，利用已知的航向α和未知的截距b描述位置约束，其数学表达式是：
yk＝tanα·xk+b
相对应的速度约束是：
Figure BDA0001722901470000121
为了计算位置及相对应的速度约束关系，将未知的截距作为状态分量估计。增广后的状态方程是：
Figure BDA0001722901470000122
其中
Figure BDA0001722901470000123
Figure BDA0001722901470000124
和
Figure BDA0001722901470000125
分别表示增广后的状态转移矩阵，增广后的状态向量和增广后的噪声增益矩阵。
Figure BDA0001722901470000126
是增广后的过程噪声，
Figure BDA0001722901470000127
是增广后的过程噪声协方差。
(3)伪量测
本发明基于位置约束关系和相对应的速度约束关系构建伪量测，为了整合状态约束到估计器，原来的量测向量被增广。
利用上面提及的约束关系，构建了伪量测ξk和ηk，其值都是常数。伪量测的数学表达式是：
ξk＝yk-tanα·xk-b
Figure BDA0001722901470000128
增广后的量测方程是：
Figure BDA0001722901470000131
本发明中上标a表示某些矩阵或向量的增广，下标k表示对应第k时刻，下标k-1表示对应第k-1时刻。从上式后两项得出，伪量测ξk和ηk是无噪声的，即状态变量满足给定的约束关系。量测噪声协方差的增广
Figure BDA0001722901470000132
是：
Figure BDA0001722901470000133
因为伪量测中包含目标位置和速度的先验信息，被整合到估计器时，可以明显提高估计的精度，并且把原来的约束问题转变成常规的滤波问题。
(4)航向约束卡尔曼滤波
首先利用两点差分法求解状态变量的初始估计，为了状态估计的无偏化，引入无偏因子，转换极坐标变量到笛卡尔坐标。
计算原来状态向量的初始值，其解表示为：
Figure BDA0001722901470000134
T是采样周期，μθ表示无偏因子，表达式是：
Figure BDA0001722901470000135
原来状态向量的初始协方差是：
Figure BDA0001722901470000136
Figure BDA0001722901470000137
和
Figure BDA0001722901470000138
都是块矩阵：
Figure BDA0001722901470000141
转换量测误差的协方差是：
Figure BDA0001722901470000142
Figure BDA0001722901470000143
请参阅图2，为车辆在多车道公路行驶的示意图，存在多条可能的车道和可能的轨迹。车辆的航向被约束在公路的方向，即已知运动目标的航向及轨迹形状，且车辆的直线轨迹存在唯一的截距b。借助这些先验信息及优化算法可以确定目标的轨迹。
根据位置分量之间的约束关系，截距b的方程是：
bk＝yk-tanα·xk.
利用k＝2时刻的量测初始化截距b，则b的初始值是：
bk＝μθrksinθk-tanα·(μθrkcosθk)
截距b的协方差是：
Pk,b＝Rk,yy+(tanα)2×Rk,xx-2tanα×Rk,xy
根据原来状态向量的初始值及增广截距的初始值，被增广状态即增广后状态向量的初始值是：
Figure BDA0001722901470000144
相对应的初始协方差矩阵是：
Figure BDA0001722901470000145
该卡尔曼滤波包括以下两大步骤：
a、滤波的预测步骤：
状态向量预测值为：
Figure BDA0001722901470000151
协方差的预测值为：
Figure BDA0001722901470000152
b、滤波的更新步骤：
计算新息协方差：
Figure BDA0001722901470000153
计算状态向量与量测之间的协方差：
Figure BDA0001722901470000154
计算卡尔曼增益：
Figure BDA0001722901470000155
计算状态向量的更新值：
Figure BDA0001722901470000156
计算协方差的更新值：
Figure BDA0001722901470000157
上式中
Figure BDA0001722901470000158
表示新息。I是n维的单位阵，n是状态向量的维度。本发明中状态向量或者协方差的两个下标相同时，表示更新值，如果下标为“k，k-1”时，表示根据k-1时刻的更新值，通过状态方程得出的k时刻的预测值。
本发明还提供了一种航向约束下的状态估计***。该实施例提供的***包括：
初始向量计算单元，用于约束直线的截距增广的状态向量和协方差矩阵，计算其初始值；该初始向量计算单元的计算过程与上述方法中步骤S1一致，在此不再赘述。
量测计算单元，用于基于位置约束关系和相对应的速度约束关系构建伪量测，并利用该伪量测增广量测方程；该量测计算单元的计算过程与上述方法中步骤S2一致，在此不再赘述。
卡尔曼滤波单元，用于基于增广后的状态方程和增广后的量测方程，在状态向量和协方差矩阵初始值的条件下，利用扩展卡尔曼滤波得到目标位置的滤波结果。该卡尔曼滤波单元的计算过程与上述方法中步骤S3一致，在此不再赘述。
本发明所述的航向约束下的状态估计方法和***解决了不完整约束条件下的状态估计问题，充分利用了先验已知的信息，避免了信息的浪费。传统的约束滤波算法高度依赖约束先验信息的完整性，直接整合约束条件到优化算法，估计目标的状态，不能解决约束条件模糊的估计问题。而航向约束卡尔曼滤波算法可以解决现实情况中存在的多车道轨迹估计问题，合理利用了先验已知的航向和轨迹形状信息，确定了车辆的行驶轨道，提高了目标状态的估计精度。
为展示本发明的性能，实验设置了目标行驶的仿真场景，利用两组不同的量测协方差验证算法的性能。将本发明提出的航向约束卡尔曼滤波(HCKF)方法与两种经典的约束估计算法进行比较，包括完美量测法(PMKF)和估计后投影法(EPKF)。采用不同的权重矩阵，估计后投影法(EPKF)有不同的滤波形式，分别表示为：EPKF(W＝P-1)和EPKF(W＝I)。选取均方根误差(RMSE)作为性能的量度，进行500次蒙特卡洛仿真。仿真结果如图3a和3b所示。可以明显看出，HCKF的位置均方根误差小于无约束的EKF。因为HCKF的约束先验信息不完整，其位置的均方根误差稍大于约束完整的PMKF和EPKFs。
为了定量描述不同估计器之间的差别，给定了每种滤波器的时间平均均方根误差。如下表所示：
表格1
Figure BDA0001722901470000161
Figure BDA0001722901470000171
当距离和方位角量测的协方差变化，其它条件不变时，验证所述算法的有效性。仿真结果如图4a和4b所示。从图中看出，HCKF的位置均方根误差小于无约束的EKF。随着模拟步长的变化，5种估计器的速度均方根误差趋于稳定且相差不大。
应该理解地是，本发明中航向约束下的状态估计方法及***的原理相同，因此对航向约束下的状态估计方法的实施例的详细阐述也适用于航向约束下的状态估计***。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种航向约束下的状态估计方法，其特征在于，包括以下步骤：
S1、利用约束直线的截距增广状态向量和协方差矩阵，并计算初始值；
S2、基于位置约束关系和相对应的速度约束关系构建伪量测，并利用该伪量测增广量测方程；
S3、基于增广后的状态方程和增广后的量测方程，在状态向量和协方差矩阵初始值的条件下，利用扩展卡尔曼滤波得到目标位置的滤波结果；
所述步骤S1中增广后的状态向量的初始值为：
Figure FDA0002615031820000011
式中，T是采样周期，μθ表示无偏因子，
Figure FDA0002615031820000012
和
Figure FDA0002615031820000013
分别表示第k时刻和k-1时刻的距离，
Figure FDA0002615031820000014
和
Figure FDA0002615031820000015
分别表示第k时刻和k-1时刻的方位角；α表示已知的航向，其中截距通过μθrksinθk-tanα·(μθrkcosθk)表示；
步骤S1中增广后的协方差矩阵的初始值为：
Figure FDA0002615031820000016
式中，
Figure FDA0002615031820000017
和
Figure FDA0002615031820000018
为块矩阵，Pk,b为截距的协方差，Pk,b＝Rk,yy+(tanα)2×Rk,xx-2tanα×Rk,xy，Rk,yy为y方向的量测噪声协方差，Rk,xx为x方向的量测噪声协方差，Rk,xy为交叉协方差。
2.根据权利要求1所述的航向约束下的状态估计方法，其特征在于，所述步骤S2中构建的伪量测为：
ξk＝yk-tanα·xk-b
Figure FDA0002615031820000021
式中，xk,
Figure FDA0002615031820000022
分别为x方向的位置和相对应的速度，yk,
Figure FDA0002615031820000023
分别为y方向的位置和相对应的速度，α表示已知的航向，b为未知的截距；
基于上述伪量测增广后的量测方程为：
Figure FDA0002615031820000024
式中，wk为量测噪声。
3.根据权利要求2所述的航向约束下的状态估计方法，其特征在于，所述步骤S3包括迭代执行的以下步骤：
(1)滤波的预测步骤：
通过以下公式计算第k时刻的状态向量预测值
Figure FDA0002615031820000025
和协方差预测值
Figure FDA0002615031820000026
Figure FDA0002615031820000027
Figure FDA0002615031820000028
式中，
Figure FDA0002615031820000029
为第k-1时刻增广后的状态向量的更新值，
Figure FDA00026150318200000210
为第k-1时刻增广后的状态转移矩阵，
Figure FDA00026150318200000211
为第k-1时刻增广后的协方差矩阵，
Figure FDA00026150318200000212
为第k-1时刻增广后的噪声增益矩阵，
Figure FDA00026150318200000213
为第k-1时刻增广后的过程噪声的协方差；
(2)滤波的更新步骤：
通过以下公式计算第k时刻增广后的状态向量
Figure FDA00026150318200000214
的更新值，以及第k时刻增广后的协方差
Figure FDA00026150318200000215
的更新值，令k＝k+1，转步骤(1)进行下一时刻的滤波：
Figure FDA0002615031820000031
Figure FDA0002615031820000032
式中，
Figure FDA0002615031820000033
为卡尔曼增益，并且
Figure FDA0002615031820000034
其中
Figure FDA0002615031820000035
为新息协方差，
Figure FDA0002615031820000036
表示新息，
Figure FDA0002615031820000037
为观测算子，
Figure FDA0002615031820000038
为增广后的量测方程，
Figure FDA0002615031820000039
为增广后的量测噪声协方差；
Figure FDA00026150318200000310
为状态向量与量测之间的协方差
Figure FDA00026150318200000311
4.根据权利要求3所述的航向约束下的状态估计方法，其特征在于，所述步骤S3中基于CV模型表示增广后的状态转移矩阵
Figure FDA00026150318200000312
和噪声增益矩阵
Figure FDA00026150318200000313
Figure FDA00026150318200000314
5.一种航向约束下的状态估计***，其特征在于，包括：
初始向量计算单元，用于约束直线的截距增广的状态向量和协方差矩阵，计算初始值；
量测计算单元，用于基于位置约束关系和相对应的速度约束关系构建伪量测，并利用该伪量测增广量测方程；
卡尔曼滤波单元，用于基于增广后的状态方程和增广后的量测方程，在状态向量和协方差矩阵初始值条件下，利用扩展卡尔曼滤波得到目标位置的滤波结果；
所述初始向量计算单元计算的增广后的状态向量的初始值为：
Figure FDA0002615031820000041
式中，T是采样周期，μθ表示无偏因子，
Figure FDA0002615031820000042
和
Figure FDA0002615031820000043
分别表示第k时刻和k-1时刻的距离，
Figure FDA0002615031820000044
和
Figure FDA0002615031820000045
分别表示第k时刻和k-1时刻的方位角，α表示已知的航向，其中截距通过μθrksinθk-tanα·(μθrkcosθk)表示；
增广后的协方差矩阵的初始值为：
Figure FDA0002615031820000046
式中，
Figure FDA0002615031820000047
和
Figure FDA0002615031820000048
为块矩阵，Pk,b为截距的协方差，Pk,b＝Rk,yy+(tanα)2×Rk,xx-2tanα×Rk,xy，Rk,yy为y方向的量测噪声协方差，Rk,xx为x方向的量测噪声协方差，Rk,xy为交叉协方差。
6.根据权利要求5所述的航向约束下的状态估计***，其特征在于，所述量测计算单元构建的伪量测为：
ξk＝yk-tanα·xk-b
Figure FDA0002615031820000049
式中，xk,
Figure FDA00026150318200000410
分别为x方向的位置和相对应的速度，yk,
Figure FDA00026150318200000411
分别为y方向的位置和相对应的速度，α表示已知的航向，b为未知的截距；
基于上述伪量测增广后的量测方程为：
Figure FDA00026150318200000412
式中，wk为量测噪声。
7.根据权利要求6所述的航向约束下的状态估计***，其特征在于，所述卡尔曼滤波单元迭代执行以下步骤：
(1)滤波的预测步骤：
通过以下公式计算第k时刻的状态向量预测值
Figure FDA0002615031820000051
和协方差预测值
Figure FDA0002615031820000052
Figure FDA0002615031820000053
Figure FDA0002615031820000054
式中，
Figure FDA0002615031820000055
为第k-1时刻增广后的状态向量的更新值，
Figure FDA0002615031820000056
为第k-1时刻增广后的状态转移矩阵，
Figure FDA0002615031820000057
为第k-1时刻增广后的协方差矩阵，
Figure FDA0002615031820000058
为第k-1时刻增广后的噪声增益矩阵，
Figure FDA0002615031820000059
为第k-1时刻增广后的过程噪声的协方差；
(2)滤波的更新步骤：
通过以下公式计算第k时刻增广后的状态向量
Figure FDA00026150318200000510
的更新值，以及第k时刻增广后的协方差
Figure FDA00026150318200000511
的更新值，令k＝k+1，转步骤(1)进行下一时刻的滤波：
Figure FDA00026150318200000512
Figure FDA00026150318200000513
式中，
Figure FDA00026150318200000514
为卡尔曼增益，并且
Figure FDA00026150318200000515
其中
Figure FDA00026150318200000516
为新息协方差，
Figure FDA00026150318200000517
表示新息，
Figure FDA00026150318200000518
为量测算子，
Figure FDA00026150318200000519
为增广后的量测方程，
Figure FDA00026150318200000520
为增广后的量测噪声协方差；
Figure FDA00026150318200000521
为状态向量与量测之间的协方差
Figure FDA00026150318200000522
8.根据权利要求7所述的航向约束下的状态估计***，其特征在于，所述卡尔曼滤波单元基于CV模型表示增广后的状态转移矩阵
Figure FDA00026150318200000523
和噪声增益矩阵
Figure FDA00026150318200000524
Figure FDA0002615031820000061
CN201810739573.4A 2018-07-06 2018-07-06 一种航向约束下的状态估计方法及*** Active CN108871365B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810739573.4A CN108871365B (zh) 2018-07-06 2018-07-06 一种航向约束下的状态估计方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810739573.4A CN108871365B (zh) 2018-07-06 2018-07-06 一种航向约束下的状态估计方法及***

Publications (2)

Publication Number Publication Date
CN108871365A CN108871365A (zh) 2018-11-23
CN108871365B true CN108871365B (zh) 2020-10-20

Family

ID=64299978

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810739573.4A Active CN108871365B (zh) 2018-07-06 2018-07-06 一种航向约束下的状态估计方法及***

Country Status (1)

Country Link
CN (1) CN108871365B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110793518B (zh) * 2019-11-11 2021-05-11 中国地质大学（北京） 一种海上平台的定位定姿方法及***
CN112763980B (zh) * 2020-12-28 2022-08-05 哈尔滨工程大学 一种基于方位角及其变化率的目标运动分析方法
CN112904393B (zh) * 2021-01-19 2023-11-10 江苏大学 一种导航路径几何约束辅助的农业机械自主导航方法
CN116702479B (zh) * 2023-06-12 2024-02-06 哈尔滨工程大学 一种水下航行器未知输入与位置估计方法及***

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104614751A (zh) * 2015-01-30 2015-05-13 上海电机学院 基于约束信息的目标定位方法
CN106054170A (zh) * 2016-05-19 2016-10-26 哈尔滨工业大学 一种约束条件下的机动目标跟踪方法
CN107015945A (zh) * 2017-04-10 2017-08-04 哈尔滨工业大学 一种基于混合转移分布的高阶交互式多模型滤波方法
CN107045125A (zh) * 2017-03-17 2017-08-15 电子科技大学 一种基于预测值量测转换的交互多模型雷达目标跟踪方法
WO2018081348A1 (en) * 2016-10-26 2018-05-03 The Charles Stark Draper Laboratory, Inc. Vision-inertial navigation with variable contrast tracking residual

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104614751A (zh) * 2015-01-30 2015-05-13 上海电机学院 基于约束信息的目标定位方法
CN106054170A (zh) * 2016-05-19 2016-10-26 哈尔滨工业大学 一种约束条件下的机动目标跟踪方法
WO2018081348A1 (en) * 2016-10-26 2018-05-03 The Charles Stark Draper Laboratory, Inc. Vision-inertial navigation with variable contrast tracking residual
CN107045125A (zh) * 2017-03-17 2017-08-15 电子科技大学 一种基于预测值量测转换的交互多模型雷达目标跟踪方法
CN107015945A (zh) * 2017-04-10 2017-08-04 哈尔滨工业大学 一种基于混合转移分布的高阶交互式多模型滤波方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Fixed-lag smoothing with linear equality constraints;Keyi Li et al.;《2017 20th International Conference on Information Fusion》;20170713;第1-7页 *

Also Published As

Publication number Publication date
CN108871365A (zh) 2018-11-23

Similar Documents

Publication Publication Date Title
CN107045125B (zh) 一种基于预测值量测转换的交互多模型雷达目标跟踪方法
CN111985093B (zh) 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法
CN108871365B (zh) 一种航向约束下的状态估计方法及***
CN111178385B (zh) 一种鲁棒在线多传感器融合的目标跟踪方法
CN110503071B (zh) 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法
CN110208792B (zh) 同时估计目标状态和轨迹参数的任意直线约束跟踪方法
CN111722214B (zh) 雷达多目标跟踪phd实现方法
CN110231620B (zh) 一种噪声相关***跟踪滤波方法
Agate et al. Road-constrained target tracking and identification using a particle filter
CN111711432B (zh) 一种基于ukf和pf混合滤波的目标跟踪算法
CN107015944A (zh) 一种用于目标跟踪的混合平方根容积卡尔曼滤波方法
CN111562570A (zh) 基于毫米波雷达的面向自动驾驶的车辆感知方法
CN115204212A (zh) 一种基于stm-pmbm滤波算法的多目标跟踪方法
CN116500575A (zh) 一种基于变分贝叶斯理论的扩展目标跟踪方法和装置
CN114137562B (zh) 一种基于改进全局最邻近的多目标跟踪方法
CN111291319A (zh) 一种应用于非高斯噪声环境下的移动机器人状态估计方法
CN114236480A (zh) 一种机载平台传感器***误差配准算法
CN113759364A (zh) 一种基于激光地图的毫米波雷达连续定位方法及装置
CN116224320B (zh) 一种极坐标系下处理多普勒量测的雷达目标跟踪方法
CN113344970A (zh) 基于多伯努利的非规则多扩展目标联合跟踪与分类方法
CN107886058B (zh) 噪声相关的两阶段容积Kalman滤波估计方法及***
CN105549003A (zh) 一种汽车雷达目标跟踪方法
CN115114985A (zh) 一种基于集合理论的传感器***分布式融合方法
CN111998854B (zh) 基于Cholesky分解计算的精确扩展Stirling插值滤波方法
CN112285697A (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