CN110018489B - 基于激光雷达的目标追踪方法、装置及控制器和存储介质 - Google Patents
基于激光雷达的目标追踪方法、装置及控制器和存储介质 Download PDFInfo
- Publication number
- CN110018489B CN110018489B CN201910340665.XA CN201910340665A CN110018489B CN 110018489 B CN110018489 B CN 110018489B CN 201910340665 A CN201910340665 A CN 201910340665A CN 110018489 B CN110018489 B CN 110018489B
- Authority
- CN
- China
- Prior art keywords
- target
- tracking
- point
- key points
- movement
- 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.)
- Active
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/66—Tracking systems using electromagnetic waves other than radio waves
Abstract
本发明公开了一种基于激光雷达的目标追踪方法和装置，该方法包括：实时获取激光雷达采集的点云数据；基于点云数据获取多个目标点云；提取多个目标点云中每个目标点云的多个关键点；检测多个目标点云中是否存在与多个关键点匹配的追踪目标；若存在追踪目标，则将与追踪目标相匹配的多个关键点确定为多个目标关键点，并根据多个目标关键点确定追踪目标的运行状态。本发明通过在目标点云提取出多个关键点，并根据该多个关键点来判断追踪目标的动静状态，以及确定追踪目标运动信息，有效地提高了判断追踪目标的动静状态的准确性和运行信息的精度，进而提高了无人驾驶车辆的行驶安全，并保证了车内人员的人身安全。
Description
技术领域
本发明涉及自动驾驶技术领域，尤其涉及一种基于激光雷达的目标追踪方法、目标追踪装置及控制器和计算机可读存储介质。
背景技术
目前，在无人驾驶车辆行驶过程中，通过激光雷达采集的点云数据中的目标点云的运动信息的估计，主要根据目标点云的质心点的位置来确定，但是由于目标自身或者其他目标的遮挡，运动目标的质心点的位置很难保持稳定，容易出现运动信息估计不准、动静状态判断错误等情况的发生，对后续目标的轨迹和行为预测造成很大影响，最终给无人驾驶车辆的行为决策带来风险。
发明内容
本发明所要解决的技术问题在于，提供一种基于激光雷达的目标追踪方法和目标追踪装置，通过在目标点云提取出多个关键点，并根据该多个关键点来判断追踪目标的动静状态，以及确定追踪目标运动信息，有效地提高了判断追踪目标的动静状态的准确性和运动信息的精度，进而提高了无人驾驶车辆的行驶安全，并保证了车内人员的人身安全。
为了解决上述技术问题，根据本发明一方面，提供了一种基于激光雷达的目标追踪方法，包括:
实时获取激光雷达采集的点云数据；
基于所述点云数据获取多个目标点云；
提取所述多个目标点云中每个目标点云的多个关键点；
检测所述多个目标点云中是否存在与所述多个关键点匹配的追踪目标；
若存在所述追踪目标，则将与所述追踪目标相匹配的所述多个关键点确定为多个目标关键点，并根据所述多个目标关键点确定所述追踪目标的运行状态。
进一步的，所述运行状态包括：动静状态和运动信息；
所述若存在所述追踪目标，则根据与所述追踪目标相匹配的多个目标关键点确定所述追踪目标的运行状态的步骤，包括：
通过贝叶斯滤波器与所述多个目标关键点判断所述追踪目标的所述动静状态；
当所述追踪目标处于运动状态时，通过卡尔曼滤波器与所述多个目标关键点确定所述追踪目标的所述运动信息。
进一步的，所述通过贝叶斯滤波器与所述多个目标关键点判断所述追踪目标的所述动静状态的步骤，包括：
计算当前所述多个目标关键点和上一帧所述多个目标关键点的每个关键点的位移距离，通过先验设定方法赋予所述多个位移距离分别对应的第一移动概率值；
计算连续的预设数量帧的所述多个目标关键点中每个关键点的位移距离之和，通过先验设定方法赋予多个所述位移距离之和分别对应的第二移动概率值；
根据多个所述第一移动概率值和多个所述第二移动概率值计算出所述追踪目标的目标移动概率值；
根据所述目标移动概率值确定所述追踪目标的所述动静状态。
进一步的，所述根据多个所述第一移动概率值和多个所述第二移动概率值计算出所述追踪目标的目标移动概率值的计算公式为：
log_odds+＝log(pmoving/(1-pmoving))
P＝1-1/(1+elog_odds)
其中，log_odds+为多个所述第一概率值和多个所述第二概率值的累加值，pmoving为多个所述第一概率值和多个所述第二概率值，P为所述目标移动概率值。
进一步的，所述运动信息包括：追踪目标的运动速度、加速度、航向和角速度；
所述当所述追踪目标处于运动状态时，通过卡尔曼滤波器与所述多个目标关键点确定所述追踪目标的运动信息的步骤，包括：
根据当前所述多个目标关键点与上一帧所述多个目标关键点的移动距离与时间间隔计算出多个目标运动速度；
基于所述多个目标运动速度通过所述卡尔曼滤波器确定所述追踪目标的运动速度和加速度；
计算当前所述多个目标关键点与上一帧所述多个目标关键点的运动方向；
基于所述运动方向通过所述卡尔曼滤波器确定出所述追踪目标的航向和角速度。
进一步的，所述多个关键点包括：质心点、长边中心点和最近点；以及
所述提取所述多个目标点云中每个目标点云的多个关键点的步骤，包括：
计算每个所述目标点云中各点的三维坐标的平均值，将所述平均值所对应的点作为所述质心点；
采用直线拟合方法提取每个所述目标点云的最长边，并将所述最长边的中点作为所述长边中心点；
遍历每个所述目标点云的全部点，将距离所述激光雷达的原点最近的点作为所述最近点。
进一步地，所述基于所述点云数据获得多个目标点云的步骤，包括：
将所述点云数据进行地面去除和分割聚类，得到所述多个目标点云。
根据本发明另一方面，提供一种基于激光雷达的目标追踪装置，包括：
第一获取模块，配置为实时获取激光雷达采集的点云数据；
第二获取模块，配置为基于所述点云数据获取多个目标点云；
提取模块，配置为提取所述多个目标点云中每个目标点云的多个关键点；
检测模块，配置为检测所述多个目标点云中是否存在与所述多个关键点匹配的追踪目标；
确定模块，配置为在所述多个目标点云中存在所述追踪目标时，将与所述追踪目标相匹配的所述多个关键点确定为多个目标关键点，并根据所述多个目标关键点确定所述追踪目标的运行状态。
进一步的，所述运行状态包括：动静状态和运动信息；
所述确定模块包括：
判断子模块，配置为通过贝叶斯滤波器与所述多个目标关键点判断所述追踪目标的动静状态；
确定子模块，配置为在所述追踪目标处于运动状态时，通过卡尔曼滤波器与所述多个目标关键点确定所述追踪目标的运动信息。
进一步的，所述判断子模块包括：
第一计算单元，配置为计算当前所述多个目标关键点和上一帧所述多个目标关键点的每个关键点的位移距离，通过先验设定方法赋予所述多个位移距离分别对应的第一移动概率值；
所述第一计算单元还配置为计算连续的预设数量帧的所述多个目标关键点中每个关键点的位移距离之和，通过先验设定方法赋予多个所述位移距离之和分别对应的第二移动概率值；
所述第一计算单元还配置为根据多个所述第一移动概率值和多个所述第二移动概率值计算出所述追踪目标的目标移动概率值；
第一确定单元，配置为根据所述目标移动概率值确定所述追踪目标的所述动静状态。
进一步的，所述根据多个所述第一移动概率值和多个所述第二移动概率值计算出所述追踪目标的目标移动概率值的计算公式为：
log_odds+＝log(pmoving/(1-pmoving))
P＝1-1/(1+elog_odds)
其中，log_odds+为多个所述第一概率值和多个所述第二概率值的累加值，pmoving为多个所述第一概率值和多个所述第二概率值，P为所述目标移动概率值。
进一步地，所述运动信息包括：追踪目标的运动速度、加速度、航向和角速度；
所述确定子模块包括：
第二计算单元，配置为根据当前所述多个目标关键点与上一帧所述多个目标关键点的移动距离与时间间隔计算出多个目标运动速度；
第二确定单元，配置为基于所述多个目标运动速度通过所述卡尔曼滤波器确定所述追踪目标的所述运动速度和所述加速度；
所述第二计算单元还配置为计算当前所述多个目标关键点与上一帧所述多个目标关键点的运动方向；
所述第二确定单元还配置为基于所述运动方向通过所述卡尔曼滤波器确定出所述追踪目标的所述航向和所述角速度。
进一步地，所述多个关键点包括：质心点、长边中心点和最近点；以及
所述提取模块包括：
计算子模块，配置为计算每个所述目标点云中各点的三维坐标的平均值，将所述平均值所对应的点确定所述质心点；
提取子模块，配置为采用直线拟合方法提取每个所述目标点云的最长边，并将所述最长边的中点确定为所述长边中心点；
遍历子模块，配置为遍历每个所述目标点云的全部点，将距离所述激光雷达的原点最近的点确定为所述最近点。
进一步地，所述第二获取模块具体用于：将所述点云数据进行地面去除和分割聚类，得到所述多个目标点云。
根据本发明又一方面，提供一种控制器，其包括存储器与处理器，所述存储器存储有计算机程序，所述程序在被所述处理器执行时能够实现上述任一项所述的基于激光雷达的目标追踪方法的步骤。
根据本发明再一方面，提供一种计算机可读存储介质，用于存储计算机程序，所述程序在由一计算机或处理器执行时实现上述任意一项所述的基于激光雷达的目标追踪方法的步骤。
本发明与现有技术相比具有明显的优点和有益效果。借由上述技术方案，本发明的一种基于激光雷达的目标追踪方法和目标追踪装置可以达到相当的技术进步性及实用性，并具有产业上的广泛利用价值，其至少具有下列优点：
(1)通过对点云数据进行地面去除和分割聚类处理，能够有效地提升目标点云中多个关键点的提取精度；
(2)通过在目标点云提取出多个关键点，并根据该多个关键点来判断追踪目标的动静状态，以及确定追踪目标运动信息，有效地提高了判断追踪目标动静状态的准确性和运行信息的精度，进而提高了无人驾驶车辆的行驶安全，并保证了车内人员的人身安全。
(3)通过采用贝叶斯滤波器检测每个关键点的位置变化，来确定追踪目标的动静状态，能够有效地提升动静状态判断的准确性。
(4)对每个特征点使用卡尔曼滤波器更新追踪目标的运动信息，能够有效地提升运动信息判断的精度。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图,详细说明如下。
附图说明
图1示出了本发明一实施例的基于激光雷达的目标追踪方法的流程图；
图2示出了本发明实施例的基于激光雷达的目标追踪装置的结构框图；
图3示出了图2所示的提取模块的结构框图；
图4示出了图2所示的确定模块的结构框图；
图5示出了图4所示的判断子模块的结构框图；
图6示出了图4所示的确定子模块的结构框图；
图7示出了本发明另一实施例的基于激光雷达的目标追踪方法的流程图；
图8示出了本发明实施例的目标点云中确定关键点的示意图；
图9示出了本发明一实施例的移动距离与运动概率的对应关系曲线图；
图10示出了本发明另一实施例的移动距离与运动概率的对应关系曲线图。
具体实施方式
为更进一步阐述本发明为达成预定发明目的所采取的技术手段及功效,以下结合附图及较佳实施例，对依据本发明提出的一种基于激光雷达的目标追踪方法和目标追踪装置的具体实施方式及其功效，详细说明如后。
如图1所示，本发明实施例的一种基于激光雷达的目标追踪方法，该方法包括:
步骤S11：实时获取激光雷达采集的点云数据。
具体地，在无人驾驶车辆的运行过程中激光雷达实时扫描车辆附近的路况，实时获取激光雷达采集到的路况的点云数据。
步骤S12：基于点云数据获取多个目标点云。
具体地，首先将获取到的点云数据进行地面去除、聚类等操作，进而对处理后的点云数据进行分割为多个目标点云。
步骤S13：提取多个目标点云中每个目标点云的多个关键点。
可知的是，目前多采用根据目标点云的质心点来估计追踪目标的情况，但是在实际应用中，由于目标本身或者其他的目标的遮挡，而使得目标点云的质心点很难保持稳定，进而导致目标情况判断不准的情况发生。因此，可以通过提取多个目标点云中每个目标点云的多个关键点来解决上述问题的发生。
在一个实施例中，上述多个关键点可以包括目标点云的质心点、长边中心点和最近点，当然在其他实施例中，同样可以提取上述三个关键点的一个或两个，当然还可以提取出其他的关键点。
在该实施例中，质心点的提取是通过计算每个目标点云中各点的三维坐标的平均值，并将该平均值所对应的点作为质心点；长边中心点的提取是采用直线拟合方法提取出每个目标点云的最长边，并将最长边的中点作为长边中心点，具体地，可以采用最小二乘或RANSAC等直线拟合方法来提取最长边；最近点的提取是通过遍历每个目标点云的全部点，并将距离激光雷达原点最近的点作为最近点。
步骤S14：检测多个目标点云中是否存在与多个关键点匹配的追踪目标。
具体地，可以通过最近邻匹配或者匈牙利匹配等数据关联算法来获得与多个关键点所匹配的追踪目标，若未检测到相匹配的追踪目标则初始化一个追踪目标，若检测到相匹配的追踪目标则执行步骤S15。
步骤S15：若存在追踪目标，则将与追踪目标相匹配的多个关键点确定为多个目标关键点，并根据多个目标关键点确定追踪目标的运行状态。
具体地，当存在与多个关键点匹配的追踪目标时，则将该多个关键点确定为多个目标关键点，进而可以通过多个目标关键点和贝叶斯滤波器来判断出追踪目标的动静状态，即判断追踪目标是处于静止状态还是运动状态。
在一个实施例中，通过计算当前目标点云的多个目标关键点和上一帧目标点云的多个目标关键点中的每个关键点的位移距离，并通过先验设定方法赋予多个位移距离分别对应的第一移动概率值。
通过计算连续的预设数量帧目标点云的多个目标关键点的每个关键点的位移距离之和，并通过先验设定方法赋予多个所述位移距离之和分别对应的第二移动概率值。
在一个实施例中，可以通过转换函数来将位移距离转换为移动概率值，还可以调整分段函数的参数值，来得到不同形态的移动概率值与移动距离的对应曲线。
由上述可知，目标点云的关键点具有三个，因此第一移动概率值和第二移动概率值均具有三个，并利用贝叶斯滤波器将六个移动概率值进行累加，具体公式如下：
log_odds+＝log(pmoving/(1-pmoving))，其中，log_odds+为多个第一概率值和多个第二概率值的累加值，pmoving为多个第一概率值和多个第二概率值；而后，通过计算出的多个第一概率值和多个第二概率值的累加值计算出追踪目标的移动概率值，具体计算公式如下：
P＝1-1/(1+e log_odds)，其中P为追踪目标的目标移动概率值，进而可以根据计算出的目标移动概率值来确定出追踪目标的运动状态，可以设定为目标移动概率值大于或等于0.5时，则判定追踪目标处于运动状态，当然也可以设定为其他的值来进行判定。
通过上述实施例，在判定出追踪目标处于运动状态时，通过卡尔曼滤波器与多个目标关键点确定追踪目标的所述运动信息，其中，该运动信息包括运动速度、加速度、航向和角速度。
在一个实施例中，可以根据当前多个目标关键点与上一帧多个目标关键点的移动距离与时间间隔计算出多个目标运动速度，该多个目标运动速度为多个目标关键点中每个关键点的运动速度，进而基于多个目标运动速度通过卡尔曼滤波器确定出追踪目标的运动速度和加速度。还可以计算当前多个目标关键点与上一帧多个目标关键点的运动方向，并基于该运动方向通过卡尔曼滤波器确定出追踪目标的航向和角速度。
本发明实施例还提供一种基于激光雷达的目标追踪装置，如图2所示，该装置包括：第一获取模块10、第二获取模块20、提取模块30、检测模块40和确定模块50。
其中，获取模块10配置为实时获取激光雷达采集的点云数据。
具体地，在无人驾驶车辆的运行过程中激光雷达实时扫描车辆附近的路况，第一获取模块10实时获取激光雷达采集到的路况的点云数据。
第二获取模块20配置为基于点云数据获取多个目标点云。
在一实施例中，第二获取模块20首先将获取到的点云数据进行地面去除、聚类等操作，进而对处理后的点云数据进行分割为多个目标点云。
提取模块30配置为提取多个目标点云中每个目标点云的多个关键点。
可知的是，目前多采用根据目标点云的质心点来估计追踪目标的情况，但是在实际应用中，由于目标本身或者其他的目标的遮挡，而使得目标点云的质心点很难保持稳定，进而导致目标情况判断不准的情况发生。因此，可以通过提取多个目标点云中每个目标点云的多个关键点来解决上述问题的发生。
在一个实施例中，上述多个关键点可以包括目标点云的质心点、长边中心点和最近点，当然在其他实施例中，同样可以提取上述三个关键点的一个或两个，当然还可以提取出其他的关键点。
在该实施例中，如图3所示，提取模块30包括：计算子模块301、提取子模块302和遍历子模块303；其中，计算子模块301配置为通过计算每个目标点云中各点的三维坐标的平均值，并将该平均值所对应的点作为质心点；提取子模块302配置为采用直线拟合方法提取出每个目标点云的最长边，并将最长边的中点作为长边中心点，具体地，可以采用最小二乘或RANSAC等直线拟合方法来提取最长边；遍历子模块303配置为通过遍历每个目标点云的全部点，并将距离激光雷达原点最近的点作为最近点。
检测模块40配置为检测多个目标点云中是否存在与多个关键点匹配的追踪目标。
具体地，检测模块40可以通过最近邻匹配或者匈牙利匹配等数据关联算法来获得与多个关键点所匹配的追踪目标，若未检测到相匹配的追踪目标则初始化一个追踪目标，若检测到相匹配的追踪目标则由确定模块50进行处理。
确定模块50配置为在多个目标点云中存在追踪目标时，将与追踪目标相匹配的多个关键点确定为多个目标关键点，并根据多个目标关键点确定追踪目标的运行状态。
具体地，如图4所示，确定模块50包括：判断子模块501和确定子模块502；其中判断子模块501在存在与多个关键点匹配的追踪目标时，将该多个关键点确定为多个目标关键点，进而可以通过多个目标关键点和贝叶斯滤波器来判断出追踪目标的动静状态，即判断追踪目标是处于静止状态还是运动状态。
在一个实施例中，如图5所示，判断子模块501包括：第一计算单元5011和第一确定单元5012；其中，第一计算单元5011通过计算当前目标点云的多个目标关键点和上一帧目标点云的多个目标关键点中的每个关键点的位移距离，并通过先验设定方法赋予多个位移距离分别对应的第一移动概率值。
第一计算单元5011还配置为通过计算连续的预设数量帧目标点云的多个目标关键点的每个关键点的位移距离之和，并通过先验设定方法赋予多个所述位移距离之和分别对应的第二移动概率值。
第一计算单元5011还配置为根据多个第一移动概率值和多个第二移动概率值计算出追踪目标的目标移动概率值。
第一确定单元5012配置为根据目标移动概率值确定追踪目标的动静状态。
在一个实施例中，可以通过转换函数来将位移距离转换为移动概率值，还可以调整分段函数的参数值，来得到不同形态的移动概率值与移动距离的对应曲线。
由上述可知，目标点云的关键点具有三个，因此第一移动概率值和第二移动概率值均具有三个，并利用贝叶斯滤波器将六个移动概率值进行累加，具体公式如下：
log_odds+＝log(pmoving/(1-pmoving))，其中，log_odds+为多个第一概率值和多个第二概率值的累加值，pmoving为多个第一概率值和多个第二概率值；而后，通过计算出的多个第一概率值和多个第二概率值的累加值计算出追踪目标的移动概率值，具体计算公式如下：
P＝1-1/(1+elog_odds)，其中P为追踪目标的目标移动概率值，进而可以根据计算出的目标移动概率值来确定出追踪目标的运动状态，可以设定为目标移动概率值大于或等于0.5时，则判定追踪目标处于运动状态，当然也可以设定为其他的值来进行判定。
在该实施例中，确定子模块502配置为在判定出追踪目标处于运动状态时，通过卡尔曼滤波器与多个目标关键点确定追踪目标的运动信息，其中，该运动信息包括运动速度、加速度、航向和角速度。
在一个实施例中，如图6所示，确定子模块502包括：第二计算单元5021和第二确定单元5022；其中第二计算单元5021配置为根据当前多个目标关键点与上一帧多个目标关键点的移动距离与时间间隔计算出多个目标运动速度，该多个目标运动速度为多个目标关键点中每个关键点的运动速度；第二确定单元5022配置为基于多个目标运动速度通过卡尔曼滤波器确定出追踪目标的运动速度和加速度。第二计算单元5021还配置为计算当前多个目标关键点与上一帧多个目标关键点的运动方向；第二确定单元5022还配置为基于该运动方向通过卡尔曼滤波器确定出追踪目标的航向和角速度。
本发明一个具体实施例提供一种基于激光雷达的目标追踪方法，如图7所示，包括：
步骤S21：获取激光雷达采集的点云数据。
步骤S22：对点云数据进行地面去除、点云分割聚类，以得到多个目标点云。
具体地，对于每帧激光点云数据，首先经过地面去除，聚类等操作，将激光点云数据分割为包含多个目标点云的集合。
步骤S23：提取每个目标点云的质心点、长边中心点和最近点。
具体地，提取多个目标点云中每个目标点云的多个关键点，如图8所示，该多个关键点包括：质心点A、长边中心点B和最近点C。
其中，目标点云的质心点A提取方法为，直接对目标点云的各个点的三维坐标求和取平均值，并将该平均值对应的点作为质心点A；目标点云的长边中心点B的提取方法为，对目标点云使用直线拟合方法，提取目标点云中最长的边沿，并求取最长边沿的中心点作为长边中心点B，该直线提取方法可以使用最小二乘或RANSAC等直线拟合方法；目标点云的最近点C提取方法为，遍历目标点云的所有点，得到距离激光雷达原点最近的点作为最近点C。
步骤S24：确定是否存在与三种关键点匹配的追踪目标，若是，则执行步骤S25，否则执行步骤S27，初始化追踪目标。
具体地，根据上述提取方法获得三种跟踪关键点，通过最近邻匹配或者匈牙利匹配等数据关联算法，获得与当前观测特征点匹配的跟踪目标。若未找到匹配的跟踪目标，则执行步骤S27，初始化一个追踪目标；若找到了匹配的跟踪目标，则执行步骤S25。
步骤S25：基于关键点与贝叶斯滤波器更新目标动静状态。
具体地，对目标动静状态判断的方法为，计算当前关键点与上一帧关键点的位移，即分别计算三个关键点中的每个关键点当前帧和上一帧的位移距离，共可得出三个位移距离，通过先验设定，对不同关键点的不同位移距离赋予对应的目标移动概率值，计算过去5帧关键点位移之和，即分别计算三个关键点中的过去5帧的位移距离之和，共可得出三个位移距离，同样根据先验知识转换为目标移动概率值。具体地，可以通过转换函数来将位移距离转换为目标移动概率值，还可以调整分段函数的参数值，来得到不同形态的移动概率值与移动距离的对应曲线。如图9和图10所示的位移距离和目标移动概率值的对应曲线。
利用贝叶斯公式累加这些概率，对于每个概率值，更新公式如下：
log_odds+＝log(pmoving/(1-pmoving))；
则最终判断目标的移动的概率为：
P＝1-1/(1+elog_odds)；
对概率P超过0.5的目标，认为该目标当前正在运动,当然，可以根据实际需求调整该设定的阈值，比如，设定为0.6或0.7等。
步骤S26：基于关键点与卡尔曼滤波器更新动态目标的运动信息。
具体地，运动信息的估计方法为，用当前3个关键点与上一帧关键点的距离除以时间间隔，获得3个目标运动速度的观测，作为卡尔曼滤波器的输入，估计目标的运动速度和加速度，选取使卡尔曼滤波器更新后误差协方差最小的观测作为当前的更新。对于运动目标的航向，通过关键点计算出两帧之间的方向作为目标航向的观测，作为卡尔曼滤波器的输入，估计目标的航向和角速度。同样选取使卡尔曼滤波器更新后误差协方差最小的航向观测作为当前的更新。
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明,任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内,当可利用上述揭示的技术内容作出些许更动或修饰为等同变化的等效实施例,但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。
Claims (14)
1.一种基于激光雷达的目标追踪方法，其特征在于，包括：
实时获取激光雷达采集的点云数据；
基于所述点云数据获取多个目标点云；
提取所述多个目标点云中每个目标点云的多个关键点；
检测所述多个目标点云中是否存在与所述多个关键点匹配的追踪目标；
若存在所述追踪目标，计算当前所述多个目标关键点和上一帧所述多个目标关键点的每个关键点的位移距离，通过先验设定方法赋予所述多个位移距离分别对应的第一移动概率值；
计算连续的预设数量帧的所述多个目标关键点中每个关键点的位移距离之和，通过先验设定方法赋予多个所述位移距离之和分别对应的第二移动概率值；
根据多个所述第一移动概率值和多个所述第二移动概率值计算出所述追踪目标的目标移动概率值；
根据所述目标移动概率值确定所述追踪目标的动静状态。
2.根据权利要求1所述的基于激光雷达的目标追踪方法，其特征在于，还包括：
当所述追踪目标处于运动状态时，通过卡尔曼滤波器与所述多个目标关键点确定所述追踪目标的运动信息。
3.根据权利要求1所述的基于激光雷达的目标追踪方法，其特征在于，所述根据多个所述第一移动概率值和多个所述第二移动概率值计算出所述追踪目标的目标移动概率值的计算公式为：
log_odds+=log（pmoving/（1- pmoving））
P=1-1/（1+e log_odds）
其中，log_odds+为多个所述第一移动概率值和多个所述第二移动概率值的累加值，pmoving为多个所述第一移动概率值和多个所述第二移动概率值，P为所述目标移动概率值。
4.根据权利要求2所述的基于激光雷达的目标追踪方法，其特征在于，所述运动信息包括：追踪目标的运动速度、加速度、航向和角速度；
所述当所述追踪目标处于运动状态时，通过卡尔曼滤波器与所述多个目标关键点确定所述追踪目标的运动信息的步骤，包括：
根据当前所述多个目标关键点与上一帧所述多个目标关键点的移动距离与时间间隔计算出多个目标运动速度；
基于所述多个目标运动速度通过所述卡尔曼滤波器确定所述追踪目标的运动速度和加速度；
计算当前所述多个目标关键点与上一帧所述多个目标关键点的运动方向；
基于所述运动方向通过所述卡尔曼滤波器确定出所述追踪目标的航向和角速度。
5.根据权利要求1-4中任一项所述的基于激光雷达的目标追踪方法，其特征在于，所述多个关键点包括：质心点、长边中心点和最近点；以及
所述提取所述多个目标点云中每个目标点云的多个关键点的步骤，包括：
计算每个所述目标点云中各点的三维坐标的平均值，将所述平均值所对应的点作为所述质心点；
采用直线拟合方法提取每个所述目标点云的最长边，并将所述最长边的中点作为所述长边中心点；
遍历每个所述目标点云的全部点，将距离所述激光雷达的原点最近的点作为所述最近点。
6.根据权利要求5所述的基于激光雷达的目标追踪方法，其特征在于，所述基于所述点云数据获得多个目标点云的步骤，包括：
将所述点云数据进行地面去除和分割聚类，得到所述多个目标点云。
7.一种基于激光雷达的目标追踪装置，其特征在于，包括：
第一获取模块，配置为实时获取激光雷达采集的点云数据；
第二获取模块，配置为基于所述点云数据获取多个目标点云；
提取模块，配置为提取所述多个目标点云中每个目标点云的多个关键点；
检测模块，配置为检测所述多个目标点云中是否存在与所述多个关键点匹配的追踪目标；
确定模块，配置为在所述多个目标点云中存在所述追踪目标时，
计算当前所述多个目标关键点和上一帧所述多个目标关键点的每个关键点的位移距离，通过先验设定方法赋予所述多个位移距离分别对应的第一移动概率值；
计算连续的预设数量帧的所述多个目标关键点中每个关键点的位移距离之和，通过先验设定方法赋予多个所述位移距离之和分别对应的第二移动概率值；
根据多个所述第一移动概率值和多个所述第二移动概率值计算出所述追踪目标的目标移动概率值；
根据所述目标移动概率值确定所述追踪目标的动静状态。
8.根据权利要求7所述的基于激光雷达的目标追踪装置，其特征在于，
所述确定模块还配置为：
配置为在所述追踪目标处于运动状态时，通过卡尔曼滤波器与所述多个目标关键点确定所述追踪目标的运动信息。
9.根据权利要求7所述的基于激光雷达的目标追踪装置，其特征在于，所述根据多个所述第一移动概率值和多个所述第二移动概率值计算出所述追踪目标的目标移动概率值的计算公式为：
log_odds+=log（pmoving/（1- pmoving））
P=1-1/（1+e log_odds）
其中，log_odds+为多个所述第一移动概率值和多个所述第二移动概率值的累加值，pmoving为多个所述第一移动概率值和多个所述第二移动概率值，P为所述目标移动概率值。
10.根据权利要求8所述的基于激光雷达的目标追踪装置，其特征在于，所述运动信息包括：追踪目标的运动速度、加速度、航向和角速度；
所述确定子模块包括：
第二计算单元，配置为根据当前所述多个目标关键点与上一帧所述多个目标关键点的移动距离与时间间隔计算出多个目标运动速度；
第二确定单元，配置为基于所述多个目标运动速度通过所述卡尔曼滤波器确定所述追踪目标的运动速度和加速度；
所述第二计算单元还配置为计算当前所述多个目标关键点与上一帧所述多个目标关键点的运动方向；
所述第二确定单元还配置为基于所述运动方向通过所述卡尔曼滤波器确定出所述追踪目标的航向和角速度。
11.根据权利要求7-10中任一项所述的基于激光雷达的目标追踪装置，其特征在于，所述多个关键点包括：质心点、长边中心点和最近点；以及
所述提取模块包括：
计算子模块，配置为计算每个所述目标点云中各点的三维坐标的平均值，将所述平均值所对应的点确定所述质心点；
提取子模块，配置为采用直线拟合方法提取每个所述目标点云的最长边，并将所述最长边的中点确定为所述长边中心点；
遍历子模块，配置为遍历每个所述目标点云的全部点，将距离所述激光雷达的原点最近的点确定为所述最近点。
12.根据权利要求11所述的基于激光雷达的目标追踪装置，其特征在于，所述第二获取模块具体配置为：
将所述点云数据进行地面去除和分割聚类，得到所述多个目标点云。
13.一种控制器，其包括存储器与处理器，所述存储器存储有计算机程序，所述程序在被所述处理器执行时能够实现权利要求1至6中任意一项权利要求所述的方法的步骤。
14.一种计算机可读存储介质，用于存储计算机程序，所述程序在由一计算机或处理器执行时实现如权利要求1至6中任意一项权利要求所述的方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910340665.XA CN110018489B (zh) | 2019-04-25 | 2019-04-25 | 基于激光雷达的目标追踪方法、装置及控制器和存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910340665.XA CN110018489B (zh) | 2019-04-25 | 2019-04-25 | 基于激光雷达的目标追踪方法、装置及控制器和存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110018489A CN110018489A (zh) | 2019-07-16 |
| CN110018489B true CN110018489B (zh) | 2022-11-08 |
Family
ID=67192479
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910340665.XA Active CN110018489B (zh) | 2019-04-25 | 2019-04-25 | 基于激光雷达的目标追踪方法、装置及控制器和存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110018489B (zh) |
Families Citing this family (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112558036B (zh) * | 2019-09-24 | 2024-04-02 | 北京百度网讯科技有限公司 | 用于输出信息的方法和装置 |
| CN112166458B (zh) * | 2019-10-17 | 2024-04-26 | 深圳市大疆创新科技有限公司 | 目标检测与跟踪方法、系统、设备及存储介质 |
| CN110780305B (zh) * | 2019-10-18 | 2023-04-21 | 华南理工大学 | 一种基于多线激光雷达的赛道锥桶检测及目标点追踪方法 |
| CN111239766B (zh) * | 2019-12-27 | 2021-12-07 | 北京航天控制仪器研究所 | 基于激光雷达的水面多目标快速识别跟踪方法 |
| CN113192110A (zh) * | 2020-01-14 | 2021-07-30 | 中寰卫星导航通信有限公司 | 多目标跟踪方法、装置、设备及存储介质 |
| CN111337941B (zh) * | 2020-03-18 | 2022-03-04 | 中国科学技术大学 | 一种基于稀疏激光雷达数据的动态障碍物追踪方法 |
| CN113498529B (zh) * | 2020-06-28 | 2022-08-26 | 华为技术有限公司 | 一种目标跟踪方法及其装置 |
| CN111813120A (zh) * | 2020-07-10 | 2020-10-23 | 北京林业大学 | 机器人的移动目标的识别方法、装置和电子设备 |
| CN111932943B (zh) * | 2020-10-15 | 2021-05-14 | 深圳市速腾聚创科技有限公司 | 动态目标的检测方法、装置、存储介质及路基监测设备 |
| CN112462381A (zh) * | 2020-11-19 | 2021-03-09 | 浙江吉利控股集团有限公司 | 一种基于车路协同的多激光雷达融合方法 |
| CN113298950B (zh) * | 2021-05-13 | 2024-04-19 | 京东鲲鹏(江苏)科技有限公司 | 一种物体属性的确定方法、装置、电子设备和存储介质 |
| CN113298016A (zh) * | 2021-06-10 | 2021-08-24 | 蔚来汽车科技(安徽)有限公司 | 基于神经网络的跟踪目标动静状态判断方法及其系统 |
| CN114063079B (zh) * | 2021-10-12 | 2022-06-21 | 福瑞泰克智能系统有限公司 | 目标置信度获取方法、装置、雷达系统和电子装置 |
| CN115575923B (zh) * | 2022-12-08 | 2023-03-28 | 千巡科技(深圳)有限公司 | 一种地面机器人静止判断方法、系统、装置以及存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107341819A (zh) * | 2017-05-09 | 2017-11-10 | 深圳市速腾聚创科技有限公司 | 目标跟踪方法及存储介质 |
| CN108664841A (zh) * | 2017-03-27 | 2018-10-16 | 郑州宇通客车股份有限公司 | 一种基于激光点云的动静态目标物识别方法及装置 |
| CN108985171A (zh) * | 2018-06-15 | 2018-12-11 | 上海仙途智能科技有限公司 | 运动状态估计方法和运动状态估计装置 |
| CN109298725A (zh) * | 2018-11-29 | 2019-02-01 | 重庆大学 | 一种基于phd滤波的群体机器人分布式多目标跟踪方法 |
| CN109521756A (zh) * | 2017-09-18 | 2019-03-26 | 百度在线网络技术（北京）有限公司 | 用于无人驾驶车辆的障碍物运动信息生成方法和装置 |
-
2019
- 2019-04-25 CN CN201910340665.XA patent/CN110018489B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108664841A (zh) * | 2017-03-27 | 2018-10-16 | 郑州宇通客车股份有限公司 | 一种基于激光点云的动静态目标物识别方法及装置 |
| CN107341819A (zh) * | 2017-05-09 | 2017-11-10 | 深圳市速腾聚创科技有限公司 | 目标跟踪方法及存储介质 |
| CN109521756A (zh) * | 2017-09-18 | 2019-03-26 | 百度在线网络技术（北京）有限公司 | 用于无人驾驶车辆的障碍物运动信息生成方法和装置 |
| CN108985171A (zh) * | 2018-06-15 | 2018-12-11 | 上海仙途智能科技有限公司 | 运动状态估计方法和运动状态估计装置 |
| CN109298725A (zh) * | 2018-11-29 | 2019-02-01 | 重庆大学 | 一种基于phd滤波的群体机器人分布式多目标跟踪方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110018489A (zh) | 2019-07-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110018489B (zh) | 基于激光雷达的目标追踪方法、装置及控制器和存储介质 | |
| US20190086923A1 (en) | Method and apparatus for generating obstacle motion information for autonomous vehicle | |
| CN106991389B (zh) | 确定道路边沿的装置和方法 | |
| KR101572851B1 (ko) | 동적 환경에서 모바일 플랫폼의 지도 작성방법 | |
| JP4637690B2 (ja) | 道路形状認識装置 | |
| JP6492469B2 (ja) | 自車走行レーン推定装置及びプログラム | |
| US10101448B2 (en) | On-board radar apparatus and region detection method | |
| CN110738690A (zh) | 一种基于多目标追踪框架的无人机视频中车速校正方法 | |
| WO2022001323A1 (zh) | 目标车辆的控制方法、装置、电子设备及存储介质 | |
| CN113189975A (zh) | 确定车辆的位置数据和/或运动数据的方法 | |
| CN115840221B (zh) | 基于4d毫米波雷达实现目标特征提取与多目标跟踪的方法 | |
| Zhang et al. | Single camera visual odometry based on random finite set statistics | |
| CN103047982A (zh) | 基于角度信息的目标自适应跟踪方法 | |
| CN112037257A (zh) | 一种目标跟踪方法、终端及其计算机可读存储介质 | |
| Hultqvist et al. | Detecting and positioning overtaking vehicles using 1D optical flow | |
| CN114609609A (zh) | Fmcw激光雷达随机采样提取静止点云的速度估计方法 | |
| CN111832343B (zh) | 跟踪方法和装置、存储介质 | |
| Lim et al. | A fusion method of data association and virtual detection for minimizing track loss and false track | |
| Vanpoperinghe et al. | Model-based detection and tracking of vehicle using a scanning laser rangefinder: A particle filtering approach | |
| CN112731335B (zh) | 一种基于全区域激光扫描的多无人机协作定位方法 | |
| JP2004219300A (ja) | 目標追尾装置 | |
| CN111239761B (zh) | 一种用于室内实时建立二维地图的方法 | |
| CN107256382A (zh) | 基于图像识别的虚拟保险杠控制方法和系统 | |
| Tlig et al. | Multi-Object tracking based on Kalman Filtering Combining Radar and Image Measurements | |
| CN113281736B (zh) | 基于多假设singer模型的雷达机动交汇目标跟踪方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |