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

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

A method and device for obstacle tracking based on Calman filtering

The present invention provides an obstacle tracking method and device based on Calman filter. The method includes the following steps: Step 1: identify obstacles: Step 2: set the motion model of the obstacle to a uniform motion, predict the position of the obstacle, and get the prediction position of the obstacle; step 3: according to the position of the obstacle. The prediction error and observation error are obtained, and the position gain of the obstacle is obtained. Step 4: the optimal solution of the position is calculated according to the predicted position, the observation position and the position gain, and it is used as the actual position of the obstacle. The invention provides an obstacle tracking method based on Calman filter. The point cloud method is used to determine the observation position of the obstacle, and then the actual position of the obstacle is predicted by the Calman filter. Because the method of detecting the obstacle location provided by the invention is simpler, it does not need to spend a lot of calculation. Time, so it can ensure the real-time location of obstacles.

【技术实现步骤摘要】
一种基于卡尔曼滤波的障碍物跟踪方法和装置
本专利技术属于车辆环境感知
，具体涉及一种基于卡尔曼滤波的障碍物跟踪方法和装置。
技术介绍
随着科学技术的发展，人们对车辆的智能化需求也越来越高，其中提高车辆的环境感知能力是提高车辆智能化的重要发展方向。提高车辆的环境感知能力，首先需要提高车辆的障碍物识别和跟踪能力。障碍物识别和跟踪包括对原始数据进行分类、障碍物类型识别和障碍物速度识别。由于激光雷达检测精度高，数据输出频率高，且近年来激光雷达在汽车领域的发展趋于产业化，成本逐渐降低，因此被越来越多的应用到汽车自动驾驶中。如公布号为CN104931977A的专利文件所公开的一种用于智能车辆的障碍物识别方法，就是采用激光雷达获取障碍物的障碍点，即采用激光雷达获取障碍物的点云数据，然后对障碍物进行判断。这种方法虽然能够提高对障碍物类型识别的准确率，但是点云数据量巨大，算法也比较复杂，运算过程需要花费大量的时间，不能保证对障碍物识别的实时性。同时，这种方法只能对障碍物进行静态的识别，不能准确计算出障碍物的移动速度。
技术实现思路
本专利技术提供一种基于卡尔曼滤波的障碍物跟踪方法和装置，用于解决现有技术中对障碍物识别时算法复杂，运算过程需要花费大量时间的问题。一种基于卡尔曼滤波的障碍物跟踪方法，包括如下步骤：识别障碍物：(1)获取点云；(2)将点云粗栅格化，得到点云的初始聚类，计算出各初始聚类的几何中心；(3)将点云细栅格化，根据各点云的位置和各初始聚类的几何中心位置，得到点云的新聚类，所述细栅格化的间隔长度小于粗栅格化的栅格间隔长度；(4)计算各新聚类的几何中心；将各新聚类作为相应的障碍物；将每个新聚类的几何中心作为障碍物的观测位置；将障碍物的运动模型设置为匀速运动，对障碍物的位置进行预测，得到障碍物的预测位置；根据障碍物位置的预测误差和观测误差，得到障碍物的位置增益；根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置。本专利技术提供的一种基于卡尔曼滤波的障碍物跟踪方法，采用点云法确定障碍物的观测位置，然后采用卡尔曼滤波的方法对障碍的实际位置进行预测，由于本专利技术所提供的检测障碍物位置的方法比较简单，不需要花费大量的计算时间，所以能够保证对障碍物位置计算的实时性。进一步的，还包括对障碍物速度的计算：(1)将障碍物的运动模型设为匀速运动模型，得到障碍物的预测速度；(2)根据两次检测到的障碍物的实际位置和两次检测的时间间隔，计算出障碍物的观测速度；(3)根据障碍物速度的预测误差和观测误差，计算出障碍物速度的最优解，将其作为障碍物的实际速度。增设计算障碍物速度的步骤，能够实时得到障碍物的运动状态，进一步的提高对障碍物跟踪的可靠性。进一步的，如果获取的是三维点云，则将其映射到平面中，得到平面内的点云。如果只需检测障碍物的观测位置，不需要确定障碍物的形状，将点云数据映射到平面中，在二维的平面中计算，能够进一步的减少算法的复杂程度，提高运算的速度。进一步的，将点云粗栅格化后，采用区域增长法对点云进行初始聚类：选中某个存在平面点云的栅格作为一个窗口，判断该窗口各相邻的栅格内是否存在平面点云，如果存在，则将其划分到该窗口内，将该窗口内的平面点云归为同一初始聚类。进一步的，将点云细栅格化后，计算各点云与各初始聚类几何中心之间的距离，作为该点云与相应初始聚类之间的距离；将各点云归于与其距离最近的初始聚类，得到新聚类。一种基于卡尔曼滤波的障碍物跟踪装置，包括如下模块：识别障碍物的模块：(1)获取点云；(2)将点云粗栅格化，得到点云的初始聚类，计算出各初始聚类的几何中心；(3)将点云细栅格化，根据各点云的位置和各初始聚类的几何中心位置，得到点云的新聚类，所述细栅格化的间隔长度小于粗栅格化的栅格间隔长度；(4)计算各新聚类的几何中心；将各新聚类作为相应的障碍物；将每个新聚类的几何中心作为障碍物的观测位置；将障碍物的运动模型设置为匀速运动，对障碍物的位置进行预测，得到障碍物的预测位置的模块；根据障碍物位置的预测误差和观测误差，得到障碍物的位置增益的模块；根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置的模块。进一步的，还包括对障碍物速度计算的模块：(1)将障碍物的运动模型设为匀速运动模型，得到障碍物的预测速度；(2)根据两次检测到的障碍物的实际位置和两次检测的时间间隔，计算出障碍物的观测速度；(3)根据障碍物速度的预测误差和观测误差，计算出障碍物速度的最优解，将其作为障碍物的实际速度。进一步的，如果获取的是三维点云，则将其映射到平面中，得到平面内的点云。进一步的，将点云粗栅格化后，采用区域增长法对点云进行初始聚类：选中某个存在平面点云的栅格作为一个窗口，判断该窗口各相邻的栅格内是否存在平面点云，如果存在，则将其划分到该窗口内，将该窗口内的平面点云归为同一初始聚类。进一步的，将点云细栅格化后，计算各点云与各初始聚类几何中心之间的距离，作为该点云与相应初始聚类之间的距离；将各点云归于与其距离最近的初始聚类，得到新聚类。附图说明图1为采用点云算法识别障碍物的流程图；图2为计算障碍物实际位置的流程图；图3为计算障碍物实际速度的流程图。具体实施方式本专利技术提供一种基于卡尔曼滤波的障碍物跟踪方法和装置，用于解决现有技术中由于对障碍物速度观测的准确性低造成的对障碍物位置预测不准确的问题。一种基于卡尔曼滤波的障碍物跟踪方法，包括如下步骤：识别障碍物：(1)获取点云；(2)将点云粗栅格化，得到点云的初始聚类，计算出各初始聚类的几何中心；(3)将点云细栅格化，根据各点云的位置和各初始聚类的几何中心位置，得到点云的新聚类，；所述细栅格化的间隔长度小于粗栅格化的栅格间隔长度；(4)计算各新聚类的几何中心；将各新聚类作为相应的障碍物；将每个新聚类的几何中心作为障碍物的观测位置；将障碍物的运动模型设置为匀速运动，对障碍物的位置进行预测，得到障碍物的预测位置；根据障碍物位置的预测误差和观测误差，得到障碍物的位置增益；根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置。本专利技术提供的一种基于卡尔曼滤波的障碍物跟踪方法，采用点云法确定障碍物的观测位置，然后采用卡尔曼滤波的方法对障碍的实际位置进行预测，由于本专利技术所提供的检测障碍物位置的方法比较简单，不需要花费大量的计算时间，所以能够保证对障碍物位置计算的实时性。下面结合附图对本专利技术进行详细说明：方法实施例：本实施例提供一种基于卡尔曼滤波的障碍物跟踪方法，首先采用点云法对障碍物进行识别，并得到障碍物的观测位置，然后采用卡尔曼滤波原理计算出障碍物的实际位置和实际速度。点云法识别障碍物：本实施例所提供的障碍物识别方法流程如图1所示，具体步骤如下：(1)采用激光雷达获取障碍物的三维点云；(2)将三维点云映射到二维平面中，得到平面点云；(3)对平面点云粗栅格化，采用区域增长算法获取平面点云的初始聚类；粗栅格化时选取的间隔长度一般在1-2m之间取值；区域增长算法是指先选中某个存在平面点云的栅格作为一个窗口，判断该窗口各相邻的栅格内是否存在平面点云，如果存在，则将其划分到该窗口内；依次对该窗口进行扩展，直到与该窗口相邻的栅格内不存在平面点云为止；将该窗口内的平面点云本文档来自技高网...

【技术保护点】
1.一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，包括如下步骤：识别障碍物：(1)获取点云；(2)将点云粗栅格化，得到点云的初始聚类，计算出各初始聚类的几何中心；(3)将点云细栅格化，根据各点云的位置和各初始聚类的几何中心位置，得到点云的新聚类，所述细栅格化的间隔长度小于粗栅格化的栅格间隔长度；(4)计算各新聚类的几何中心；将各新聚类作为相应的障碍物；将每个新聚类的几何中心作为障碍物的观测位置；将障碍物的运动模型设置为匀速运动，对障碍物的位置进行预测，得到障碍物的预测位置；根据障碍物位置的预测误差和观测误差，得到障碍物的位置增益；根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置。

【技术特征摘要】
1.一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，包括如下步骤：识别障碍物：(1)获取点云；(2)将点云粗栅格化，得到点云的初始聚类，计算出各初始聚类的几何中心；(3)将点云细栅格化，根据各点云的位置和各初始聚类的几何中心位置，得到点云的新聚类，所述细栅格化的间隔长度小于粗栅格化的栅格间隔长度；(4)计算各新聚类的几何中心；将各新聚类作为相应的障碍物；将每个新聚类的几何中心作为障碍物的观测位置；将障碍物的运动模型设置为匀速运动，对障碍物的位置进行预测，得到障碍物的预测位置；根据障碍物位置的预测误差和观测误差，得到障碍物的位置增益；根据预测位置、观测位置和位置增益计算出位置的最优解，将其作为障碍物的实际位置。2.根据权利要求1所述的一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，还包括对障碍物速度的计算：(1)将障碍物的运动模型设为匀速运动模型，得到障碍物的预测速度；(2)根据两次检测到的障碍物的实际位置和两次检测的时间间隔，计算出障碍物的观测速度；(3)根据障碍物速度的预测误差和观测误差，计算出障碍物速度的最优解，将其作为障碍物的实际速度。3.根据权利要求1所述的一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，如果获取的是三维点云，则将其映射到平面中，得到平面内的点云。4.根据权利要求1所述的一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，将点云粗栅格化后，采用区域增长法对点云进行初始聚类：选中某个存在平面点云的栅格作为一个窗口，判断该窗口各相邻的栅格内是否存在平面点云，如果存在，则将其划分到该窗口内，将该窗口内的平面点云归为同一初始聚类。5.根据权利要求1所述的一种基于卡尔曼滤波的障碍物跟踪方法，其特征在于，将点云细栅格化后，计算各点云与各初始聚类几何中心之间的距离，作为该点云与相应初始聚类之间的距离；将各点云归于与其距离最近的初始聚类，得到新聚类。6.一种基于卡...

【专利技术属性】
技术研发人员：路晓静，张磊，
申请(专利权)人：郑州宇通客车股份有限公司，
类型：发明
国别省市：河南,41