一种基于ICP与多特征数据关联的障碍物预测与跟踪方法技术

本发明专利技术提供了一种基于ICP与多特征数据关联的障碍物预测与跟踪方法，根据激光雷达返回点云特征采用欧几里得聚类后再进行二次聚类的方法对雷达检测出的每一帧障碍物进行聚类；然后通过连续帧之间的聚类信息进行最近距离与点云强度信息多特征数据关联；最后，采用卡尔曼滤波器对障碍物进行稳定、持续的跟踪预测。本发明专利技术所述的一种基于ICP与多特征数据关联的障碍物预测与跟踪方法提高了点云聚类精度，而且为无人车提供了精确的动态障碍物信息。息。息。

【技术实现步骤摘要】
一种基于ICP与多特征数据关联的障碍物预测与跟踪方法


[0001]本专利技术属于激光雷达信息预测领域，尤其是涉及一种基于ICP与多特征数据关联的障碍物预测与跟踪方法。

技术介绍

[0002]激光雷达（Lidar）是智能驾驶领域中不可或缺的外部传感器，Lidar充当智能驾驶车的“眼睛”，通过自身扫描及反馈的激光点云（Lidar cloud points）来告诉智能车所面临的一切感知信息。虽然近年来视觉相机对周围信息感知技术得到了迅速发展，但是很容易受到光线的影响提供错误的信息。因此Lidar能够对周围环境的信息进行高精度的测量（位置、距离、3D cloud points），完成对障碍物的检测与跟踪并成功应用在智能车中。
[0003]障碍物的检测与跟踪对智能车决策控制及路径规划有着重要的意义，为智能车提供一个高精度、连续可持续的目标检测与跟踪是非常有必要的。但因lidar点云聚类算法太过直接及单特征进行数据关联，导致跟踪及预测不准确。

技术实现思路

[0004]有鉴于此，本专利技术旨在提出一种基于ICP与多特征数据关联的障碍物预测与跟踪方法，以解决激光雷达点云聚类算法太过直接及单特征进行数据关联，导致跟踪及预测不准确的问题。
[0005]为达到上述目的，本专利技术的技术方案是这样实现的：一种基于ICP与多特征数据关联的障碍物预测与跟踪方法，具体步骤如下：S1、根据雷达返回的点云特征进行以雷达为中心的多个区域划分，对每个区域内的点云设定聚类半径r并进行欧几里得点云聚类，获得每簇点云聚类的质心坐标；对于相邻区域两簇点云聚类的质心坐标的最近距离小于对应的聚类半径r时，进行二次聚类，即合并两簇点云并重新计算该簇合并点云的质心坐标；S2、通过最近邻迭代算法ICP建立当前帧所有点云聚类数据与相邻帧所有点云聚类数据之间的目标函数；在目标函数的基础上，引进雷达点云强度概率函数进行多特征数据关联，获得多特征数据关联函数；S3、根据步骤S2中连续两帧数据的多特征数据关联函数，并结合卡尔曼滤波获得点云状态预测方程，实现障碍物跟踪。
[0006]进一步的，步骤S1中，以雷达为中心的多个区域划分的具体方法：以雷达为中心依次向外划分多个区域，除中心区域位圆形外，其他区域均为环形；以激光雷达为中心的多个区域划分依次为0m~8m，8m~18m，18m~30m，30m~44m，44m~60m，且多个区域依次从雷达向外对应的聚类半径为r1，r2，r3，r4，r5；每簇点云聚类的质心坐标为，，其中为为某一簇的点云总个数，
为当前点云的序列号。
[0007]进一步的，通过最近邻迭代算法ICP建立T0时刻所有点云聚类数据与T1时刻所有点云聚类数据之间的目标函数，具体方法如下：S21、将当前帧T0时刻所有点云聚类数据存放至列表L0中，相邻帧T1时刻所有点云聚类数据存放至列表L1中，每一簇点云聚类的结果包括点云序号、质点坐标、强度信息；S22、在待关联的当前帧T0时刻所有点云聚类数据与相邻帧T1时刻所有点云聚类数据中，分别选取两个点集来表示源点集和目标点集，通过ICP建立源点集和目标点集之间的空间变换，形成目标函数，使得源点集和目标点集之间的距离最小。
[0008]进一步的，通过ICP建立源点集和目标点集之间的空间变换，形成目标函数，具体公式如下：设定源点集为P,目标点集为Q，旋转矩阵为R，平移矩阵为 t ，其中，n 为两个点集中的点云总个数，i为当前点云的序号。
[0009]进一步的，在关联函数的基础上，引进雷达点云强度概率函数进行多特征数据关联，获得多特征数据关联函数，具体公式如下：其中，为雷达点云强度概率函数，；为点云数据Q的一维离散强度概率函数；为点云数据P的一维离散强度概率函数；α=0.7；β=0.3。
[0010]进一步的，步骤S3中，根据步骤S2中连续两帧数据的多特征数据关联函数，并结合卡尔曼滤波获得点云状态预测方程，实现障碍物跟踪，具体步骤如下：S31、根据多特征数据关联函数D，建立初始预测方程：其中，为多特征数据关联函数D某一时刻状态的状态矩阵，为状态转移矩阵，与时间相关，为状态矩阵的同一时刻状态的状态协方差矩阵，
为过程协方差矩阵，根据环境噪声人为经验设定；S32、通过观测值减预测值获得残差方程；其中，为观测值，通过雷达观测获得；为装换矩阵，；S33、建立卡尔曼增益矩阵；其中，为新的残差协方差矩阵，；为观测噪声，根据传感器噪声设定；S34、获得点云状态预测方程，进行障碍物预测，具体方程如下：S34、获得点云状态预测方程，进行障碍物预测，具体方程如下：为单位矩阵。
[0011]相对于现有技术，本专利技术所述的一种基于ICP与多特征数据关联的障碍物预测与跟踪方法具有以下有益效果：（1）本专利技术所述的一种基于ICP与多特征数据关联的障碍物预测与跟踪方法，根据激光雷达返回点云特征采用欧几里得聚类后再进行二次聚类的方法对雷达检测出的每一帧障碍物进行聚类；然后通过连续帧之间的聚类信息进行最近距离与点云强度信息多特征数据关联；最后，采用卡尔曼滤波器对障碍物进行稳定、持续的跟踪预测，提高了点云聚类精度，而且为无人车提供了精确的动态障碍物信息。
[0012]（2）本专利技术所述的一种基于ICP与多特征数据关联的障碍物预测与跟踪方法，对于相邻区域两簇点云聚类的质心坐标的最近距离小于对应的聚类半径时，进行二次聚类，解决了现有的聚类过拟合的问题。
附图说明
[0013]构成本专利技术的一部分的附图用来提供对本专利技术的进一步理解，本专利技术的示意性实施例及其说明用于解释本专利技术，并不构成对本专利技术的不当限定。在附图中：图1为本专利技术实施例所述的一种基于ICP与多特征数据关联的障碍物预测与跟踪方法流程图；图2为本专利技术实施例所述的欧几里得点云聚类与二次聚类配合示意图。
具体实施方式
[0014]需要说明的是，在不冲突的情况下，本专利技术中的实施例及实施例中的特征可以相互组合。
[0015]在本专利技术的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本专利技术和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本专利技术的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本专利技术的描述中，除非另有说明，“多个”的含义是两个或两个以上。
[0016]在本专利技术的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于ICP与多特征数据关联的障碍物预测与跟踪方法，其特征在于，具体步骤如下：S1、根据雷达返回的点云特征进行以雷达为中心的多个区域划分，对每个区域内的点云设定聚类半径r并进行欧几里得点云聚类，获得每簇点云聚类的质心坐标；对于相邻区域两簇点云聚类的质心坐标的最近距离小于对应的聚类半径r时，进行二次聚类，即合并两簇点云并重新计算该簇合并点云的质心坐标；S2、通过最近邻迭代算法ICP建立当前帧所有点云聚类数据与相邻帧所有点云聚类数据之间的目标函数；在目标函数的基础上，引进雷达点云强度概率函数进行多特征数据关联，获得多特征数据关联函数；S3、根据步骤S2中连续两帧数据的多特征数据关联函数，并结合卡尔曼滤波获得点云状态预测方程，实现障碍物跟踪。2.根据权利要求1所述的一种基于ICP与多特征数据关联的障碍物预测与跟踪方法，其特征在于：步骤S1中，以雷达为中心的多个区域划分的具体方法：以雷达为中心依次向外划分多个区域，除中心区域位圆形外，其他区域均为环形；以激光雷达为中心的多个区域划分依次为0m~8m，8m~18m，18m~30m，30m~44m，44m~60m，且多个区域依次从雷达向外对应的聚类半径为r1，r2，r3，r4，r5；每簇点云聚类的质心坐标为，，其中为为某一簇的点云总个数，为当前点云的序列号。3.根据权利要求1所述的一种基于ICP与多特征数据关联的障碍物预测与跟踪方法，其特征在于：通过最近邻迭代算法ICP建立T0时刻所有点云聚类数据与T1时刻所有点云聚类数据之间的目标函数，具体方法如下：S21、将当前帧T0时刻所有点云聚类数据存放至列表L0中，相邻帧T1时刻所有点云聚类数据存放至列表L1中，每一簇点云聚类的结果包括点云序号、质点坐标、强度信息；S22、在待关联的当前帧T0时刻所有点云聚类数据与相邻帧T1时刻所...

【专利技术属性】
技术研发人员：郭蓬，王庆闪，杨建森，夏海鹏，张登权，蔡聪，
申请(专利权)人：中国汽车技术研究中心有限公司，
类型：发明
国别省市：