CN114966736A - 一种基于点云数据进行目标速度预测的处理方法 - Google Patents
一种基于点云数据进行目标速度预测的处理方法 Download PDFInfo
- Publication number
- CN114966736A CN114966736A CN202210581422.7A CN202210581422A CN114966736A CN 114966736 A CN114966736 A CN 114966736A CN 202210581422 A CN202210581422 A CN 202210581422A CN 114966736 A CN114966736 A CN 114966736A
- Authority
- CN
- China
- Prior art keywords
- target
- frame
- point
- point cloud
- current
- 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
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/87—Combinations of systems using electromagnetic waves other than radio waves
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
- G06V10/80—Fusion, i.e. combining data from various sources at the sensor level, preprocessing level, feature extraction level or classification level
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/60—Type of objects
- G06V20/64—Three-dimensional objects
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V2201/00—Indexing scheme relating to image or video recognition or understanding
- G06V2201/07—Target detection
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Remote Sensing (AREA)
- Multimedia (AREA)
- Radar, Positioning & Navigation (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Computing Systems (AREA)
- Artificial Intelligence (AREA)
- Databases & Information Systems (AREA)
- Evolutionary Computation (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- Image Analysis (AREA)
Abstract
本发明实施例涉及一种基于点云数据进行目标速度预测的处理方法,所述方法包括:获取相邻时刻的前后两帧激光雷达点云数据作为对应的前帧点云和后帧点云;基于点云目标检测模型分别对前帧、后帧点云进行点云目标检测处理得到多个第一目标检测框和多个第二目标检测框;对前、后帧点云的第一、第二目标检测框进行目标关联处理;根据每对关联的第一、第二目标检测框预测对应目标的速度。通过本发明,可以克服传统点云目标检测模型不能进行速度检测的缺陷。
Description
技术领域
本发明涉及数据处理技术领域,特别涉及一种基于点云数据进行目标速度预测的处理方法。
背景技术
自动驾驶感知系统的激光雷达通过对自车周围环境的进行激光扫描可以得到对应的点云数据,感知系统使用点云目标检测模型对原始点云数据进行检测可以识别出多类障碍物目标(诸如人、车辆、自行车、植物、动物、交通标志物等)以及各类目标对应的三维目标检测框(bounding box),然而传统的点云目标检测模型无法确认各类目标的速度。为此,感知系统还需另行配置用于测速的毫米波雷达与之配合,通过数据融合模块将毫米波雷达点云中的速度特征增加到激光雷达点云中去。
发明内容
本发明的目的,就是针对现有技术的缺陷,提供一种基于点云数据进行目标速度预测的处理方法、电子设备及计算机可读存储介质,先对相邻时刻的两帧激光雷达点云分别进行目标识别,再对两帧点云上的目标识别框进行关联从而得到多对关联的目标识别框,再基于迭代最近点(Iterative Closest Point,ICP)算法对每对关联目标识别框中的点云进行配准从而得到前后时刻的位姿变换矩阵T,并基于位姿变换矩阵T对前一时刻目标识别框上的指定位置点在下一时刻的位姿进行预测,并根据该点前一时刻的位姿和下一时刻的预测位姿算出对应目标的速度。通过本发明,对传统点云目标检测模型的输出进行持续关联和点云位姿配准就能获得点云中各个目标的速度,不但可以克服传统点云目标检测模型不能进行速度检测的缺陷,还可以降低感知系统对毫米波雷达的依赖,减少数据融合模块的点云融合计算量,提高感知系统的整体工作效率。
为实现上述目的,本发明实施例第一方面提供了一种基于点云数据进行目标速度预测的处理方法,所述方法包括:
获取相邻时刻的前后两帧激光雷达点云数据作为对应的前帧点云和后帧点云;
基于点云目标检测模型分别对所述前帧、后帧点云进行点云目标检测处理得到多个第一目标检测框和多个第二目标检测框;所述第一目标检测框对应所述前帧点云,所述第二目标检测框对应所述后帧点云;
对所述前、后帧点云的所述第一、第二目标检测框进行目标关联处理;
根据每对关联的所述第一、第二目标检测框预测对应目标的速度。
优选的,所述点云目标检测模型包括VoxelNet模型、SECOND模型和PointPillars模型,默认使用所述PointPillars模型;
各个所述第一目标检测框对应一组第一检测框参数;所述第一检测框参数包括第一目标框中心点坐标(x1,y1,z1)、第一目标框深度l1、第一目标框宽度w1、第一目标框高度h1和第一目标框朝向角yaw1;
各个所述第二目标检测框对应一组第二检测框参数;所述第二检测框参数包括第二目标框中心点坐标(x2,y2,z2)、第二目标框深度l2、第二目标框宽度w2、第二目标框高度h2和第二目标框朝向角yaw2。
优选的,所述对所述前、后帧点云的所述第一、第二目标检测框进行目标关联处理,具体包括:
统计所述第一检测框参数的数量记为第一数量m,并统计所述第二检测框参数的数量记为第二数量n;
基于目标框索引i对各个所述第一目标框中心点坐标进行顺序编码得到对应的第一目标框中心点坐标(x1,i,y1,i,z1,i),1≤i≤m;基于目标框索引j对各个所述第二目标框中心点坐标进行顺序编码得到对应的第二目标框中心点坐标(x2,j,y2,j,z2,j),1≤j≤n;
由m*n个所述中心点间距si,j构建一个形状为m*n的矩阵向量记为第一矩阵向量;并将所述第一矩阵向量输入深度匈牙利网络DHN基于改进的匈牙利算法进行匹配度计算得到对应的形状为m*n的关联矩阵向量A;所述关联矩阵向量A包括m*n个匹配度ai,j,0<ai,j≤1;每个所述匹配度ai,j基于所述目标框索引i和所述目标框索引j对应一对所述第一目标检测框和所述第二目标检测框;
将同一个所述目标框索引i对应的所述第二数量n的所述匹配度ai,j归入同一个匹配度集合Di;并提取各个所述匹配度集合Di中取值最大的所述匹配度ai,j作为对应的最大匹配度bi;
对各个所述最大匹配度bi进行判断;若当前最大匹配度bi小于预设的最小匹配度阈值amin,则确认所述当前最大匹配度bi对应的一对所述第一目标检测框和所述第二目标检测框之间没有关联关系;若所述当前最大匹配度bi大于或等于所述最小匹配度阈值amin,则在所述当前最大匹配度bi对应的一对所述第一目标检测框和所述第二目标检测框之间创建关联关系。
优选的,所述根据每对关联的所述第一、第二目标检测框预测对应目标的速度,具体包括:
将每对关联的所述第一、第二目标检测框记为对应的第一关联框和第二关联框;
从所述前帧点云中提取所述第一关联框内的点云作为第一点云;并从所述后帧点云中提取所述第二关联框内的点云作为第二点云;
将所述第一点云对应的所述第一关联框上指定位置点对应的点云坐标记为第一点坐标(x1,c,y1,c,z1,c);并基于所述位姿变换矩阵T对所述第一点坐标(x1,c,y1,c,z1,c)进行刚体位姿变换得到对应的第二点坐标(x2,c,y2,c,z2,c);
将所述第一、第二关联框的对应目标记为当前目标;并根据所述第一、第二点坐标以及前后时刻的时间差估算出所述当前目标的速度向量V。
进一步的,所述基于迭代最近点ICP算法对所述第一、第二点云进行点云位姿配准得到对应的位姿变换矩阵T,具体包括:
步骤51,将所述第一点云作为当前源点云、将所述第二点云作为当前目标点云;并初始化迭代计数器的值为1;并将预设的初始化旋转矩阵R0、初始化平移向量t0作为对应的前次旋转矩阵和前次平移向量;
步骤52,基于预设的最小距离阈值dmin确定所述当前源点云和所述当前目标点云中的关联点对集合;所述关联点对集合包括多个关联点对Gk,k为关联点对索引,1≤k;所述关联点对Gk包括一个当前源点云中的点pk和一个当前目标点云中的点qk,且所述点pk与所述点qk的距离小于或等于所述最小距离阈值dmin;
步骤53,统计所述关联点对集合中所述关联点对Gk的数量记为第三数量H;
步骤54,基于欧式距离变换构建对应的目标函数f(R’,t’):
R’为旋转矩阵变量,t’为平移向量变量,ck为所述点qk的法向量;
步骤55,对使所述目标函数f(R’,t’)达到最小值的两个变量R’、t’进行求解,并将求解结果作为对应的当前旋转矩阵R*和当前平移向量t*;
步骤56,对所述当前旋转矩阵R*与所述前次旋转矩阵的误差以及所述当前平移向量t*与所述前次平移向量的误差进行计算得到对应的旋转矩阵误差和平移向量误差;若所述旋转矩阵误差和所述平移向量误差不都满足各自的预设合理误差范围则转至步骤57;若所述旋转矩阵误差和所述平移向量误差都满足各自的预设合理误差范围则转至步骤59;
步骤57,对所述迭代计数器的值加1;若加1后所述迭代计数器的值未超过预设的最大迭代次数则转至步骤58;若加1后所述迭代计数器的值超过了所述最大迭代次数则转至步骤59;
步骤58,由所述当前旋转矩阵R*和所述当前平移向量t*构成对应的当前位姿变换矩阵T*,并基于所述当前位姿变换矩阵T*对所述当前源点云中各点进行位姿变换,并将变换后得到的点云作为新的所述当前源点云;并将所述当前旋转矩阵R*作为新的所述前次旋转矩阵,将所述当前平移向量t*作为新的所述前次平移向量;并转至步骤52根据新的所述当前源点云、所述前次旋转矩阵和所述前次旋转矩阵继续进行迭代运算;
进一步的,所述根据所述第一、第二点坐标以及前后时刻的时间差估算出所述当前目标的速度向量V,具体包括:
将前后时刻的时间差记为时间差δ;并根据所述第一点坐标(x1,c,y1,c,z1,c)、所述第二点坐标(x2,c,y2,c,z2,c)和所述时间差δ对所述当前目标的三个速度分量进行估算得到对应的vx、vy和vz;并由三个速度分量vx、vy和vz构成所述当前目标的所述速度向量V;vx=(x2,c-x1,c)/δ,vy=(y2,c-y1,c)/δ,vz=(z2,c-z1,c)/δ。
本发明实施例第二方面提供了一种电子设备,包括:存储器、处理器和收发器;
所述处理器用于与所述存储器耦合,读取并执行所述存储器中的指令,以实现上述第一方面所述的方法步骤;
所述收发器与所述处理器耦合,由所述处理器控制所述收发器进行消息收发。
本发明实施例第三方面提供了一种计算机可读存储介质,所述计算机可读存储介质存储有计算机指令,当所述计算机指令被计算机执行时,使得所述计算机执行上述第一方面所述的方法的指令。
本发明实施例提供了一种基于点云数据进行目标速度预测的处理方法、电子设备及计算机可读存储介质,先对相邻时刻的两帧激光雷达点云分别进行目标识别,再对两帧点云上的目标识别框进行关联从而得到多对关联的目标识别框,再基于I CP算法对每对关联目标识别框中的点云进行配准从而得到前后时刻的位姿变换矩阵T,并基于位姿变换矩阵T对前一时刻目标识别框上的指定位置点在下一时刻的位姿进行预测,并根据该点前一时刻的位姿和下一时刻的预测位姿算出对应目标的速度。通过本发明,对传统点云目标检测模型的输出进行持续关联和点云位姿配准就能获得点云中各个目标的速度,不但克服了传统点云目标检测模型不能进行速度检测的缺陷,还降低了感知系统对毫米波雷达的依赖,减少了数据融合模块的点云融合计算量,提高了感知系统的整体工作效率。
附图说明
图1为本发明实施例一提供的一种基于点云数据进行目标速度预测的处理方法示意图;
图2为本发明实施例二提供的一种电子设备的结构示意图。
具体实施方式
为了使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明作进一步地详细描述,显然,所描述的实施例仅仅是本发明一部份实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
本发明实施例一提供一种基于点云数据进行目标速度预测的处理方法,如图1为本发明实施例一提供的一种基于点云数据进行目标速度预测的处理方法示意图所示,本方法主要包括如下步骤:
步骤1,获取相邻时刻的前后两帧激光雷达点云数据作为对应的前帧点云和后帧点云。
这里,设当前时刻为t,前一时刻为t-1,则前帧点云为一个或一组激光雷达在时刻t-1时对自车环境按设定的扫描范围进行扫描得到的激光雷达点云数据,后帧点云即是由同一个或同一组激光雷达在时刻t时对自车环境按相同的扫描范围进行扫描得到的激光雷达点云数据。
步骤2,基于点云目标检测模型分别对前帧、后帧点云进行点云目标检测处理得到多个第一目标检测框和多个第二目标检测框;
其中,点云目标检测模型包括VoxelNet模型、SECOND模型和PointPillars模型,默认使用PointPillars模型;第一目标检测框对应前帧点云,第二目标检测框对应后帧点云;各个第一目标检测框对应一组第一检测框参数;第一检测框参数包括第一目标框中心点坐标(x1,y1,z1)、第一目标框深度l1、第一目标框宽度w1、第一目标框高度h1和第一目标框朝向角yaw1;各个第二目标检测框对应一组第二检测框参数;第二检测框参数包括第二目标框中心点坐标(x2,y2,z2)、第二目标框深度l2、第二目标框宽度w2、第二目标框高度h2和第二目标框朝向角yaw2。
这里,本发明实施例可使用多种点云目标检测模型完成对前、后帧点云的目标检测,其中包括VoxelNet模型、SECOND模型和PointPillars模型,这三个模型的计算效能逐个增强,所以默认采用计算效能最高的PointPillars模型,关于VoxelNet模型、SECOND模型和PointPillars模型的具体实现可以分别参考对应的技术论文《VoxelNet:End-to-EndLearning for Point Cloud Based 3D Object Detection》、《SECOND:Sparsely EmbeddedConvolutional Detection》、《PointPillars:Fast Encoders for Object Detectionfrom Point Clouds》,在此不做进一步赘述。需要说明的是,基于每种点云目标检测模型都可以对识别出的各类目标输出一个对应的目标检测框(bounding box)也就是第一、第二目标检测框,每个目标检测框对应一组由目标框中心点坐标、目标框深度l、目标框宽度w、标框高度h和目标框朝向角yaw构成的目标检测框参数也就是第一、第二检测框参数;此处,第一目标检测框就是前一时刻t-1对应的前帧点云上检测出的各类目标的目标检测框,第二目标检测框就是当前时刻t对应的后帧点云上检测出的各类目标的目标检测框。
步骤3,对前、后帧点云的第一、第二目标检测框进行目标关联处理;
这里,对前、后帧点云的第一、第二目标检测框进行目标关联处理实际就是将前后两帧点云中属于相同目标的两个目标检测框进行关联;
具体包括:步骤31,统计第一检测框参数的数量记为第一数量m,并统计第二检测框参数的数量记为第二数量n;
这里,第一数量m实际就是前帧点云被检测出的目标总数,第二数量n实际就是后帧点云被检测出的目标总数,理想状态下m=n;但在现实情况中,经常出现二者不相等的情况,例如,前一时刻自车周围只有两辆车即对应的m=2,当前时刻突然从另外道路上快速并入了第三辆车则对应的n=3;又例如,前一时刻自车周围有三辆车即对应的m=3但其中一辆发生了紧急制动,那么当前时刻自车周围则可能就剩下了两辆车则对应的n=2;
步骤32,基于目标框索引i对各个第一目标框中心点坐标进行顺序编码得到对应的第一目标框中心点坐标(x1,i,y1,i,z1,i),1≤i≤m;基于目标框索引j对各个第二目标框中心点坐标进行顺序编码得到对应的第二目标框中心点坐标(x2,j,y2,j,z2,j),1≤j≤n;
例如,前帧点云被检测出有4个目标,分别对应第一目标检测框1-4,则m=4,对应的4个第一目标框中心点坐标分别为(x1,i=1,y1,i=1,z1,i=1)、(x1,i=2,y1,i=2,z1,i=2)、(x1,i=3,y1,i=3,z1,i=3)、(x1,i=4,y1,i=4,z1,i=4);后帧点云被检测出有3个目标,分别对应第二目标检测框1-3,则n=3,对应的3个第二目标框中心点坐标分别为(x2,j=1,y2,j=1,z2,j=1)、(x2,j=2,y2,j=2,z2,j=2)、(x2,j=3,y2,j=3,z2,j=3);
这里,卡尔曼滤波器的原理可参考公知的卡尔曼滤波器算法,在此不做进一步赘述;本发明实施例预先为各类目标(诸如人、车辆、自行车、植物、动物、交通标志物等)指定一种对应的物体运动模型(诸如静止模型、匀速运动模型、匀加速运动模型等),再基于该指定的物体运动模型对卡尔曼滤波器的状态方程进行配置,之后就能使用配置后的状态方程对各类目标下一时刻的位置进行预测;此处,本发明实施例默认各个目标在静止或运动时都不会发生形变也就是满足刚性运动形态,在刚性运动形态下单个目标上所有点的运动状态都可视为一致,也即是说前后时刻所有点位置的变化量都是一致的,所以此处只选择各个目标检测框上的一点即目标框中心点进行预测;选择前一时刻t-1的第一目标框中心点坐标(x1,i,y1,i,z1,i)进行预测得到的就是当前时刻t的中心点坐标理想值;
例如,前帧点云中获得了4个第一目标框中心点坐标:(x1,i=1,y1,i=1,z1,i=1)、(x1,i=2,y1,i=2,z1,i=2)、(x1,i=3,y1,i=3,z1,i=3)、(x1,i=4,y1,i=4,z1,i=4);那么,通过当前步骤就可得到4个理想的预测目标框中心点坐标分别为:
例如,已知有3个第二目标框中心点坐标分别为(x2,j=1,y2,j=1,z2,j=1)、(x2,j=2,y2,j=2,z2,j=2)、(x2,j=3,y2,j=3,z2,j=3),有4个预测目标框中心点坐标分别为:
那么,就会得到4*3=12个中心点间距si,j,分别是:
si=1,j=1、si=1,j=2、si=1,j=3,
si=2,j=1、si=2,j=2、si=2,j=3,
si=3,j=1、si=3,j=2、si=3,j=3,
si=4,j=1、si=4,j=2、si=4,j=3;
步骤35,由m*n个中心点间距si,j构建一个形状为m*n的矩阵向量记为第一矩阵向量;并将第一矩阵向量输入深度匈牙利网络(Deep Hungarian Net,DHN)基于改进的匈牙利算法进行匹配度计算得到对应的形状为m*n的关联矩阵向量A;
其中,关联矩阵向量A包括m*n个匹配度ai,j,0<ai,j≤1;每个匹配度ai,j基于目标框索引i和目标框索引j对应一对第一目标检测框和第二目标检测框;
这里,匈牙利算法是一种公知的用于计算匹配度的算法,深度匈牙利网络(DeepHungarian Net,DHN)则是基于改进的匈牙利算法构建的用于对形状为m*n输入矩阵进行匹配度计算并对等输出相同形状m*n关联矩阵向量A的神经网络,其具体模型实现可参考论文《How To Train Your Deep Multi-Object Tracker》,在此不做一一赘述;需要说明的是,由论文实现可知关联矩阵向量A的结构与输入矩阵也就是第一矩阵向量的形状相同且呈对应关系,关联矩阵向量A由m*n个取值在0-1之间的匹配度ai,j构成,每个匹配度ai,j用于标称对应的第一目标检测框i和第二目标检测框j的匹配度;
步骤36,将同一个目标框索引i对应的第二数量n的匹配度ai,j归入同一个匹配度集合Di;并提取各个匹配度集合Di中取值最大的匹配度ai,j作为对应的最大匹配度bi;
对应目标框索引i=1的匹配度集合Di=1应为{a1,1,a1,2,a1,3},最大匹配度bi=1也就是{a1,1,a1,2,a1,3}中的最大值;
对应目标框索引i=2的匹配度集合Di=2应为{a2,1,a2,2,a2,3},最大匹配度bi=2也就是{a2,1,a2,2,a2,3}中的最大值;
对应目标框索引i=3的匹配度集合Di=3应为{a3,1,a3,2,a3,3},最大匹配度bi=3也就是{a3,1,a3,2,a3,3}中的最大值;
对应目标框索引i=4的匹配度集合Di=4应为{a4,1,a4,2,a4,3},最大匹配度bi=4也就是{a4,1,a4,2,a4,3}中的最大值;
步骤37,对各个最大匹配度bi进行判断;若当前最大匹配度bi小于预设的最小匹配度阈值amin,则确认当前最大匹配度bi对应的一对第一目标检测框和第二目标检测框之间没有关联关系;若当前最大匹配度bi大于或等于最小匹配度阈值amin,则在当前最大匹配度bi对应的一对第一目标检测框和第二目标检测框之间创建关联关系。
这里,最小匹配度阈值amin是一个预先设定的参考阈值,本发明实施例规定但凡任何匹配度ai,j低于该参考阈值就说明该匹配度ai,j对应的两个目标检测框也就是第一目标检测框i和第二目标检测框j是不匹配的也即是二者之间没有关联性、二者分别对应的是不同的目标;反之,若任何匹配度ai,j高于该阈值则说明该匹配度ai,j对应的第一目标检测框i和第二目标检测框j有可能匹配,但对于同一个第一目标检测框i而言可能存在多个高于该参考阈值的匹配度ai,j,本发明实施例取其中最大匹配度也就是最大匹配度bi对应的第二目标检测框作为当前第一目标检测框i的最匹配对象,并为二者建立关联关系。
例如,得到的4个最大匹配度bi=1、bi=2、bi=3、bi=4,其中bi=1、bi=2、bi=4都大于最小匹配度阈值amin,但bi=3小于最小匹配度阈值amin;那么,第一目标检测框3没有任何第二目标检测框与之关联,也就是说前帧点云中第一目标检测框3的对应目标并未出现在当前时刻的后帧点云中;设bi=1为a1,1、bi=2为a2,3、bi=3为a3,2,那么,与第一目标检测框1关联的就是第二目标检测框1,与第一目标检测框2关联的就是第二目标检测框3,与第一目标检测框3关联的就是第二目标检测框2。
步骤4,根据每对关联的第一、第二目标检测框预测对应目标的速度;
这里,实际就是通过对一对关联的第一、第二目标检测框中的点云进行配准得到由物体运动产生的点云位姿变换关系也就是位姿变换矩阵T,再基于该位姿变换矩阵T对第一目标检测框的指定位置点在下一时刻的位姿进行预测,然后就可根据该指定位置点前后时刻的位姿变化进行速度估算;
具体包括:步骤41,将每对关联的第一、第二目标检测框记为对应的第一关联框和第二关联框;
步骤42,从前帧点云中提取第一关联框内的点云作为第一点云;并从后帧点云中提取第二关联框内的点云作为第二点云;
步骤43,基于迭代最近点ICP算法对第一、第二点云进行点云位姿配准得到对应的位姿变换矩阵T;
具体包括:步骤431,将第一点云作为当前源点云、将第二点云作为当前目标点云;并初始化迭代计数器的值为1;并将预设的初始化旋转矩阵R0、初始化平移向量t0作为对应的前次旋转矩阵和前次平移向量;
步骤432,基于预设的最小距离阈值dmin确定当前源点云和当前目标点云中的关联点对集合;
其中,关联点对集合包括多个关联点对Gk,k为关联点对索引,1≤k;关联点对Gk包括一个当前源点云中的点pk和一个当前目标点云中的点qk,且点pk与点qk的距离小于或等于最小距离阈值dmin;
这里,确立关联点对的实现方式有多种;其中一种具体为:对当前目标点云中的各点进行遍历,遍历时基于最小距离阈值dmin以当前遍历点为球心构建一个半径为dmin的球形空间,并将当前源点云中点云坐标落入该球形空间的点纳入到同一个匹配点集合中,若匹配点集合中的点数不为0则将当前遍历点作为一个点qk并从该匹配点集合中选出与当前遍历点最近的点作为对应的点pk从而得到一组关联点对Gk;遍历结束后,由得到的所有关联点对Gk组成关联点对集合;
步骤433,统计关联点对集合中关联点对Gk的数量记为第三数量H;
步骤434,基于欧式距离变换构建对应的目标函数f(R’,t’):
其中,R’为旋转矩阵变量,t’为平移向量变量,ck为点qk的法向量;
这里,目标函数f(R’,t’)是根据点到面(point-to-plane)ICP算法的目标函数结构进行构建的;基于点到面ICP算法,将每个pk视为一个源点、每个qk视为一个目标点,pk经过变换之后得到的点(R′pk+t′)应更靠近目标点qk,二者的距离向量(R′pk+t′-qk)在qk的法向量ck上的投影也就是(R′pk+t′-qk)·ck应更小;完成配准的两个点集(源点集、目标点集)的上所有源点-目标点对的(R′pk+t′-qk)·ck的总和也就是目标函数f(R’,t’)应为一个最小值;
步骤435,对使目标函数f(R’,t’)达到最小值的两个变量R’、t’进行求解,并将求解结果作为对应的当前旋转矩阵R*和当前平移向量t*;
步骤436,对当前旋转矩阵R*与前次旋转矩阵的误差以及当前平移向量t*与前次平移向量的误差进行计算得到对应的旋转矩阵误差和平移向量误差;若旋转矩阵误差和平移向量误差不都满足各自的预设合理误差范围则转至步骤437;若旋转矩阵误差和平移向量误差都满足各自的预设合理误差范围则转至步骤439;
这里,基于点到面ICP算法的迭代若不设置退出条件,则可能无限制的计算下去;为节约计算资源本发明实施例设定一种退出条件:合理误差范围;合理误差范围有两个分别对应旋转矩阵误差和平移向量误差,当旋转矩阵误差和平移向量误差全都进入各自对应的合理误差范围时说明转换后的源点已经非常逼近目标点了,此时可不再继续进行迭代从而转至步骤439;
步骤437,对迭代计数器的值加1;若加1后迭代计数器的值未超过预设的最大迭代次数则转至步骤438;若加1后迭代计数器的值超过了最大迭代次数则转至步骤439;
这里,基于点到面ICP算法的迭代过程中有时候旋转矩阵误差和平移向量误差很难同时合理收敛到各自对应的合理误差范围,此时若不另外再设置一个退出条件也可能造成计算资源大量消耗;为节约计算资源本发明实施例还设定了一种退出条件:最大迭代次数;无论旋转矩阵误差和平移向量误差的收敛情况如何,一旦迭代计数器表示的迭代次数超过最大迭代次数都会转至步骤439强制停止迭代;
步骤438,由当前旋转矩阵R*和当前平移向量t*构成对应的当前位姿变换矩阵T*,并基于当前位姿变换矩阵T*对当前源点云中各点进行位姿变换,并将变换后得到的点云作为新的当前源点云;并将当前旋转矩阵R*作为新的前次旋转矩阵,将当前平移向量t*作为新的前次平移向量;并转至步骤432根据新的当前源点云、前次旋转矩阵和前次旋转矩阵继续进行迭代运算;
这里,在旋转矩阵误差和平移向量误差并未完全收敛且迭代次数尚未达到最大迭代次数时,本发明实施例会根据当前旋转矩阵R*和当前平移向量t*对当前源点云进行位姿变换从而得到与目标点云更逼近的新源点云并将之作为下一次迭代的当前源点云,对应的也会将当前旋转矩阵R*和当前平移向量t*作为下一次迭代的前次旋转矩阵和前次旋转矩阵;完成下一次迭代所需数据的准备之后,本发明实施例会继续转至步骤432根据下一次迭代所需数据(新的当前源点云、前次旋转矩阵和前次旋转矩阵)进行下一次迭代处理;
这里,在旋转矩阵误差和平移向量误差完全收敛或迭代次数达到最大迭代次数时,本发明实施例停止本轮迭代并将最后得到的当前旋转矩阵R*和当前平移向量t*作为对应的旋转矩阵R和平移向量t输出,并由此获得对应的位姿变换矩阵T;
步骤44,将第一点云对应的第一关联框上指定位置点对应的点云坐标记为第一点坐标(x1,c,y1,c,z1,c);并基于位姿变换矩阵T对第一点坐标(x1,c,y1,c,z1,c)进行刚体位姿变换得到对应的第二点坐标(x2,c,y2,c,z2,c);
这里,指定位置点有多种设定方式,其中一种为:不分目标类型统一设定为第一关联框对应的第一目标框中心点坐标(x1,i,y1,i,z1,i);其中另一种为:不分目标类型统一设定为第一关联框与自车距离最近的底边的中心点;其中另一种为按目标类型进区分定义,具体为:若第一关联框对应的目标类型为人、自行车、动物、植物或交通标志物则将指定位置点设为第一关联框对应的第一目标框中心点坐标(x1,i,y1,i,z1,i),若第一关联框对应的目标类型为车辆则将指定位置点设为第一关联框与自车距离最近的底边的中心点或第一关联框上与自车后轴中心点匹配的点;
步骤45,将第一、第二关联框的对应目标记为当前目标;并根据第一、第二点坐标以及前后时刻的时间差估算出当前目标的速度向量V;
具体包括:将前后时刻的时间差记为时间差δ;并根据第一点坐标(x1,c,y1,c,z1,c)、第二点坐标(x2,c,y2,c,z2,c)和时间差δ对当前目标的三个速度分量进行估算得到对应的vx、vy和vz;并由三个速度分量vx、vy和vz构成当前目标的速度向量V;vx=(x2,c-x1,c)/δ,vy=(y2,c-y1,c)/δ,vz=(z2,c-z1,c)/δ。
图2为本发明实施例二提供的一种电子设备的结构示意图。该电子设备可以为前述的终端设备或者服务器,也可以为与前述终端设备或者服务器连接的实现本发明实施例方法的终端设备或服务器。如图2所示,该电子设备可以包括:处理器301(例如CPU)、存储器302、收发器303;收发器303耦合至处理器301,处理器301控制收发器303的收发动作。存储器302中可以存储各种指令,以用于完成各种处理功能以及实现前述方法实施例描述的处理步骤。优选的,本发明实施例涉及的电子设备还包括:电源304、系统总线305以及通信端口306。系统总线305用于实现元件之间的通信连接。上述通信端口306用于电子设备与其他外设之间进行连接通信。
在图2中提到的系统总线305可以是外设部件互连标准(Peripheral ComponentInterconnect,PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture,EISA)总线等。该系统总线可以分为地址总线、数据总线、控制总线等。为便于表示,图2中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。通信接口用于实现数据库访问装置与其他设备(例如客户端、读写库和只读库)之间的通信。存储器可能包含随机存取存储器(Random Access Memory,RAM),也可能还包括非易失性存储器(Non-Volatile Memory),例如至少一个磁盘存储器。
上述的处理器可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(Network Processor,NP)、图形处理器(Graphics Processing Unit,GPU)等;还可以是数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(FieldProgrammable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
需要说明的是,本发明实施例还提供一种计算机可读存储介质,该计算机可读存储介质中存储有指令,当其在计算机上运行时,使得计算机执行上述实施例中提供的方法和处理过程。
本发明实施例还提供一种运行指令的芯片,该芯片用于执行前述方法实施例描述的处理步骤。
本发明实施例提供了一种基于点云数据进行目标速度预测的处理方法、电子设备及计算机可读存储介质,先对相邻时刻的两帧激光雷达点云分别进行目标识别,再对两帧点云上的目标识别框进行关联从而得到多对关联的目标识别框,再基于ICP算法对每对关联目标识别框中的点云进行配准从而得到前后时刻的位姿变换矩阵T,并基于位姿变换矩阵T对前一时刻目标识别框上的指定位置点在下一时刻的位姿进行预测,并根据该点前一时刻的位姿和下一时刻的预测位姿算出对应目标的速度。通过本发明,对传统点云目标检测模型的输出进行持续关联和点云位姿配准就能获得点云中各个目标的速度,不但克服了传统点云目标检测模型不能进行速度检测的缺陷,还降低了感知系统对毫米波雷达的依赖,减少了数据融合模块的点云融合计算量,提高了感知系统的整体工作效率。
专业人员应该还可以进一步意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、计算机软件或者二者的结合来实现,为了清楚地说明硬件和软件的可互换性,在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块,或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上所述的具体实施方式,对本发明的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上所述仅为本发明的具体实施方式而已,并不用于限定本发明的保护范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (8)
1.一种基于点云数据进行目标速度预测的处理方法,其特征在于,所述方法包括:
获取相邻时刻的前后两帧激光雷达点云数据作为对应的前帧点云和后帧点云;
基于点云目标检测模型分别对所述前帧、后帧点云进行点云目标检测处理得到多个第一目标检测框和多个第二目标检测框;所述第一目标检测框对应所述前帧点云,所述第二目标检测框对应所述后帧点云;
对所述前、后帧点云的所述第一、第二目标检测框进行目标关联处理;
根据每对关联的所述第一、第二目标检测框预测对应目标的速度。
2.根据权利要求1所述的基于点云数据进行目标速度预测的处理方法,其特征在于,
所述点云目标检测模型包括VoxelNet模型、SECOND模型和PointPillars模型,默认使用所述PointPillars模型;
各个所述第一目标检测框对应一组第一检测框参数;所述第一检测框参数包括第一目标框中心点坐标(x1,y1,z1)、第一目标框深度l1、第一目标框宽度w1、第一目标框高度h1和第一目标框朝向角yaw1;
各个所述第二目标检测框对应一组第二检测框参数;所述第二检测框参数包括第二目标框中心点坐标(x2,y2,z2)、第二目标框深度l2、第二目标框宽度w2、第二目标框高度h2和第二目标框朝向角yaw2。
3.根据权利要求2所述的基于点云数据进行目标速度预测的处理方法,其特征在于,所述对所述前、后帧点云的所述第一、第二目标检测框进行目标关联处理,具体包括:
统计所述第一检测框参数的数量记为第一数量m,并统计所述第二检测框参数的数量记为第二数量n;
基于目标框索引i对各个所述第一目标框中心点坐标进行顺序编码得到对应的第一目标框中心点坐标(x1,i,y1,i,z1,i),1≤i≤m;基于目标框索引j对各个所述第二目标框中心点坐标进行顺序编码得到对应的第二目标框中心点坐标(x2,j,y2,j,z2,j),1≤j≤n;
由m*n个所述中心点间距si,j构建一个形状为m*n的矩阵向量记为第一矩阵向量;并将所述第一矩阵向量输入深度匈牙利网络DHN基于改进的匈牙利算法进行匹配度计算得到对应的形状为m*n的关联矩阵向量A;所述关联矩阵向量A包括m*n个匹配度ai,j,0<ai,j≤1;每个所述匹配度ai,j基于所述目标框索引i和所述目标框索引j对应一对所述第一目标检测框和所述第二目标检测框;
将同一个所述目标框索引i对应的所述第二数量n的所述匹配度ai,j归入同一个匹配度集合Di;并提取各个所述匹配度集合Di中取值最大的所述匹配度ai,j作为对应的最大匹配度bi;
对各个所述最大匹配度bi进行判断;若当前最大匹配度bi小于预设的最小匹配度阈值amin,则确认所述当前最大匹配度bi对应的一对所述第一目标检测框和所述第二目标检测框之间没有关联关系;若所述当前最大匹配度bi大于或等于所述最小匹配度阈值amin,则在所述当前最大匹配度bi对应的一对所述第一目标检测框和所述第二目标检测框之间创建关联关系。
4.根据权利要求1所述的基于点云数据进行目标速度预测的处理方法,其特征在于,所述根据每对关联的所述第一、第二目标检测框预测对应目标的速度,具体包括:
将每对关联的所述第一、第二目标检测框记为对应的第一关联框和第二关联框;
从所述前帧点云中提取所述第一关联框内的点云作为第一点云;并从所述后帧点云中提取所述第二关联框内的点云作为第二点云;
将所述第一点云对应的所述第一关联框上指定位置点对应的点云坐标记为第一点坐标(x1,c,y1,c,z1,c);并基于所述位姿变换矩阵T对所述第一点坐标(x1,c,y1,c,z1,c)进行刚体位姿变换得到对应的第二点坐标(x2,c,y2,c,z2,c);
将所述第一、第二关联框的对应目标记为当前目标;并根据所述第一、第二点坐标以及前后时刻的时间差估算出所述当前目标的速度向量V。
5.根据权利要求4所述的基于点云数据进行目标速度预测的处理方法,其特征在于,所述基于迭代最近点ICP算法对所述第一、第二点云进行点云位姿配准得到对应的位姿变换矩阵T,具体包括:
步骤51,将所述第一点云作为当前源点云、将所述第二点云作为当前目标点云;并初始化迭代计数器的值为1;并将预设的初始化旋转矩阵R0、初始化平移向量t0作为对应的前次旋转矩阵和前次平移向量;
步骤52,基于预设的最小距离阈值dmin确定所述当前源点云和所述当前目标点云中的关联点对集合;所述关联点对集合包括多个关联点对Gk,k为关联点对索引,1≤k;所述关联点对Gk包括一个当前源点云中的点pk和一个当前目标点云中的点qk,且所述点pk与所述点qk的距离小于或等于所述最小距离阈值dmin;
步骤53,统计所述关联点对集合中所述关联点对Gk的数量记为第三数量H;
步骤54,基于欧式距离变换构建对应的目标函数f(R’,t’):
R’为旋转矩阵变量,t’为平移向量变量,ck为所述点qk的法向量;
步骤55,对使所述目标函数f(R’,t’)达到最小值的两个变量R’、t’进行求解,并将求解结果作为对应的当前旋转矩阵R*和当前平移向量t*;
步骤56,对所述当前旋转矩阵R*与所述前次旋转矩阵的误差以及所述当前平移向量t*与所述前次平移向量的误差进行计算得到对应的旋转矩阵误差和平移向量误差;若所述旋转矩阵误差和所述平移向量误差不都满足各自的预设合理误差范围则转至步骤57;若所述旋转矩阵误差和所述平移向量误差都满足各自的预设合理误差范围则转至步骤59;
步骤57,对所述迭代计数器的值加1;若加1后所述迭代计数器的值未超过预设的最大迭代次数则转至步骤58;若加1后所述迭代计数器的值超过了所述最大迭代次数则转至步骤59;
步骤58,由所述当前旋转矩阵R*和所述当前平移向量t*构成对应的当前位姿变换矩阵T*,并基于所述当前位姿变换矩阵T*对所述当前源点云中各点进行位姿变换,并将变换后得到的点云作为新的所述当前源点云;并将所述当前旋转矩阵R*作为新的所述前次旋转矩阵,将所述当前平移向量t*作为新的所述前次平移向量;并转至步骤52根据新的所述当前源点云、所述前次旋转矩阵和所述前次旋转矩阵继续进行迭代运算;
6.根据权利要求4所述的基于点云数据进行目标速度预测的处理方法,其特征在于,所述根据所述第一、第二点坐标以及前后时刻的时间差估算出所述当前目标的速度向量V,具体包括:
将前后时刻的时间差记为时间差δ;并根据所述第一点坐标(x1,c,y1,c,z1,c)、所述第二点坐标(x2,c,y2,c,z2,c)和所述时间差δ对所述当前目标的三个速度分量进行估算得到对应的vx、vy和vz;并由三个速度分量vx、vy和vz构成所述当前目标的所述速度向量V;vx=(x2,c-x1,c)/δ,vy=(y2,c-y1,c)/δ,vz=(z2,c-z1,c)/δ。
7.一种电子设备,其特征在于,包括:存储器、处理器和收发器;
所述处理器用于与所述存储器耦合,读取并执行所述存储器中的指令,以实现权利要求1-6任一项所述的方法步骤;
所述收发器与所述处理器耦合,由所述处理器控制所述收发器进行消息收发。
8.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质存储有计算机指令,当所述计算机指令被计算机执行时,使得所述计算机执行权利要求1-6任一项所述的方法的指令。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210581422.7A CN114966736A (zh) | 2022-05-26 | 2022-05-26 | 一种基于点云数据进行目标速度预测的处理方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210581422.7A CN114966736A (zh) | 2022-05-26 | 2022-05-26 | 一种基于点云数据进行目标速度预测的处理方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114966736A true CN114966736A (zh) | 2022-08-30 |
Family
ID=82956080
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210581422.7A Pending CN114966736A (zh) | 2022-05-26 | 2022-05-26 | 一种基于点云数据进行目标速度预测的处理方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114966736A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116540252A (zh) * | 2023-07-06 | 2023-08-04 | 上海云骥跃动智能科技发展有限公司 | 基于激光雷达的速度确定方法、装置、设备及存储介质 |
CN118072000A (zh) * | 2024-04-17 | 2024-05-24 | 中国科学院合肥物质科学研究院 | 一种基于新型目标识别算法的鱼类检测方法 |
-
2022
- 2022-05-26 CN CN202210581422.7A patent/CN114966736A/zh active Pending
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116540252A (zh) * | 2023-07-06 | 2023-08-04 | 上海云骥跃动智能科技发展有限公司 | 基于激光雷达的速度确定方法、装置、设备及存储介质 |
CN116540252B (zh) * | 2023-07-06 | 2023-09-26 | 上海云骥跃动智能科技发展有限公司 | 基于激光雷达的速度确定方法、装置、设备及存储介质 |
CN118072000A (zh) * | 2024-04-17 | 2024-05-24 | 中国科学院合肥物质科学研究院 | 一种基于新型目标识别算法的鱼类检测方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Oleynikova et al. | Signed distance fields: A natural representation for both mapping and planning | |
CN110264502B (zh) | 点云配准方法和装置 | |
CN109087510B (zh) | 交通监测方法及装置 | |
CN114966736A (zh) | 一种基于点云数据进行目标速度预测的处理方法 | |
US9286524B1 (en) | Multi-task deep convolutional neural networks for efficient and robust traffic lane detection | |
KR20200021626A (ko) | 객체 검출 모델 트레이닝 장치 및 방법 | |
CN111123927A (zh) | 轨迹规划方法、装置、自动驾驶设备和存储介质 | |
Liu et al. | Dynamic vehicle detection with sparse point clouds based on PE-CPD | |
US20230386076A1 (en) | Target detection method, storage medium, electronic device, and vehicle | |
CN115436910B (zh) | 一种对激光雷达点云进行目标检测的数据处理方法和装置 | |
CN111062405A (zh) | 训练图像识别模型的方法和装置以及图像识别方法和装置 | |
CN114463736A (zh) | 一种基于多模态信息融合的多目标检测方法及装置 | |
Liu et al. | A co-point mapping-based approach to drivable area detection for self-driving cars | |
WO2022247303A1 (zh) | 预测控制的方法、装置、设备及计算机可读存储介质 | |
CN113281779B (zh) | 一种3d物体快速检测方法、装置、设备及介质 | |
CN114241448A (zh) | 障碍物航向角的获取方法、装置、电子设备及车辆 | |
CN111765883B (zh) | 机器人蒙特卡罗定位方法、设备及存储介质 | |
CN116844124A (zh) | 三维目标检测框标注方法、装置、电子设备和存储介质 | |
CN115542301A (zh) | 激光雷达外部参数的校准方法、装置、设备及存储介质 | |
CN114663478A (zh) | 一种根据多参考点预测信息估计锚点位置的方法 | |
CN114882458A (zh) | 一种目标跟踪方法、系统、介质及设备 | |
CN116863325A (zh) | 一种用于多个目标检测的方法和相关产品 | |
CN114140660A (zh) | 一种车辆检测方法、装置、设备及介质 | |
CN115544191A (zh) | 基于三维点云众包式语义地图的更新方法和装置 | |
CN113361601A (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 |