基于协方差交叉融合算法的V2X协同感知（多目标跟踪）

论文标题：A Novel Probabilistic V2X Data Fusion Framework for Cooperative Perception

问题：由于视觉遮挡或感知传感器故障，CAV通常无法使用本地传感器探测到其他道路使用者，需要在其路径规划和决策中考虑V2X协同感知策略。
1） 现有的基于V2X协同感知的数据融合方法大多假设从其他ITS-S接收到的感知信息是不相关的，而实际中信息通常在任意CV对之间交换，它们是互相关的。
2） 在V2X通信中共享目标轨迹可促进不同类型和不同制造商的ITS-S之间的互操作性，但是结合来自多个ITS-S的轨迹，需要处理来自单一平台和不同平台的共同先验信息。因为，ITS-S之间共享的目标的轨迹取决于先前的估计。
CV如何有效地、持续地将其本地感知信息与从其他ITS-Ss收到的数据相融合？

智能交通系统站（ITS-S）：网联车辆（CV）、智能路边装置（IRSU）

1 方法

提出了一个数据融合方法，其考虑了互相关性，以更好地融合CV本地感知数据和通过V2X接收到的感知数据。

1.1 协同多目标跟踪系统

CV通过V2X通信从其他ITS-Ss接收远程感知信息CPM，融合中心将接收到的信息与CV本地感知数据进行数据融合处理，然后更新的目标轨迹并将其编码为CPMs，再通过V2X通信传输给其他ITS-S。

1. 附近ITS-Ss通过V2X无线通信向Ego车发送CPM；
2. Ego车接收CPM并对齐进行解码，再将其转换到Ego车的坐标系以实现空间对齐；
3. 解码后的CPM将在融合中心上进行聚合；
   1）传感器原始数据${\mathrm{Z}}_{\mathrm{r}}$：采用传统的贝叶斯滤波方法（如卡尔曼滤波）来进行融合；
   2）目标轨迹信息$T_r$：采用协方差交叉融合（CI）算法来将接收到的CPM中包含的目标轨迹$T_r$与Ego车生成的目标轨迹$T_l$进行融合。
4. 融合中心采用高斯混合概率假设密度（GMPHD）滤波器的一个变体来跟踪多个道路使用者；
5. 更新的目标轨迹$T_{l+}$并将其编码为CPM，然后通过V2X无线通信传送给其他ITS-S。

1.2 数据融合方案

1.2.1 Tracklet融合管理

在V2X环境中，Ego车可以同时与其他多个合作的ITS-S进行通信，这里简单描述只涵盖一个Ego车和一个ITS-S进行协作的情况，同样的融合可以迭代地应用于其他每个合作的ITS-S。（假设在第t个时间步，Ego车从另一个ITS-S接收到的CPM包含N个远程目标轨迹，同时，Ego车感知产生了M个本地目标轨迹。）

• 远程目标轨迹信息（发送车辆发送的CPM）的集合表示为 T r = { T r j } j = 1 N T_r = \{T^{j}_r\}^{N}_{j=1} Tr​={Trj​}j=1N​，其中每个$T_r$是一个元组：
  
  其中，¯xr是轨迹平均矢量，Σr是协方差矩阵，sr是发送ITS-S的唯一ID，dr代表轨迹ID。
• Ego车本地轨迹信息的集合用 T l = { T l i } i = 1 M T_l = \{T^{i}_l\}^{M}_{i=1} Tl​={Tli​}i=1M​表示，每个$T_l$：
  
  其中，¯xl是轨迹平均矢量，Σl是协方差矩阵，w是轨迹的相关权重，dl代表轨迹ID，并引入别名ID列表 $\mathrm{R}=[{\mathrm{s}}_{\mathrm{r}}^{\mathrm{j}}{\mathrm{d}}_{\mathrm{r}}^{\mathrm{j}}{]}_{\mathrm{j}=1}^{\mathrm{P}}$（列表中的每一对指的是本地轨迹$T_l$被发送ITS-S称为的别名ID，而相同的轨迹可以存在于其他的ITS-S中；最初，列表是空的）。

一个轨迹包含状态估计和它在发送ITS-S内分配的轨迹ID，不同的ITS-S有自己的发送和管理轨迹ID的方式。因此，现实世界中的同一个目标物体在不同的ITS-S中可能具有不同的轨迹ID。

1.2.2 轨迹匹配融合算法

采用了协方差交叉融合（CI）算法对接收到的目标轨迹和本地感知轨迹进行融合。因为，
1）远程目标轨迹的状态向量通常是目标跟踪器中保留的本地轨迹的一个子集，因此，远程和本地轨迹的状态向量维度往往是不一样的。协方差交叉融合（CI）算法可以适应状态向量维度不匹配的情况。
2）各个目标轨迹之间的相关性是未知的，而CI算法融合两个高斯估计值时，不需要计算它们的互协方差。

协方差交叉融合（CI）：在多传感器融合时往往需要知道各个传感器估计误差是不相关的或需要计算它们的互协方差。实际中互协方差很难获得，协方差交叉融合（CI融合）方法不需要计算互协方差，可以有效地解决互协方差未知的系统的融合问题。

为了确定接收到的远程轨迹是否与本地轨迹相匹配，引入了一个轨迹匹配融合算法。最初，每个本地轨迹的别名ID列表是都空的，对于每个远程轨迹$T_r$：

• 搜索每个本地轨迹Tl的别名ID列表 $R\in T_l$以进行匹配检查，
  1） 如果发送ITS-S的ID ($s_r\in T_r$)在$R\in T_l$中找不到（不匹配），就对$T_l$和$T_r$进行概率性的轨迹关联；一旦$T_r$与$T_l$融合了，$[s_r{d}_r]$被添加到$R$中，以供后续时间步的匹配检查。
  2） 如果发送ITS-S的ID ($s_r\in T_r$)和该远程轨迹的ID ($d_r\in T_r$) 存在$R$中（匹配），说明$T_l$之前已经与来自某个其他ITS-S的信息融合过了，那么直接采用协方差交叉融合方法将$T_r$与$T_l$进行融合。
  
• 更新没有被发送ITS-S检测到的本地轨迹，并给定其被检测到的概率$p_{D,r}(.)$。

基于CI的轨迹融合

给定一对准备进行融合的$T_l$和$T_r$，基于CI的轨迹融合分为两个步骤：
1）通过解决优化问题，使得融合后的轨迹的状态估计协方差的迹或行列式最小化，获得CI算法的最优参数ω∈(0，1)

2）更新轨迹状态估计和协方差，轨迹的相关权重：

最终，Ego车的本地轨迹集合为未匹配到的和更新的所有本地轨迹子集的并集：

• T mt l+ : 匹配到的并融合的本地轨迹集合
• T um l+ : 通过CI融合更新后的未匹配的本地轨迹集合
• T ud l+: 未被发送ITS-S检测到的未匹配和更新的本地轨迹的集合

可以选择使用一些后处理策略来有效控制T l+中轨迹的增长：
1）可以通过删除权重低于阈值的轨迹来引入裁剪过程；
2）可以根据一些距离度量，例如马氏距离，来合并接近的轨迹；
3）可以设置每次迭代中更新的本地轨迹的最大数量。

2 实验

通过数值模拟、CARLA模拟和包含V2X智能平台的真实场景实验来验证所提出的方法，其中，现实世界的实验包括一辆自行车、一辆联网自动驾驶汽车（CAV）和一个加装了视觉和LiDAR的智能路边装置（IRSU）。

3 总结

提出了一个基于CI的概率轨迹融合方法应用于V2X CP，可在不需要知道任何互相关未知的情况下，对本地感知信息和从其他ITS-S收到的感知目标信息进行融合；支持Ego CV中不同类型的感知信息的融合，包括其本地感知数据、传感器原始数据和通过V2X通信从其他合作ITS-S收到的目标轨迹。