阅读说明：本技术 基于单目相机与毫米波雷达融合的障碍物识别与定位方法 (Obstacle identification and positioning method based on fusion of monocular camera and millimeter wave radar ) 是由 黄攀峰 余航 张帆 孟中杰 张夷斋 刘正雄 于 2020-07-15 设计创作，主要内容包括：本发明涉及一种基于单目相机与毫米波雷达融合的障碍物识别与定位方法,包括传感器数据采集与时间同步、毫米波雷达与单目相机测量对象匹配和使用扩展卡尔曼滤波算法对两种传感器的测量数据进行滤波与融合；本发明采用多传感器融合的方式,使机器人能够感知比单个传感器更加丰富且精确的障碍物信息。相对单纯基于坐标转换的融合方式来说,本发明所使用的扩展卡尔曼滤波数据融合方式获取的数据更加接近真实值,且大大减少了传感器测量噪声带来的影响。(The invention relates to an obstacle identification and positioning method based on the fusion of a monocular camera and a millimeter wave radar, which comprises the steps of collecting data of a sensor and synchronizing time, matching a millimeter wave radar with a monocular camera measuring object and filtering and fusing the measuring data of the two sensors by using an extended Kalman filtering algorithm; the invention adopts a multi-sensor fusion mode, so that the robot can sense more abundant and accurate obstacle information than a single sensor. Compared with a fusion mode based on coordinate conversion, the data acquired by the extended Kalman filtering data fusion mode used by the invention is closer to a true value, and the influence brought by the measurement noise of the sensor is greatly reduced.)

基于单目相机与毫米波雷达融合的障碍物识别与定位方法

技术领域

本发明涉及多传感器融合技术领域，尤其涉及一种基于单目相机与毫米波雷达融合的障碍物识别与定位方法。

背景技术

随着无人驾驶、无人机等智能机器人的发展，环境感知技术取得了巨大的进步。目前较为成熟的目标识别与定位方法是利用深度学习算法对图像进行处理，包括RCNN，YOLO，SSD等系列，这些深度学习算法能够实现对障碍物的高准确率识别，甚至得到精确的物体轮廓边界。然而单一视觉传感器的感知能力是有限的，且容易受到环境的干扰，因此将其他传感器与视觉传感器进行融合能够切实提高机器人的感知能力。

毫米波雷达是一种通过发射毫米波来判断物体距离的主动式传感器。毫米波雷达获取的障碍物深度信息精度高且测量范围广，具有可靠性好、环境鲁棒性强、成本合理等优势。将单目相机与毫米波雷达进行融合，既可以获取障碍物的轮廓信息，又能得到精确的三维坐标，克服了单目相机无法获取精确深度信息以及毫米波雷达点云过于稀疏与噪点干扰的问题。

目前已有的单目相机与毫米波雷达融合的处理方法大多是：分别采集单目相机数据以及毫米波雷达数据，进行时间同步处理；使用深度学***面，与视觉获取的轮廓位置进行匹配得到最终的障碍物信息。这种方法虽然能够融合两种传感器的测量信息，但是并没有考虑毫米波雷达的噪声干扰，最终融合得到的数据可能会出现突变、漏检等现象，整个算法的鲁棒性较低。使用扩展卡尔曼滤波算法对传感器数据进行融合，既能获取更加准确的坐标信息，又能保证测量的鲁棒性。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于单目相机与毫米波雷达数据融合的障碍物识别与定位方法，结合单目相机与毫米波雷达的优势，能够使机器人获取环境中障碍物的精确位置与轮廓，相对于现有方法提升了测量精度与鲁棒性。

技术方案

一种基于单目相机与毫米波雷达融合的障碍物识别与定位方法，其特征在于步骤如下：

步骤1：传感器数据采集与时间同步

1.1)采集毫米波雷达与单目相机的数据，根据毫米波雷达提供的通信协议读取二维点云数据，使用OpenCV读取单目相机的图像信息；

1.2)根据点云与图像的时间戳信息将毫米波雷达与单目相机的数据进行时间序列配准，保证两个传感器融合数据的时间一致；

步骤2：毫米波雷达与单目相机测量对象匹配；

2.1)使用提前训练好的深度学习模型对视觉图像中的障碍物进行识别，获取包络障碍物的最小矩形框；

2.2)根据雷达相机外参矩阵与相机内参矩阵将雷达点云坐标(x_ri,y_ri)转换到像素坐标(u_ri,v_ri)，具体转换关系如下：

其中，z_c为空间点到相机平面的实际深度，K表示相机的内参矩阵R和T分别表示相机与雷达之间的旋转矩阵与平移向量，可以提前标定或直接测量获取，H表示毫米波雷达与相机之间的高度差；

2.3)遍历视觉检测的矩形框的中心像素位置(u_ci,v_ci)，距离(u_ri,v_ri)最近的点即为毫米波雷达与单目相机共同检测到的障碍物；

步骤3：使用扩展卡尔曼滤波算法对两种传感器的测量数据进行滤波与融合；

3.1)状态量的预测：融合后的状态量为雷达坐标系下的坐标与速度x_f(p_x,p_y,p_z,v_x,v_y)，根据卡尔曼滤波方程，状态量及其不确定度的预测值可表示为：

其中，x′_f为x_f的预测值，F表示线性系统状态量的变化系数，P、P′分别表示系统的不确定度以及不确定度的预测，Q表示线性系统x′_f＝Fx_f的额外干扰因素；

3.2)毫米波雷达测量更新：毫米波雷达测量得到的障碍物位置和速度为x_r(x_r,y_r,v_xr,v_yr)，根据卡尔曼滤波方程，状态量及其不确定度的更新值可表示为：

其中，x′_fr、P′_r为根据毫米波雷达测量数据更新后的状态量以及不确定度，H表示状态量与毫米波雷达测量值之间的转换关系，R_radar表示毫米波雷达测量值的不确定度，可以根据雷达的出厂参数获取；

3.3)单目相机测量更新：步骤2中与毫米波雷达匹配到的视觉检测到的障碍物的中心像素坐标为x_s(u,v)，根据卡尔曼滤波方程，状态量及其不确定度的更新值可表示为：

其中，f(x′_f)表示状态量向像素坐标系投影的坐标转换关系，使用针孔相机模型，因此坐标转换为非线性映射，H_j表示非线性映射关系中的像素坐标关于状态量的雅克比矩阵，R_cam表示单目相机测量值的不确定度，为单位矩阵；

3.4)按照步骤1中配准的时间序列，获取毫米波雷达及单目相机的测量值，并不断更新状态量，每更新一次执行一次预测，由此获得更加精确的障碍物位置信息。

步骤2.2中的K可以通过matlab或者OpenCV中的张正友法进行标定获取。

有益效果

本发明提出的一种基于单目相机与毫米波雷达融合的障碍物识别与定位方法，包括传感器数据采集与时间同步、毫米波雷达与单目相机测量对象匹配和使用扩展卡尔曼滤波算法对两种传感器的测量数据进行滤波与融合。本发明采用多传感器融合的方式，使机器人能够感知比单个传感器更加丰富且精确的障碍物信息。相对单纯基于坐标转换的融合方式来说，本发明所使用的扩展卡尔曼滤波数据融合方式获取的数据更加接近真实值，且大大减少了传感器测量噪声带来的影响。

附图说明

图1是毫米波雷达与单目相机数据融合的流程图；

图2是毫米波雷达与单目相机之间的坐标转换关系。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

如图1所示，传感器数据采集与时间同步；

1.1)采集毫米波雷达与单目相机的数据，根据毫米波雷达提供的通信协议解析传感器数据，获取二维点云信息，使用OpenCV读取单目相机的图像信息；

1.2)根据点云与图像的时间戳信息将毫米波雷达与单目相机的数据进行时间序列配准，保证两个传感器融合数据的时间一致；

2)如图1所示，将毫米波雷达与单目相机测量对象匹配；

2.1)提前采集具有障碍物信息的图片，使用基于YOLO-v3的深度学习训练模型，使模型能够对视觉图像中的障碍物进行识别，获取包络障碍物的最小矩形框；

2.2)如图2所示为毫米波雷达与单目相机的坐标转换关系，O_rX_rY_rZ_r，O_cX_cY_cZ_c和O_sX_sY_s分别表示雷达坐标系、相机坐标系和像素坐标系，根据雷达相机外参矩阵与相机内参矩阵将雷达点云坐标(x_ri,y_ri)转换到像素坐标(u_ri,v_ri)，具体转换关系如下：

其中，z_c为空间点到相机平面的实际深度，K表示相机的内参矩阵可以通过matlab或者OpenCV中的张正友法进行标定获取，R和T分别表示相机与雷达之间的旋转矩阵与平移向量，可以提前标定或直接测量获取，H表示毫米波雷达与相机之间的高度差，可直接测量获取。

2.3)遍历视觉检测的矩形框的中心像素位置(u_ci,v_ci)，距离(u_ri,v_ri)最近的点即为毫米波雷达与单目相机共同检测到的障碍物；

3)如图1右边所示，使用扩展卡尔曼滤波算法对两种传感器的测量数据进行滤波与融合；

3.1)状态量的预测：融合后的状态量为雷达坐标系下的坐标与速度x_f(p_x,p_y,p_z,v_x,v_y)，根据卡尔曼滤波方程，状态量及其不确定度的预测值可表示为：

其中，x′_f为x_f的预测值，P、P′分别表示系统的不确定度以及不确定度的预测，F表示线性系统状态量的变化系数，由于是线性系统，可知：

Q表示线性系统x′_f＝Fx_f的额外干扰因素，由于假设成了线性系统，因此干扰量与加速度有关：

其中，v＝Ga表示加速度对速度的增量。

3.2)毫米波雷达测量更新：毫米波雷达测量得到的障碍物位置和速度为x_r(x_r,y_r,v_xr,v_yr)，根据卡尔曼滤波方程，状态量及其不确定度的更新值可表示为：

其中，x′_fr、P′_r为根据毫米波雷达测量数据更新后的状态量以及不确定度，H＝[I,0]表示状态量与毫米波雷达测量值之间的转换关系，R_radar表示毫米波雷达测量值的不确定度，可以根据雷达的出厂参数获取。

3.3)单目相机测量更新：步骤2中与毫米波雷达匹配到的视觉检测到的障碍物的中心像素坐标为x_s(u,v)，根据卡尔曼滤波方程，状态量及其不确定度的更新值可表示为：

其中，R_cam表示单目相机测量值的不确定度，为单位矩阵。f(x′_f)表示状态量向像素坐标系投影的坐标转换关系，使用针孔相机模型，因此坐标转换为非线性映射：

f(x_f)＝[u,v]^T＝KT[p_x,p_y,p_z,1]^T

H_j表示非线性映射关系中的像素坐标关于状态量的雅克比矩阵：

3.4)按照步骤1中配准的时间序列，获取毫米波雷达及单目相机的测量值，并不断更新状态量，每更新一次执行一次预测，由此获得更加精确的障碍物位置信息。