CN107193012A - 基于imm‑mht算法的智能车激光雷达机动多目标跟踪方法 - Google Patents

Abstract

本发明公开了基于IMM‑MHT算法的智能车激光雷达机动多目标跟踪方法，属于智能车辆技术领域，本方法首先生成跟踪门；接收量测值并对其进行检测识别；接着将航迹和量测联合假设；其次简化和管理假设；然后对航迹进行检测识别；最后对航迹进行滤波和预测。包括以下步骤：步骤(1)：形成航迹跟踪门；步骤(2)：接收t时刻量测值，通过图像处理方法对其进行识别；步骤(3)将跟踪门内量测与航迹生成关联假设；步骤(4)：假设管理；步骤(5)：计算航迹的速度、角速度等信息，对航迹进行识别；步骤(6)：通过交互多模型算法对航迹进行滤波和预测。本发明结合交互多模型算法和多假设跟踪算法，能够更好地处理复杂环境下智能车机动多目标跟踪问题。

Description

基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法

技术领域

本发明属于智能车辆技术领域，具体涉及一种基于IMM-MHT算法的智能车激光雷达 机动多目标跟踪方法。

背景技术

由于激光雷达具有精度高、受环境影响小等优点，所以其在智能驾驶技术中应用越来 越广泛。智能汽车在道路中使用激光雷达跟踪障碍物是一种机动多目标的跟踪。道路中的 动态障碍物多为行人和车辆，机动车辆由于其运动无法预测，所以表现为机动特性，单模 型滤波不再适用。雷达扫描范围中障碍物数量无法确定，跟踪过程因包含跟踪航迹的生成 与消除。

交互多模型算法(IMM)是一种解决机动目标跟踪的算法，原理是在多模型算法的基 础上，使用马尔科夫链切换不同模型，得到目标的总的状态估计。IMM算法的性能很大取决于所使用的模型集。

多假设跟踪算法(MHT)是基于贝叶斯的多目标跟踪算法的一种，通过生成假设并进 行概率计算来考虑每一种可能性。在理想条件下，MHT被认为是处理数据互联的最有效方 法。

激光雷达由于其较高的精度，可以较好的获得目标的距离信息和表面轮廓信息，对目标进 行识别与检测。而通过在跟踪中加入目标检测环节可以很好地对模型进行选择与修正，并 能够剔除不合理的目标互联，减少计算量，提高准确率与实时性。

发明内容

为了解决在智能驾驶中，车载激光雷达机动多目标跟踪问题，本发明提出了一种基于 MHT和IMM算法的，并且充分利用激光雷达检测识别障碍物优势的跟踪精度高、实时性好的目标跟踪方法。

本发明的技术解决方案是：一种基于IMM-MHT算法的智能车激光雷达机动多目标跟 踪方法。实现步骤如下：

首先生成跟踪门；接收量测值并对其进行检测识别；接着将航迹和量测联合假设；其 次简化和管理假设；然后对航迹进行检测识别；最后对航迹进行滤波和预测。具体包括以 下步骤：

步骤(1)：形成航迹跟踪门；

步骤(2)：接收t时刻量测值，通过图像处理方法对其进行识别；

步骤(3)将跟踪门内量测与航迹生成关联假设；

步骤(4)：假设管理；

步骤(5)：计算航迹的速度、角速度等信息，对航迹进行识别；

步骤(6)：通过交互多模型算法对航迹进行滤波和预测。

所述步骤(1)对航迹进行预测，形成跟踪门，具体如下：

跟踪门是指以被跟踪目标的预测位置为中心，用来确定该目标观测值可能出现范围的 一块区域。在本发明中，以k-1时刻IMM算法所预测的位置建立矩形跟踪门。采用的矩形 波门的体积为：K_G为波门常数。

所述步骤(2)接收t时刻量测值，通过图像处理方法对其进行识别，具体如下：

接收K时刻激光雷达数据后，首先建立占据栅格地图，并通过网格聚类算法将各个障 碍物进行聚类，提取出障碍物外部轮廓图，利用模版匹配识别障碍物类型。

设{Z_i}(i＝1，…，N)是当前帧检测到的量测。将其进行栅格化得到栅格地图。单元格状态表示为p(s＝1)表示为占据概率，p(s＝0)表示为空白概率。通过贝叶斯公式，单元格更新概率可以用加减法来表示。

障碍物提取使用网格聚类算法。根据输入的密度阈值判断每个网格单元是否为高密度 单元，搜索临近八个网格，存在高密度单元则合并形成簇，遍历整个地图。

模版匹配通过计算模板与子图像的相关系数δ来识别障碍物类型。当δ大于阈值λ时， 认为障碍物属于此类。生成量测属性列表O_Z(i)＝H；其中，H＝1为机动车辆，H＝0为行人， H＝2为其他障碍物。当跟踪门里没有出现任何量测，删除该航迹。

所述步骤(3)将跟踪门内量测与航迹生成关联假设，具体如下：

设{T_j}(j＝1，…，M)表示k-1时刻航迹集，航迹属性列表为O_T(j)＝G，其中G＝1为机动车辆，G＝0为行人，G＝2为其他障碍物。考虑生成假设的概率受到量测类型与航迹类型的影响，所以引入类型因子σ。

假设量测与航迹属于同种类型障碍物，则σ＝1；当量测属于其他障碍物时，σ取量测相对于此航迹类型的模版匹配相关系数δ。

关联假设概率如下：

其中，C为归一化常数因子；为量测源于已确认航迹数量；为量测源于新目标数量；为量测为虚警或杂波数量；μ_F、μ_N为假量测数和新目标数的先验概率 质量函数；为航迹的探测概率，m_k为跟踪门内候选回波数，V为跟踪门体积，Z_k为量 测值，θ_k表示k时刻与量测有关的事件，与航迹互联的量测服从高斯分布。

所述步骤(4)假设管理，具体如下：

假设管理包括了N-SCAN假设剪枝与K-BEST最优假设提取。

所述步骤(5)计算航迹的速度、角速度等信息，对航迹进行识别，具体如下：

航迹和量测相当于两帧连续的数据，通过障碍物目标在两帧中的相对位置来获得其速 度和方向。

航迹的状态信息表示为：

其中航迹速度表示为：

V＝{[(X_Z-X_T)/T]²+[(Y_Z-Y_T)/T]²}^1/2

X_Z为量测X轴坐标；X_T为航迹X轴坐标；Y_Z为量测Y轴坐标；Y_T为航迹Y轴坐标； T为扫描周期。

航迹加速度表示为：

a＝(V-V_T)/T

航迹角度与角速度表示为：

θ＝tan^-1{(Y_Z-Y_T)/(X_Z-X_T)}

其中V_T为上一时刻航迹的速度；θ_T为上一时刻航迹的角度。

通过几何轮廓特征(模版匹配)和运动状态特征对航迹进行联合识别检测。经过上述 步骤，获得k时刻航迹集{T_j}(j＝1，…，M)，航迹属性列表为O_T(j)＝G，其中G＝1为机 动车辆，G＝0为行人，G＝2为其他障碍物。

所述步骤(6)通过交互多模型算法对航迹进行滤波和预测，具体如下：

IMM算法模型集包括匀速直线运动模型(CV)、匀加速直线运动模型(CA)、匀速圆周运 动模型(CT)。滤波器采用卡尔曼滤波器。

K时刻IMM算法预测输出为：

其中表示为总的状态估计，也是k+1时刻跟踪门建立的原点；u_k(i)为k时刻i 模型的概率；为滤波器i的状态估计；P(k|k)为k时刻状态协方差。

本发明的有益效果：

本发明结合了交互多模型算法和多假设跟踪算法，可以更好地处理复杂环境下智能车 机动多目标跟踪问题；通过引入目标识别环节，将原始的多假设跟踪算法与图像处理联合 起来，降低非同一类型目标的联合假设概率，进一步提高跟踪算法的准确性。

附图说明

图1为本发明提供的基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法流程 图。

图2为跟踪门内量测与航迹生成关联假设举例。

具体实施方式

下面结合附图及具体实施方式详细介绍本发明。

本发明的技术解决方案是：一种基于IMM-MHT算法的智能车激光雷达机动多目标跟 踪方法。实现步骤如下：首先生成跟踪门；接收量测值并对其进行检测识别；接着将轨迹和量测联合假设；其次简化和管理假设；然后对轨迹进行检测识别；最后对轨迹进行滤波和预测。其实施流程如图1所示，具体包括以下步骤：

所述步骤(1)对航迹进行预测，形成跟踪门如下：

航迹起始是多目标跟踪的首要问题，起始航迹的正确性是减轻多目标跟踪固有的组合 爆炸所带来的计算负担的有效措施。跟踪门是指以被跟踪目标的预测位置为中心，用来确 定该目标观测值可能出现范围的一块区域。在本发明中，以k-1时刻IMM算法所预测的位 置建立矩形跟踪门。采用的矩形波门的体积为：K_G为波门常数。

所述步骤(2)接收t时刻量测值，通过图像处理方法对其进行识别如下：

接收K时刻激光雷达数据后，首先建立占据栅格地图，并通过网格聚类算法将各个障 碍物进行聚类，提取出障碍物外部轮廓图,利用模版匹配识别障碍物类型。

{Z_i}(i＝1，…，N)是当前帧检测到的量测。将其进行栅格化得到栅格地图。单元格状态表示为p(s＝1)表示为占据概率，p(s＝0)表示为空白概率。通过贝叶斯公式，单元格更新概率可以用加减法来表示。

障碍物提取使用网格聚类算法。根据输入的密度阈值判断每个网格单元是否为高密度 单元，搜索临近八个网格，存在高密度单元则合并形成簇，遍历整个地图。

模版匹配通过计算模板与子图像的相关系数δ来识别障碍物类型。当δ大于阈值λ时， 认为障碍物属于此类。生成量测属性列表O_Z(i)＝H,H＝1为机动车辆，H＝0为行人，H＝2 为其他障碍物。当跟踪门里没有出现任何量测，就把该航迹删除。

所述步骤(3)将跟踪门内量测与航迹生成关联假设如下：

设{T_j}(j＝1，…，M)表示k-1时刻航迹集，航迹属性列表为O_T(j)＝G,G＝1为机动车辆，G＝0为行人,G＝2为其他障碍物。生成假设的概率应该受到量测类型与航迹类型的影响， 所以引入类型因子σ。

假设量测与航迹属于同种类型障碍物，则σ＝1；当量测属于其他障碍物时，σ取量测 相对于此航迹类型的模版匹配相关系数δ。

关联假设概率如下：

其中，C为归一化常数因子；τ为量测源于已确认航迹数量；γ为量测源于新目标数量； 为量测为虚警或杂波数量；μ_F、μ_N为假量测数和新目标数的先验概率质量函数；为航 迹的探测概率，与航迹互联的量测服从高斯分布。

现通过图2来对关联假设的生成进行举例说明，T1、T2为预测K时刻航迹位置，黑色矩形分别为T1、T2跟踪门。门内量测Z1、Z2、Z3与T1、T2建立联合假设。其中Z1 为步骤2已检测的车辆目标；Z2为行人目标；Z3为其他类型目标；T1为已检测的车辆目 标，T2为行人目标。生成的假设中，T1与Z1、T2与Z2因为是同种类别，假设类型因子 σ＝1；而Z3属于其他障碍物，其与T1、T2生成假设的类型因子为模板匹配系数。

所述步骤(4)假设管理如下：

假设管理包括了N-SCAN假设剪枝与K-BEST最优假设提取。所采用的方法主要是为了解决MHT算法计算量过大的问题。其中，K-BEST假设提取是在不列举所有假设的情况 下，把K个可能性最高的假设提取出来。通过构造分配矩阵，运用匈牙利算法来进行求解。 这样，就可以控制每层假设数量，减少了算法的复杂度。N-SCAN假设剪枝是一种通过限 制航迹树深度来控制假设数量的算法。在K+N时刻，查找假设概率最大的分支，并向上 回溯N个时刻，保留其节点，其余分支将会被删除。

所述步骤(5)计算航迹的速度、角速度等信息，对航迹进行识别如下：

在当前帧量测中无法得到障碍物目标的速度、方向等信息，这样就使识别过程不够准 确，所以在对航迹进行识别时需要加入这些信息。航迹和量测相当于两帧连续的数据，我 们可以通过障碍物目标在两帧中的相对位置来获得其速度和方向。

航迹的状态信息可以表示为：

其中航迹速度表示为：

V＝{[(X_Z-X_T)/T]²+[(Y_Z-Y_T)/T]²}^1/2

X_Z为量测X轴坐标；X_T为航迹X轴坐标；Y_Z为量测Y轴坐标；Y_T为航迹Y轴坐标； T为扫描周期。

航迹加速度表示为：

a＝(V-V_T)/T

航迹角度与角速度表示为：

θ＝tan^-1{(Y_Z-Y_T)/(X_Z-X_T)}

其中V_T为上一时刻航迹的速度；θ_T为上一时刻航迹的角度。

通过几何轮廓特征(模版匹配)和运动状态特征对航迹进行联合识别检测。经过上述 步骤，获得k时刻航迹集{T_j}(j＝1，…，M)，航迹属性列表为O_T(j)＝G,G＝1为机动车辆，G＝0为行人,G＝2为其他障碍物。

所述步骤(6)通过交互多模型算法对航迹进行滤波和预测如下：

IMM算法模型集包括匀速直线运动模型(CV)、匀加速直线运动模型(CA)、匀速圆周运 动模型(CT)。滤波器采用卡尔曼滤波器。

K时刻IMM算法预测输出为：

其中表示为总的状态估计，也是k+1时刻跟踪门建立的原点；uk(i)为k时刻i 模型的概率；为滤波器i的状态估计；P(k|k)为k时刻状态协方差。

上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它 们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施方式或变更 均应包含在本发明的保护范围之内。

Claims (7)

1.基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法，其特征在于，包括如下步骤：

步骤(1)：形成航迹跟踪门；

步骤(2)：接收t时刻量测值，通过图像处理方法对其进行识别；

步骤(3)将跟踪门内量测与航迹生成关联假设；

步骤(4)：假设管理；

步骤(5)：计算航迹的速度、角速度信息，对航迹进行识别；

步骤(6)：通过交互多模型算法对航迹进行滤波和预测。

2.根据权利要求1所述的基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法，其特征在于，所述步骤(1)形成航迹跟踪门，具体包括如下：

以k-1时刻IMM算法所预测的位置建立矩形跟踪门，采用矩形波门的体积为：K_G为波门常数。

3.根据权利要求1所述的基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法，其特征在于，所述步骤(2)接收t时刻量测值，通过图像处理方法对其进行识别，具体如下：

接收K时刻激光雷达数据后，首先建立占据栅格地图，并通过网格聚类算法将各个障碍物进行聚类，提取出障碍物外部轮廓图，利用模版匹配识别障碍物类型；

设{Z_i}(i＝1，…，N)是当前帧检测到的量测，将其进行栅格化得到栅格地图；单元格状态表示为p(s＝1)表示为占据概率，p(s＝0)表示为空白概率；通过贝叶斯公式，单元格更新概率可以用加减法来表示；

障碍物提取使用网格聚类算法，根据输入的密度阈值判断每个网格单元是否为高密度单元，搜索临近八个网格，存在高密度单元则合并形成簇，遍历整个地图；

模版匹配通过计算模板与子图像的相关系数δ来识别障碍物类型；当δ大于阈值λ时，认为障碍物属于此类；生成量测属性列表O_Z(i)＝H；其中，H＝1为机动车辆，H＝0为行人，H＝2为其他障碍物；当跟踪门里没有出现任何量测，删除该航迹。

4.根据权利要求1所述的基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法，其特征在于，所述步骤(3)将跟踪门内量测与航迹生成关联假设，具体如下：

设{T_j}(j＝1，…，M)表示k-1时刻航迹集，航迹属性列表为O_T(j)＝G，其中G＝1为机动车辆，G＝0为行人，G＝2为其他障碍物；考虑到生成假设的概率受到量测类型与航迹类型的影响，因而引入类型因子σ；

假设量测与航迹属于同种类型障碍物，则σ＝1；当量测属于其他障碍物时，σ取量测相对于此航迹类型的模版匹配相关系数δ：

<mrow> <mi>&amp;sigma;</mi> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mn>1</mn> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>H</mi> <mo>=</mo> <mi>G</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mi>H</mi> <mo>&amp;NotEqual;</mo> <mi>G</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

关联假设概率如下：

其中，C为归一化常数因子；τ为量测源于已确认航迹数量；γ为量测源于新目标数量；为量测为虚警或杂波数量；μ_F、μ_N为假量测数和新目标数的先验概率质量函数；为航迹的探测概率，与航迹互联的量测服从高斯分布。

5.根据权利要求1所述的基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法，其特征在于，所述步骤(4)假设管理包括：N-SCAN假设剪枝与K-BEST最优假设提取。

6.根据权利要求1所述的基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法，其特征在于，所述步骤(5)计算航迹的速度、角速度信息，对航迹进行识别，具体如下：

航迹和量测相当于两帧连续的数据，通过障碍物目标在两帧中的相对位置来获得其速度和方向；

航迹的状态信息表示为：

<mrow> <mi>S</mi> <mo>=</mo> <mrow> <mo>(</mo> <mi>v</mi> <mo>,</mo> <mi>a</mi> <mo>,</mo> <mi>&amp;theta;</mi> <mo>,</mo> <mover> <mi>&amp;theta;</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>)</mo> </mrow> </mrow>

其中航迹速度表示为：

V＝{[(X_Z-X_T)/T]²+[(Y_Z-Y_T)/T]²}^1/2

X_Z为量测X轴坐标；X_T为航迹X轴坐标；Y_Z为量测Y轴坐标；Y_T为航迹Y轴坐标；T为扫描周期；

航迹加速度表示为：

a＝(V-V_T)/T

航迹角度与角速度表示为：

θ＝tan^-1{(Y_Z-Y_T)/(X_Z-X_T)}

<mrow> <mover> <mi>&amp;theta;</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>-</mo> <msub> <mi>&amp;theta;</mi> <mi>T</mi> </msub> <mo>)</mo> </mrow> <mo>/</mo> <mi>T</mi> </mrow>

其中：V_T为上一时刻航迹的速度；θ_T为上一时刻航迹的角度；

通过几何轮廓特征和运动状态特征对航迹进行联合识别检测；

经过上述步骤，获得k时刻航迹集{T_j}(j＝1，…，M)，航迹属性列表为O_T(j)＝G，其中G＝1为机动车辆，G＝0为行人，G＝2为其他障碍物。

7.根据权利要求1所述的基于IMM-MHT算法的智能车激光雷达机动多目标跟踪方法，其特征在于，所述步骤(6)通过交互多模型算法对航迹进行滤波和预测，具体如下：

IMM算法模型集包括匀速直线运动模型CV、匀加速直线运动模型CA、匀速圆周运动模型CT；滤波器采用卡尔曼滤波器；

K时刻IMM算法预测输出为：

<mrow> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>i</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <msub> <mi>u</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </mrow>

<mrow> <mi>P</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </msubsup> <msub> <mi>u</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> <mo>{</mo> <msup> <mi>P</mi> <mi>i</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <mo>&amp;lsqb;</mo> <msup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>i</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>i</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mo>&amp;prime;</mo> </msup> <mo>}</mo> <mo>;</mo> </mrow>

其中表示为总的状态估计，也是k+1时刻跟踪门建立的原点；u_k(i)为k时刻i模型的概率；为滤波器i的状态估计；P(k|k)为k时刻状态协方差。