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

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

【技术实现步骤摘要】
一种基于变分贝叶斯的自适应协同目标跟踪方法


[0001]本专利技术涉及智能网联汽车目标跟踪
，尤其是涉及一种基于变分贝叶斯的自适应协同目标跟踪方法。

技术介绍

[0002]目标跟踪系统是智能网联汽车环境感知系统的重要组成部分，为了实现车辆自适应巡航、碰撞预警及其他车辆自动驾驶控制功能，需要准确地检测并估计智能网联车辆周围环境中各类目标的准确位置及运动状态信息。
[0003]随着车联网技术的成熟与普及，越来越多的车辆通过车间通信技术能够在车辆之间完成信息的共享。基于车车通信的协同目标跟踪不仅有较高的跟踪精度而且扩大了自车的感知范围，缩小了驾驶盲区。此外，由于基于车车通信的协同跟踪能充分利用协同车辆发布的信息，因此可以减少单车传感器种类或数量，降低智能汽车的制造成本。
[0004]但是传统的协同跟踪算法在实际跟踪场景中会有一定的局限性，原因在于这些算法本身的建立依赖于观测噪声的统计特性已知，而在实际交通场景中，由于主车、协同和目标车的运动工况十分复杂，观测噪声的统计特性往往难以确定，此外在实际交通场景中，主车、协同车和目标车的运动工况本身存在摄动，同时容易受到外界交通环境的干扰，跟踪系统的观测噪声也可能随之发生变化，这种情形下，传统的协同跟踪算法的工作性能就会发生退化，极端情形下甚至无法正常工作。
[0005]经过检索，中国专利技术专利申请CN113971752A公开了一种抗观测数据干扰的多车协同状态估计方法，但是该方法假设目标车的位置噪声服从t分布，控制参数过多，协同跟踪起始阶段状态估计收敛更慢，在复杂噪声环境下跟踪结果甚至会发散，此外，估计的参数增加，因此消耗的计算资源更大，执行时间更长，不利于车载控制器上实现实时目标跟踪。
[0006]为了解决时变观测噪声下普通的协同跟踪算法无法自适应地估计观测噪声的大小，从而导致跟踪精度降低的问题以及实时性跟踪需求，亟需设计一种自适应协同目标跟踪方法。

技术实现思路

[0007]本专利技术的目的就是为了克服上述现有技术存在的缺陷而提供了一种基于变分贝叶斯的自适应协同目标跟踪方法，采用贝叶斯框架，对协同车与目标的状态及观测噪声协方差矩阵的后验分布进行联合估计，通过变分贝叶斯推断迭代更新协同车与目标的状态及观测噪声协方差矩阵的参数，可以在不增加车载传感器硬件成本的基础上，提高目标跟踪的精度，采用目标车的位置噪声服从高斯分布的设置，跟踪效果鲁棒性更好，降低了计算资源，提高了实时性。
[0008]本专利技术的目的可以通过以下技术方案来实现：
[0009]根据本专利技术的第一方面，提供了一种基于变分贝叶斯的自适应协同目标跟踪方法，该方法包括以下步骤：
[0010]步骤1、主车和协同车分别对同一目标车进行感知，协同车将感知到的观测数据和自车状态数据发送给主车；
[0011]步骤2、主车对接收到的观测数据进行同步处理和坐标变换处理，然后将协同车和目标车的状态及观测组合；
[0012]步骤3、在k＝0时刻，初始化增广状态分布为其中，表示服从均值为和协方差矩阵为P0的高斯分布，并返回步骤1；
[0013]步骤4、采用扩展卡尔曼滤波预测算法预测k时刻增广状态分布步骤4、采用扩展卡尔曼滤波预测算法预测k时刻增广状态分布其中，和P
k|k
‑1分别表示预测的增广状态及相应的误差协方差矩阵；
[0014]步骤5、预测k时刻观测噪声协方差矩阵Σ
k
的分布参数v
k|k
‑1、V
k|k
‑1，初始化Σ
k
的分布参数及数学期望E[Σ
k
]0，并初始化变分迭代次数i＝0；
[0015]步骤6、根据变分贝叶斯框架，输入k时刻的观测y
k
，利用扩展卡尔曼滤波EKF的修正算法计算第i次迭代增广状态x
k|k
的变分后验分布的变分后验分布
[0016]步骤7、计算第i次迭代增广观测噪声协方差矩阵Σ
k
的变分后验分布及数学期望E[∑
k
]i
，其中，表示自由度为尺度参数为的逆威沙特分布；
[0017]步骤8、判断变分贝叶斯迭代是否收敛，如果收敛则转到步骤9，否则返回步骤6，迭代次数i增加1；
[0018]步骤9、输出增广状态误差协方差矩阵变分参数并判断是否结束协同目标跟踪，如果结束则算法终止，否则返回步骤1。
[0019]优选地，所述步骤1具体为：定义t1时刻主车对目标车的观测为t2时刻协同车对目标车的观测为
[0020]协同车发送给主车的数据包括：协同车的感知时刻t2、协同车对目标车的观测协同车通过车载定位设备获得的经纬度和航向角。
[0021]优选地，所述步骤S2具体为：
[0022]增广观测y
k
：主车将接收到的观测数据进行同步处理和坐标变换，经过数据预处理后，得到主车对协同车和目标的观测协同车对目标的观测以及协同车相对于主车的航向角θ
k
，组合后得到增广观测
[0023]增广状态x
k
：协同车的运动状态为其中，为协同车在主车坐标系x,y方向的位置与速度，(θ
k
,ω
k
)为协同车在
主车坐标系下的航向角和角速度；目标车的运动状态为表示目标车在主车坐标系下x,y方向的位置与速度；组合后得到增广状态
[0024]则观测方程为：
[0025][0026]式中，θ为协同车相对于主车坐标系的航向角。
[0027]优选地，所述步骤4中的增广状态及协方差矩阵P
k|k
‑1，具体数学表达式分别为：
[0028][0029][0030]式中，分别为k
‑
1时刻更新后的增广状态和k时刻预测的增广状态，F
k
∈R
n
×
n
是状态转移矩阵；P
k
‑
1|k
‑1、P
k|k
‑1∈R
n
×
n
分别为k
‑
1时刻更新后的增广状态协方差矩阵和k时刻预测的状态协方差矩阵，Q
k
∈R
n
×
n
表示过程噪声协方差矩阵，n表示增广状态维度。
[0031]优选地，所述步骤5包括：
[0032]预测k时刻观测噪声协方差矩阵Σ
k
的分布参数v
k|k
‑1、V
k|k
‑1，并初始化Σ
k
的后验分布参数及数学期望E[Σ
k
]0，具体的数学表达式分别为：
[0033]v
k|k
‑1＝ρ(v
k
‑
1|k
‑1‑
n
‑
1)+n+1
[0034]V
k|k
‑1＝BV
...

【技术保护点】

【技术特征摘要】
1.一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，该方法包括以下步骤：步骤1、主车和协同车分别对同一目标车进行感知，协同车将感知到的观测数据和自车状态数据发送给主车；步骤2、主车对接收到的观测数据进行同步处理和坐标变换处理，然后将协同车和目标车的状态及观测组合；步骤3、在k＝0时刻，初始化增广状态分布为其中，表示服从均值为和协方差矩阵为P0的高斯分布，并返回步骤1；步骤4、采用扩展卡尔曼滤波预测算法预测k时刻增广状态分布步骤4、采用扩展卡尔曼滤波预测算法预测k时刻增广状态分布其中，和P
k|k
‑1分别表示预测的增广状态及相应的误差协方差矩阵；步骤5、预测k时刻观测噪声协方差矩阵Σ
k
的分布参数v
k|k
‑1、V
k|k
‑1，初始化Σ
k
的分布参数及数学期望E[Σ
k
]0，并初始化变分迭代次数i＝0；步骤6、根据变分贝叶斯框架，输入k时刻的观测y
k
，利用扩展卡尔曼滤波EKF的修正算法计算第i次迭代增广状态x
k|k
的变分后验分布的变分后验分布步骤7、计算第i次迭代增广观测噪声协方差矩阵Σ
k
的变分后验分布及数学期望E[∑
k
]
i
，其中，表示自由度为尺度参数为的逆威沙特分布；步骤8、判断变分贝叶斯迭代是否收敛，如果收敛则转到步骤9，否则返回步骤6，迭代次数i增加1；步骤9、输出增广状态误差协方差矩阵变分参数并判断是否结束协同目标跟踪，如果结束则算法终止，否则返回步骤1。2.根据权利要求1所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤1具体为：定义t1时刻主车对目标车的观测为t2时刻协同车对目标车的观测为协同车发送给主车的数据包括：协同车的感知时刻t2、协同车对目标车的观测协同车通过车载定位设备获得的经纬度和航向角。3.根据权利要求2所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤S2具体为：增广观测y
k
：主车将接收到的观测数据进行同步处理和坐标变换，经过数据预处理后，得到主车对协同车和目标的观测协同车对目标的观测以及协同车相对于主车的航向角θ
k
，组合后得到增广观测
增广状态x
k
：协同车的运动状态为其中，为协同车在主车坐标系x,y方向的位置与速度，(θ
k
,ω
k
)为协同车在主车坐标系下的航向角和角速度；目标车的运动状态为表示目标车在主车坐标系下x,y方向的位置与速度；组合后得到增广状态则观测方程为：式中，θ为协同车相对于主车坐标系的航向角。4.根据权利要求2所述的一种基于变分贝叶斯的自适应协同目标跟踪方法，其特征在于，所述步骤4中的增广状态及协方差矩阵P
k|k
‑1，具体数学表达式分别为：，具体数学表达式分别为：式中，分别为k
‑
1时刻更新后的增广状态和k时刻预测的增广状态，F
k
∈R
n
×
n
是状态转移矩阵；P
k
‑
1|k
‑1、...

【专利技术属性】
技术研发人员：冀建宇，顾海雷，何豆，王潇屹，曹寅，江玥，高瑶，侯俊杰，
申请(专利权)人：上海智能新能源汽车科创功能平台有限公司，
类型：发明
国别省市：