CN108345823B

CN108345823B - 一种基于卡尔曼滤波的障碍物跟踪方法和装置

Info

Publication number: CN108345823B
Application number: CN201710057168.XA
Authority: CN
Inventors: 路晓静; 张磊
Original assignee: Zhengzhou Yutong Bus Co Ltd
Current assignee: Yutong Bus Co Ltd
Priority date: 2017-01-23
Filing date: 2017-01-23
Publication date: 2021-02-23
Anticipated expiration: 2037-01-23
Also published as: CN108345823A

Abstract

本发明提供一种基于卡尔曼滤波的障碍物跟踪方法和装置，方法包括如下步骤：步骤1：识别障碍物：步骤2：将障碍物的运动模型设置为匀速运动，对障碍物的位置进行预测，得到障碍物的预测位置；步骤3：根据障碍物位置的预测误差和观测误差，得到障碍物的位置增益；步骤4：根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置。本发明提供的一种基于卡尔曼滤波的障碍物跟踪方法，采用点云法确定障碍物的观测位置，然后采用卡尔曼滤波的方法对障碍的实际位置进行预测，由于本发明所提供的检测障碍物位置的方法比较简单，不需要花费大量的计算时间，所以能够保证对障碍物位置计算的实时性。

Description

一种基于卡尔曼滤波的障碍物跟踪方法和装置

技术领域

本发明属于车辆环境感知技术领域，具体涉及一种基于卡尔曼滤波的障碍物跟踪方法和装置。

背景技术

随着科学技术的发展，人们对车辆的智能化需求也越来越高，其中提高车辆的环境感知能力是提高车辆智能化的重要发展方向。

提高车辆的环境感知能力，首先需要提高车辆的障碍物识别和跟踪能力。障碍物识别和跟踪包括对原始数据进行分类、障碍物类型识别和障碍物速度识别。由于激光雷达检测精度高，数据输出频率高，且近年来激光雷达在汽车领域的发展趋于产业化，成本逐渐降低，因此被越来越多的应用到汽车自动驾驶中。如公布号为CN104931977A的专利文件所公开的一种用于智能车辆的障碍物识别方法，就是采用激光雷达获取障碍物的障碍点，即采用激光雷达获取障碍物的点云数据，然后对障碍物进行判断。这种方法虽然能够提高对障碍物类型识别的准确率，但是点云数据量巨大，算法也比较复杂，运算过程需要花费大量的时间，不能保证对障碍物识别的实时性。同时，这种方法只能对障碍物进行静态的识别，不能准确计算出障碍物的移动速度。

发明内容

本发明提供一种基于卡尔曼滤波的障碍物跟踪方法和装置，用于解决现有技术中对障碍物识别时算法复杂，运算过程需要花费大量时间的问题。

一种基于卡尔曼滤波的障碍物跟踪方法，包括如下步骤：

识别障碍物：

(1)获取点云；

(2)将点云粗栅格化，得到点云的初始聚类，计算出各初始聚类的几何中心；

(3)将点云细栅格化，根据各点云的位置和各初始聚类的几何中心位置，得到点云的新聚类，；所述细栅格化的间隔长度小于粗栅格化的栅格间隔长度；

(4)计算各新聚类的几何中心；

将各新聚类作为相应的障碍物；将每个新聚类的几何中心作为障碍物的观测位置；

将障碍物的运动模型设置为匀速运动，对障碍物的位置进行预测，得到障碍物的预测位置；

根据障碍物位置的预测误差和观测误差，得到障碍物的位置增益；

根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置。

本发明提供的一种基于卡尔曼滤波的障碍物跟踪方法，采用点云法确定障碍物的观测位置，然后采用卡尔曼滤波的方法对障碍的实际位置进行预测，由于本发明所提供的检测障碍物位置的方法比较简单，不需要花费大量的计算时间，所以能够保证对障碍物位置计算的实时性。

进一步的，还包括对障碍物速度的计算：

(1)将障碍物的运动模型设为匀速运动模型，得到障碍物的预测速度；

(2)根据两次检测到的障碍物的实际位置和两次检测的时间间隔，计算出障碍物的观测速度；

(3)根据障碍物速度的预测误差和观测误差，计算出障碍物速度的最优解，将其作为障碍物的实际速度。

增设计算障碍物速度的步骤，能够实时得到障碍物的运动状态，进一步的提高对障碍物跟踪的可靠性

进一步的，如果获取的是三维点云，则将其映射到平面中，得到平面内的点云。

如果只需检测障碍物的观测位置，不需要确定障碍物的形状，将点云数据映射到平面中，在二维的平面中计算，能够进一步的减少算法的复杂程度，提高运算的速度。

进一步的，将点云粗栅格化后，采用区域增长法对点云进行初始聚类：选中某个存在平面点云的栅格作为一个窗口，判断该窗口各相邻的栅格内是否存在平面点云，如果存在，则将其划分到该窗口内，将该窗口内的平面点云归为同一初始聚类。

进一步的，将点云细栅格化后，计算各点云与各初始聚类几何中心之间的距离，作为该点云与相应初始聚类之间的距离；将各点云归于与其距离最近的初始聚类，得到新聚类。

一种基于卡尔曼滤波的障碍物跟踪装置，包括如下模块：

识别障碍物的模块：

(1)获取点云；

(4)计算各新聚类的几何中心；

将障碍物的运动模型设置为匀速运动，对障碍物的位置进行预测，得到障碍物的预测位置的模块；

根据障碍物位置的预测误差和观测误差，得到障碍物的位置增益的模块；

根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置的模块。

进一步的，还包括对障碍物速度计算的模块：

附图说明

图1为采用点云算法识别障碍物的流程图；

图2为计算障碍物实际位置的流程图；

图3为计算障碍物实际速度的流程图。

具体实施方式

本发明提供一种基于卡尔曼滤波的障碍物跟踪方法和装置，用于解决现有技术中由于对障碍物速度观测的准确性低造成的对障碍物位置预测不准确的问题。

一种基于卡尔曼滤波的障碍物跟踪方法，包括如下步骤：

识别障碍物：

(1)获取点云；

(4)计算各新聚类的几何中心；

下面结合附图对本发明进行详细说明：

方法实施例：

本实施例提供一种基于卡尔曼滤波的障碍物跟踪方法，首先采用点云法对障碍物进行识别，并得到障碍物的观测位置，然后采用卡尔曼滤波原理计算出障碍物的实际位置和实际速度。

点云法识别障碍物：本实施例所提供的障碍物识别方法流程如图1所示，具体步骤如下：

(1)采用激光雷达获取障碍物的三维点云；

(2)将三维点云映射到二维平面中，得到平面点云；

(3)对平面点云粗栅格化，采用区域增长算法获取平面点云的初始聚类；

粗栅格化时选取的间隔长度一般在1-2m之间取值；

区域增长算法是指先选中某个存在平面点云的栅格作为一个窗口，判断该窗口各相邻的栅格内是否存在平面点云，如果存在，则将其划分到该窗口内；依次对该窗口进行扩展，直到与该窗口相邻的栅格内不存在平面点云为止；将该窗口内的平面点云归为同一初始聚类，以此得到平面点云的所有初始聚类；

(4)计算各初始聚类的几何中心；

设某个初始聚类的上下左右四个边界的栅格坐标分别为(x_up，y_up)、(x_buttom，y_buttom)、(x_left，y_left)、(x_right，y_right),则该初始聚类几何中心的横轴坐标为x_center＝(x_left+x_right)/2，纵轴坐标为y_center＝(y_left+y_right)/2；依次计算出各初始聚类的几何中心坐标；

(5)将平面点云细栅格化，计算出各平面点云与各初始聚类几何中心的距离，各平面点云与各初始聚类几何中心之间的距离即为各平面点云与相应初始聚类之间的距离；将各平面点云归入与其距离最近的初始聚类，得到新聚类；

细栅格化时选取的间隔长度一般在0.1-0.5m之间取值；

设某个点云的栅格坐标为(x_{point_cloud}，y_{point_cloud})，某个初始聚类的几何中心为(x_center，y_center)，则该点云与该初始聚类之间的距离为

(6)按照上述计算初始聚类几何中心的方法计算出各新聚类的几何中心；

为了保证对各新聚类几何中心计算的准确性，对同一个新聚类进行多次计算，当某个新聚类连续两次计算出的几何中心之间距离小于设定值时，判断为该新聚类的几何中心收敛平衡，选取该两次计算出的几何中心中任意一个，或者两个几何中心的中点作为该新聚类的几何中心；上述设定值一般在0.5-1m之间取值；

此时得到的各新聚类即为相应的障碍物，各新聚类的几何中心即为相应障碍物的观测位置。

计算障碍物的实际位置：本实施例所提供的计算障碍物实际位置的流程如图2所示，具体步骤如下：

(1)将障碍物的运动模型设定为匀速直线运动，则障碍物的预测位置为：

s(k|k-1)＝s(k-1|k-1)+v(k-1|k-1)×Δt

其中s(k|k-1)为障碍物当前时刻的观测位置，s(k-1|k-1)为障碍物上一时刻的实际位置，v(k-1|k-1)为障碍物上一时刻的实际速度，Δt为障碍物当前时刻与上一时刻之间的时间差；

(2)计算障碍物的位置增益

其中p为障碍物位置预测值的误差，q为障碍物位置观测值的误差；

(3)计算出障碍物位置的最优解s(k|k)＝s(k|k-1)+K_g×(z-s(k|k-1))，其中z为障碍物位置的观测值，障碍物位置的最优解s(k|k)即为障碍物的实际位置；

计算障碍物的实际速度：本实施例所提供的计算障碍物实际速度的流程如图3所示，具体步骤如下：

(1)计算障碍物的观测速度

(2)计算障碍物的预测速度v(k|k-1)＝v(k-1|k-1))；

(3)计算障碍物速度的增益

其中p_v障碍物速度预测值的误差，q_v为障碍物速度观测值的误差；

(4)计算障碍物速度的最优解v(k|k)＝v(k|k-1)+K_g×(z_v-v(k|k-1))；

障碍物速度的最优解即为障碍物的实际速度。

在本实施例中，设有计算障碍物实际速度的步骤；作为其他实施方法，当不需要检测障碍物速度时，可以不设置检测障碍物速度的步骤。

本实施例所提供的一种障碍物识别方法，为了计算方便，采用激光雷达获取障碍物的三维点云后将其映射到二维平面中，得到平面点云；作为其他实施方式，可以直接对三维点云进行计算，将上述平面点云的计算方法转化为三维点云的计算方法即可。

在本实施例中，采用区域增长算法划分初始聚类；作为其他实施方法，可以采用其他方法划分初始聚类，如根据图像中物体的轮廓得到初始聚类等。

在本实施例中，根据各点云与各初始聚类几何中心之间的距离得到新聚类；作为其他实施方式，可以采用其他方法，根据各点云的位置与各初始聚类几何中心的位置得到新聚类，如根据区域增长法得到新聚类。

装置实施例：

一种基于卡尔曼滤波的障碍物跟踪装置，包括如下模块：

识别障碍物的模块：

(1)获取点云；

(4)计算各新聚类的几何中心；

本实施例提供的一种基于卡尔曼滤波的障碍物跟踪装置，其中各模块并不是硬件模块，而是按照上述方法实施例所提供的方法进行编程，得到的软件模块，运行在车辆的整车控制器或者其他处理器中，可存储在移动存储装置或者固定存储装置中。

以上给出了本发明涉及的具体实施方式，但本发明不局限于所描述的实施方式。在本发明给出的思路下，采用对本领域技术人员而言容易想到的方式对上述实施例中的技术手段进行变换、替换、修改，并且起到的作用与本发明中的相应技术手段基本相同、实现的发明目的也基本相同，这样形成的技术方案是对上述实施例进行微调形成的，这种技术方案仍落入本发明的保护范围内。

Claims

1.一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，包括如下步骤：

识别障碍物：

(1)获取点云；

(3)将点云细栅格化，根据各点云的位置和各初始聚类的几何中心位置，得到点云的新聚类，所述细栅格化的间隔长度小于粗栅格化的栅格间隔长度；

(4)计算各新聚类的几何中心；

根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置；

将点云细栅格化后，计算各点云与各初始聚类几何中心之间的距离，作为该点云与相应初始聚类之间的距离；将各点云归于与其距离最近的初始聚类，得到新聚类；对同一个新聚类进行多次计算，当某个新聚类连续两次计算出的几何中心之间距离小于设定值时，判断为该新聚类的几何中心收敛平衡，选取该两次计算出的几何中心中任意一个，或者两个几何中心的中点作为该新聚类的几何中心。

2.根据权利要求1所述的一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，还包括对障碍物速度的计算：

3.根据权利要求1所述的一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，如果获取的是三维点云，则将其映射到平面中，得到平面内的点云。

4.根据权利要求1所述的一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，将点云粗栅格化后，采用区域增长法对点云进行初始聚类：选中某个存在平面点云的栅格作为一个窗口，判断该窗口各相邻的栅格内是否存在平面点云，如果存在，则将其划分到该窗口内，将该窗口内的平面点云归为同一初始聚类。

5.一种基于卡尔曼滤波的障碍物跟踪装置，其特征在于，包括如下模块：

识别障碍物的模块：

(1)获取点云；

(4)计算各新聚类的几何中心；

将点云细栅格化后，计算各点云与各初始聚类几何中心之间的距离，作为该点云与相应初始聚类之间的距离；将各点云归于与其距离最近的初始聚类，得到新聚类；对同一个新聚类进行多次计算，当某个新聚类连续两次计算出的几何中心之间距离小于设定值时，判断为该新聚类的几何中心收敛平衡，选取该两次计算出的几何中心中任意一个，或者两个几何中心的中点作为该新聚类的几何中心；

6.根据权利要求5所述的一种基于卡尔曼滤波的障碍物跟踪装置，其特征在于，还包括对障碍物速度计算的模块：

7.根据权利要求5所述的一种基于卡尔曼滤波的障碍物跟踪装置，其特征在于，如果获取的是三维点云，则将其映射到平面中，得到平面内的点云。

8.根据权利要求5所述的一种基于卡尔曼滤波的障碍物跟踪装置，其特征在于，将点云粗栅格化后，采用区域增长法对点云进行初始聚类：选中某个存在平面点云的栅格作为一个窗口，判断该窗口各相邻的栅格内是否存在平面点云，如果存在，则将其划分到该窗口内，将该窗口内的平面点云归为同一初始聚类。