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

一种基于icp与多特征数据关联的障碍物预测与跟踪方法
技术领域
1.本发明属于激光雷达信息预测领域，尤其是涉及一种基于icp与多特征数据关联的障碍物预测与跟踪方法。


背景技术：

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


技术实现要素：

4.有鉴于此，本发明旨在提出一种基于icp与多特征数据关联的障碍物预测与跟踪方法，以解决激光雷达点云聚类算法太过直接及单特征进行数据关联，导致跟踪及预测不准确的问题。
5.为达到上述目的，本发明的技术方案是这样实现的：一种基于icp与多特征数据关联的障碍物预测与跟踪方法，具体步骤如下：s1、根据雷达返回的点云特征进行以雷达为中心的多个区域划分，对每个区域内的点云设定聚类半径r并进行欧几里得点云聚类，获得每簇点云聚类的质心坐标；对于相邻区域两簇点云聚类的质心坐标的最近距离小于对应的聚类半径r时，进行二次聚类，即合并两簇点云并重新计算该簇合并点云的质心坐标；s2、通过最近邻迭代算法icp建立当前帧所有点云聚类数据与相邻帧所有点云聚类数据之间的目标函数；在目标函数的基础上，引进雷达点云强度概率函数进行多特征数据关联，获得多特征数据关联函数；s3、根据步骤s2中连续两帧数据的多特征数据关联函数，并结合卡尔曼滤波获得点云状态预测方程，实现障碍物跟踪。
6.进一步的，步骤s1中，以雷达为中心的多个区域划分的具体方法：以雷达为中心依次向外划分多个区域，除中心区域位圆形外，其他区域均为环形；以激光雷达为中心的多个区域划分依次为0m~8m，8m~18m，18m~30m，30m~44m，44m~60m，且多个区域依次从雷达向外对应的聚类半径为r1，r2，r3，r4，r5；每簇点云聚类的质心坐标为，，其中为为某一簇的点云总个数，
为当前点云的序列号。
7.进一步的，通过最近邻迭代算法icp建立t0时刻所有点云聚类数据与t1时刻所有点云聚类数据之间的目标函数，具体方法如下：s21、将当前帧t0时刻所有点云聚类数据存放至列表l0中，相邻帧t1时刻所有点云聚类数据存放至列表l1中，每一簇点云聚类的结果包括点云序号、质点坐标、强度信息；s22、在待关联的当前帧t0时刻所有点云聚类数据与相邻帧t1时刻所有点云聚类数据中，分别选取两个点集来表示源点集和目标点集，通过icp建立源点集和目标点集之间的空间变换，形成目标函数，使得源点集和目标点集之间的距离最小。
8.进一步的，通过icp建立源点集和目标点集之间的空间变换，形成目标函数，具体公式如下：设定源点集为p,目标点集为q，旋转矩阵为r，平移矩阵为 t ，其中，n 为两个点集中的点云总个数，i为当前点云的序号。
9.进一步的，在关联函数的基础上，引进雷达点云强度概率函数进行多特征数据关联，获得多特征数据关联函数，具体公式如下：其中，为雷达点云强度概率函数，；为点云数据q的一维离散强度概率函数；为点云数据p的一维离散强度概率函数；α=0.7；β=0.3。
10.进一步的，步骤s3中，根据步骤s2中连续两帧数据的多特征数据关联函数，并结合卡尔曼滤波获得点云状态预测方程，实现障碍物跟踪，具体步骤如下：s31、根据多特征数据关联函数d，建立初始预测方程：其中，为多特征数据关联函数d某一时刻状态的状态矩阵，为状态转移矩阵，与时间相关，为状态矩阵的同一时刻状态的状态协方差矩阵，
为过程协方差矩阵，根据环境噪声人为经验设定；s32、通过观测值减预测值获得残差方程；其中，为观测值，通过雷达观测获得；为装换矩阵，；s33、建立卡尔曼增益矩阵；其中，为新的残差协方差矩阵，；为观测噪声，根据传感器噪声设定；s34、获得点云状态预测方程，进行障碍物预测，具体方程如下：s34、获得点云状态预测方程，进行障碍物预测，具体方程如下：为单位矩阵。
11.相对于现有技术，本发明所述的一种基于icp与多特征数据关联的障碍物预测与跟踪方法具有以下有益效果：（1）本发明所述的一种基于icp与多特征数据关联的障碍物预测与跟踪方法，根据激光雷达返回点云特征采用欧几里得聚类后再进行二次聚类的方法对雷达检测出的每一帧障碍物进行聚类；然后通过连续帧之间的聚类信息进行最近距离与点云强度信息多特征数据关联；最后，采用卡尔曼滤波器对障碍物进行稳定、持续的跟踪预测，提高了点云聚类精度，而且为无人车提供了精确的动态障碍物信息。
12.（2）本发明所述的一种基于icp与多特征数据关联的障碍物预测与跟踪方法，对于相邻区域两簇点云聚类的质心坐标的最近距离小于对应的聚类半径时，进行二次聚类，解决了现有的聚类过拟合的问题。
附图说明
13.构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：图1为本发明实施例所述的一种基于icp与多特征数据关联的障碍物预测与跟踪方法流程图；图2为本发明实施例所述的欧几里得点云聚类与二次聚类配合示意图。
具体实施方式
14.需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
15.在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
16.在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。
17.下面将参考附图并结合实施例来详细说明本发明。
18.如图1、图2所示，一种基于icp与多特征数据关联的障碍物预测与跟踪方法，具体步骤如下：s1、根据雷达返回的点云特征进行以雷达为中心的多个区域划分，对每个区域内的点云设定聚类半径r并进行欧几里得点云聚类，获得每簇点云聚类的质心坐标；对于相邻区域两簇点云聚类的质心坐标的最近距离小于对应的聚类半径r时，进行二次聚类，即合并两簇点云并重新计算该簇合并点云的质心坐标；相邻区域两簇点云聚类的质心坐标的最近距离d与聚类半径r进行对比，若相邻区域两簇点云聚类的质心坐标的最近距离d大于r，则是两个不同的类；若相邻区域两簇点云聚类的质心坐标的最近距离d小于等于r，出现了过拟合现象，进行两簇点云的合并，并重新计算该簇点云的质心坐标，进而完成二次聚类。
19.s2、通过最近邻迭代算法icp建立当前帧所有点云聚类数据与相邻帧所有点云聚类数据之间的目标函数；在目标函数的基础上，引进雷达点云强度概率函数进行多特征数据关联，获得多特征数据关联函数；s3、根据步骤s2中连续两帧数据的多特征数据关联函数，并结合卡尔曼滤波获得点云状态预测方程，实现障碍物跟踪。
20.如图1、图2所示，步骤s1中，以雷达为中心的多个区域划分的具体方法：以雷达为中心依次向外划分多个区域，除中心区域位圆形外，其他区域均为环形；以激光雷达为中心的多个区域划分依次为0m~8m，8m~18m，18m~30m，30m~44m，44m~60m，且多个区域依次从雷达向外对应的聚类半径为r1，r2，r3，r4，r5；每簇点云聚类的质心坐标为，，其中为为某一簇的点云总个数，
为当前点云的序列号。
21.欧几里得点云聚类算法如下：1）找到某一区域中的点p，使用近邻查询算法kd-tree找到离他最近的n个点，判断这n个点到p的距离，将距离小于该区域的聚类半径的点p12，p13,p14...放在q里；2）在q\p里找到一点p12，重复1）；3）在q\p\p12找到一点，重复1），找到p22，p23，p24....全部放进q里；4）当q再也不能有新点加入了，则完成搜索，实现欧几里得聚类。
22.如图1所示，通过最近邻迭代算法icp建立t0时刻所有点云聚类数据与t1时刻所有点云聚类数据之间的目标函数，具体方法如下：s21、将当前帧t0时刻所有点云聚类数据存放至列表l0中，相邻帧t1时刻所有点云聚类数据存放至列表l1中，每一簇点云聚类的结果包括点云序号、质点坐标、强度信息；所述强度信息通过雷达获得。
23.s22、在待关联的当前帧t0时刻所有点云聚类数据与相邻帧t1时刻所有点云聚类数据中，分别选取两个点集来表示源点集和目标点集，通过icp建立源点集和目标点集之间的空间变换，形成目标函数，使得源点集和目标点集之间的距离最小。
24.如图1所示，通过icp建立源点集和目标点集之间的空间变换，形成目标函数，具体公式如下：设定源点集为p,目标点集为q，旋转矩阵为r，平移矩阵为 t ，其中，n 为两个点集中的点云总个数，i为当前点云的序号。
25.用目标函数e(r,t)来表示源点集p在变换矩阵(r,t)下与目标点集q 之间的误差，则求解最优变换矩阵的问题可以转化为满足min e(r,t)的最优解 (r,t)。
26.如图1所示，在关联函数的基础上，引进雷达点云强度概率函数进行多特征数据关联，获得多特征数据关联函数，具体公式如下：其中，为雷达点云强度概率函数，；为点云数据q的一维离散强度概率函数；为点云数据p的一维离散强度概率函数；α=0.7；β=0.3。
27.越小则关联程度越高，根据实际场景及lidar返回的周围点云信息进行测试，最终α和β权重分别为0.7和0.3。
28.如图1所示，步骤s3中，根据步骤s2中连续两帧数据的多特征数据关联函数，并结
合卡尔曼滤波获得点云状态预测方程，实现障碍物跟踪，具体步骤如下：s31、根据多特征数据关联函数d，建立初始预测方程：其中，为多特征数据关联函数d某一时刻状态的状态矩阵，为状态转移矩阵，与时间相关，为状态矩阵的同一时刻状态的状态协方差矩阵，为过程协方差矩阵，根据环境噪声人为经验设定；s32、通过观测值减预测值获得残差方程；其中，为观测值，通过雷达观测获得；为装换矩阵，；s33、建立卡尔曼增益矩阵；其中，为新的残差协方差矩阵，；为观测噪声，根据传感器噪声设定；s34、获得点云状态预测方程，进行障碍物预测，具体方程如下：s34、获得点云状态预测方程，进行障碍物预测，具体方程如下：为单位矩阵。
29.以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。