多雷达融合算法设计

0.前言

    现有的多传感器融合算法很少涉及多雷达融合算法，这里整理了多雷达融合算法的一些方法，分享出来共同探讨。

    博客主要描述五个雷达融合算法的详细设计过程，包括数据接收、缓存、坐标转换、关联、滤波等过程。

1.数据接收和时间同步

    融合算法接收前雷达和四个角雷达的数据，并开辟缓存存储这些数据。

    如果存储数据超过队列长度，则删除队列头数据，新的数据放到队列尾部。

    每个周期从缓冲读取最新数据，如果没有最新数据，则不更新融合目标，只做外推。

    使用最新的数据作为量测，并作后续的关联滤波处理，融合的时间也和最新数据的时间保持同步。

    如果发现时戳异常，则做相应判断和处理。

2.量测筛选

    雷达的输入作为量测，但有些输入不符合融合指标要求，可按照动静、距离、方位等状态筛选感兴趣的输入作为量测。

    对于后角雷达，由于后方只有它能覆盖，因此一般不做距离筛选，只做存在概率的筛选。

    前雷达和前角雷达有较大重叠区域，而且通常前角雷达精度较低，特别在远处，横向偏差较大，因此可以只使用前角雷达中近程的目标。

3.坐标转换

    所有的量测应当转换到统一坐标系下，需要确定每个雷达的安装位置和坐标系，并确定统一自车的坐标系，将雷达坐标转换到自车坐标系。

4.融合处理

4.1 起始策略

    目标生成时按照存在概率、量测状态等进行筛选，同时记录传感器ID和量测ID。

4.2 多级关联策略

    第1次关联：目标优先关联上一帧匹配的传感器ID和量测ID。

    第2次关联：如果没有关联上匹配的目标，则在关联门限内搜索目标。

    如果关联1个量测，则用这个量测更新。

    如果关联多个量测，则计算量测的得分，距离速度越接近，得分越高。

    第3次关联：放宽速度门限，在关联门限内搜索目标。

    如果3次都关联不上，则用预测值更新融合目标，累计量测丢失时间；如果丢失时间过长，则删除目标。

5.跟踪滤波

    根据目标特性设计运动模型和滤波算法，一般使用匀加速直线运动模型和卡尔曼滤波算法。

    根据模型确定状态转移矩阵A，确定量测矩阵H

    根据模型和算法确定初始的噪声协方差矩阵Q、R和P。

    根据关联的目标和时间差更新噪声协方差矩阵Qk，Rk和Pk。

    再根据卡尔曼滤波方程计算增益矩阵Kk，得到状态更新Xk。

6.流程框图

    下面是使用前雷达ars、前角雷达fc、后角雷达rc做的多雷达融合算法流程。