一种低分辨率毫米波雷达与光学相机的标定方法与流程

1.本发明涉及多传感器融合的外参标定技术领域，具体涉及一种低分辨率毫米波雷达与光学相机的标定方法。


背景技术：

2.三维环境感知一直是无人车定位、路径规划导航、三维重建等任务的关键，在三维环境感知的任务中激光雷达、毫米波雷达以及光学相机等传感器都扮演着重要的角色。相比于雷达，光学相机因其可以拿到稠密的场景rgb信息而受到广泛关注，但是无论是单目还是双目相机都无法拿到场景准确的三维信息。因此在实际应用中常与其他测距传感器例如激光雷达进行融合，用于完成具有尺度的三维重建、实时定位与建图等工作。近年来，毫米波雷达因其可以用于大雾、大雨等殊环境而受到国内外学者的高度关注，产生了许多毫米波雷达的相关研究成果。但是目前市面上的毫米波雷达相比与激光雷达依然有着很多缺点，例如：单帧的点云数据稀疏、精度低、易受干扰、杂点多等。这也使得毫米波雷达与其他传感器的空间标定任务更加困难。而多传感器的空间标定通常是完成多源信息融合感知算法的关键一环。
3.毫米波雷达与相机的标定工作一般使用雷达所测得目标的速度、距离以及水平方位角信息与图像或图像流进行信息的匹配，例如基于强反射标定物并在标定方程中添加了几何约束实现360
°
的毫米波雷达与相机的配准的方法，使用雷达所测目标的速度维信息，并在图像序列中寻找对应的目标来对毫米波雷达与相机进行标定的方法等。但是上述标定工作主要用于大型室外环境的三维重建，上述标定工作采用的毫米波雷达分辨率较高，标定方案也不具有普适性，在实际工程中，限于价格，在大多数应用场景中都采用数据较为稀疏的毫米波雷达，使得在毫米波雷达与光学相机空间标定工作中无法使用上述的标定方法来完成毫米波雷达与相机的标定。


技术实现要素：

4.本发明的目的在于提供一种低分辨率毫米波雷达与光学相机的标定方法，标定结果的准确率高，且在较为空旷的环境以及杂物较多的环境具有有效性与鲁棒性，应用场景广泛。
5.为实现以上目的，本发明采用以下技术方案：
6.一种低分辨率毫米波雷达与光学相机的标定方法，包括步骤：
7.s1、搭建标定板以及标定采集设备，标定采集设备包括相机、毫米波雷达、位姿传感器；
8.s2、相机采集标定板原始图像信息，毫米波雷达采集标定板单帧雷达点云信息，位姿传感器采集移动平台的位姿信息；
9.s3、将单帧雷达点云信息与位姿信息融合，输出包含标定板位置信息的点云地图；
10.s4、利用点云滤波算法以及聚类算法对点云地图进行处理，以提取出标定板的毫
米波雷达点云块；
11.s5、使用目标分割算法分割出标定板在原始图像信息中的二维边缘特征点位置信息；
12.s6、通过标定板的结构先验构建其三维模型，并将其与毫米波雷达点云块进行配准，从而得到拟合后的标定板三维边缘特征点位置信息，并与步骤s5中的二维边缘特征点位置信息进行匹配，以得到多组二维与三维的匹配特征点对信息；
13.s7、将多组二维与三维的匹配特征点对信息，作为pnp算法的输入，以计算得到用于毫米波雷达与相机标定的外参矩阵。
14.作为优选方案，步骤s3中，包括步骤：
15.s3.1、划定雷达点云信息中的有效区域，并滤除其单帧的低精度点；
16.s3.2、基于预设的cfar阈值滤除毫米波雷达噪点；
17.s3.3、根据每一帧雷达点云信息对应的位姿信息，将雷达点云信息统一至全局坐标系下；
18.s3.4、使用概率模型，判断给定测量值时体素被占据的概率大小来确定地图点是否采用；
19.s3.5、根据步骤s3.4中被采用的地图点，完成毫米波雷达建图，输出包含标定板位置信息的点云地图。
20.作为优选方案，步骤s3.3中所述根据每一帧雷达点云信息对应的位姿信息，将雷达点云信息统一至全局坐标系下，采用的计算公式为：
[0021][0022]
其中，表示t时刻下毫米波雷达检测得到的在全局坐标系下的单帧雷达点云坐标，表示t时刻下毫米波雷达检测得到的在局部坐标系下的单帧雷达点云坐标，表示t时刻下位全局坐标系到相机坐标系的三维旋转矩阵，表示t时刻下全局坐标系到相机坐标系的三维平移向量。
[0023]
作为优选方案，步骤s3.4使用的概率模型为：
[0024][0025]
其中为t时刻之前给定测量值时节点n被占据的概率，p(n)为先验概率，为t-1时刻给定测量值时节点n被占据的概率，表示t时刻给定测量值时节点n被占据的概率，表示t时刻之前毫米波雷达检测得到的在全局坐标系下的所有帧的雷达点云坐标。
[0026]
作为优选方案，步骤s4中，包括步骤：
[0027]
s4.1、对点云地图进行下采样，并利用点云滤波算法滤除离群点；
[0028]
s4.2、使用欧式聚类算法对点云地图中的不同部分进行聚类；
[0029]
s4.3、根据每个点云类的边缘特征以及面特征，从中提取出标定板的毫米波雷达
点云块。
[0030]
作为优选方案，步骤s1具体为：
[0031]
使用两块成90
°
夹角的反射板作为标定板，在移动平台上固定安装毫米波雷达以及相机。
[0032]
作为优选方案，步骤s6中所述三维模型基于标定板的真实尺寸以及夹角进行构建。
[0033]
作为优选方案，步骤s6中，采用迭代最近点算法将标定板三维模型与毫米波雷达点云块进行匹配。
[0034]
作为优选方案，位姿传感器采用轮式里程计。
[0035]
作为优选方案，步骤s7中，包括步骤：
[0036]
s7.1、构建优化问题：
[0037][0038]
s7.2、将外参矩阵的求解转换为非线性最小二乘问题：
[0039][0040]
s7.3、使用lm优化方法求解标定矩阵；
[0041]
其中，zc为尺度因子，m为相机的内参矩阵，r为毫米波雷达坐标系坐标到相机坐标系坐标的三维旋转矩阵，t为毫米波雷达坐标系与相机坐标系的三维平移向量，p'
p
为二维边缘特征点坐标估计值，pr为标定板三维边缘特征点位置坐标，pr为二维边缘特征点坐标。
[0042]
本发明的有益效果是：
[0043]
1、本发明将毫米波雷达采集的单帧雷达点云信息与位姿传感器采集的位姿信息融合，输出包含标定板位置信息的点云地图；利用点云滤波算法以及聚类算法对点云地图进行处理，以提取出标定板的毫米波雷达点云；使用目标分割算法分割出标定板在原始图像信息中的二维边缘特征点位置信息；通过标定板的结构先验构建其三维模型，并将其与毫米波雷达点云块进行配准，从而得到拟合后的标定板三维边缘特征点位置信息，并与分割出的二维边缘特征点位置信息进行匹配，得到多组二维与三维的匹配特征点对信息；将多组二维与三维的匹配特征点对信息，作为pnp算法的输入，计算得到用于毫米波雷达与相机标定的外参矩阵。使用本发明所述算法得到的外参矩阵，在毫米波雷达和光学相机的标定任务中，得到的标定结果准确率高，且在较为空旷的环境以及杂物较多的环境中具有有效性与鲁棒性，应用场景广泛。
[0044]
2、本发明首次使用标定板完成低分辨率毫米波雷达与光学相机的标定任务，搭建的标定环境和标定采集设备简单且易于操作。
附图说明
[0045]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0046]
图1是本发明所述的一种低分辨率毫米波雷达与光学相机的标定方法的流程图。
[0047]
图2是毫米波雷达与光学相机在移动平台上固定安装的位置图。
[0048]
图3是在标定环境和场景一中分别使用人工提取标定板特征点和本发明所述算法提取标定板特征点得到的重投影效果的精度量化结果的数据对比图。
[0049]
图4是使用本发明所述的算法分别在场景一、场景二下两次实验得到的重投影效果的精度量化结果图。
具体实施方式
[0050]
以下通过特定的具体实施例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
[0051]
参照图1，本实施例提供了一种低分辨率毫米波雷达与光学相机的标定方法，包括步骤：
[0052]
s1、搭建标定板以及标定采集设备，标定采集设备包括相机、毫米波雷达、位姿传感器；
[0053]
s2、相机采集标定板原始图像信息，毫米波雷达采集标定板单帧雷达点云信息，位姿传感器采集移动平台的位姿信息；
[0054]
s3、将单帧雷达点云信息与位姿信息融合，输出包含标定板位置信息的点云地图；
[0055]
s4、利用点云滤波算法以及聚类算法对点云地图进行处理，以提取出标定板的毫米波雷达点云块；
[0056]
s5、使用目标分割算法分割出标定板在原始图像信息中的二维边缘特征点位置信息；
[0057]
s6、通过标定板的结构先验构建其三维模型，并将其与毫米波雷达点云块进行配准，从而得到拟合后的标定板三维边缘特征点位置信息，并与步骤s5中的二维边缘特征点位置信息进行匹配，以得到多组二维与三维的匹配特征点对信息；
[0058]
s7、将多组二维与三维的匹配特征点对信息，作为pnp算法的输入，以计算得到用于毫米波雷达与相机标定的外参矩阵。
[0059]
使用本发明所述算法得到的外参矩阵，在毫米波雷达与光学相机的标定任务中，得到的标定结果准确率高，且在较为空旷的环境以及杂物较多的环境中具有有效性与鲁棒性，应用场景广泛。
[0060]
具体的：
[0061]
步骤s3中，包括步骤：
[0062]
s3.1、划定雷达点云信息中的有效区域，并滤除其单帧的低精度点；
[0063]
s3.2、基于预设的cfar阈值滤除毫米波雷达噪点；
[0064]
s3.3、根据每一帧雷达点云信息对应的位姿信息，将雷达点云信息统一至全局坐标系下；
[0065]
s3.4、使用概率模型，判断给定测量值时体素被占据的概率大小来确定地图点是
否采用；
[0066]
s3.5、根据步骤s3.4中被采用的地图点，完成毫米波雷达建图，输出包含标定板位置信息的点云地图。
[0067]
步骤s3.3中所述根据每一帧雷达点云信息对应的位姿信息，将雷达点云信息统一至全局坐标系下，采用的计算公式为：
[0068][0069]
其中，表示t时刻下毫米波雷达检测得到的在全局坐标系下的单帧雷达点云坐标，表示t时刻下毫米波雷达检测得到的在局部坐标系下的单帧雷达点云坐标，表示t时刻下位全局坐标系到相机坐标系的三维旋转矩阵，表示t时刻下全局坐标系到相机坐标系的三维平移向量。
[0070]
步骤s3.4使用的概率模型为：
[0071][0072]
其中为t时刻之前给定测量值时节点n被占据的概率，p(n)为先验概率，为t-1时刻给定测量值时节点n被占据的概率，表示t时刻给定测量值时节点n被占据的概率，表示t时刻之前毫米波雷达检测得到的在全局坐标系下的所有帧的雷达点云坐标。
[0073]
步骤s4中，包括步骤：
[0074]
s4.1、对点云地图进行下采样，并利用点云滤波算法滤除离群点；
[0075]
s4.2、使用欧式聚类算法对点云地图中的不同部分进行聚类；
[0076]
s4.3、根据每个点云类的边缘特征以及面特征，从中提取出标定板的毫米波雷达点云块。
[0077]
步骤s1具体为：
[0078]
使用两块成90
°
夹角的反射板作为标定板，在移动平台上固定安装毫米波雷达以及相机。本发明的光学相机使用zed2双目相机的左相机作为单目相机，其分辨率为672
×
376，水平视角110
°
，垂直视角最大70
°
，相机内参默认使用zed2的官方数据。毫米波雷达使用ti公司的iwr1443，其水平视角最大为120
°
，俯仰角最大30
°
，距离分辨率最好为4cm，水平角分辨率为15
°
，垂直角分辨率为60
°
。移动机器人平台使用turtlebot2。毫米波雷达和相机的固定安装位置参见图2。
[0079]
步骤s2中所述的位姿传感器采用轮式里程计。位姿传感器采集的移动平台的位姿信息为移动平台的位置和姿态，位姿信息的变化包括平移和旋转两种变化。
[0080]
步骤s6中所述三维模型基于标定板的真实尺寸以及夹角进行构建。
[0081]
步骤s6中，采用迭代最近点算法将标定板三维模型与毫米波雷达点云块进行匹配。
[0082]
步骤s7中，包括步骤：
[0083]
s7.1、构建优化问题：
[0084][0085]
s7.2、将外参矩阵的求解转换为非线性最小二乘问题：
[0086][0087]
s7.3、使用lm优化方法求解标定矩阵；
[0088]
其中，zc为尺度因子，m为相机的内参矩阵，r为毫米波雷达坐标系坐标到相机坐标系坐标的三维旋转矩阵，t为毫米波雷达坐标系与相机坐标系的三维平移向量，p'
p
为二维边缘特征点坐标估计值，pr为标定板三维边缘特征点位置坐标，pr为二维边缘特征点坐标。
[0089]
以下通过具体实验，以说明本实施例所述标定方法的优势：
[0090]
实验中的标定环境为，在一个空旷的房间内，只有两块作为标定板的成90
°
夹角的反射板；场景一为，在上述空旷的房间内，除两块标定板外，还存在有其他干扰物体；场景二为，在场景一基础上包含更多杂物的房间。
[0091]
分别用人工和本发明所述算法在标定板点云块上提取六个三维特征点。在标定环境和场景一中，分别将使用人工以及本发明所述算法提取的三维特征点位置信息与分割出的二维特征点位置信息匹配，分别将使用上述两种方法得到的六组二维与三维的匹配特征点对信息作为标定算法的输入，得到标定结果。在搭建的标定环境以及场景一中，使用两种不同方法得到的重投影效果的精度量化结果数据对比参见图3。
[0092]
根据图3所示的实验结果，使用人工提取特征点的方法，在标定环境中重投影效果更好，但其得到的是错误的局部最优解，导致在场景一中并不能很好得将点云投影到目标的像素区域。而使用本发明所述的算法提取特征点，在标定环境和场景一中，重投影准确率都很高，且在场景一中，重投影效果远好于使用人工提取特征点的方法。
[0093]
使用本发明所述算法得到的外参矩阵，分别在场景一和场景二下做两次实验，得到的重投影效果的精度量化结果参见图4。根据图4所示的实验结果，使用本发明所述算法得到的外参矩阵，在空旷的环境以及杂物较多的环境中具有有效性与鲁棒性，重投影验证精度平均在0.7左右。
[0094]
以上所述的实施例仅仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明的保护范围内。

技术特征：
1.一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，包括步骤：s1、搭建标定板以及标定采集设备，标定采集设备包括相机、毫米波雷达、位姿传感器；s2、相机采集标定板原始图像信息，毫米波雷达采集标定板单帧雷达点云信息，位姿传感器采集移动平台的位姿信息；s3、将单帧雷达点云信息与位姿信息融合，输出包含标定板位置信息的点云地图；s4、利用点云滤波算法以及聚类算法对点云地图进行处理，以提取出标定板的毫米波雷达点云块；s5、使用目标分割算法分割出标定板在原始图像信息中的二维边缘特征点位置信息；s6、通过标定板的结构先验构建其三维模型，并将其与毫米波雷达点云块进行配准，从而得到拟合后的标定板三维边缘特征点位置信息，并与步骤s5中的二维边缘特征点位置信息进行匹配，以得到多组二维与三维的匹配特征点对信息；s7、将多组二维与三维的匹配特征点对信息，作为pnp算法的输入，以计算得到用于毫米波雷达与相机标定的外参矩阵。2.根据权利要求1所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，步骤s3中，包括步骤：s3.1、划定雷达点云信息中的有效区域，并滤除其单帧的低精度点；s3.2、基于预设的cfar阈值滤除毫米波雷达噪点；s3.3、根据每一帧雷达点云信息对应的位姿信息，将雷达点云信息统一至全局坐标系下；s3.4、使用概率模型，判断给定测量值时体素被占据的概率大小来确定地图点是否采用；s3.5、根据步骤s3.4中被采用的地图点，完成毫米波雷达建图，输出包含标定板位置信息的点云地图。3.根据权利要求2所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，步骤s3.3中所述根据每一帧雷达点云信息对应的位姿信息，将雷达点云信息统一至全局坐标系下，采用的计算公式为：其中，表示t时刻下毫米波雷达检测得到的在全局坐标系下的单帧雷达点云坐标，表示t时刻下毫米波雷达检测得到的在局部坐标系下的单帧雷达点云坐标，表示t时刻下位全局坐标系到相机坐标系的三维旋转矩阵，t
to
表示t时刻下全局坐标系到相机坐标系的三维平移向量。4.根据权利要求3所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，步骤s3.4使用的概率模型为：其中为t时刻之前给定测量值时节点n被占据的概率，p(n)为先验概率，
为t-1时刻给定测量值时节点n被占据的概率，表示t时刻给定测量值时节点n被占据的概率，表示t时刻之前毫米波雷达检测得到的在全局坐标系下的所有帧的雷达点云坐标。5.根据权利要求1所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，步骤s4中，包括步骤：s4.1、对点云地图进行下采样，并利用点云滤波算法滤除离群点；s4.2、使用欧式聚类算法对点云地图中的不同部分进行聚类；s4.3、根据每个点云类的边缘特征以及面特征，从中提取出标定板的毫米波雷达点云块。6.根据权利要求1所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，步骤s1具体为：使用两块成90
°
夹角的反射板作为标定板，在移动平台上固定安装毫米波雷达以及相机。7.根据权利要求6所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，步骤s6中所述三维模型基于标定板的真实尺寸以及夹角进行构建。8.根据权利要求7所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，步骤s6中，采用迭代最近点算法将标定板三维模型与毫米波雷达点云块进行匹配。9.根据权利要求1所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，位姿传感器采用轮式里程计。10.根据权利要求1所述的一种低分辨率毫米波雷达与光学相机的标定方法，其特征在于，步骤s7中，包括步骤：s7.1、构建优化问题：s7.2、将外参矩阵的求解转换为非线性最小二乘问题：s7.3、使用lm优化方法求解标定矩阵；其中，z
c
为尺度因子，m为相机的内参矩阵，r为毫米波雷达坐标系坐标到相机坐标系坐标的三维旋转矩阵，t为毫米波雷达坐标系与相机坐标系的三维平移向量，p'
p
为二维边缘特征点坐标估计值，p
r
为标定板三维边缘特征点位置坐标，p
r
为二维边缘特征点坐标。

技术总结
本发明公开一种低分辨率毫米波雷达与光学相机的标定方法，将毫米波雷达采集的单帧雷达点云信息与位姿传感器采集的位姿信息融合，输出包含标定板位置信息的点云地图；利用点云滤波算法以及聚类算法对点云地图进行处理，以提取出标定板的毫米波雷达点云；使用目标分割算法分割出标定板在原始图像信息中的二维边缘特征点位置信息；通过标定板的结构先验构建其三维模型，并将其与毫米波雷达点云块进行配准，从而得到拟合后的标定板三维边缘特征点位置信息，并与分割出的二维边缘特征点位置信息进行匹配，得到多组二维与三维的匹配特征点对信息；将多组二维与三维的匹配特征点对信息，作为PnP算法的输入，计算得到用于毫米波雷达与相机标定的外参矩阵。与相机标定的外参矩阵。与相机标定的外参矩阵。


技术研发人员：周叶剑 李琛玮 李冰凝 陈法龙 彭同友 陈博 张文安
受保护的技术使用者：中国人民解放军61191部队
技术研发日：2022.12.09
技术公布日：2023/7/11