视觉毫米波简单后融合方法

视觉毫米波简单后融合方法

相机、激光雷达和毫米波雷达是自动驾驶领域较为重要的三个传感器，三者互为备份，优势互补。不依赖lidar的低成本方案一直是各家的研究重点。这里我们简单介绍一种使用相机和毫米波雷达的结果做后融合的方法。本方法的特点是充分考虑了各传感器的误差特性，实现了一定程度上的优势互补，简洁明了。

附赠自动驾驶学习资料和量产经验：链接

1.单帧融合

1.1相机测量误差模型

针孔相机的成像原理可以用小孔成像模型来描述，如下图所示。图像中的一个像素点对应的是3D空间中的一条射线，也就是一个角度，所有射线的交点就是相机中心点C。由于像素是稠密的，相机有很高的角度分辨率。以常见的30度长焦相机举例，当图像分辨率为1080P时，其角度分辨精度为0.015度。某种意义上来说，相机可以被视为高精度的角度传感器。

图1 相机模型

图像只有2D信息，在3D空间上缺失深度信息。一般单目目标感知会根据2D信息找到目标中心点的像素坐标，然后计算或拟合出一个深度值补齐缺失的深度信息。相机深度估计存在着一定的误差，且为比例误差。综上所述，相机的角度测量误差较小，深度测量误差较大。如图2所示，我们以相机中心点为坐标原点，建立一个X轴、Y轴分别与车体坐标系X轴、Y轴平行的BEV视角下的极坐标系（θ , r），该坐标系下θ和r是独立变量，其协方差矩阵可以写成下式：

图2 极坐标系下相机测量的分布

根据图2中所示的极坐标与直角坐标转换式的一阶泰勒展开式，结合视觉深度误差为百分比误差这一特性，我们可以得到视觉测量在直角坐标系下的协方差矩阵。如下图所示，其中我们使用的单目测距网络（Mono 3d）测距百分比误差一般不超过0.1（10%）；中心点误差在十个像素以内，按十个像素记，1080P分辨率下30度相机视觉目标角度的误差一般在0.0027左右，60度相机角度误差一般在0.0054左右。

图3 笛卡尔坐标系下mono3d测量的协方差

1.2毫米波测量误差模型

Radar传感器利用毫米波的频差来测距、测速。其径向测量精度较高，角分辨率较低，与视觉传感器天然互补。由于角分辨率的原因，Radar目标的中心点往往有一定的横向偏差，且没有Z方向的信息，因此在BEV坐标下其观测协方差矩阵可以写成如下形式：

图4 毫米波误差分布

1.3位置融合

综合考虑相机和Radar的观测误差分布规律，可以使用极大似然估计的方法来获取融合后的位置。在这里我们举一个实例来说明算法流程。如图5 所示，黄色方块为自车位置，红色框为mono3d检测结果， 天蓝色框为毫米波雷达检测结果，绿色框为vision-radar融合结果，白色框为根据激光雷达标注的真值。图中一个方格代表10m * 10m的区域。

图5 位置融合结果

• 对一个待匹配视觉目标（图5白色圆圈内的红色框），首先剔除大量欧氏距离差距较大的无效Radar目标，得到候选目标（图5白色圆圈内的天蓝色框）

• 根据相机观测误差分布，获得视觉目标的概率分布，如图6所示

图6 视觉目标的位置分布

• 根据Radar观测误差分布，获得Radar目标的概率分布，如图7所示

图7 毫米波目标的位置分布

• 假设关联后的目标位置符合二维高斯分布，对每一个候选毫米波目标，假设其与待匹配视觉目标可匹配，利用极大似然估计求取匹配后目标分布的均值和协方差矩阵。匹配后目标的概率分布函数的极值可以作为视觉与毫米波之间的距离度量，并记录下来

• 在计算完所有视觉-毫米波目标对之间的距离度量后，利用图像处理中常用的crosscheck准则进行匹配

• 匹配成功后，利用极大似然估计得到的目标位置分布函数的极值点位置即为新的关联后目标的位置，如图8所示

图8 融合后概率分布

2.时序融合

待续