CN114280611A - 一种融合毫米波雷达与摄像头的路侧感知方法 - Google Patents

一种融合毫米波雷达与摄像头的路侧感知方法 Download PDF

Info

Publication number
CN114280611A
CN114280611A CN202111313286.5A CN202111313286A CN114280611A CN 114280611 A CN114280611 A CN 114280611A CN 202111313286 A CN202111313286 A CN 202111313286A CN 114280611 A CN114280611 A CN 114280611A
Authority
CN
China
Prior art keywords
data
camera
coordinate system
millimeter wave
wave radar
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111313286.5A
Other languages
English (en)
Inventor
王亚飞
殷承良
周志松
吴胜杰
陈炜邦
高瑞金
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Shanghai Intelligent and Connected Vehicle R&D Center Co Ltd
Original Assignee
Shanghai Jiaotong University
Shanghai Intelligent and Connected Vehicle R&D Center Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University, Shanghai Intelligent and Connected Vehicle R&D Center Co Ltd filed Critical Shanghai Jiaotong University
Priority to CN202111313286.5A priority Critical patent/CN114280611A/zh
Publication of CN114280611A publication Critical patent/CN114280611A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明涉及一种融合毫米波雷达与摄像头的路侧感知方法，该方法包括以下步骤：步骤1：通过毫米波雷达获取毫米波雷达数据，对其进行有效目标的提取，以去除静止目标和随机噪声；步骤2：通过设置在路侧立杆的摄像头获取目标的图像数据，基于YOLOv3和Deep SORT跟踪算法进行目标检测与跟踪，得到摄像头数据；步骤3：基于融合算法将毫米波雷达数据与摄像头数据进行融合，以实现路侧设备对目标的实时检测、跟踪与定位，与现有技术相比，本发明具有减少计算量、实现数据融合后稳定地进行检测跟踪以及提高定位精度等优点。

Description

一种融合毫米波雷达与摄像头的路侧感知方法
技术领域
本发明涉及路侧感知技术领域，尤其是涉及一种融合毫米波雷达与摄像头的路侧感知方法。
背景技术
现阶段车辆大多采用车载传感器和GPS定位系统来实现对车辆周围环境的感知和实时定位，存在单车车本高，较难应对复杂交通场景等问题。针对该问题，人们提出了路侧感知的解决方法，它通过融合路侧端的传感器感知结果为车辆提供准确、实时的交通信息，从而增强自动驾驶车辆的感知范围以提高车辆的行驶安全性。按照传感器方案可以分为单传感器方案和多传感器融合方案。
路侧感知系统中广泛用到的传感器有摄像头、毫米波雷达和激光雷达，单一传感器总是存在无法应对的特定场景，比如摄像头夜间成像效果差、激光雷达在雨雪天气可能失效等。因此采用单传感器方案面对复杂交通环境，无法满足高质量、高精度的要求，多传感器融合方案由于融合多个传感器数据，可以实现冗余保护和应对复杂场景，现有方案一般为基于激光雷达和摄像头的感知方案以及基于毫米波雷达和摄像头的方案。前者由于采用了激光雷达，不仅成本高昂，而且需要较强的计算资源来处理点云数据；后者价格较低，可以全天候工作，数据量较小。
现有基于毫米波雷达和摄像头融合方案一般是基于毫米波雷达的多目标跟踪结果去匹配单帧的摄像头检测结果，由于摄像头的单帧检测结果并不稳定，存在较多的漏检、虚警的结果，只是匹配单帧检测结果，会将错误的结果匹配到毫米波雷达跟踪目标，难以实现融合后稳定的检测跟踪结果，且现有基于毫米波雷达和摄像头融合方案在进行数据融合时，直接采用加权求和，忽略了不同传感器的特性，没有发挥各个传感器的优势。摄像头在近距离测量时，横向位置测量精度相比毫米波雷达更高，而毫米波雷达在纵向距离测量方面则更有优势，因此可以采用自适应权重的方法对二者的数据进行融合达到更高的定位精度。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种融合毫米波雷达与摄像头的路侧感知方法。
本发明的目的可以通过以下技术方案来实现：
一种融合毫米波雷达与摄像头的路侧感知方法，该方法包括以下步骤：
步骤1：通过毫米波雷达获取毫米波雷达数据，对其进行有效目标的提取，去除静止目标和随机噪声；
步骤2：通过设置在路侧立杆上的摄像头获取图像数据，基于YOLOv3和Deep SORT跟踪算法进行目标检测与多目标跟踪，得到摄像头数据；
步骤3：基于融合算法将毫米波雷达数据与摄像头数据进行多传感器融合，以实现路侧设备对目标的实时检测、跟踪与定位。
所述的步骤1中，进行有效目标的提取的过程具体包括以下步骤：
步骤101：通过设置在路侧立杆上的毫米波雷达获取毫米波雷达数据，毫米波雷达数据包括目标的ID、纵向距离、横向距离、纵向速度、横向速度和反射截面积；
步骤102：基于固定的路侧立杆获取设置的道路区域的道路位置信息，根据道路位置信息筛选得到位于道路区域内的目标；
步骤103：基于设定的速度阈值去除位于道路区域内的静止目标；
步骤104：设置目标在一段连续时间内存在的次数阈值，根据目标的ID索引目标在前4个扫描周期内存在的次数，去除存在的次数小于3次的索引结果，进而得到有效目标。
所述的步骤2中，基于YOLOv3和Deep SORT跟踪算法进行目标检测与跟踪的过程具体包括以下步骤：
步骤201：通过设置在路侧立杆上的摄像头获取图像数据；
步骤202：对图像数据进行标注并创建数据集，基于数据集对采用的YOLOv3 模型进行训练，首先冻结主干提取网路，只训练预测网络，训练50个周期后，对整个模型进行训练，采用Early Stopping结束训练；
步骤203：将训练后的YOLOv3模型作为Deep SORT算法的检测器，进行多目标跟踪，最终的输出为[idc,score,xmin,ymin,xmax,ymax,class]m，即摄像头数据，idc为摄像头检测到的目标的ID，score为置信度，(xmin,ymin)为检测框的左上顶点， (xmax,ymax)为检测框的右下顶点，class为类别，m表示摄像头检测到的目标数量。
所述的步骤3中，基于融合算法将毫米波雷达数据与摄像头数据进行融合的过程具体包括以下步骤：
步骤301：对毫米波雷达数据和摄像头数据进行空间融合，将雷达坐标系下的点通过坐标系转换到摄像机对应的像素坐标系下，即可实现两者空间融合，具体为：
设Xw-Yw-Zw为世界坐标系，Ow为世界坐标系的原点，Xr-Yr-Zr为雷达坐标系， Or为雷达坐标系的原点，Xc-Yc-Zc为摄像头坐标系，Oc为摄像头坐标系的原点，平面Ow-Xw-Yw与地面重合，轴Ow-Zw沿着路侧立杆朝上，摄像头坐标系的原点Oc到世界坐标系的原点Ow的垂直高度为hc，摄像头坐标系的原点Oc到世界坐标系的原点Ow的纵向距离为lc，轴Oc-Zc与摄像头的光轴重合，轴Oc-Zc与轴Ow-Xw的夹角为θ，平面Oc-Yc-Zc与平面Ow-Xw-Zw重合，雷达坐标系的原点Or到世界坐标系的原点Ow的垂直高度为hr，雷达坐标系的原点Or到世界坐标系的原点Ow的纵向距离为lr，平面Or-Xr-Zr与平面Ow-Xw-Zw重合，轴Or-Xr与轴Ow-Xw平行，u-v为像素坐标系，将摄像头坐标系中的点(Xc,Yc,Zc)转换至像素坐标系下的坐标的计算公式为：
Figure RE-GDA0003495509960000031
其中，fx和fy为相机的等效焦距，(u0,v0)为图片中心的像素坐标，fx、fy、 u0和v0均为摄像头的内部参数，(Xc,Yc,Zc)为摄像头坐标系的三维坐标，(u,v)为像素坐标系的二维坐标；
根据雷达坐标系与摄像头坐标系的相对位置关系将雷达坐标系中的点 (Xr,Yr,Zr)转换至像素坐标系下的坐标的计算公式为：
Figure RE-GDA0003495509960000032
其中，(Xr,Yr,Zr)为雷达坐标系的三维坐标，hr为雷达坐标系的原点Or到世界坐标系的原点Ow的垂直高度，lr为雷达坐标系的原点Or到世界坐标系的原点Ow的纵向距离，hc为摄像头坐标系的原点Oc到世界坐标系的原点Ow的垂直高度，lc为摄像头坐标系的原点Oc到世界坐标系的原点Ow的纵向距离，θ为轴Oc-Zc与轴Ow-Xw的夹角；
将世界坐标系中的点(Xw,Yw,Zw)转换为像素坐标系下的坐标的计算公式为：
Figure RE-GDA0003495509960000041
其中，(Xw,Yw,Zw)为世界坐标系的三维坐标；
步骤302：对毫米波雷达与摄像头的时间进行融合，将毫米波雷达数据和摄像头数据的时间戳对准，以进行时间融合，即根据摄像头数据的时间戳，采用自适应算法进行对准，完成对毫米波雷达数据和摄像头数据的时间融合，
步骤303：基于融合算法对毫米波雷达数据和摄像头数据进行融合。
所述的步骤303中，基于融合算法对毫米波雷达数据和摄像头数据进行融合的过程具体包括以下步骤：
步骤303A：通过坐标变换对毫米波雷达数据和摄像头数据进行数据扩展；
步骤303B：对毫米波雷达数据和摄像头数据进行匹配数据初选；
步骤303C：将进行匹配数据初选后的毫米波雷达数据和扩展后的摄像头数据进行数据关联，计算每个毫米波雷达的数据属于每一个检测框的代价，即采用像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离以及世界坐标系下毫米波雷达数据和摄像头数据之间的欧氏距离作为代价矩阵，并基于匈牙利算法获取全局代价最小的匹配方式；
步骤303D：以视觉跟踪结果为主，匹配毫米波雷达关于该目标的数据，即以摄像头数据为主，将摄像头数据与毫米波雷达数据进行匹配和融合，并基于卡尔曼滤波进行更新，得到融合数据的航迹。
所述的步骤303A中，进行数据扩展的过程具体为：
步骤303A1：对毫米波雷达数据进行有效目标的提取后的输出为：
[idr,xr,yr,vxr,vyr]n
其中，idr为毫米雷达波检测到的目标的ID，(xr,yr)为毫米雷达波检测到的目标在雷达坐标系下的雷达坐标，vxr和vyr分别为目标在雷达坐标系下的纵向速度和横向速度，n表示毫米波雷达检测到的目标数量；
步骤303A2：通过空间融合将雷达坐标(xr,yr)投影到像素坐标系下得到像素坐标(xr_c,yr_c)，进而将毫米波雷达数据扩展为：
[idr,xr,yr,vxr,vyr,xr_c,yr_c]n；
其中，(xr_c,yr_c)为雷达坐标(xr,yr)投影到像素坐标系下的像素坐标；
步骤303A3：对路侧的摄像头获取目标的图像数据进行目标检测与跟踪的输出为：
[idc,score,xmin,ymin,xmax,ymax,class]m
其中，idc为摄像头检测到的目标的ID，score为置信度，xmin和ymin分别为检测框的左上顶点的横坐标和纵坐标，xmax和ymax分别为检测框的右下顶点的横坐标和纵坐标，class为类别，m表示摄像头检测到的目标数量；
步骤303A4：以检测框下边界的中心点((xmin+xmax)/2,ymax)作为参考点，该参考点对应的世界坐标系下的坐标在Zw＝0的平面上，该点对应的世界坐标系下的坐标的计算公式为：
Figure RE-GDA0003495509960000051
其中，(Xw,Yw,Zw)为世界坐标系的三维坐标，(xc_w,yc_w)为摄像头检测到的目标在世界坐标系下的坐标；
步骤303A5：进而将摄像头数据扩展为：
[idc,score,xmin,ymin,xmax,ymax,class,xc_w,yc_w]m。
所述的步骤303B中，对毫米波雷达数据和摄像头数据进行匹配数据初选的过程具体为：
一组经过空间融合和时间融合后的毫米波雷达数据的像素坐标为(xr_c,yr_c)，检测框的左上顶点坐标为(xmin,ymin)，右下顶点为(xmax,ymax)，扩展检测框左上顶点的坐标为(xmin-Δx,ymin-Δy)，右下顶点的坐标为(xmax+Δx,ymax+Δy)，即在检测框的基础上沿u轴和v轴得到扩展检测框，通过扩大后的检测扩展框筛选匹配的毫米波雷达数据，筛选后的摄像头数据中剩下M个目标，毫米波数据中剩下N个目标，即摄像头数据的数量为M，毫米波雷达数据的数量为N。
所述的步骤303C中，进行数据关联的过程具体包括以下步骤：
步骤303C1：至少有一个毫米波雷达数据落在每个摄像头输出的检测框内，即摄像头数据的数量M小于等于毫米波雷达数据的数量N，设摄像头数据和毫米波雷达数据的表达式分别为：
[idc,score,xmin,ymin,xmax,ymax,class,xc_w,yc_w]M
[idr,xr,yr,vxr,vyr,xr_c,yr_c]N
其中，idc为摄像头检测到的目标的ID，score为置信度，xmin和ymin分别为检测框左上顶点的横坐标和纵坐标，xmax和ymax分别为检测框右下顶点的横坐标和纵坐标，class为类别，(xc_w,yc_w)为检测框下边界的中心点在世界坐标系下的坐标， M为摄像头检测到的目标的数量，即摄像头数据的数量，idr为毫米雷达波检测到的目标的ID，(xr,yr)为毫米雷达波检测到的目标在雷达坐标系下的雷达坐标，vxr和 vyr分别为毫米雷达波检测到的目标在雷达坐标系下的纵向速度和横向速度， (xr_c,yr_c)为毫米雷达波检测到的目标的像素坐标，M为摄像头检测到的目标的数量，即摄像头数据的数量，N为毫米雷达波检测到的目标的数量，即毫米雷达波数据的数量；
步骤303C2：计算像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离：
对于摄像头检测到的第i个目标，若该目标为首次检测到的目标，则以检测框的中心对摄像头数据的像素坐标进行初始化，以单位阵I2×2对协方差矩阵进行初始化，摄像头数据的像素坐标和协方差矩阵的表达式为：
Figure RE-GDA0003495509960000061
其中，
Figure RE-GDA0003495509960000062
为检测框的中心坐标，di为摄像头检测到的第i个目标对应的像素坐标，Si为摄像头检测到的第i个目标对应的协方差矩阵；
若该目标属于现存的融合数据的航迹，则计算观测变量和观测空间的协方差矩阵，由于不涉及速度信息，因此取观测变量的位置信息对应的像素坐标(xw_c,yw_c) 对像素坐标di进行赋值，取观测空间协方差矩阵中的位置协方差对协方差矩阵Si赋值：
Figure RE-GDA0003495509960000063
Figure RE-GDA0003495509960000071
其中，Sk+1为k+1时刻的观测空间协方差矩阵，h为2×4的矩阵，T表示矩阵的转置；
根据像素坐标di和协方差矩阵Si计算像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离，马氏距离的计算公式为：
Figure RE-GDA0003495509960000072
dj＝[xr_c,yr_c]
其中，disimage为像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离， dj为毫米波雷达检测到的第j个目标对应的像素坐标；
步骤303C3：计算世界坐标系下毫米波雷达数据和摄像头数据之间的欧氏距离，欧氏距离的计算公式为：
Figure RE-GDA0003495509960000073
其中，disworld为世界坐标系下毫米波雷达数据和摄像头数据之间的欧氏距离，(xr+lr,yr)为毫米波雷达检测到的第j个目标对应的世界坐标，(xc_w,yc_w)为摄像头检测到的第i个目标对应的世界坐标；
步骤303C4：对得到的马氏距离和欧氏距离分别归一化，再进行加权求和，得到第j个毫米波雷达数据匹配第i个摄像头数据的代价，代价的计算公式为：
Figure RE-GDA0003495509960000074
其中，task_matrixij为第j个雷达数据匹配第i个摄像头数据的代价，λ为马氏距离和欧氏距离之间的权重；
步骤303C5：获取全局代价矩阵，并根据全局代价矩阵基于匈牙利算法得到全局代价最优的匹配结果，对于某一个匹配结果，若对应的代价大于设定的代价阈值，则认为该匹配结果是错误的，并将该匹配结果去掉。
所述的步骤303D中，若目标的id不属于现存的融合数据的航迹，基于卡尔曼滤波进行更新的过程具体包括以下步骤：
步骤303D1：目标的id不属于现存的融合数据的航迹，即摄像头数据没有匹配的毫米雷达波数据，对状态变量进行初始化，则状态变量和状态协方差矩阵的表达式分别为：
Figure RE-GDA0003495509960000081
其中，I4×4为4×4的单位阵；
过程噪声协方差矩阵qk的表达式为：
qk＝0.1I4×4；
观测噪声协方差矩阵rk的表达式为：
rk＝0.1I4×4
状态转移矩阵fk的表达式为：
Figure RE-GDA0003495509960000082
其中，t为当前时刻与上一时刻的时间差；
观测矩阵hk的表达式为：
hk＝I4×4；
步骤303D2：进行预测与更新，获取预测状态向量和预测状态协方差矩阵：
Figure RE-GDA0003495509960000083
其中，Xk+1|k为k+1时刻的预测状态向量，fk+1为k+1时刻的状态转移矩阵，Pk+1|k为k+1时刻的预测状态协方差矩阵，qk+1为k+1时刻的过程噪声协方差矩阵，T表示转置；
根据预测状态向量和预测状态协方差矩阵得到观测向量和观测空间协方差矩阵：
Figure RE-GDA0003495509960000084
其中，Yk+1为k+1时刻的观测向量，hk+1为k+1时刻的状态转移矩阵，Sk+1为k+1 时刻的观测空间协方差矩阵，rk+1为k+1时刻的观测噪声协方差矩阵；
k+1时刻的实际测量值Zk+1和观测噪声协方差矩阵rk+1的表达式为：
Zk+1＝[xc_w,yc_w,0,0]T
rk+1＝I4×4
对状态向量和状态协方差矩阵进行更新得到状态向量和状态协方差矩阵的后验估计值：
Figure RE-GDA0003495509960000091
Kk+1为k+1时刻的增益，Zk+1为k+1时刻的实际测量值，Xk+1为k+1时刻的状态向量的后验估计值，Pk+1为k+1时刻的状态协方差矩阵的后验估计值；
若目标的id属于现存的融合数据的航迹，基于卡尔曼滤波进行更新的过程具体包括以下步骤：
步骤303D3：目标的id属于现存的融合数据的航迹，即摄像头数据有匹配的毫米雷达波数据，则状态变量、状态协方差矩阵和观测变量的表达式分别为：
Figure RE-GDA0003495509960000092
其中，xr+lr为毫米雷达波数据在世界坐标系下的纵向坐标，λ1和λ2为权重，Xk为状态变量，dis和vel分别为目标在世界坐标系下的位置和速度，disx、disy、velx和vely分别为目标在世界坐标系下的纵向距离、横向距离、纵向速度和横向速度， Yk为观测变量，I为单位阵；
权重λ1的表达式为：
Figure RE-GDA0003495509960000093
其中，xr为毫米雷达波检测到的目标在雷达坐标系下的纵向坐标；
步骤303D4：进行预测与更新，获取预测状态向量和预测状态协方差矩阵：
Figure RE-GDA0003495509960000094
其中，Xk+1|k为k+1时刻的预测状态向量，fk+1为k+1时刻的状态转移矩阵，Pk+1|k为k+1时刻的预测状态协方差矩阵，qk+1为k+1时刻的过程噪声协方差矩阵，T表示转置；
根据预测状态向量和预测状态协方差矩阵得到观测向量和观测空间协方差矩阵：
Figure RE-GDA0003495509960000101
其中，Yk+1为k+1时刻的观测向量，hk+1为k+1时刻的状态转移矩阵，Sk+1为k+1 时刻的观测空间协方差矩阵，rk+1为k+1时刻的观测噪声协方差矩阵；
k+1时刻的实际测量值Zk+1的表达式为：
Zk+1＝[λ1(xr+lr)+(1-λ1)xc_w,λ2yr+(1-λ2)xc_w,vxr,vyr]T
对状态向量和状态协方差矩阵进行更新得到状态向量和状态协方差矩阵的后验估计值：
Figure RE-GDA0003495509960000102
Kk+1为k+1时刻的增益，Zk+1为k+1时刻的实际测量值，Xk+1为k+1时刻的状态向量的后验估计值，Pk+1为k+1时刻的状态协方差矩阵的后验估计值；
步骤303D5：得到目标最新的融合数据为：
[id,class,score,xmin,ymin,xmax,ymax,disx,disy,velx,vely]M
其中，id为融合目标的ID，disx、disy、vel和vely为基于卡尔曼滤波进行更新后的后验估计值，disx、disy、vel和vely分别表示融合目标在世界坐标系下的纵向距离、横向距离、纵向速度和横向速度；
步骤303D6：获取融合数据的航迹，采用age和moment管理融合数据的航迹的产生和消失。
所述的步骤303D6中，采用age和moment管理融合数据的航迹的产生和消失具体为：
摄像头检测到一个目标后产生一条航迹，融合航迹基于摄像头的航迹进行初始化，此时将所有航迹的age设为1，将所有航迹的moment设为当前数据的时间戳，当下一周期接收的摄像头数据中出现新的摄像头的航迹时，在融合数据的航迹中添加新的摄像头的航迹，并将该航迹的age设为1，将该航迹的moment设为对应摄像头数据的时间戳，若摄像头的航迹与上一周期存在的航迹对应的id相同，则采用新的摄像头的航迹进行更新，age仍为1，moment更新为新的摄像头的航迹对应摄像头数据的时间戳，若上一周期存在的航迹在本周期内不存在对应的航迹，则将该航迹的age增加1，moment保持不变，一个航迹的age达到设定的阈值后认为该航迹已离开场景，并将其从航迹集内删除。
与现有技术相比，本发明具有以下优点：
1、通过毫米波雷达有效目标提取方法，有效地滤除了大量噪声和虚警信号，减少了后期计算量。
2、通过自建的路侧数据集，有效解决了路侧摄像头安装位置高，可视范围大，小目标较多的难点。
3、采用加权距离特征进行数据关联，将关联后的数据进行加权融合作为测量值进行卡尔曼滤波更新，提高了融合模型距离跟踪精度。
4、在数据融合时，考虑了毫米波雷达和摄像头的特性，近距离测量时，横向位置测量精度相比毫米波雷达更高，而毫米波雷达在纵向距离测量方面则更有优势，因此采用了自适应权重的方法对二者的数据进行融合达到更高的定位精度。
附图说明
图1为本发明的结构框图。
图2为坐标系的相对位置示意图。
图3为时间融合算法的示意图。
图4为融合算法的结构框图。
图5为匹配数据初选的示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
如图1所示，本发明提供了一种融合毫米波雷达与摄像头的路侧感知方法，该方法包括以下步骤：
步骤1：通过毫米波雷达获取毫米波雷达数据，对其进行有效目标的提取，以去除静止目标和随机噪声；
步骤2：通过设置在路侧的摄像头获取图像数据，基于YOLOv3和Deep SORT 跟踪算法进行目标检测与多目标跟踪，得到摄像头数据；
步骤3：基于融合算法将毫米波雷达数据与摄像头数据进行多传感器融合，以实现路侧设备对目标的实时检测、跟踪与定位。
在步骤1中，通过设置感兴趣区域和速度阈值去除静止目标，通过设置目标在一段连续时间内存在的次数阈值去除随机噪声，提取可能存在的有效目标，即通过设置在路侧立杆上的毫米波雷达获取毫米波雷达数据，毫米波雷达数据包括目标的 ID、纵向距离、横向距离、纵向速度、横向速度和反射截面积，由于路侧立杆是固定不动的，能够提前获取道路位置信息，从而筛选出位于道路区域内的目标，并通过设定的速度阈值去除静止目标，毫米波雷达数据中存在少量的随机噪声和虚警结果，根据ID索引目标在前4个扫描周期内存在的次数，滤除存在的次数小于3 次的索引结果，进而得到有效目标。
在步骤2中，基于机器视觉进行目标检测与跟踪，采用路侧立杆上设置的摄像头获取图像数据，对图像数据进行标注并创建数据集，基于数据集对采用的 YOLOv3模型进行训练，训练过程具体为：
首先冻结主干提取网路，只训练预测网络，训练50个周期后，对整个模型进行训练，并采用Early Stopping结束训练。
训练后的YOLOv3模型作为Deep SORT算法的检测器，进行多目标跟踪，最终的输出为[idc,score,xmin,ymin,xmax,ymax,class]m，即摄像头数据，idc为摄像头检测到的目标的ID，score为置信度，(xmin,ymin)为检测框的左上顶点，(xmax,ymax)为检测框的右下顶点，class为类别，m表示摄像头检测到的目标数量。
在步骤3中，基于融合算法将毫米波雷达数据与摄像头数据进行融合的过程具体包括以下步骤：
步骤301：对毫米波雷达数据和摄像头数据进行空间融合，将雷达坐标系下的点通过坐标系转换到摄像机对应的像素坐标系下，即可实现两者空间融合：
Xw-Yw-Zw为世界坐标系，Ow为世界坐标系的原点，Xr-Yr-Zr为雷达坐标系， Or为雷达坐标系的原点，Xc-Yc-Zc为摄像头坐标系，Oc为摄像头坐标系的原点，，平面Ow-Xw-Yw与地面重合，轴Ow-Zw沿着路侧立杆朝上，摄像头坐标系的原点Oc到世界坐标系的原点Ow的垂直高度为hc，摄像头坐标系的原点Oc到世界坐标系的原点Ow的纵向距离为lc，轴Oc-Zc与摄像头的光轴重合，轴Oc-Zc与轴Ow-Xw的夹角为θ，平面Oc-Yc-Zc与平面Ow-Xw-Zw重合，雷达坐标系的原点Or到世界坐标系的原点Ow的垂直高度为hr，雷达坐标系的原点Or到世界坐标系的原点Ow的纵向距离为lr，平面Or-Xr-Zr与平面Ow-Xw-Zw重合，轴Or-Xr与轴Ow-Xw平行，u-v为像素坐标系，将摄像头坐标系中的点(Xc,Yc,Zc)转换至像素坐标系下的坐标的计算公式为：
Figure RE-GDA0003495509960000131
其中，fx和fy为相机的等效焦距、(u0,v0)为图片中心的像素坐标，fx、fy、 u0和v0均为摄像头的内部参数，(Xc,Yc,Zc)为摄像头坐标系的三维坐标，(u,v)为像素坐标系的二维坐标；
根据雷达坐标系与摄像头坐标系的相对位置关系将雷达坐标系中的点 (Xr,Yr,Zr)转换至像素坐标系下的坐标：
Figure RE-GDA0003495509960000132
其中，(Xr,Yr,Zr)为雷达坐标系的三维坐标，hr为雷达坐标系的原点Or到世界坐标系的原点Ow的垂直高度，lr为雷达坐标系的原点Or到世界坐标系的原点Ow的纵向距离，hc为摄像头坐标系的原点Oc到世界坐标系的原点Ow的垂直高度，lc为摄像头坐标系的原点Oc到世界坐标系的原点Ow的纵向距离，θ为轴Oc-Zc与轴 Ow-Xw的夹角；
将世界坐标系中的点(Xw,Yw,Zw)转换为像素坐标系下的坐标：
Figure RE-GDA0003495509960000133
其中，(Xw,Yw,Zw)为世界坐标系的三维坐标；
步骤302：对毫米波雷达与摄像头的时间进行融合，将毫米波雷达数据和摄像头数据的时间戳对准，以进行时间融合，即根据摄像头数据的时间戳，采用自适应算法进行对准，完成对毫米波雷达数据和摄像头数据的时间融合，如图3所示，黑色直线表示摄像头和毫米波雷达发布的数据流，黑色的点代表示摄像头和毫米波雷达在一个完整周期内发布的数据，判断两者数据的时间戳差值是否在设定的时间差阈值范围内，若是，则认为二者是对准的，并用虚线连接。
步骤303：基于融合算法对毫米波雷达数据和摄像头数据进行融合：
步骤1对毫米波雷达的数据进行解析和过滤后的输出，即对毫米波雷达数据进行有效目标的提取后的输出为：
[idr,xr,yr,vxr,vyr]n
其中，idr为毫米雷达波检测到的目标的ID，(xr,yr)为目标在雷达坐标系下的雷达坐标，vxr和vyr分别为目标在雷达坐标系下的纵向速度和横向速度，n表示毫米波雷达检测到的目标数量；
通过空间融合将雷达坐标(xr,yr)投影到像素坐标系下得到像素坐标(xr_c,yr_c)，进而将毫米波雷达数据扩展为：
[idr,xr,yr,vxr,vyr,xr_c,yr_c]n；
其中，(xr_c,yr_c)为雷达坐标(xr,yr)投影到像素坐标系下的像素坐标；
步骤2对路测的摄像头获取目标的图像数据进行目标检测与跟踪的输出，即摄像头数据为：
[idc,score,xmin,ymin,xmax,ymax,class]m
其中，idc为摄像头检测到的目标的ID，score为置信度，xmin和ymin分别为检测框的左上顶点的横坐标和纵坐标，xmax和ymax分别为检测框的右下顶点的横坐标和纵坐标，class为类别，m表示摄像头检测到的目标数量；
以检测框下边界的中心点((xmin+xmax)/2,ymax)作为参考点，该参考点对应的世界坐标系下的坐标在Zw＝0的平面上，该点对应的世界坐标系下的坐标的计算公式为：
Figure RE-GDA0003495509960000141
其中，(Xw,Yw,Zw)为世界坐标系的三维坐标，(xc_w,yc_w)为摄像头检测到的目标在世界坐标系下的坐标；
进而将摄像头数据扩展为：
[idc,score,xmin,ymin,xmax,ymax,class,xc_w,yc_w]m
其中，(xc_w,yc_w)为检测框下边界的中心点在世界坐标系下的坐标；
路侧摄像头视野范围广，遮挡面积小，检测与跟踪性能表现良好，以摄像头数据为主，匹配毫米波雷达关于该目标的数据，基于卡尔曼滤波进行更新，完成数据融合。
如图4所示的融合算法的框架流程图，通过坐标变换对毫米波雷达数据和摄像头数据进行数据扩展，并对毫米波雷达的数据和摄像头数据进行匹配数据初选，再将扩展后的毫米波雷达和扩展后的摄像头数据进行数据关联，将摄像头数据与毫米波雷达数据进行匹配和融合，并基于卡尔曼滤波进行更新，得到融合数据的航迹。
基于卡尔曼滤波进行更新的过程具体包括以下步骤：
步骤a：进行初始化，若摄像头数据有匹配的毫米雷达波数据，则
Figure RE-GDA0003495509960000151
其中，xr+lr为毫米雷达波数据在世界坐标系下的纵向坐标，lr＝0.16m，λ1和λ2为权重，取λ2＝0.8，Xk为状态变量，disx、disy、velx和vely分别为融合目标在世界坐标系下的纵向距离、横向距离、纵向速度和横向速度，Yk＝Xk为观测变量；
权重λ1与目标在雷达坐标系下的纵向坐标xr的关系表达式为：
Figure RE-GDA0003495509960000152
若摄像头数据没有匹配的毫米雷达波数据，对状态变量进行初始化，则状态变量和状态协方差矩阵的表达式分别为：
Figure RE-GDA0003495509960000153
其中，I4×4为4×4的单位阵；
过程噪声协方差矩阵qk的表达式为：
qk＝0.1I4×4；
观测噪声协方差矩阵rk的表达式为：
rk＝0.1I4×4
状态转移矩阵fk的表达式为：
Figure RE-GDA0003495509960000161
其中，t为当前时刻与上一时刻的时间差；
观测矩阵hk的表达式为：
hk＝I4×4；
步骤b：进行预测与更新，获取预测状态向量和预测状态协方差矩阵：
Figure RE-GDA0003495509960000162
其中，Xk+1|k为k+1时刻的预测状态向量，fk+1为k+1时刻的状态转移矩阵，Pk+1|k为k+1时刻的预测状态协方差矩阵，qk+1为k+1时刻的过程噪声协方差矩阵，T表示转置；
根据预测状态向量和预测状态协方差矩阵得到观测向量和观测空间协方差矩阵：
Figure RE-GDA0003495509960000163
其中，Yk+1为k+1时刻的观测向量，hk+1为k+1时刻的状态转移矩阵，Sk+1为k+1 时刻的观测空间协方差矩阵，rk+1为k+1时刻的观测噪声协方差矩阵；
对状态向量和状态协方差矩阵进行更新得到状态向量和状态协方差矩阵的后验估计值：
Figure RE-GDA0003495509960000164
Kk+1为k+1时刻的增益，Zk+1为k+1时刻的实际测量值，Xk+1为k+1时刻的状态向量的后验估计值，Pk+1为k+1时刻的状态协方差矩阵的后验估计值；
通过对毫米波雷达的数据进行有效目标提取，能够有效滤除干扰信号，但仍然存在无关信号，即存在非行人或者非车辆的信号，而且行人或者车辆有可能没有雷达信号，因此需要筛选出参与数据关联的摄像头数据和雷达数据，即对毫米波雷达的数据和摄像头数据进行匹配数据初选，如图5所示，一组经过空间融合和时间融合后的毫米波雷达数据的像素坐标为(xr_c,yr_c)，检测框的左上顶点坐标为 (xmin,ymin)，右下顶点为(xmax,ymax)，扩展检测框左上顶点的坐标为 (xmin-Δx,ymin-Δy)，右下顶点的坐标为(xmax+Δx,ymax+Δy)，毫米雷达波接收的回波信号是由目标的某一部分反射回来的，因此将雷达坐标投影到像素坐标下，该坐标在目标检测框内，由于标定过程存在误差且目标检测框与包围目标的最小矩形框也存在偏差，因此需要拓展检测框，可选择Δx＝2Δy＝20个像素，即在检测框的基础上沿u轴和v轴得到扩展检测框，通过扩大后的检测扩展框筛选匹配的毫米波雷达数据。
通过匹配数据初选后，得到M个摄像头数据和N个毫米波雷达的数据，设摄像头数据和毫米波雷达数据的表达式分别为：
[idc,score,xmin,ymin,xmax,ymax,class,xc_w,yc_w]M
[idr,xr,yr,vxr,vyr,xr_c,yr_c]N
其中，idc为摄像头检测到的目标的ID，score为置信度，xmin和ymin分别为检测框左上顶点的横坐标和纵坐标，xmax和ymax分别为检测框右下顶点的横坐标和纵坐标，class为类别，(xc_w,yc_w)为检测框下边界的中心点在世界坐标系下的坐标， M为摄像头检测到的目标的数量，即摄像头数据的数量，idr为毫米雷达波检测到的目标的ID，(xr,yr)为毫米雷达波检测到的目标在雷达坐标系下的雷达坐标，vxr和 vyr分别为毫米雷达波检测到的目标在雷达坐标系下的纵向速度和横向速度， (xr_c,yr_c)为毫米雷达波检测到的目标的像素坐标，M为摄像头检测到的目标的数量，即摄像头数据的数量，N为毫米雷达波检测到的目标的数量，即毫米雷达波数据的数量；
由于至少有一个毫米波雷达数据落在每个摄像头输出的检测扩展框，因此摄像头数据总是小于等于毫米波雷达的数据，即M≤N，如图5所示，一个检测扩展框可能包含多个雷达数据点，一个毫米波雷达数据可能落入多个检测扩展框内，因此需要进一步计算每个毫米波雷达数据属于每一个检测扩展框的代价，本发明采用像素坐标系下毫米波雷达的数据和摄像头数据之间的马氏距离以及世界坐标系下毫米波雷达的数据和摄像头数据之间的欧氏距离作为代价矩阵，并基于匈牙利算法获取全局代价最小的匹配方式：
1)计算像素坐标系下毫米波雷达的数据和摄像头数据之间的马氏距离：
对于摄像头检测到的第i个目标，若该目标为首次检测到的目标，则以检测框的中心对摄像头数据的像素坐标进行初始化，以单位阵I2×2对协方差矩阵进行初始化，摄像头数据的像素坐标和协方差矩阵的表达式为：
Figure RE-GDA0003495509960000181
其中，
Figure RE-GDA0003495509960000182
为检测框的中心坐标，di为摄像头检测到的第 i个目标对应的像素坐标，Si为摄像头检测到的第i个目标对应的协方差矩阵；
若该目标属于现存的融合数据的航迹，则计算观测变量和观测空间的协方差矩阵，由于不涉及速度信息，因此取观测变量的位置信息对应的像素坐标对像素坐标 di进行赋值，取观测空间协方差矩阵中的位置协方差对协方差矩阵Si赋值：
Figure RE-GDA0003495509960000183
Figure RE-GDA0003495509960000184
其中，Sk+1为k+1时刻的观测空间协方差矩阵，h为2×4的矩阵，T表示矩阵的转置；
根据di和Si计算像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离，马氏距离的计算公式为：
Figure RE-GDA0003495509960000185
dj＝[xr_c,yr_c]
其中，disimage为像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离， dj为毫米波雷达检测到的第j个目标对应的像素坐标；
2)计算像素坐标系下毫米波雷达的数据和摄像头数据之间的欧氏距离：
毫米波雷达检测到的第j个目标在世界坐标系下的坐标为(xr+lr,yr)，摄像头检测到的第i个目标在世界坐标系下的坐标为(xc_w,yc_w)，计算二者欧氏距离的公式为：
Figure RE-GDA0003495509960000186
其中，disworld为世界坐标系下毫米波雷达数据和摄像头数据之间的欧氏距离，(xr+lr,yr)为毫米波雷达检测到的第j个目标对应的世界坐标，(xc_w,yc_w)为摄像头检测到的第i个目标对应的世界坐标；
对得到的马氏距离和欧氏距离分别归一化，再进行加权求和作为第j个雷达数据属于第i个摄像头数据的代价，代价的表达式为：
Figure RE-GDA0003495509960000191
其中，task_matrixij为第j个雷达数据匹配第i个摄像头数据的代价，λ为马氏距离和欧氏距离之间的权重，取0.9；
获取全局代价矩阵，并根据全局代价矩阵基于匈牙利算法得到全局代价最优的匹配结果，对于某一个匹配结果，若对应的代价大于设定的代价阈值，则认为该匹配结果是错误的，并将该匹配结果去掉，例如对应的代价大于0.99995，则认为该匹配结果是错误的，并去掉该项匹配结果，因为存在摄像头检测到目标，毫米雷达波未检测到目标，又恰好有一个未被滤除的干扰信号落入该检测扩展框内的可能。
经过匹配数据初选和数据关联后进行融合与更新：
首先判断目标id是否属于现存航迹，若否，则对状态变量进行初始化，若是，即对于匹配到毫米雷达波数据的摄像头数据，实际测量值Zk+1为 [λ1(xr+lr)+(1-λ1)xc_w,λ2yr+(1-λ2)xc_w,vxr,vyr]T进行更新，得到后验估计值Xk+1；
若否，即对于没有匹配到毫米雷达波数据的摄像头数据，将[xc_w,yc_w,0,0]T作为实际测量值Zk+1进行更新，但由于摄像头距离估计误差较大，因此需要增大观测噪声协方差矩阵rk，取rk＝I4×4，得到后验估计值Xk+1。
3)融合与更新后得到目标最新的融合数据为：
[id,class,score,xmin,ymin,xmax,ymax,disx,disy,velx,vely]M
其中，disx、disy、velx和vely为基于卡尔曼滤波进行更新后的后验估计值，disx、disy、velx和vely分别表示融合目标在世界坐标系下的纵向距离、横向距离、纵向速度和横向速度；
4)采用age和moment管理融合数据的航迹的产生和消失，摄像头检测到一个目标后产生一条航迹，融合航迹基于摄像头的航迹进行初始化，此时将所有航迹的age设为1，将所有航迹的moment设为当前数据的时间戳，当下一周期接收的摄像头数据中出现新的摄像头的航迹时，在融合数据的航迹中添加新的摄像头的航迹，并将该航迹的age设为1，将该航迹的moment设为对应摄像头数据的时间戳，若摄像头的航迹与上一周期存在的航迹对应的id相同，则采用新的摄像头的航迹进行更新，age仍为1，moment更新为新的摄像头的航迹对应摄像头数据的时间戳，若上一周期存在的航迹在本周期内不存在对应的航迹，则将该航迹的age增加1， moment保持不变，一个航迹的age达到30后认为该航迹已离开场景，并将其从航迹集内删除。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，该方法包括以下步骤：
步骤1：通过毫米波雷达获取毫米波雷达数据，对其进行有效目标的提取，去除静止目标和随机噪声；
步骤2：通过设置在路侧立杆上的摄像头获取图像数据，基于YOLOv3和Deep SORT跟踪算法进行目标检测与多目标跟踪，得到摄像头数据；
步骤3：基于融合算法将毫米波雷达数据与摄像头数据进行多传感器融合，以实现路侧设备对目标的实时检测、跟踪与定位。
2.根据权利要求1所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤1中，进行有效目标的提取的过程具体包括以下步骤：
步骤101：通过设置在路侧立杆上的毫米波雷达获取毫米波雷达数据，毫米波雷达数据包括目标的ID、纵向距离、横向距离、纵向速度、横向速度和反射截面积；
步骤102：基于固定的路侧立杆获取设置的道路区域的道路位置信息，根据道路位置信息筛选得到位于道路区域内的目标；
步骤103：基于设定的速度阈值去除位于道路区域内的静止目标；
步骤104：设置目标在一段连续时间内存在的次数阈值，根据目标的ID索引目标在前4个扫描周期内存在的次数，去除存在的次数小于3次的索引结果，进而得到有效目标。
3.根据权利要求1所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤2中，基于YOLOv3和Deep SORT跟踪算法进行目标检测与跟踪的过程具体包括以下步骤：
步骤201：通过设置在路侧立杆上的摄像头获取图像数据；
步骤202：对图像数据进行标注并创建数据集，基于数据集对采用的YOLOv3模型进行训练，首先冻结主干提取网路，只训练预测网络，训练50个周期后，对整个模型进行训练，采用Early Stopping结束训练；
步骤203：将训练后的YOLOv3模型作为Deep SORT算法的检测器，进行多目标跟踪，最终的输出为[idc,score,xmin,ymin,xmax,ymax,class]m，即摄像头数据，idc为摄像头检测到的目标的ID，score为置信度，(xmin,ymin)为检测框的左上顶点，(xmax,ymax)为检测框的右下顶点，class为类别，m表示摄像头检测到的目标数量。
4.根据权利要求1所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤3中，基于融合算法将毫米波雷达数据与摄像头数据进行融合的过程具体包括以下步骤：
步骤301：对毫米波雷达数据和摄像头数据进行空间融合，将雷达坐标系下的点通过坐标系转换到摄像机对应的像素坐标系下，即可实现两者空间融合，具体为：
设Xw-Yw-Zw为世界坐标系，Ow为世界坐标系的原点，Xr-Yr-Zr为雷达坐标系，Or为雷达坐标系的原点，Xc-Yc-Zc为摄像头坐标系，Oc为摄像头坐标系的原点，平面Ow-Xw-Yw与地面重合，轴Ow-Zw沿着路侧立杆朝上，摄像头坐标系的原点Oc到世界坐标系的原点Ow的垂直高度为hc，摄像头坐标系的原点Oc到世界坐标系的原点Ow的纵向距离为lc，轴Oc-Zc与摄像头的光轴重合，轴Oc-Zc与轴Ow-Xw的夹角为θ，平面Oc-Yc-Zc与平面Ow-Xw-Zw重合，雷达坐标系的原点Or到世界坐标系的原点Ow的垂直高度为hr，雷达坐标系的原点Or到世界坐标系的原点Ow的纵向距离为lr，平面Or-Xr-Zr与平面Ow-Xw-Zw重合，轴Or-Xr与轴Ow-Xw平行，u-v为像素坐标系，将摄像头坐标系中的点(Xc,Yc,Zc)转换至像素坐标系下的坐标的计算公式为：
Figure FDA0003342617720000021
其中，fx和fy为相机的等效焦距，(u0,v0)为图片中心的像素坐标，fx、fy、u0和v0均为摄像头的内部参数，(Xc,Yc,Zc)为摄像头坐标系的三维坐标，(u,v)为像素坐标系的二维坐标；
根据雷达坐标系与摄像头坐标系的相对位置关系将雷达坐标系中的点(Xr,Yr,Zr)转换至像素坐标系下的坐标的计算公式为：
Figure FDA0003342617720000022
其中，(Xr,Yr,Zr)为雷达坐标系的三维坐标，hr为雷达坐标系的原点Or到世界坐标系的原点Ow的垂直高度，lr为雷达坐标系的原点Or到世界坐标系的原点Ow的纵向距离，hc为摄像头坐标系的原点Oc到世界坐标系的原点Ow的垂直高度，lc为摄像头坐标系的原点Oc到世界坐标系的原点Ow的纵向距离，θ为轴Oc-Zc与轴Ow-Xw的夹角；
将世界坐标系中的点(Xw,Yw,Zw)转换为像素坐标系下的坐标的计算公式为：
Figure FDA0003342617720000031
其中，(Xw,Yw,Zw)为世界坐标系的三维坐标；
步骤302：对毫米波雷达与摄像头的时间进行融合，将毫米波雷达数据和摄像头数据的时间戳对准，以进行时间融合，即根据摄像头数据的时间戳，采用自适应算法进行对准，完成对毫米波雷达数据和摄像头数据的时间融合，
步骤303：基于融合算法对毫米波雷达数据和摄像头数据进行融合。
5.根据权利要求4所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤303中，基于融合算法对毫米波雷达数据和摄像头数据进行融合的过程具体包括以下步骤：
步骤303A：通过坐标变换对毫米波雷达数据和摄像头数据进行数据扩展；
步骤303B：对毫米波雷达数据和摄像头数据进行匹配数据初选；
步骤303C：将进行匹配数据初选后的毫米波雷达数据和扩展后的摄像头数据进行数据关联，计算每个毫米波雷达的数据属于每一个检测框的代价，即采用像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离以及世界坐标系下毫米波雷达数据和摄像头数据之间的欧氏距离作为代价矩阵，并基于匈牙利算法获取全局代价最小的匹配方式；
步骤303D：以视觉跟踪结果为主，匹配毫米波雷达关于该目标的数据，即以摄像头数据为主，将摄像头数据与毫米波雷达数据进行匹配和融合，并基于卡尔曼滤波进行更新，得到融合数据的航迹。
6.根据权利要求5所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤303A中，进行数据扩展的过程具体为：
步骤303A1：对毫米波雷达数据进行有效目标的提取后的输出为：
[idr,xr,yr,vxr,vyr]n
其中，idr为毫米雷达波检测到的目标的ID，(xr,yr)为毫米雷达波检测到的目标在雷达坐标系下的雷达坐标，vxr和vyr分别为目标在雷达坐标系下的纵向速度和横向速度，n表示毫米波雷达检测到的目标数量；
步骤303A2：通过空间融合将雷达坐标(xr,yr)投影到像素坐标系下得到像素坐标(xr_c,yr_c)，进而将毫米波雷达数据扩展为：
[idr,xr,yr,vxr,vyr,xr_c,yr_c]n；
其中，(xr_c,yr_c)为雷达坐标(xr,yr)投影到像素坐标系下的像素坐标；
步骤303A3：对路侧的摄像头获取目标的图像数据进行目标检测与跟踪的输出为：
[idc,score,xmin,ymin,xmax,ymax,class]m
其中，idc为摄像头检测到的目标的ID，score为置信度，xmin和ymin分别为检测框的左上顶点的横坐标和纵坐标，xmax和ymax分别为检测框的右下顶点的横坐标和纵坐标，class为类别，m表示摄像头检测到的目标数量；
步骤303A4：以检测框下边界的中心点((xmin+xmax)/2,ymax)作为参考点，该参考点对应的世界坐标系下的坐标在Zw＝0的平面上，该点对应的世界坐标系下的坐标的计算公式为：
Figure FDA0003342617720000041
其中，(Xw,Yw,Zw)为世界坐标系的三维坐标，(xc_w,yc_w)为摄像头检测到的目标在世界坐标系下的坐标；
步骤303A5：进而将摄像头数据扩展为：
[idc,score,xmin,ymin,xmax,ymax,class,xc_w,yc_w]m。
7.根据权利要求5所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤303B中，对毫米波雷达数据和摄像头数据进行匹配数据初选的过程具体为：
一组经过空间融合和时间融合后的毫米波雷达数据的像素坐标为(xr_c,yr_c)，检测框的左上顶点坐标为(xmin,ymin)，右下顶点为(xmax,ymax)，扩展检测框左上顶点的坐标为(xmin-Δx,ymin-Δy)，右下顶点的坐标为(xmax+Δx,ymax+Δy)，即在检测框的基础上沿u轴和v轴得到扩展检测框，通过扩大后的检测扩展框筛选匹配的毫米波雷达数据，筛选后的摄像头数据中剩下M个目标，毫米波数据中剩下N个目标，即摄像头数据的数量为M，毫米波雷达数据的数量为N。
8.根据权利要求5所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤303C中，进行数据关联的过程具体包括以下步骤：
步骤303C1：至少有一个毫米波雷达数据落在每个摄像头输出的检测框内，即摄像头数据的数量M小于等于毫米波雷达数据的数量N，设摄像头数据和毫米波雷达数据的表达式分别为：
[idc,score,xmin,ymin,xmax,ymax,class,xc_w,yc_w]M
[idr,xr,yr,vxr,vyr,xr_c,yr_c]N
其中，idc为摄像头检测到的目标的ID，score为置信度，xmin和ymin分别为检测框左上顶点的横坐标和纵坐标，xmax和ymax分别为检测框右下顶点的横坐标和纵坐标，class为类别，(xc_w,yc_w)为检测框下边界的中心点在世界坐标系下的坐标，M为摄像头检测到的目标的数量，即摄像头数据的数量，idr为毫米雷达波检测到的目标的ID，(xr,yr)为毫米雷达波检测到的目标在雷达坐标系下的雷达坐标，vxr和vyr分别为毫米雷达波检测到的目标在雷达坐标系下的纵向速度和横向速度，(xr_c,yr_c)为毫米雷达波检测到的目标的像素坐标，M为摄像头检测到的目标的数量，即摄像头数据的数量，N为毫米雷达波检测到的目标的数量，即毫米雷达波数据的数量；
步骤303C2：计算像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离：
对于摄像头检测到的第i个目标，若该目标为首次检测到的目标，则以检测框的中心对摄像头数据的像素坐标进行初始化，以单位阵I2×2对协方差矩阵进行初始化，摄像头数据的像素坐标和协方差矩阵的表达式为：
Figure FDA0003342617720000051
其中，
Figure FDA0003342617720000052
为检测框的中心坐标，di为摄像头检测到的第i个目标对应的像素坐标，Si为摄像头检测到的第i个目标对应的协方差矩阵；
若该目标属于现存的融合数据的航迹，则计算观测变量和观测空间的协方差矩阵，由于不涉及速度信息，因此取观测变量的位置信息对应的像素坐标(xw_c,yw_c)对像素坐标di进行赋值，取观测空间协方差矩阵中的位置协方差对协方差矩阵Si赋值：
Figure FDA0003342617720000061
Figure FDA0003342617720000062
其中，Sk+1为k+1时刻的观测空间协方差矩阵，h为2×4的矩阵，T表示矩阵的转置；
根据像素坐标di和协方差矩阵Si计算像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离，马氏距离的计算公式为：
Figure FDA0003342617720000063
dj＝[xr_c,yr_c]
其中，disimage为像素坐标系下毫米波雷达数据和摄像头数据之间的马氏距离，dj为毫米波雷达检测到的第j个目标对应的像素坐标；
步骤303C3：计算世界坐标系下毫米波雷达数据和摄像头数据之间的欧氏距离，欧氏距离的计算公式为：
Figure FDA0003342617720000064
其中，disworld为世界坐标系下毫米波雷达数据和摄像头数据之间的欧氏距离，(xr+lr,yr)为毫米波雷达检测到的第j个目标对应的世界坐标，(xc_w,yc_w)为摄像头检测到的第i个目标对应的世界坐标；
步骤303C4：对得到的马氏距离和欧氏距离分别归一化，再进行加权求和，得到第j个毫米波雷达数据匹配第i个摄像头数据的代价，代价的计算公式为：
Figure FDA0003342617720000065
其中，task_matrixij为第j个雷达数据匹配第i个摄像头数据的代价，λ为马氏距离和欧氏距离之间的权重；
步骤303C5：获取全局代价矩阵，并根据全局代价矩阵基于匈牙利算法得到全局代价最优的匹配结果，对于某一个匹配结果，若对应的代价大于设定的代价阈值，则认为该匹配结果是错误的，并将该匹配结果去掉。
9.根据权利要求5所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤303D中，若目标的id不属于现存的融合数据的航迹，基于卡尔曼滤波进行更新的过程具体包括以下步骤：
步骤303D1：目标的id不属于现存的融合数据的航迹，即摄像头数据没有匹配的毫米雷达波数据，对状态变量进行初始化，则状态变量和状态协方差矩阵的表达式分别为：
Figure FDA0003342617720000071
其中，I4×4为4×4的单位阵；
过程噪声协方差矩阵qk的表达式为：
qk＝0.1I4×4；
观测噪声协方差矩阵rk的表达式为：
rk＝0.1I4×4
状态转移矩阵fk的表达式为：
Figure FDA0003342617720000072
其中，t为当前时刻与上一时刻的时间差；
观测矩阵hk的表达式为：
hk＝I4×4；
步骤303D2：进行预测与更新，获取预测状态向量和预测状态协方差矩阵：
Figure FDA0003342617720000073
其中，Xk+1|k为k+1时刻的预测状态向量，fk+1为k+1时刻的状态转移矩阵，Pk+1|k为k+1时刻的预测状态协方差矩阵，qk+1为k+1时刻的过程噪声协方差矩阵，T表示转置；
根据预测状态向量和预测状态协方差矩阵得到观测向量和观测空间协方差矩阵：
Figure FDA0003342617720000081
其中，Yk+1为k+1时刻的观测向量，hk+1为k+1时刻的状态转移矩阵，Sk+1为k+1时刻的观测空间协方差矩阵，rk+1为k+1时刻的观测噪声协方差矩阵；
k+1时刻的实际测量值Zk+1和观测噪声协方差矩阵rk+1的表达式为：
Zk+1＝[xc_w,yc_w,0,0]T
rk+1＝I4×4
对状态向量和状态协方差矩阵进行更新得到状态向量和状态协方差矩阵的后验估计值：
Figure FDA0003342617720000082
Kk+1为k+1时刻的增益，Zk+1为k+1时刻的实际测量值，Xk+1为k+1时刻的状态向量的后验估计值，Pk+1为k+1时刻的状态协方差矩阵的后验估计值；
若目标的id属于现存的融合数据的航迹，基于卡尔曼滤波进行更新的过程具体包括以下步骤：
步骤303D3：目标的id属于现存的融合数据的航迹，即摄像头数据有匹配的毫米雷达波数据，则状态变量、状态协方差矩阵和观测变量的表达式分别为：
Figure FDA0003342617720000083
其中，xr+lr为毫米雷达波数据在世界坐标系下的纵向坐标，λ1和λ2为权重，Xk为状态变量，dis和vel分别为目标在世界坐标系下的位置和速度，disx、disy、velx和vely分别为目标在世界坐标系下的纵向距离、横向距离、纵向速度和横向速度，Yk为观测变量，I为单位阵；
权重λ1的表达式为：
Figure FDA0003342617720000084
其中，xr为毫米雷达波检测到的目标在雷达坐标系下的纵向坐标；
步骤303D4：进行预测与更新，获取预测状态向量和预测状态协方差矩阵：
Figure FDA0003342617720000091
其中，Xk+1|k为k+1时刻的预测状态向量，fk+1为k+1时刻的状态转移矩阵，Pk+1|k为k+1时刻的预测状态协方差矩阵，qk+1为k+1时刻的过程噪声协方差矩阵，T表示转置；
根据预测状态向量和预测状态协方差矩阵得到观测向量和观测空间协方差矩阵：
Figure FDA0003342617720000092
其中，Yk+1为k+1时刻的观测向量，hk+1为k+1时刻的状态转移矩阵，Sk+1为k+1时刻的观测空间协方差矩阵，rk+1为k+1时刻的观测噪声协方差矩阵；
k+1时刻的实际测量值Zk+1的表达式为：
Zk+1＝[λ1(xr+lr)+(1-λ1)xc_w,λ2yr+(1-λ2)xc_w,vxr,vyr]T
对状态向量和状态协方差矩阵进行更新得到状态向量和状态协方差矩阵的后验估计值：
Figure FDA0003342617720000093
Kk+1为k+1时刻的增益，Zk+1为k+1时刻的实际测量值，Xk+1为k+1时刻的状态向量的后验估计值，Pk+1为k+1时刻的状态协方差矩阵的后验估计值；
步骤303D5：得到目标最新的融合数据为：
[id,class,score,xmin,ymin,xmax,ymax,disx,disy,velx,vely]M
其中，id为融合目标的ID，disx、disy、vel和vely为基于卡尔曼滤波进行更新后的后验估计值，disx、disy、vel和vely分别表示融合目标在世界坐标系下的纵向距离、横向距离、纵向速度和横向速度；
步骤303D6：获取融合数据的航迹，采用age和moment管理融合数据的航迹的产生和消失。
10.根据权利要求9所述的一种融合毫米波雷达与摄像头的路侧感知方法，其特征在于，所述的步骤303D6中，采用age和moment管理融合数据的航迹的产生和消失具体为：
摄像头检测到一个目标后产生一条航迹，融合航迹基于摄像头的航迹进行初始化，此时将所有航迹的age设为1，将所有航迹的moment设为当前数据的时间戳，当下一周期接收的摄像头数据中出现新的摄像头的航迹时，在融合数据的航迹中添加新的摄像头的航迹，并将该航迹的age设为1，将该航迹的moment设为对应摄像头数据的时间戳，若摄像头的航迹与上一周期存在的航迹对应的id相同，则采用新的摄像头的航迹进行更新，age仍为1，moment更新为新的摄像头的航迹对应摄像头数据的时间戳，若上一周期存在的航迹在本周期内不存在对应的航迹，则将该航迹的age增加1，moment保持不变，一个航迹的age达到设定的阈值后认为该航迹已离开场景，并将其从航迹集内删除。
CN202111313286.5A 2021-11-08 2021-11-08 一种融合毫米波雷达与摄像头的路侧感知方法 Pending CN114280611A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111313286.5A CN114280611A (zh) 2021-11-08 2021-11-08 一种融合毫米波雷达与摄像头的路侧感知方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111313286.5A CN114280611A (zh) 2021-11-08 2021-11-08 一种融合毫米波雷达与摄像头的路侧感知方法

Publications (1)

Publication Number Publication Date
CN114280611A true CN114280611A (zh) 2022-04-05

Family

ID=80868818

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111313286.5A Pending CN114280611A (zh) 2021-11-08 2021-11-08 一种融合毫米波雷达与摄像头的路侧感知方法

Country Status (1)

Country Link
CN (1) CN114280611A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114972434A (zh) * 2022-06-06 2022-08-30 电子科技大学 一种级联检测和匹配的端到端多目标跟踪系统
CN115236656A (zh) * 2022-09-22 2022-10-25 中国电子科技集团公司第十研究所 用于飞机避障的多源传感器目标关联方法、设备及介质
CN116363623A (zh) * 2023-01-28 2023-06-30 苏州飞搜科技有限公司 基于毫米波雷达与视觉融合的车辆检测方法
CN116758110A (zh) * 2023-08-15 2023-09-15 中国科学技术大学 复杂运动场景下的鲁棒多目标跟踪方法

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114972434A (zh) * 2022-06-06 2022-08-30 电子科技大学 一种级联检测和匹配的端到端多目标跟踪系统
CN115236656A (zh) * 2022-09-22 2022-10-25 中国电子科技集团公司第十研究所 用于飞机避障的多源传感器目标关联方法、设备及介质
CN115236656B (zh) * 2022-09-22 2022-12-06 中国电子科技集团公司第十研究所 用于飞机避障的多源传感器目标关联方法、设备及介质
CN116363623A (zh) * 2023-01-28 2023-06-30 苏州飞搜科技有限公司 基于毫米波雷达与视觉融合的车辆检测方法
CN116363623B (zh) * 2023-01-28 2023-10-20 苏州飞搜科技有限公司 基于毫米波雷达与视觉融合的车辆检测方法
CN116758110A (zh) * 2023-08-15 2023-09-15 中国科学技术大学 复杂运动场景下的鲁棒多目标跟踪方法
CN116758110B (zh) * 2023-08-15 2023-11-17 中国科学技术大学 复杂运动场景下的鲁棒多目标跟踪方法

Similar Documents

Publication Publication Date Title
Lim et al. Radar and camera early fusion for vehicle detection in advanced driver assistance systems
CN109459750B (zh) 一种毫米波雷达与深度学习视觉融合的前方多车辆跟踪方法
CN110988912B (zh) 自动驾驶车辆的道路目标与距离检测方法、系统、装置
CN110032949B (zh) 一种基于轻量化卷积神经网络的目标检测与定位方法
CN110675418B (zh) 一种基于ds证据理论的目标轨迹优化方法
TWI722355B (zh) 用於基於障礙物檢測校正高清晰度地圖的系統和方法
CN112149550B (zh) 一种基于多传感器融合的自动驾驶车辆3d目标检测方法
CN107330925B (zh) 一种基于激光雷达深度图像的多障碍物检测和跟踪方法
CN114280611A (zh) 一种融合毫米波雷达与摄像头的路侧感知方法
US9429650B2 (en) Fusion of obstacle detection using radar and camera
CN113269098A (zh) 一种基于无人机的多目标跟踪定位与运动状态估计方法
CN115113206B (zh) 井下有轨车辅助驾驶的行人与障碍物检测方法
CN114495064A (zh) 一种基于单目深度估计的车辆周围障碍物预警方法
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
CN112541938A (zh) 一种行人速度测量方法、系统、介质及计算设备
CN113848545A (zh) 一种基于视觉和毫米波雷达的融合目标检测和跟踪方法
CN111123262A (zh) 自动驾驶3d建模方法、装置及系统
CN114693909A (zh) 一种基于多传感器机器视觉融合的微观车辆轨迹感知设备
CN110824495B (zh) 基于激光雷达的果蝇视觉启发的三维运动目标检测方法
Yu et al. Camera-radar data fusion for target detection via Kalman filter and Bayesian estimation
CN116978009A (zh) 基于4d毫米波雷达的动态物体滤除方法
Hussain et al. Multiple objects tracking using radar for autonomous driving
CN115144828A (zh) 一种智能汽车多传感器时空融合的自动在线标定方法
Jibrin et al. An object detection and classification method using radar and camera data fusion
CN117606497A (zh) 一种实时地图构建方法、系统及导盲眼镜

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination