使用3-D激光测距仪的目标和车辆检测及跟踪

摘要

一种使用三维激光测距仪检测和跟踪车辆附近的目标的方法和系统。所述方法从激光测距仪接收多个点，所述点表示测距仪感测一些目标存在的空间位置。算法首先基于先前地平面位置、来自于车载传感器的数据、以及应用于点数据的特征矢量计算来估计地平面位置。接下来，基于点数据结合地平面计算静止目标的平面图占位映射和标高映射。最后，检测和跟踪动态目标，感测移动的目标，例如其它车辆、行人和动物。方法的输出是静止和动态目标集合，包括其形状、距离和速度。该输出可以在以下方面使用，例如碰撞避免或半自主驾驶系统。

说明书

技术领域

本发明总体上涉及目标检测方法和系统，且更具体地涉及使用三维激光测距仪来检测和跟踪车辆附近的静止和移动目标两者的目标检测方法和系统，从而给下游车辆控制应用提供输入源。

背景技术

许多现代车辆包括用于安全和导航辅助的各种复杂电子系统。在一些车辆中，这些系统包括目标检测和适应性巡航控制系统，其可以警告驾驶员车辆附近的障碍物，或甚至控制车辆制动器或转向装置以保持距另一个车辆或目标的安全距离。然而，本领域已知且在市场上可用的目标检测系统在它们可以多好地检测和跟踪目标以及目标数据可以多可靠地用于车辆控制目的方面具有限制。

当今车辆中可用的大多数目标检测系统使用基于雷达或照相机的视觉技术。这些系统通常具有在实际驾驶情况中通常发现的杂乱环境中的目标之间进行区分的问题。它们可能能够检测目标的二维形状，但是不能确定目标高度，从而不能确定目标实际上是否为地面或道路表面特征。或者它们可能能够检测目标的存在，但是不能基于相对速度或三维特性在紧密隔开的目标之间进行区分。最后，由于缺乏反射或缺乏目标的颜色差异，现有基于雷达或照相机的目标检测系统可能完全不能检测一些目标。

需要一种可靠和稳固的目标检测系统，其可以将目标彼此或者与地平面进行区分，其可以可靠地跟踪所检测目标相对于地面以及相对于主车辆的位置和速度两者，其可以在实际世界驾驶的杂乱环境中执行这些任务。这种系统可以允许在开发先进车辆系统（例如，半自主驾驶系统）中的显著突破。

发明内容

根据本发明的教导，公开了一种使用三维激光测距仪来检测和跟踪车辆附近的目标的方法和系统。所述方法从激光测距仪接收点云，其中，所述点表示测距仪感测一些目标存在的空间位置。算法首先基于先前地平面位置、来自于底盘动态传感器的数据、以及应用于点云数据的特征矢量计算来估计地平面位置。也可以构造不平坦的分段地表面。接下来，基于点云数据结合地表面计算静止目标的平面图占位映射和标高映射。最后，检测和跟踪动态目标，感测相对于地面移动的目标，例如其它车辆、行人和动物。方法的输出是静止和动态目标集合，包括其大小和形状、距离以及相对于地面和主车辆的速度。该输出数据可以由下游应用使用，例如碰撞避免或半自主驾驶系统。

方案1. 一种用于检测和跟踪车辆附近的目标的方法，所述方法包括：

从激光测距仪提供多个扫描点，所述扫描点表示车辆周围空间中的激光测距仪已经检测到目标的位置；

使用扫描点和车辆动态数据来建立地平面的位置；

使用扫描点和地平面的位置来构造目标占位映射和目标标高映射；以及

使用扫描点和目标占位映射来生成动态目标数据，其中，所述动态目标数据识别和跟踪移动的目标。

方案2. 根据方案1所述的方法，其中，使用扫描点和车辆动态数据来建立地平面的位置包括：

使用车辆俯仰角和倾侧角数据来从先前地平面位置计算预测地平面位置；

在扫描点上执行选通操作，其中，选通操作使用预测地平面位置来识别要在地平面计算中包括的点集合；

生成矩阵，所述矩阵包含要在地平面计算中包括的点集合；

在矩阵上执行特征值分解，以识别最小特征值以及与最小特征值相关的特征矢量；以及

使用与最小特征值相关的特征矢量来确定地平面的方程。

方案3. 根据方案2所述的方法，还包括使用卡尔曼滤波器提供更新地平面位置。

方案4. 根据方案1所述的方法，其中，使用扫描点和地平面的位置来构造目标占位映射和目标标高映射包括：

将每个扫描点投影到地平面以确定每个扫描点的地平面相交点和法向矢量；

使用所有扫描点的法向矢量来构建目标标高映射，其中，所述目标标高映射示出了在车辆周围空间中检测的所有目标的高度信息；以及

使用所有扫描点的地平面相交点来构建目标占位映射，其中，所述目标占位映射示出了在车辆周围空间中检测的所有目标的位置信息。

方案5. 根据方案1所述的方法，其中，使用扫描点和目标占位映射来生成动态目标数据包括：

从扫描点去除地面点以生成目标点集合；

使用最近计算的目标占位映射；

构造相似性曲线以确定哪些目标点一起移动；

将一起移动的目标点组聚类成动态目标；

将动态目标分类成目标类型，其中，目标类型包括车辆、行人、自行车和动物；

计算动态目标的质心；以及

跟踪动态目标以生成动态目标数据。

方案6. 根据方案5所述的方法，其中，将一起移动的目标点组聚类成动态目标使用深度优先搜索策略。

方案7. 根据方案5所述的方法，其中，跟踪动态目标使用卡尔曼滤波器。

方案8. 根据方案5所述的方法，其中，动态目标数据包括每个动态目标相对于地面的纵向和横向位置以及纵向和横向速度。

方案9. 根据方案1所述的方法，还包括在车辆应用中使用目标占位映射和动态目标数据。

方案10. 一种用于检测和跟踪车辆附近的目标的方法，所述方法包括：

从激光测距仪提供多个扫描点，所述扫描点表示车辆周围空间中的激光测距仪已经检测到目标的位置；

使用扫描点和车辆动态数据来建立地表面的定义；

使用扫描点和地表面的定义来构造目标占位映射和目标标高映射；

使用扫描点和目标占位映射来生成动态目标数据，其中，所述动态目标数据识别和跟踪移动的目标；以及

在车辆应用中使用目标占位映射和动态目标数据。

方案11. 根据方案10所述的方法，其中，使用扫描点和车辆动态数据来建立地表面的定义包括使用维诺图方法来构造不平坦地表面分段。

方案12. 根据方案10所述的方法，其中，使用扫描点和地表面的定义来构造目标占位映射和目标标高映射包括：

将每个扫描点投影到地表面以确定每个扫描点的地表面相交点和法向矢量；

使用所有扫描点的法向矢量来构建目标标高映射，其中，所述目标标高映射示出了在车辆周围空间中检测的所有目标的高度信息；以及

使用所有扫描点的地表面相交点来构建目标占位映射，其中，所述目标占位映射示出了在车辆周围空间中检测的所有目标的位置信息。

方案13. 根据方案10所述的方法，其中，使用扫描点和目标占位映射来生成动态目标数据包括：

从扫描点去除地面点以生成目标点集合；

使用最近计算的目标占位映射；

构造相似性曲线以确定哪些目标点一起移动；

将一起移动的目标点组聚类成动态目标；

将动态目标分类成目标类型，其中，目标类型包括车辆、行人、自行车和动物；

计算动态目标的质心；以及

跟踪动态目标以生成动态目标数据。

方案14. 根据方案13所述的方法，其中，将一起移动的目标点组聚类成动态目标包括构造相似性曲线、使用深度优先搜索策略以及使用具有散列表的网格数据结构。

方案15. 一种用于检测和跟踪车辆附近的目标的系统，所述系统包括：

车辆中的激光测距仪，用于提供多个扫描点，所述扫描点表示车辆周围空间中的激光测距仪已经检测到目标的位置；

第一处理器，所述第一处理器配置成使用扫描点来计算地表面且生成目标占位映射；

第二处理器，所述第二处理器配置成使用目标占位映射和扫描点来识别和跟踪动态目标；以及

共享存储器模块，用于在第一处理器和第二处理器之间共享数据。

方案16. 根据方案15所述的系统，其中，第一处理器包括地表面跟踪模块，所述地表面跟踪模块包括：选通算法，用于识别要在地平面计算矩阵中包括的点集合；以及特征值分解算法，用于从地平面计算矩阵计算地平面的方程。

方案17. 根据方案16所述的系统，其中，第一处理器中的地表面跟踪模块还包括地表面分段算法，其使用维诺图方法来构造不平坦地表面分段。

方案18. 根据方案15所述的系统，其中，第一处理器包括目标占位模块，所述目标占位模块包括：地表面投影算法，用于将所有扫描点正交地投影到地表面；标高映射构造算法，用于构造标高映射，标高映射示出了在车辆周围空间中检测的所有目标的高度信息；以及占位映射构造算法，用于构造占位映射，占位映射示出了在车辆周围空间中检测的所有目标的位置信息。

方案19. 根据方案15所述的系统，其中，第二处理器包括目标聚类模块，用于将一起移动的目标点聚类成动态目标；目标分类模块，用于将动态目标分类成目标类型；以及目标跟踪模块，用于提供关于每个动态目标的位置和速度信息。

方案20. 根据方案15所述的系统，还包括车辆中的第二激光测距仪。

本发明的附加特征从以下说明和所附权利要求结合附图显而易见。

附图说明

图1是根据本发明实施例的可以用于目标检测和跟踪的硬件系统的框图；

图2是使用图1的硬件系统的元件来检测和跟踪目标的软件系统的框图；

图3是由图2的软件系统的地平面跟踪模块使用的过程的流程图；

图4是用于计算分段地表面的投影到地平面（可能不是平坦的）的点集合的维诺图；

图5是示出了扫描点如何以网格设置以改进分段地表面计算的效率的简图；

图6是由图2的软件系统的静止占位映射模块使用的过程的流程图；

图7是由图2的软件系统的静止占位映射模块产生的二元占位映射的示例；

图8是由图2的软件系统的动态目标跟踪模块使用的过程的流程图；和

图9是示出了在图2的软件系统的动态目标跟踪模块中扫描点如何聚类成目标的简图。

具体实施方式

涉及使用三维激光测距仪的目标检测和跟踪的方法和系统的本发明实施例的以下阐述本质上仅仅是示例性的且绝不旨在限制本发明或其应用或使用。

车辆碰撞避免和半自主驾驶系统需要关于其周围环境中的目标的非常精确的信息以便有效。使用基于雷达或照相机的视觉技术的常规目标检测系统可能具有辨别杂乱环境中的目标的问题，这在实际世界驾驶情况下是典型的。例如，基于雷达的系统可能不会可靠地检测非金属目标，例如分立路边、石头和植物。基于照相机的视觉系统可能难以在类似颜色或亮度的目标之间进行辨别，且不能直接地测量距目标的测距或范围。在本发明中，三维（3-D）激光测距仪用于获取点云，表示主车辆附近的目标，特定算法用于计算车辆周围空间中的地平面以及静止和动态目标的映射。

图1是可以用于根据本发明的目标检测和跟踪的硬件系统10的框图。车辆12包含能够三维地快速扫描车辆12周围环境的3-D激光测距仪14。如果适当，可以使用多于一个的激光测距仪14，以更完全和快速地扫描整个360度视界。激光测距仪14使用有时称为光检测和测距（LIDAR）的技术，且提供表示激光碰到的目标的点云数据。所述点可以由激光测距仪14按照方位和仰角加上测距来表示，其可以容易地转换为相对于附连到车辆12的本地坐标框架的（x, y, z）点数据。激光测距仪14将点云数据提供给可现场编程的门阵列（FPGA）16，其是设计成由顾客配置（在该情况下是车辆制造商，在制造之后）的集成电路。

FPGA 16从测距仪14接收点云数据且执行涉及地平面和目标检测的一系列计算。FPGA 16的能够可以由任何合适处理器处理，包括通用电子控制单元或者专用集成电路（ASIC）。专用集成电路是针对特定用途定制的集成电路，而不旨在用于通用用途。由于ASIC的高开发成本以及全功能电子控制单元的高成本，在硬件系统10的优选实施例中设想FPGA 16。

FPGA 16将其输出发送到另一个处理器，在该情况下是数字信号处理器（DAP）18。DSP 18基于点云数据和先前计算的目标数据来执行下述的附加计算，从而得到车辆12附近的地平面、静止目标和动态目标的完整表示。FPGA 16和DSP 18两者均接入共享存储器模块20。

图2是使用硬件系统10的元件检测和跟踪目标的软件系统30的框图。总图像处理模块32管理整个软件系统30的功能。地平面跟踪模块34使用点云数据和其它信息以估计地表面的当前位置，如下文详细所述。接下来，静止占位映射模块36使用点云数据和先前计算的地表面作为输入来计算所有检测目标的位置和高度。下文还将提供静止占位映射模块36中的计算细节。最后，动态目标跟踪模块38使用下文详细描述的方法来计算移动目标的形状、位置和速度。

图3是由软件系统30的地平面跟踪模块34使用的过程的流程图40。过程在框42开始，由总图像处理模块32指导，例如在车辆发动机启动时。过程在决策菱形块44等待来自于先前地平面估计的新数据变得可用。在框46，来自于先前计算步骤的估计地平面用作新地平面计算的基础或起始点。在车辆12启动之后过程的第一关，在没有紧接先前地平面数据可用时，在框46可以使用来自于车辆12先前操作的最后一个地平面或者在框46可以使用基于车辆标称行驶高度的缺省值。在框48，作出关于地平面相对于来自于框46的先前地平面的预期移动的预测。框48处的预测基于许多现代车辆中可用类型的车载惯性测量单元（IMU）。来自于框48处的预测中使用的IMU的数据包括车辆俯仰角和侧倾角，如果这些角度从先前地平面计算以来已经发生变化，其将对地平面的位置和取向具有显著影响。

在框50，提供来自于激光测距仪14的最近点云数据。这包括所有点数据，包括表示远高于地面水平的高目标的点。在框52，在点云数据上进行选通或滤波，以仅仅选择被估计表示地平面的点。框52处的选通操作通过计算每个点距预测地平面的距离来进行。限定阈值高度，例如0.25米，且距地平面距离小于阈值的任何点被认为是地面点。所有其它点（即，距地平面距离大于阈值距离的点）被认为是目标点，且在地平面计算中不使用。

在框54，表示地平面的点集合用于进一步分析。要注意的是，在框54处提供的点不都处于一个平面内。相反，从框52处的选通操作输出的点（其在框54使用）是被认为由地面或道路表面返回的点，且这些点用于确定最佳地拟合所述点的平面的计算。在框56，来自于框54的地面点放置在4×N矩阵中，且在矩阵上进行特征值分解，目标是寻找由以下方程表示的地平面：

Ax+By+Cz+D＝0    （1）

框56的动作如下进行。来自于框54的N个地面点中的每个的（x, y, z）坐标用于填充4×N矩阵的前三列，第四列为数字1。即，矩阵的第一行将包括第一个地面点的坐标加上数字1，如下：[x₁, y₁, z₁, 1]。矩阵的第二行将包括第二个地面点的坐标加上数字1，如下：[x₂, y₂, z₂, 1]。由此，4×N矩阵用来自于框54的所有地面点填充。然后，可以在矩阵上进行特征值分解，从而产生一组特征值和特征矢量，如下文所述。最小特征值可以认为具有来自于最佳地拟合矩阵中包含的点数据的平面方程的系数[A, B, C, D]。在框58，使用方程（1）和在框56刚刚确定的系数[A, B, C, D]限定更新地平面，且过程循环回到决策菱形块44。

以下是在流程图40所示过程中使用的计算的详细讨论。在框48，假定主车辆的俯仰角速度和侧倾角速度的IMU测量值分别为                                                和，其为法线n相对于地平面的俯仰角（）和侧倾角（）的变化速率。坐标系O_xyz与车辆车架一起固定。假定要估计的地平面表示为：

n^Tp+d＝0                 （2）

其中，n表示地平面的单位法线矢量（即，||n||＝1），|d|表示从3D传感器到地平面的距离，p表示平面上的点。

地平面的法线n可以表示为相对于主车辆坐标系的俯仰角（）和侧倾角（）。地平面的状态矢量定义为。

只要新数据到达，算法就开始。假定先前时间瞬时的地平面为x，当前时间瞬时的状态矢量表示为x′。那么系统动态方程可以写为：

或者简短地写为：

其中，ΔT是取样时间间隔，是控制输入矢量，是噪音项，建模为零均值白色高斯分布。

预测地平面x′可以转换为笛卡尔方程形式：，其中，，，且||·||是欧几里得范数算子。

假定是从点p到地平面的距离。在流程图40中框52处的选通操作通过以下标准从来自于3D传感器的收集3D点云选择地面点：如果D≤T_H，那么点p是地面点且被选择；否则p是目标或障碍物点。在此，T_H是高度阈值，例如0.25米。

假定地面点集合表示为：。通过使得以下最小二乘最小化，地面点使用平面方程A_mx+B_my+C_mz+d_m＝0拟合：

其中，，且

方程（4）的解是矩阵D的最小特征值的特征矢量，即：

其中，λ_m是最小特征值。

假定且。那么，，，且。

因而，卡尔曼滤波器测量方程可以写成俯仰角()、侧倾角()和距离偏移（d）的函数：

或者简短地写为：

其中，且是测量噪音项，通常建模为白色零均值高斯随机矢量。

基于方程（4）和（9）的扩展卡尔曼滤波器过程在流程图40中的框58处应用于更新地平面操作。

要注意的是，地平面跟踪模块34以及流程图40的过程的输出是数学意义上的真实平面。即，地平面不由3-D空间中的可能模糊点群表示，而相反由方程（2）的确定数学公式表示。

上述地平面仅仅是道路表面的近似。存在道路表面不平坦的许多情况：例如，排水梯度、趋近斜坡道路、诸如坑和石头的小不连续物、以及具有超高的道路。为了考虑此，还可以将道路表示为分块或分段表面，而不是平面表面。

假定平面地面近似为方程n^Tp+d＝0（即，上述地平面跟踪模块的结果），那么3D点p可以表示为从地平面投影点p′到标高e的映射或者由样本集合表示的函数。

图4示出了地平面上的投影点{p′}的维诺图200。在图200中，点p′的邻域可以定义为维诺单元与单元p′共用共同边缘的点；例如，单元204-216中的点a、b、c、d、e、f、g是维诺图意义上的单元202中的点p′的邻域。因而，可以构造曲线G₁＝（V, E），其中，矢量集合是投影点集合{p′}；边缘集合E仅仅包括在维诺图200中将每个点连接到其直接邻域的边缘。

每个边缘具有相应权重，其是相邻元素v_i和v_j之间的相异性的非负度量。在不平坦地表面分段的情况下，V中的元素是近似地平面上的投影点，边缘的权重是由边缘连接的两个点（即在维诺图中共用侧边的点）之间的一种相异性度量。可以用于计算边缘权重的值或相异性值的示例是标高差，如下：

其中e_i和e_j分别是顶点v_i和v_j的标高。

接下来，基于曲线的有效方法用于将3D点云分段成一组平滑表面。分段是将V分成不重叠子集，使得每个子集V′形成子曲线，与G1的连接部分（区域）相对应。在每个分段部分中，同一部分中的元素相似，而来自于不同部分的元素相异。这表明取样部分中的两个顶点之间的边缘应当具有比来自于不同部分的点对的权重相对较低的权重。

定义谓项（二元函数）P，用于评估在两个部分Γ₁和Γ₂之间是否存在边界：

   （11）

其中，函数表示两个部分之间的相互差（相异性），定义为：

表示下文在方程（13）中定义的Γ₁,Γ₂内的组合内部差（偏差）。谓项P的非零值表示存在边界条件，因而两个部分不相交。方程（11）具有直观解释：如果其相互差度量大于两个部分的组合内部偏差，那么部分Γ₁,Γ₂不相交。

部分Γ的内部差度量定义为在部分最小生成树中的边缘中的最大权重。，其中，MST（Γ, E）是最小生成树。因而，组合内部偏差设计为：

试探函数τ调整至这样的程度，相互差必须大于其内部偏差以便确定是否存在边界条件。试探函数τ的两个示例列举如下。

第一，阈值函数基于部分的大小：

其中，|Γ|是集合Γ的大小。方程（14）意味着对于小的部分大小，我们需要边界的较强显著性。参数k_s的大值引起对大尺寸部分的偏好。

第二，阈值函数基于距主车辆所定位的原点的距离：

其中，r|Γ|表示部分Γ的平均位置，k_r是预先选定参数。

方程（15）意味着附近部分需要比遥远的部分更强的边界显著性。这是因为扫描点在附近区域比远距离区域（较少信息）中更密（更多信息）。参数k_r的较小值引起对附近区域中大尺寸部分的偏好。

现在可以定义基于曲线的分段算法。输入是具有N个顶点和M个边缘的曲线G₁＝（V, E）。输出是V分段成部分。步骤阐述如下：

1) 通过非减边缘权重将E分类为（e₁, e₂, …, e_M）

2) 用部段C⁰开始，其中，每个顶点处于其自己的部分中

3) 基于C^n-1构造分区Cⁿ，如下：

a) 直接将C^n-1复制到Cⁿ

b) 假定（v_i, v_j）为边缘e_n，排序边缘列表中的第n个边缘；如果v_i和v_j处于独立部分Γ_i和Γ_j中，且谓项P（Γ₁,Γ₂）为假，那么将两个部分Γ_i和Γ_j合并成Cⁿ的Γ_i，且从Cⁿ去除Γ_j

c) 对于n＝1, … ,M，重复步骤3）

4) 将集合C^M输出为C

一旦获得来自于上述算法的分段结果，就可以识别地表面点集合Γ_gound，其为包围主车辆位置（即车辆坐标框架的原点）的C中的最大部分。

上述内容阐述在仅知道3D点云时的一般方法。直接应用该方法可能是计算量大的。在以下说明中，假定除了3D位置之外，对于每个扫描点，已知扫描光栅网格的位置，即相应象素的行和列。在此，算法可以重新设计以利用该光栅网格信息，以便改进计算效率。

图5是示出了测距仪扫描点如何以二维网格设置的简图240。由于大多数3D测距仪传感器（例如激光测距仪14）的物理设计，扫描点可以如简图240所示组织。每个象素表示具体球面角且包含对最接近障碍物表面的距离测量。传感器沿水平扫描线242和竖直扫描线244扫描，且自然地产生网格曲线G₂＝（V, E），其中，V是投影点集合，边缘E仅包括从象素到其邻域明显的边缘。简图240示出了在内部3×3网格中边缘从象素246到其8个邻域（N、NE、E、SE、S、SW、W、NW）明显的示例。

每个边缘具有相应权重w（v_i, v_j），其定义为：

  （16）

其中，（p’_i, e_i）和（p’_j, e_j）分别表示顶点v_i和v_j的投影和标高，T_sep是距离阈值（例如，1.2 m）。要注意的是，具有无穷大权重的边缘可以从E去除。

要注意的是，修正分段算法将水平和竖直边缘处理解耦。上述四步骤分段算法首先独立地应用于每个水平扫描线，仅考虑水平边缘（即，简图240中的边缘E和W）。然后，水平扫描线中的分段片段使用竖直边缘（即，简图240中的NW、N、NE、SW、S和SE）基于限定谓项值合并。

修正分段算法的输入是具有N个顶点的网格曲线G₂＝（V, E）。边缘集合分别包括水平和竖直边缘。假定竖直边缘集合E_V具有M_V个边缘。输出是V分段成部分。详细算法阐述如下：

1）对于每个水平扫描线r：

a）构建子曲线G_r＝（V_r, E_r），其中，顶点V_r是第r个扫描线；仅包括V_r内的内部水平边缘

b）将上述四步骤分段算法应用于行曲线G_r

2）通过非减边缘权重将所有竖直边缘E_V分类为（e₁, e₂, …, ）

3）用部段C₀开始，其中，包含在步骤1）和2）中获得的所有部分

4）基于C^n-1构造分区Cⁿ，如下：

a）直接将C^n-1复制到Cⁿ

b）假定（v_i, v_j）为边缘e_n，排序竖直边缘列表中的第n个边缘；如果v_i和v_j处于独立部分Γ_i和Γ_j中，且谓项P（Γ_i,Γ_j）为假，那么将两个部分Γ_i和Γ_j合并成Cⁿ的Γ_i，且从Cⁿ去除Γ_j

c）对于G₂中的每个竖直边缘（即，n＝1, … ,M_v），重复步骤4）

5）将集合C^M输出为C

基于扫描点数据，上述讨论给计算地表面（平坦平面或分段表面表示）的准确表示提供多个方法。在定义地表面且地面点与所有扫描点集合不同的情况下，现在可以识别静止和动态目标。

图6是由软件系统30的静止占位映射模块36使用的过程的流程图60。静止占位映射模块36的目的是限定在车辆12的感兴趣范围内的所有静止目标的位置、形状和高度。每当地平面跟踪模块34提供更新地平面时或者以任何其它合适时间间隔，静止占位映射模块36可以通过总图像处理模块32调用。在框62，提供来自于激光测距仪14的3-D扫描点。在框62，可以从扫描点去除地面点，从而其余扫描点均表示地面上方的目标。在框64，从地平面跟踪模块34提供更新地平面或表面。在框66，来自于框62的所有扫描点投影到地平面或表面上，如下文详细描述中所述。在框66所有扫描点投影到地表面提供一组矢量，每个矢量从扫描点法向于地平面或表面延伸。

在框66处生成的矢量然后可以用于在框68构造2-D标高映射。2-D标高映射示出了由扫描点表示的所有目标相对于地平面或表面的高度和位置。在框70，可以构造二元占位映射，示出了车辆12的感兴趣范围内的所有目标在地平面中的位置。框70的构造二元占位映射通过获取地平面或表面中的来自于框66的法线矢量相交的所有点而开始。这些地平面投影点中的每个分配阈值目标大小，例如10平方厘米，这导致地平面中的第一组2-D形状。2-D形状可以在它们接触或重叠时聚类在一起，从而在地平面中提供第二组较大的2-D形状，每个表示由一些静止目标占据的区域。

以下是流程图60中使用的计算的详细讨论。

假定Gnd表示标记为地面的扫描点集合。其余扫描点，表示目标的扫描点，可以写为O＝Sen-Gnd，其中，Sen是传感器扫描点云集合。

假定。给定点q_k，Neighbor(q_k)表示为最接近q_k的地面点集合。假定Ω(q_k)表示基于地面点Neighbor(q_k)的拟合平面。那么q_k的标高可以定义为从q_k到平面Ω(q_k)的距离：

其中，表示Ω(q_k)和的平面方程。Elev(q_k)的正值表示点在地面上方，而负值表示点在平面下方。对于目标检测，仅仅考虑地面上方的点。每个点q_k的标高可以用于构造2D标高映射。

然后，二元静止占位映射可以构造为：

    (18)

其中，是确定每个点q_k是否是障碍物点的指示函数，且是小标高阈值。

图7是由静止占位映射模块36产生的二元静态占位映射80的示例。二元静态占位映射80示出了车辆12处于中心，由表示激光测距仪14的扫描图案的同心圆弧84环绕。多个目标86填充占位映射80，其中，每个目标86表示由激光测距仪14返回碰到的点群，点投影到地平面上。这些是确定为占位的点q_k，如上所述。在该示例中可以看出，目标86沿车辆12行驶的道路的路边定位。上述2-D标高映射给占位映射80中包含的占位信息增加高度信息。

在静止占位映射模块36已经执行之后，如上所述，动态目标跟踪模块38可以被调用。图8是由软件系统30的动态目标跟踪模块38使用的过程的流程图100。动态目标跟踪模块38的目的是限定在车辆12的感兴趣范围内所有移动目标或者具有非零地面速度的目标的位置、形状和速度。每当静止占位映射模块36提供更新占位映射时或者以任何其它合适时间间隔，动态目标跟踪模块38可以通过总图像处理模块32调用。在框102，提供来自于激光测距仪14的3-D扫描点。在框104，从扫描点数据去除地面点，其中，地面点在流程图40所示过程的框54处识别。在框104去除地面点之后，表示目标的扫描点可以被进一步处理以识别动态或移动目标。

在框106，最近静态占位映射（静态占位映射80是其中一个示例）由静止占位映射模块36提供。在框108，构造相似性曲线以确定哪些目标点与哪些其它点相关，即哪些点被认为来自于同一目标。在框110，完成目标聚类且确定动态目标的形状和大小。在框112，完成每个动态目标的分类，使得每个动态目标可以基于其形状和大小识别为车辆、行人、自行车、动物或一些其它类型的移动目标。在框114，每个动态目标的质心从目标形状计算，且在框116跟踪目标的位置和速度。在框118，输出动态目标数据，包括所有动态目标的大小、形状、潜在身份、位置和速度。

以下是流程图100中使用的计算的详细讨论。

在提供目标点集合和占位映射之后，构造相似性曲线，定义为G₃＝（V, E），其中，顶点集合定义为地面上方的点，即，且边缘集合E定义为在每个点对的距离小于阈值（T_sep）（即）时的点对集合。

本文提出了基于连接部分划分方法的有效O(N_o)聚类算法。深度优先搜索（DFS）策略应用于曲线G₃。

DFS聚类函数在下文描述，其需要输入V和T_sep，且输出集合列表L＝{S₁,…,S_c}。

1）对于V中的每个点，

a）如果未被访问，那么生成空集合S且附加到集合列表L

b）使用自变量、S、V和T_sep调用DFS函数

DFS函数在下文描述，其需要输入q、S、V和T_sep，且输出集合S。

1）将点q插入到集合S中

2）标记q为已访问点

3）如果存在另一个，使得||p-q||<T_sep

a）使用自变量p、S、V和T_sep递推地调用DFS函数

要注意的是，在上述算法中，DFS策略在G₃中采用，但是不需要曲线G₃的明确构建。DFS函数中的步骤3具有恒定时间复杂性（即，O(1)）实施。图9是示出了点如何使用DFS策略聚类成目标的简图250。网格数据结构260设计成保持V中的点。每个点基于其在X-Y平面中的位置分配给一单元，例如，点q＝（x, y, z）位于表示为q单元的单元中。如果多于两个点落入一单元中，那么使用关联列表280。

要注意的是，对于每个点q，其具有连接到q的边缘的所有直接邻域位于以保持点q的单元为中心的3×3网格中。这可以在简图250中看出，通过观察中心在单元（0，0）内的半径Δ的所有圆处于3×3网格262内。因而，DFS函数中的步骤3可以变为：

3）如果存在属于以q单元为中心的3×3网格的另一个p，使得||p-q||<T_sep

假定在3×3网格262内存在恒定数量的点，步骤3的复杂性是时间恒定的，即O(1)。如DFS聚类函数中所述，集合V中的每个点仅被访问一次，因而，整个聚类算法的复杂性是O(N_o)。

如简图250所示，网格中3D点的分布是稀疏的，即不是所有单元都有点占据。因而，散列表282可以用于实施网格。每个占据单元基于其单元指数映射到表项中：例如，单元（0，0）映射到第三表项284中，单元（-1，1）映射到第八表项286中。

目标群的质心定义为群的样本均值。假定第c个集合（目标群），包括每个点对之间的距离小于的J个点。质心可以写成：

群的几何大小可以通过以下矩阵的特征分解来计算：

即，，其中是对角矩阵，其中，对角元素为特征值，正交矩阵U＝（u₁, u₂, u₃）的列是相应特征矢量。

然后计算点p_j分别与三个主轴线（三个特征矢量）的点积的最小和最大值。例如，沿主轴线u_n的大小可以写为。群的大小可以用于将群分类为车辆、行人、自行车、动物等。

卡尔曼滤波器用于跟踪目标群的质心。轨迹的状态矢量定义为，其分别表示轨迹的纵向位移、横向位移、纵向速度和横向速度。

恒速模型（CV）用作目标动态模型。因而，轨迹的新状态矢量s′可以根据s预测：

其中，，v_CV是零均值高斯白色噪音矢量，ΔT是取样时间间隔。

假定目标群质心，跳过z分量。测量方程可以写成：

其中，，且w_c是建模测量过程的零均值高斯白色噪音矢量。

然后，卡尔曼滤波器应用于方程（21）和（22）以估计状态矢量。轨迹的几何信息使用群的几何形状的值（例如，沿主轴线的大小）。

由软件系统30的模块36和38产生的静止目标映射和动态目标跟踪数据提供车辆12附近的目标的完整和准确表示。通过使用上述激光扫描技术和特定数据处理算法，所公开的方法和系统可以克服其它目标检测系统经历的许多问题，从而给车辆上的下游应用（例如碰撞避免和半自主驾驶系统）提供质量更好的信息。

前述说明仅仅公开和描述本发明的示例性实施例。本领域技术人员从这种说明和附图以及权利要求书将容易认识到，能够对本发明进行各种变化、修改和变型，而不偏离由所附权利要求书限定的本发明的精神和范围。