CN110349192A - 一种基于三维激光点云的在线目标跟踪系统的跟踪方法 - Google Patents

Abstract

本发明公开了一种基于三维激光点云的在线目标跟踪系统的跟踪方法，包含了预处理模块、检测模块与跟踪模块三个模块：预处理模块对地面点云与道路边界外点云的滤除；检测模块对点云进行聚类与时域关联，并通过增量式的高斯混合模型对各目标进行表面建模；跟踪模块使用卡尔曼滤波，对模型的中心点进行跟踪。该标定系统依赖离线的道路边界地图，可适应于多种不同的结构化道路场景，实现了实时的三维激光点云检测与跟踪，对无人车的感知技术有着重要的意义。这一技术可广泛应用于无人车环境感知、辅助安全驾驶等领域。

Description

一种基于三维激光点云的在线目标跟踪系统的跟踪方法

技术领域

本发明属于计算机视觉与智能交通领域，特别涉及一种基于三维激光点云的在线目标跟踪系统的跟踪方法。

背景技术

智能车(Intelligent Vehicle，IV)是集环境感知、规划决策、多等级辅助驾驶等功能于一体的综合系统，它集中运用了计算机、现代传感、信息融合、通讯、人工智能及自动控制等技术，是典型的高新技术综合体，是一个国家科研实力和产业水平的重要标志。环境感知技术是智能车的关键技术之一。三维激光传感器提供了精确的障碍目标的距离信息，是智能车环境感知中重要的硬件组成部分。如何对三维激光传感器提供的原始三维点云进行处理，实时地完成道路上目标的检测与跟踪，从而增强无人车的感知能力，提高无人车的安全性能，是智能车的感知技术中非常关键的问题。

发明内容

本发明的目的在于提供一种基于三维激光点云的在线目标跟踪系统的跟踪方法，以解决上述问题。

为实现上述目的，本发明采用以下技术方案：

一种基于三维激光点云的在线目标跟踪系统的跟踪方法，所述三维激光点云的在线目标跟踪系统包括预处理模块、检测模块和跟踪模块；预处理模块、检测模块和跟踪模块依次连接；预处理模块用于对三维激光原始点云进行预处理，去除非地面点云与道路外点云；检测模块用于对于预处理模块给出的点云进行聚类，并将点云拟合成矩形模型；跟踪模块使用拟合矩形进行时域的跟踪，给出目标的速度信息；

所述基于三维激光点云的在线目标跟踪系统的跟踪方法包括以下步骤：

步骤1，预处理模块使用高斯过程回归算法分割地面，滤除地面点云；使用预先建立的道路边界地图滤除道路外点云；

步骤2，检测模块使用DBSCAN聚类算法对于三维激光点云进行聚类；对于每个聚类后的目标，使用Kuhn-Munkres算法与前一帧数据进行数据关联，并使用增量式的高斯混合体建立表面点云模型；对于每个时刻的表面模型，以平台的车体坐标系x与y方向使用矩形模型进行矩形拟合，作为检测结果；

步骤3，跟踪模块选取步骤2中拟合矩形的中点作为跟踪点。

进一步的，步骤1中具体为：使用宽40m、长80m的矩形建立栅格图，利用当前位姿数据，将全局的道路边界地图转换到车体坐标系中，并提取出与栅格图大小对应的局部道路边界地图；使用漫水填充算法对道路边界内区域的连通性进行分析，将栅格图中道路边界内的栅格设置为1，边界外栅格设置为0；将所有点云投影到栅格中，若点云所在栅格数值为0，则进行滤除，否则保留。

进一步的，步骤2中具体包括：

使用增量式混合高斯模型建立目标的表面点云模型，使用EM算法迭代地对模型进行优化，高斯混合模型可以被表示为：

其中K是高斯模型的个数，μ_k与σ_k是第k个高斯模型的均值与协方差，且 是第j帧观测中的第i个点云，R_j和t_j是将第j帧点云对齐到第一帧点云坐标系的旋转矩阵和平移向量，p_k是第k个高斯模型的混合系数；第k个高斯模型可表示为

因此，一个完整的高斯混合模型可以被表示为

其中第一部分为高斯模型参数，第二部分为点云的刚体变换参数。

进一步的，利用增量式高斯混合模型建模，并使用EM算法进行求解；高斯混合模型的隐变量Γ中的元素γ_ijk表示了点云z_ji属于第k个高斯模型的概率；求解高斯混合模型的EM算法可以被分解为三个步骤：E-步、M-步与M-GMM-步；

E-步使用高斯混合模型的参数Θ更新隐变量Γ；对于每个γ_ijk∈Γ有

其中为m-1时刻计算得到的高斯模型参数；

M-步通过E-步中得到的隐函数Γ与高斯混合模型参数Θ中的高斯模型部分对刚体变换部分进行更新；通过最小化每帧观测的点云与每个高斯模型的距离优化刚体变换矩阵，能量函数表示为

其中λ_jk表示第j帧的点作用于第k个高斯模型的概率之和

ω_jk为第j帧的点云的使用隐函数后的加权重心

M-GMM-步通过隐函数Γ与M-步中的更新的刚体变换部分对高斯模型部分进行更新：

其中

u_mk＝R^(m)ω_mk+t^(m)

N^(m)表示到m帧为止所有帧的点云个数之和；

EM算法通过设置点云间的距离阈值或30次的最大循环次数作为迭代的退出条件，最终给出优化后的高斯模型参数；使用该参数可以将所有时刻的点云拼接到当前时刻的车体坐标系中，得到目标的表面点云模型。

进一步的，步骤3中，使用卡尔曼滤波器进行跟踪。

进一步的，卡尔曼滤波器的状态定义为x＝[x,y,vx,vy]，其中x与y表示车体坐标系下目标的位置，vx与vy表示目标相对于车体的运动速度。卡尔曼滤波器的观测为 z＝[x,y]，其中x与y表示观测时刻拟合矩形的中心点坐标。

与现有技术相比，本发明有以下技术效果：

本发明实现对激光传感器数据的检测与跟踪。与传统方法相比，本发明基于高斯混合模型对跟踪目标建立表面点云模型。随着观测时间的增加，表面点云模型愈发完善，比单帧观测的结果更加全面、稳定。通过选取稳定的表面模型拟合矩形中点作为特征点进行跟踪，提高了卡尔曼滤波器中观测的稳定性，使得速度跟踪效果得到改善。

本发明基于高性能的三维激光传感器与高精度位姿系统，结合离线道路边界地图，对道路场景内的目标进行检测与跟踪，能够满足复杂交通场景下的无人车感知需求。

附图说明

图1为本发明的流程框图。

图2为本发明激光点云预处理各步骤效果图，其中包括了原始点云数据，地面滤除后的点云数据，道路边界滤除后的点云数据。

图3为本发明基于增量式高斯混合模型的目标表面点云拼接效果图。该模型使用了 425帧观测数据，包含了车辆各个角度的观测，使用增量式高斯混合模型最终拼接成完整的点云模型。

具体实施方式

以下结合附图对本发明进一步说明：

一种智能车激光传感器的在线检测与跟踪方法，包括以下步骤：

一种智能车激光传感器的在线检测与跟踪方法，包括预处理模块、检测模块与跟踪模块三个模块依次连接；

预处理模块对三维激光原始点云进行预处理：使用谌彤童于2012年提出的基于高斯过程回归的地面分割算法滤除地面点云；将预先建立的全局道路边界地图根据位姿数据转换到激光坐标系中，并根据激光传感器感知范围选择局部道路边界地图；使用局部道路边界地图对点云进行滤除。

进一步的，在使用道路边界地图进行点云滤除时，使用宽40m、长80m的矩形建立栅格图，利用当前位姿数据，将全局的道路边界地图转换到车体坐标系中，并提取出与栅格图大小对应的局部道路边界地图；使用漫水填充算法对道路边界内区域的连通性进行分析，将栅格图中道路边界内的栅格设置为1，边界外栅格设置为0；将所有点云投影到栅格中，若点云所在栅格数值为0，则进行滤除，否则保留。

检测模块对于预处理模块给出的点云进行聚类，并将点云拟合成矩形模型：使用DBSCAN聚类算法对于三维激光点云进行聚类。对于每个聚类后的目标，使用 Kuhn-Munkres算法与前一帧数据进行数据关联，并使用增量式高斯混合模型建立或更新目标的表面点云模型；对于每个时刻的表面模型，以平台的车体坐标系x与y方向使用矩形模型进行矩形拟合，作为检测结果。

进一步的，使用增量式混合高斯模型建立目标的表面点云模型，使用EM算法迭代地对模型进行优化。高斯混合模型可以被表示为：

其中K是高斯模型的个数，μ_k与σ_k是第k个高斯模型的均值与协方差，且 是第j帧观测中的第i个点云，R_j和t_j是将第j帧点云对齐到第一帧点云坐标系的旋转矩阵和平移向量，p_k是第k个高斯模型的混合系数。第k个高斯模型可表示为

因此，一个完整的高斯混合模型可以被表示为

其中第一部分为高斯模型参数，第二部分为点云的刚体变换参数。

本发明利用增量式高斯混合模型建模，并使用EM算法进行求解。高斯混合模型的隐变量Γ中的元素γ_ijk表示了点云z_ji属于第k个高斯模型的概率。求解高斯混合模型的EM算法可以被分解为三个步骤：E-步、M-步与M-GMM-步。

E-步使用高斯混合模型的参数Θ更新隐变量Γ。对于每个γ_ijk∈Γ有

其中为m-1时刻计算得到的高斯模型参数。

M-步通过E-步中得到的隐函数Γ与高斯混合模型参数Θ中的高斯模型部分对刚体变换部分进行更新。通过最小化每帧观测的点云与每个高斯模型的距离优化刚体变换矩阵，能量函数表示为

其中λ_jk表示第j帧的点作用于第k个高斯模型的概率之和

ω_jk为第j帧的点云的使用隐函数后的加权重心

M-GMM-步通过隐函数Γ与M-步中的更新的刚体变换部分对高斯模型部分进行更新：

其中

u_mk＝R^(m)ω_mk+t^(m)

N^(m)表示到m帧为止所有帧的点云个数之和。

EM算法通过设置点云间的距离阈值或30次的最大循环次数作为迭代的退出条件，最终给出优化后的高斯模型参数。使用该参数可以将所有时刻的点云拼接到当前时刻的车体坐标系中，得到目标的表面点云模型。

跟踪模块选取拟合矩形中点作为跟踪点，使用卡尔曼滤波器进行跟踪，得到最终输出速度结果。卡尔曼滤波器的状态定义为x＝[x,y,vx,vy]，其中x与y表示车体坐标系下目标的位置，vx与vy表示目标相对于车体的运动速度。卡尔曼滤波器的观测为z＝[x,y]，其中x与y表示观测时刻拟合矩形的中心点坐标。

Claims (6)

1.一种基于三维激光点云的在线目标跟踪系统的跟踪方法，其特征在于，所述三维激光点云的在线目标跟踪系统包括预处理模块、检测模块和跟踪模块；预处理模块、检测模块和跟踪模块依次连接；预处理模块用于对三维激光原始点云进行预处理，去除非地面点云与道路外点云；检测模块用于对于预处理模块给出的点云进行聚类，并将点云拟合成矩形模型；跟踪模块使用拟合矩形进行时域的跟踪，给出目标的速度信息；

所述基于三维激光点云的在线目标跟踪系统的跟踪方法包括以下步骤：

步骤1，预处理模块使用高斯过程回归算法分割地面，滤除地面点云；使用预先建立的道路边界地图滤除道路外点云；

步骤2，检测模块使用DBSCAN聚类算法对于三维激光点云进行聚类；对于每个聚类后的目标，使用Kuhn-Munkres算法与前一帧数据进行数据关联，并使用增量式的高斯混合体建立表面点云模型；对于每个时刻的表面模型，以平台的车体坐标系x与y方向使用矩形模型进行矩形拟合，作为检测结果；

步骤3，跟踪模块选取步骤2中拟合矩形的中点作为跟踪点。

2.根据权利要求1所述的一种基于三维激光点云的在线目标跟踪系统的跟踪方法，其特征在于，步骤1中具体为：使用宽40m、长80m的矩形建立栅格图，利用当前位姿数据，将全局的道路边界地图转换到车体坐标系中，并提取出与栅格图大小对应的局部道路边界地图；使用漫水填充算法对道路边界内区域的连通性进行分析，将栅格图中道路边界内的栅格设置为1，边界外栅格设置为0；将所有点云投影到栅格中，若点云所在栅格数值为0，则进行滤除，否则保留。

3.根据权利要求1所述的一种基于三维激光点云的在线目标跟踪系统的跟踪方法，其特征在于，步骤2中具体包括：

使用增量式混合高斯模型建立目标的表面点云模型，使用EM算法迭代地对模型进行优化，高斯混合模型可以被表示为：

其中K是高斯模型的个数，μ_k与σ_k是第k个高斯模型的均值与协方差，且 是第j帧观测中的第i个点云，R_j和t_j是将第j帧点云对齐到第一帧点云坐标系的旋转矩阵和平移向量，p_k是第k个高斯模型的混合系数；第k个高斯模型可表示为

因此，一个完整的高斯混合模型可以被表示为

其中第一部分为高斯模型参数，第二部分为点云的刚体变换参数。

4.根据权利要求3所述的一种基于三维激光点云的在线目标跟踪系统的跟踪方法，其特征在于，利用增量式高斯混合模型建模，并使用EM算法进行求解；高斯混合模型的隐变量Γ中的元素γ_ijk表示了点云z_ji属于第k个高斯模型的概率；求解高斯混合模型的EM算法可以被分解为三个步骤：E-步、M-步与M-GMM-步；

E-步使用高斯混合模型的参数Θ更新隐变量Γ；对于每个γ_ijk∈Γ有

其中为m-1时刻计算得到的高斯模型参数；

M-步通过E-步中得到的隐函数Γ与高斯混合模型参数Θ中的高斯模型部分对刚体变换部分进行更新；通过最小化每帧观测的点云与每个高斯模型的距离优化刚体变换矩阵，能量函数表示为

其中λ_jk表示第j帧的点作用于第k个高斯模型的概率之和

ω_jk为第j帧的点云的使用隐函数后的加权重心

M-GMM-步通过隐函数Γ与M-步中的更新的刚体变换部分对高斯模型部分进行更新：

其中

u_mk＝R^(m)ω_mk+t^(m)

N^(m)表示到m帧为止所有帧的点云个数之和；

EM算法通过设置点云间的距离阈值或30次的最大循环次数作为迭代的退出条件，最终给出优化后的高斯模型参数；使用该参数可以将所有时刻的点云拼接到当前时刻的车体坐标系中，得到目标的表面点云模型。

5.根据权利要求1所述的一种基于三维激光点云的在线目标跟踪系统的跟踪方法，其特征在于，步骤3中，使用卡尔曼滤波器进行跟踪。

6.根据权利要求5所述的一种基于三维激光点云的在线目标跟踪系统的跟踪方法，其特征在于，卡尔曼滤波器的状态定义为x＝[x,y,vx,vy]，其中x与y表示车体坐标系下目标的位置，vx与vy表示目标相对于车体的运动速度；卡尔曼滤波器的观测为z＝[x,y]，其中x与y表示观测时刻拟合矩形的中心点坐标。