CN115457510A - 一种基于变分贝叶斯的自适应协同目标跟踪方法 - Google Patents

一种基于变分贝叶斯的自适应协同目标跟踪方法 Download PDF

Info

Publication number
CN115457510A
CN115457510A CN202211189501.XA CN202211189501A CN115457510A CN 115457510 A CN115457510 A CN 115457510A CN 202211189501 A CN202211189501 A CN 202211189501A CN 115457510 A CN115457510 A CN 115457510A
Authority
CN
China
Prior art keywords
vehicle
cooperative
observation
state
target
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.)
Pending
Application number
CN202211189501.XA
Other languages
English (en)
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.)
Shanghai Intelligent New Energy Vehicle Technology Innovation Platform Co ltd
Shanghai Motor Vehicle Inspection Certification and Tech Innovation Center Co Ltd
Original Assignee
Shanghai Intelligent New Energy Vehicle Technology Innovation Platform Co ltd
Shanghai Motor Vehicle Inspection Certification and Tech Innovation Center Co Ltd
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 Shanghai Intelligent New Energy Vehicle Technology Innovation Platform Co ltd, Shanghai Motor Vehicle Inspection Certification and Tech Innovation Center Co Ltd filed Critical Shanghai Intelligent New Energy Vehicle Technology Innovation Platform Co ltd
Priority to CN202211189501.XA priority Critical patent/CN115457510A/zh
Publication of CN115457510A publication Critical patent/CN115457510A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/84—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using probabilistic graphical models from image or video features, e.g. Markov models or Bayesian networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Probability & Statistics with Applications (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明涉及一种基于变分贝叶斯的自适应协同目标跟踪方法，所述方法基于采集到的主车和协同车对目标车的感知数据以及各自的自车状态数据，对协同车与目标车的状态及观测噪声的协方差矩阵进行联合后验估计，基于变分贝叶斯框架，通过变分迭代实时更新协同车与目标车的状态及观测噪声协方差矩阵的参数，实现自适应协同目标跟踪；其中，目标车的位置噪声服从高斯分布。与现有技术相比，本发明采用多车协同跟踪，联合估计多车运动状态及观测误差协方差矩阵，在不增加车载传感器硬件成本的基础上，提高目标跟踪的精度，且目标车的位置噪声服从高斯分布的设置，跟踪效果鲁棒性更好，降低了计算资源，提高了实时性。

Description

一种基于变分贝叶斯的自适应协同目标跟踪方法
技术领域
本发明涉及智能网联汽车目标跟踪技术领域，尤其是涉及一种基于变分贝叶斯的自适应协同目标跟踪方法。
背景技术
目标跟踪系统是智能网联汽车环境感知系统的重要组成部分，为了实现车辆自适应巡航、碰撞预警及其他车辆自动驾驶控制功能，需要准确地检测并估计智能网联车辆周围环境中各类目标的准确位置及运动状态信息。
随着车联网技术的成熟与普及，越来越多的车辆通过车间通信技术能够在车辆之间完成信息的共享。基于车车通信的协同目标跟踪不仅有较高的跟踪精度而且扩大了自车的感知范围，缩小了驾驶盲区。此外，由于基于车车通信的协同跟踪能充分利用协同车辆发布的信息，因此可以减少单车传感器种类或数量，降低智能汽车的制造成本。
但是传统的协同跟踪算法在实际跟踪场景中会有一定的局限性，原因在于这些算法本身的建立依赖于观测噪声的统计特性已知，而在实际交通场景中，由于主车、协同和目标车的运动工况十分复杂，观测噪声的统计特性往往难以确定，此外在实际交通场景中，主车、协同车和目标车的运动工况本身存在摄动，同时容易受到外界交通环境的干扰，跟踪系统的观测噪声也可能随之发生变化，这种情形下，传统的协同跟踪算法的工作性能就会发生退化，极端情形下甚至无法正常工作。
经过检索，中国发明专利申请CN113971752A公开了一种抗观测数据干扰的多车协同状态估计方法，但是该方法假设目标车的位置噪声服从t分布，控制参数过多，协同跟踪起始阶段状态估计收敛更慢，在复杂噪声环境下跟踪结果甚至会发散，此外，估计的参数增加，因此消耗的计算资源更大，执行时间更长，不利于车载控制器上实现实时目标跟踪。
为了解决时变观测噪声下普通的协同跟踪算法无法自适应地估计观测噪声的大小，从而导致跟踪精度降低的问题以及实时性跟踪需求，亟需设计一种自适应协同目标跟踪方法。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供了一种基于变分贝叶斯的自适应协同目标跟踪方法，采用贝叶斯框架，对协同车与目标的状态及观测噪声协方差矩阵的后验分布进行联合估计，通过变分贝叶斯推断迭代更新协同车与目标的状态及观测噪声协方差矩阵的参数，可以在不增加车载传感器硬件成本的基础上，提高目标跟踪的精度，采用目标车的位置噪声服从高斯分布的设置，跟踪效果鲁棒性更好，降低了计算资源，提高了实时性。
本发明的目的可以通过以下技术方案来实现：
根据本发明的第一方面，提供了一种基于变分贝叶斯的自适应协同目标跟踪方法，该方法包括以下步骤：
步骤1、主车和协同车分别对同一目标车进行感知，协同车将感知到的观测数据和自车状态数据发送给主车；
步骤2、主车对接收到的观测数据进行同步处理和坐标变换处理，然后将协同车和目标车的状态及观测组合；
步骤3、在k＝0时刻，初始化增广状态分布为
Figure BDA0003868774930000021
其中，
Figure BDA0003868774930000022
表示服从均值为
Figure BDA0003868774930000023
和协方差矩阵为P0的高斯分布，并返回步骤1；
步骤4、采用扩展卡尔曼滤波预测算法预测k时刻增广状态分布
Figure BDA0003868774930000024
Figure BDA0003868774930000025
其中，
Figure BDA0003868774930000026
和Pk|k-1分别表示预测的增广状态及相应的误差协方差矩阵；
步骤5、预测k时刻观测噪声协方差矩阵Σk的分布参数vk|k-1、Vk|k-1，初始化Σk的分布参数
Figure BDA0003868774930000027
及数学期望E[Σk]0，并初始化变分迭代次数i＝0；
步骤6、根据变分贝叶斯框架，输入k时刻的观测yk，利用扩展卡尔曼滤波EKF的修正算法计算第i次迭代增广状态xk|k的变分后验分布
Figure BDA0003868774930000028
Figure BDA0003868774930000029
步骤7、计算第i次迭代增广观测噪声协方差矩阵Σk的变分后验分布
Figure BDA00038687749300000210
及数学期望E[∑k]i，其中，
Figure BDA00038687749300000211
表示自由度为
Figure BDA0003868774930000031
尺度参数为
Figure BDA0003868774930000032
的逆威沙特分布；
步骤8、判断变分贝叶斯迭代是否收敛，如果收敛则转到步骤9，否则返回步骤6，迭代次数i增加1；
步骤9、输出增广状态
Figure BDA0003868774930000033
误差协方差矩阵
Figure BDA0003868774930000034
变分参数
Figure BDA0003868774930000035
并判断是否结束协同目标跟踪，如果结束则算法终止，否则返回步骤1。
优选地，所述步骤1具体为：定义t1时刻主车对目标车的观测为
Figure BDA0003868774930000036
t2时刻协同车对目标车的观测为
Figure BDA0003868774930000037
协同车发送给主车的数据包括：协同车的感知时刻t2、协同车对目标车的观测
Figure BDA0003868774930000038
协同车通过车载定位设备获得的经纬度和航向角。
优选地，所述步骤S2具体为：
增广观测yk：主车将接收到的观测数据进行同步处理和坐标变换，经过数据预处理后，得到主车对协同车和目标的观测
Figure BDA0003868774930000039
协同车对目标的观测
Figure BDA00038687749300000310
以及协同车相对于主车的航向角θk，组合后得到增广观测
Figure BDA00038687749300000311
增广状态xk：协同车的运动状态为
Figure BDA00038687749300000312
其中，
Figure BDA00038687749300000313
为协同车在主车坐标系x,y方向的位置与速度，(θk,ωk)为协同车在主车坐标系下的航向角和角速度；目标车的运动状态为
Figure BDA00038687749300000314
表示目标车在主车坐标系下x,y方向的位置与速度；组合后得到增广状态
Figure BDA00038687749300000315
则观测方程为：
Figure BDA00038687749300000316
式中，θ为协同车相对于主车坐标系的航向角。
优选地，所述步骤4中的增广状态
Figure BDA00038687749300000317
及协方差矩阵Pk|k-1，具体数学表达式分别为：
Figure BDA00038687749300000318
Figure BDA00038687749300000319
式中，
Figure BDA00038687749300000320
分别为k-1时刻更新后的增广状态和k时刻预测的增广状态，Fk∈Rn×n是状态转移矩阵；Pk-1|k-1、Pk|k-1∈Rn×n分别为k-1时刻更新后的增广状态协方差矩阵和k时刻预测的状态协方差矩阵，Qk∈Rn×n表示过程噪声协方差矩阵，n表示增广状态维度。
优选地，所述步骤5包括：
预测k时刻观测噪声协方差矩阵Σk的分布参数vk|k-1、Vk|k-1，并初始化Σk的后验分布参数
Figure BDA0003868774930000041
及数学期望E[Σk]0，具体的数学表达式分别为：
vk|k-1＝ρ(vk-1|k-1-n-1)+n+1
Vk|k-1＝BVk-1|k-1BT
Figure BDA0003868774930000042
其中，ρ是遗忘因子，0＜ρ≤1；
Figure BDA0003868774930000043
0＜|B|≤1，E[·]表示随机变量的期望，初始化变分迭代次数为i＝0，n表示增广状态维度。
优选地，所述步骤6包括以下子步骤：
步骤6.1、根据预测的增广状态
Figure BDA0003868774930000044
及观测方程h(·)计算预测的增广观测：
Figure BDA0003868774930000045
步骤6.2、根据预测的增广状态
Figure BDA0003868774930000046
计算观测方程的雅克比矩阵Hk：
Figure BDA0003868774930000047
式中，n表示增广状态维度；
步骤6.3、计算第i次迭代增广状态xk|k的变分后验分布
Figure BDA0003868774930000048
Figure BDA0003868774930000049
的参数
Figure BDA00038687749300000410
和
Figure BDA00038687749300000411
Figure BDA00038687749300000412
Figure BDA00038687749300000413
其中，
Figure BDA00038687749300000414
Figure BDA00038687749300000415
式中，E[∑k]i为第i次迭代的k时刻观测噪声协方差矩阵；Pk|k-1为k时刻预测的状态协方差矩阵，yk为k时刻的增广观测。
优选地，所述步骤7具体为：
计算第i次迭代增广观测噪声协方差矩阵Σk的变分后验分布
Figure BDA00038687749300000416
Figure BDA0003868774930000051
的
Figure BDA0003868774930000052
及∑k的数学期望：
Figure BDA0003868774930000053
Figure BDA0003868774930000054
Figure BDA0003868774930000055
式中，h(·)为观测方程。
优选地，所述步骤8中判断变分贝叶斯迭代是否收敛，具体为：满足以下任一收敛条件，即认为变分贝叶斯迭代收敛；
收敛条件1：计算目标状态的变化量：
Figure BDA0003868774930000056
其中，
Figure BDA0003868774930000057
分别为第i次、i-1次的目标状态预测值；‖·‖2表示向量的L2范数；如果thred小于设定阈值δ，则变分迭代收敛；
收敛条件2：变分迭代次数达到设定的最大迭代次数N。
根据本发明的第二方面，提供了一种电子设备，包括存储器和处理器，所述存储器上存储有计算机程序，所述处理器执行所述程序时实现任一项所述的方法。
根据本发明的第三方面，提供了一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现任一项所述的方法。
与现有技术相比，本发明具有以下优点：
1)本发明基于目标车的位置噪声服从高斯分布的假设，控制参数较少，相比于噪声分布控制参数多的跟踪方法克服了协同跟踪起始阶段状态估计收敛慢及复杂噪声环境下状态估计可能发散的问题；采用服从于高斯分布的目标车的位置噪声设置，估计控制参数较少，降低了计算资源，缩短了执行时间，更加便于车载控制器上实现实时目标跟踪；此外，由中心极限定理知，当被跟踪目标数量N逐渐趋于无穷大时，N个抽样样本噪声的均值的频数逐渐趋于高斯分布，因此，基于目标车的位置噪声服从高斯分布更符合实际应用场景；
2)本发明能通过主车和协同车对目标车的感知进行协同目标跟踪，并采用变分近似的方法估计了时变观测噪声的大小，与基于单车车载传感器的目标跟踪相比，协同目标跟踪可以有效提升目标跟踪精度，与传统的观测噪声固定的协同跟踪方法相比，本发明可以在观测噪声时变的情况下也达到较高的跟踪精度；
3)本发明与现有单车车辆目标自适应跟踪技术相比，采用协同跟踪及自适应估计协方差矩阵的方法，不仅提高了跟踪的精度而且进一步缩小了驾驶盲区，扩大了主车的感知范围。
附图说明
图1为本发明的方法流程图；
图2为本发明的方法与单车自适应跟踪方法的跟踪误差对比图；其中，图2(a)为位置的RMSE跟踪误差对比图，图2(b)为速度的RMSE跟踪误差对比图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
实施例
如图1所示，本实施例给出了一种基于变分贝叶斯的自适应协同目标跟踪方法，该方法包括以下步骤：
步骤1：主车和协同车分别对同一目标车进行感知，然后协同车将感知到的观测数据和自身状态数据发送给主车。
步骤2：主车将接收到的观测数据进行同步处理和坐标变换，经过数据预处理后，得到主车对协同车和目标车的观测
Figure BDA0003868774930000061
协同车对目标车的观测
Figure BDA0003868774930000062
以及协同车相对于主车的航向角θk；将上述观测进行组合，得到增广观测yk，将待估计的协同车运动状态和目标车运动状态进行组合，得到增广状态xk，下标k表示同步后的第k个时刻。具体过程如下：
假设协同车、目标车均在二维笛卡尔坐标系下运动。定义协同车的运动状态为
Figure BDA0003868774930000063
其中，
Figure BDA0003868774930000064
分别为协同车在主车坐标系x、y方向的位置与速度，θk、ωk分别为协同车在主车坐标系下的航向角和角速度；定义目标车的运动状态为
Figure BDA0003868774930000065
其中，
Figure BDA0003868774930000066
分别为目标车在主车坐标系下x、y方向的位置与速度；
则增广状态矩阵
Figure BDA0003868774930000067
增广观测矩阵
Figure BDA0003868774930000071
对应的观测方程为：
Figure BDA0003868774930000072
式中，θ为协同车相对于主车坐标系的航向角；
步骤3：在k＝0时刻，初始化增广状态分布为
Figure BDA0003868774930000073
其中，
Figure BDA0003868774930000074
表示服从于均值为
Figure BDA0003868774930000075
和协方差矩阵为P0的高斯分布。在本实施例中，
Figure BDA0003868774930000076
P0＝diag[10,10,100,100,10,10,100,100,0.01,0.01]，diag[·]表示以[·]为对角元素的对角阵。
由贝叶斯概率法则可知，逆威沙特分布是均值已知的高斯分布的协方差矩阵的共轭先验分布，因此假设增广观测的噪声协方差矩阵Σk服从自由度为vk、尺度矩阵为Vk的逆威沙特分布，Σk～IW(vk,Vk)。假设初始的增广状态服从均值为
Figure BDA0003868774930000077
协方差矩阵为P0的高斯分布，
Figure BDA0003868774930000078
给定从第1时刻到第k-1时刻的增广观测y1:k-1，由查普曼-科莫高洛夫(Chapman-Kolmogorov)方程得预测的增广状态及误差协方差矩阵：
p(xk,∑k|y1:k-1)＝∫p(xk,∑k|xk-1,∑k-1)p(xk-1,∑k-1|y1:k-1)dxk-1d∑k-1 (2)
式中，假设增广状态xk与增广观测的噪声协方差矩阵Σk独立，则
p(xk,∑k|xk-1,∑k-1)＝p(xk|xk-1)p(∑k|∑k-1) (3)
假设xk-1和Σk-1的预测分布可近似为高斯分布和逆威沙特分布的乘积：
Figure BDA0003868774930000079
步骤4：利用扩展卡尔曼滤波EKF预测算法计算k(k≥1)时刻增广状态分布
Figure BDA00038687749300000710
其中
Figure BDA00038687749300000711
和Pk|k-1分别表示预测的增广状态及误差协方差矩阵，表达式分别为：
Figure BDA00038687749300000712
Figure BDA00038687749300000713
其中，
Figure BDA00038687749300000714
分别为前一时刻更新后的增广状态和当前时刻预测的增广状态，Fk∈Rn×n是状态转移矩阵，Pk-1|k-1,Pk|k-1∈Rn×n分别为前一时刻更新后的增广状态协方差矩阵和当前时刻预测的增广状态协方差矩阵，Qk∈Rn×n表示系统的过程噪声协方差矩阵，n表示增广状态的维度，在本实施例中n＝10，状态转移矩阵Fk为：
Figure BDA0003868774930000081
其中，Δt表示传感器的采样周期，在本实施例中，Δt＝0.1s。
协方差矩阵为
Figure BDA0003868774930000082
Figure BDA0003868774930000083
的高斯分布，其中
Figure BDA0003868774930000084
I是一个大小为2×2的单位矩阵，
Figure BDA0003868774930000085
表示克罗内克积。
Q2＝0.012×D×DT,
Figure BDA0003868774930000086
步骤5：预测k时刻增广观测噪声协方差矩阵Σk的分布参数vk|k-1,Vk|k-1
vk|k-1＝ρ(vk-1|k-1-n-1)+n+1 (8)
Vk|k-1＝BVk-1|k-1BT (9)
其中ρ是遗忘因子，0＜ρ≤1，
Figure BDA0003868774930000087
I1是一个大小为7×7的单位矩阵，0＜|B|≤1，E[·]表示随机变量的期望。
初始化∑k的分布参数
Figure BDA0003868774930000088
及∑k的数学期望E[∑k]0，初始化变分迭代次数为i＝0。
Figure BDA0003868774930000089
给定第k时刻的观测值yk，根据贝叶斯法则，xk,∑k的联合后验分布为：
p(xk,∑k|y1:k)∝p(yk|xk,∑k)p(xk,∑k|y1:k-1) (11)
求解上述联合后验分布涉及多重积分，难以直接计算。为了使计算易于处理，需要对后验分布做变分近似。利用平均场近似，假设变分后验分布q(xk,∑k)可被分解为：
q(xk,∑k)＝Q(xk)Q(∑k)
变分后验分布q(xk,∑k)与真实后验分布p(xk,∑k|y1:k)的差异用KL散度来度量：
Figure BDA0003868774930000091
通过最小化KL散度可知，变分后验分布的公式为：
log Q(xk)∝∫logp(yk,xk,Σk|y1:k-1)Q(Σk)dΣk (13)
log Q(Σk)∝∫logp(yk,xk,Σk|y1:k-1)Q(xk)dxk (14)
由log Q(xk)的公式经推导可知，xk服从均值为
Figure BDA0003868774930000092
协方差为Pk|k的高斯分布，即
Figure BDA0003868774930000093
由log Q(∑k)的公式经推导可知，∑k服从自由度参数为
Figure BDA0003868774930000094
尺度参数为
Figure BDA0003868774930000095
的逆威沙特分布，即Q(∑k)＝IW(∑k|vk|k,Vk|k)。
步骤6：根据变分贝叶斯框架，输入k时刻的观测yk，利用扩展卡尔曼滤波(EKF)的修正算法计算第i次迭代增广状态xk|k的变分后验分布
Figure BDA0003868774930000096
Figure BDA0003868774930000097
具体包括以下子步骤：
步骤6.1、根据预测的增广状态
Figure BDA0003868774930000098
及观测方程h(·)计算观测的预测：
Figure BDA0003868774930000099
步骤6.2、根据预测的增广状态
Figure BDA00038687749300000910
计算观测方程的雅克比矩阵：
Figure BDA00038687749300000911
在本实施例中，雅克比矩阵等于：
Figure BDA00038687749300000912
其中，
Figure BDA00038687749300000913
分别等于：
Figure BDA00038687749300000914
Figure BDA00038687749300000915
步骤6.3、更新增广状态
Figure BDA00038687749300000916
及协方差矩阵
Figure BDA00038687749300000917
Figure BDA00038687749300000918
Figure BDA0003868774930000101
Figure BDA0003868774930000102
Figure BDA0003868774930000103
步骤7：计算第i次迭代增广观测噪声协方差∑k的变分后验分布
Figure BDA0003868774930000104
及数学期望E[∑k]i，其中
Figure BDA0003868774930000105
表示自由度为
Figure BDA0003868774930000106
尺度参数为
Figure BDA0003868774930000107
的逆威沙特分布；
Figure BDA0003868774930000108
Figure BDA0003868774930000109
Figure BDA00038687749300001010
步骤8：判断变分贝叶斯迭代是否收敛，如果收敛则转到步骤9，否则返回步骤6，迭代次数加1；
收敛条件1：计算目标状态的变化量，
Figure BDA00038687749300001011
其中‖·‖2表示向量的2范数，如果thred小于阈值δ，则表示变分迭代收敛。
收敛条件2：如果变分迭代次数达到设定的最大迭代次数N，同样认为变分迭代收敛。收敛条件1或2满足其一就认为变分迭代收敛，然后转到步骤9，否则返回步骤6，且迭代次数加1。在本实施例中，δ＝5×10-6，N＝100。
步骤9：输出增广状态
Figure BDA00038687749300001012
误差协方差矩阵
Figure BDA00038687749300001013
变分参数
Figure BDA00038687749300001014
并判断是否结束协同目标跟踪，如果结束则算法终止，否则返回步骤1。
依据上述步骤，进行了计算机模拟实验。生成了一组噪声协方差随时间变化的观测数据。所有算法均用Python语言实现。位置的跟踪误差和速度的跟踪误差如表1和表2所示，位置和速度的跟踪误差的均方根误差RMSE如图2所示。
表1
Figure BDA00038687749300001015
表2
Figure BDA0003868774930000111
可以看出，本发明的目标跟踪误差在所有情况下都小于单车自适应跟踪的跟踪误差。与现有技术方案相比，本发明与现有单车车辆目标自适应跟踪技术相比，采用协同跟踪及自适应估计协方差矩阵的方法，不仅提高了跟踪的精度而且进一步缩小了驾驶盲区，扩大了主车的感知范围。因此，本技术方案对于观测噪声时变情况下实时精确估计目标的状态的理论和实际工程应用具有重要的意义。
本发明电子设备包括中央处理单元(CPU)，其可以根据存储在只读存储器(ROM)中的计算机程序指令或者从存储单元加载到随机访问存储器(RAM)中的计算机程序指令，来执行各种适当的动作和处理。在RAM中，还可以存储设备操作所需的各种程序和数据。CPU、ROM以及RAM通过总线彼此相连。输入/输出(I/O)接口也连接至总线。
设备中的多个部件连接至I/O接口，包括：输入单元，例如键盘、鼠标等；输出单元，例如各种类型的显示器、扬声器等；存储单元，例如磁盘、光盘等；以及通信单元，例如网卡、调制解调器、无线通信收发机等。通信单元允许设备通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
处理单元执行上文所描述的各个方法和处理，例如方法S1～S9。例如，在一些实施例中，方法S1～S9可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元。在一些实施例中，计算机程序的部分或者全部可以经由ROM和/或通信单元而被载入和/或安装到设备上。当计算机程序加载到RAM并由CPU执行时，可以执行上文描述的方法S1～S9的一个或多个步骤。备选地，在其他实施例中，CPU可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行方法S1～S9。
本文中以上描述的功能可以至少部分地由一个或多个硬件逻辑部件来执行。例如，非限制性地，可以使用的示范类型的硬件逻辑部件包括：场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)等等。
用于实施本发明的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本发明的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，该方法包括以下步骤：
步骤1、主车和协同车分别对同一目标车进行感知，协同车将感知到的观测数据和自车状态数据发送给主车；
步骤2、主车对接收到的观测数据进行同步处理和坐标变换处理，然后将协同车和目标车的状态及观测组合；
步骤3、在k＝0时刻，初始化增广状态分布为
Figure FDA0003868774920000011
其中，
Figure FDA0003868774920000012
表示服从均值为
Figure FDA0003868774920000013
和协方差矩阵为P0的高斯分布，并返回步骤1；
步骤4、采用扩展卡尔曼滤波预测算法预测k时刻增广状态分布
Figure FDA0003868774920000014
Figure FDA0003868774920000015
其中，
Figure FDA0003868774920000016
和Pk|k-1分别表示预测的增广状态及相应的误差协方差矩阵；
步骤5、预测k时刻观测噪声协方差矩阵Σk的分布参数vk|k-1、Vk|k-1，初始化Σk的分布参数
Figure FDA0003868774920000017
及数学期望E[Σk]0，并初始化变分迭代次数i＝0；
步骤6、根据变分贝叶斯框架，输入k时刻的观测yk，利用扩展卡尔曼滤波EKF的修正算法计算第i次迭代增广状态xk|k的变分后验分布
Figure FDA0003868774920000018
Figure FDA0003868774920000019
步骤7、计算第i次迭代增广观测噪声协方差矩阵Σk的变分后验分布
Figure FDA00038687749200000110
及数学期望E[∑k]i，其中，
Figure FDA00038687749200000111
表示自由度为
Figure FDA00038687749200000112
尺度参数为
Figure FDA00038687749200000113
的逆威沙特分布；
步骤8、判断变分贝叶斯迭代是否收敛，如果收敛则转到步骤9，否则返回步骤6，迭代次数i增加1；
步骤9、输出增广状态
Figure FDA00038687749200000114
误差协方差矩阵
Figure FDA00038687749200000115
变分参数
Figure FDA00038687749200000116
并判断是否结束协同目标跟踪，如果结束则算法终止，否则返回步骤1。
2.根据权利要求1所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤1具体为：定义t1时刻主车对目标车的观测为
Figure FDA00038687749200000117
t2时刻协同车对目标车的观测为
Figure FDA0003868774920000021
协同车发送给主车的数据包括：协同车的感知时刻t2、协同车对目标车的观测
Figure FDA0003868774920000022
协同车通过车载定位设备获得的经纬度和航向角。
3.根据权利要求2所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤S2具体为：
增广观测yk：主车将接收到的观测数据进行同步处理和坐标变换，经过数据预处理后，得到主车对协同车和目标的观测
Figure FDA0003868774920000023
协同车对目标的观测
Figure FDA0003868774920000024
以及协同车相对于主车的航向角θk，组合后得到增广观测
Figure FDA0003868774920000025
增广状态xk：协同车的运动状态为
Figure FDA0003868774920000026
其中，
Figure FDA0003868774920000027
为协同车在主车坐标系x,y方向的位置与速度，(θk,ωk)为协同车在主车坐标系下的航向角和角速度；目标车的运动状态为
Figure FDA0003868774920000028
表示目标车在主车坐标系下x,y方向的位置与速度；组合后得到增广状态
Figure FDA0003868774920000029
则观测方程为：
Figure FDA00038687749200000210
式中，θ为协同车相对于主车坐标系的航向角。
4.根据权利要求2所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤4中的增广状态
Figure FDA00038687749200000211
及协方差矩阵Pk|k-1，具体数学表达式分别为：
Figure FDA00038687749200000212
Figure FDA00038687749200000213
式中，
Figure FDA00038687749200000214
分别为k-1时刻更新后的增广状态和k时刻预测的增广状态，Fk∈Rn×n是状态转移矩阵；Pk-1|k-1、Pk|k-1∈Rn×n分别为k-1时刻更新后的增广状态协方差矩阵和k时刻预测的状态协方差矩阵，Qk∈Rn×n表示过程噪声协方差矩阵，n表示增广状态维度。
5.根据权利要求4所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤5包括：
预测k时刻观测噪声协方差矩阵Σk的分布参数vk|k-1、Vk|k-1，并初始化Σk的后验分布参数
Figure FDA0003868774920000031
及数学期望E[Σk]0，具体的数学表达式分别为：
vk|k-1＝ρ(vk-1|k-1-n-1)+n+1
Vk|k-1＝BVk-1|k-1BT
Figure FDA0003868774920000032
其中，ρ是遗忘因子，0＜ρ≤1；
Figure FDA0003868774920000033
0＜|B|≤1，E[·]表示随机变量的期望，初始化变分迭代次数为i＝0，n表示增广状态维度。
6.根据权利要求5所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤6包括以下子步骤：
步骤6.1、根据预测的增广状态
Figure FDA0003868774920000034
及观测方程h(·)计算预测的增广观测：
Figure FDA0003868774920000035
步骤6.2、根据预测的增广状态
Figure FDA0003868774920000036
计算观测方程的雅克比矩阵Hk：
Figure FDA0003868774920000037
式中，n表示增广状态维度；
步骤6.3、计算第i次迭代增广状态xk|k的变分后验分布
Figure FDA0003868774920000038
Figure FDA0003868774920000039
的参数
Figure FDA00038687749200000310
和
Figure FDA00038687749200000311
Figure FDA00038687749200000312
Figure FDA00038687749200000313
其中，
Figure FDA00038687749200000314
Figure FDA00038687749200000315
式中，E[∑k]i为第i次迭代的k时刻观测噪声协方差矩阵；Pk|k-1为k时刻预测的状态协方差矩阵，yk为k时刻的增广观测。
7.根据权利要求6所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤7具体为：
计算第i次迭代增广观测噪声协方差矩阵Σk的变分后验分布
Figure FDA00038687749200000316
Figure FDA00038687749200000317
的
Figure FDA00038687749200000318
及∑k的数学期望：
Figure FDA00038687749200000319
Figure FDA0003868774920000041
Figure FDA0003868774920000042
式中，h(·)为观测方程。
8.根据权利要求1所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤8中判断变分贝叶斯迭代是否收敛，具体为：满足以下任一收敛条件，即认为变分贝叶斯迭代收敛；
收敛条件1：计算目标状态的变化量：
Figure FDA0003868774920000043
其中，
Figure FDA0003868774920000044
分别为第i次、i-1次的目标状态预测值；‖·‖2表示向量的L2范数；如果thred小于设定阈值δ，则变分迭代收敛；
收敛条件2：变分迭代次数达到设定的最大迭代次数N。
9.一种电子设备，包括存储器和处理器，所述存储器上存储有计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1～8任一项所述的方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现如权利要求1～8中任一项所述的方法。
CN202211189501.XA 2022-09-28 2022-09-28 一种基于变分贝叶斯的自适应协同目标跟踪方法 Pending CN115457510A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211189501.XA CN115457510A (zh) 2022-09-28 2022-09-28 一种基于变分贝叶斯的自适应协同目标跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211189501.XA CN115457510A (zh) 2022-09-28 2022-09-28 一种基于变分贝叶斯的自适应协同目标跟踪方法

Publications (1)

Publication Number Publication Date
CN115457510A true CN115457510A (zh) 2022-12-09

Family

ID=84307204

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211189501.XA Pending CN115457510A (zh) 2022-09-28 2022-09-28 一种基于变分贝叶斯的自适应协同目标跟踪方法

Country Status (1)

Country Link
CN (1) CN115457510A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116500575A (zh) * 2023-05-11 2023-07-28 兰州理工大学 一种基于变分贝叶斯理论的扩展目标跟踪方法和装置
CN117095314A (zh) * 2023-08-22 2023-11-21 中国电子科技集团公司第五十四研究所 跨域多维空地环境下的目标检测与重识别方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116500575A (zh) * 2023-05-11 2023-07-28 兰州理工大学 一种基于变分贝叶斯理论的扩展目标跟踪方法和装置
CN116500575B (zh) * 2023-05-11 2023-12-22 兰州理工大学 一种基于变分贝叶斯理论的扩展目标跟踪方法和装置
CN117095314A (zh) * 2023-08-22 2023-11-21 中国电子科技集团公司第五十四研究所 跨域多维空地环境下的目标检测与重识别方法
CN117095314B (zh) * 2023-08-22 2024-03-26 中国电子科技集团公司第五十四研究所 跨域多维空地环境下的目标检测与重识别方法

Similar Documents

Publication Publication Date Title
CN115457510A (zh) 一种基于变分贝叶斯的自适应协同目标跟踪方法
EP3875907B1 (en) Method, apparatus, computing device and computer-readable storage medium for positioning
CN111178385B (zh) 一种鲁棒在线多传感器融合的目标跟踪方法
CN108332758B (zh) 一种移动机器人的走廊识别方法及装置
US11860302B2 (en) Apparatus and method for characterizing an object based on measurement samples from one or more location sensors
CN108535720B (zh) 用于改进的卡尔曼滤波目标跟踪的自适应过程噪声描述
CN105447574B (zh) 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置
CN110889862B (zh) 一种网络传输攻击环境中多目标跟踪的组合测量方法
CN108871365B (zh) 一种航向约束下的状态估计方法及系统
Attari et al. Target tracking formulation of the SVSF with data association techniques
CN109447161B (zh) 一种基于汽车雷达数据的实时增量与自适应聚类方法
CN117170354B (zh) 一种轮式机器人定位方法及系统
CN113932799A (zh) 激光地图更新方法、系统、电子设备、介质及程序产品
CN110276801B (zh) 一种物体定位方法、装置及存储介质
CN108680162B (zh) 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法
CN111968102B (zh) 一种目标设备检测方法、系统、介质及电子终端
CN116729412A (zh) 车辆定位中运动学模型自适应方法、装置、设备及介质
CN115979288A (zh) 一种航向角确定方法、电子设备和存储介质
CN114852083A (zh) 一种基于交互式多模型的自动驾驶跟踪方法
EP3654065B1 (en) Apparatus and method for characterizing an object based on measurement samples from one or more location sensors
Gulati et al. Robust cooperative localization in a dynamic environment using factor graphs and probability data association filter
CN111007553B (zh) 被测对象的导航方法、装置、计算机设备和存储介质
WO2024005063A1 (ja) 物体追跡装置
CN118091695A (zh) 基于激光雷达的车厢边界预估方法、系统、装置及介质
Steuernagel et al. Random Matrix-based Tracking of Rectangular Extended Objects with Contour Measurements

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