改进的基于L形模型的激光雷达车辆跟踪方法、系统及介质技术方案

本发明专利技术公开了一种改进的基于L形模型的激光雷达车辆跟踪方法、系统及介质，包括：获取当前帧的激光雷达点云数据，聚类得到若干点云组；基于车辆形态参数及运动状态联合演变模型和各车辆上一帧的航迹数据估计预测当前帧的航迹数据；对点云组和车辆的航迹数据预测关联得到若干关联对；利用航迹数据预测设定关联的点云组L形模型拟合方向搜索区间，对点云组进行拟合，并从中提取车辆量测；使用提取的车辆量测与多模型交互滤波方法对车辆航迹数据预测进行修正；对车辆航迹进行管理，并输出当前帧的所有车辆航迹数据，完成对当前帧的激光雷达车辆跟踪。本发明专利技术改善了激光雷达车辆跟踪的精度和稳定性。精度和稳定性。精度和稳定性。

【技术实现步骤摘要】
改进的基于L形模型的激光雷达车辆跟踪方法、系统及介质


[0001]本专利技术属于车辆跟踪领域，具体涉及一种改进的基于L形模型的激光雷达车辆跟踪方法、系统及介质。

技术介绍

[0002]自动驾驶的环境感知系统用于感知周围环境，以辅助制定驾驶决策来改善当前驾驶状态并躲避道路上的障碍物，是自动驾驶系统中的一个关键环节。其中，道路上的车辆是环境感知的一类重要目标，而激光雷达因其高精度、高分辨率的测距性能，被广泛应用于车辆感知中。在制定驾驶决策时，需要依据对周围车辆的运动状态和形态参数的准确信息才能做出最优决策。由于激光雷达仅可对满足可视条件的目标表面反射形成相应点云，且激光雷达的视点和可视条件随着车辆行驶实时变化，被感知目标的可观测部分也随之动态变化。因此，如何基于反映感知车辆动态变化的可视部分的点云数据，生成对周围车辆的运动状态及完整形态参数的精确估计是自动驾驶中激光雷达数据处理单元的关键问题。
[0003]目前，一类激光雷达数据处理算法是基于车辆外形的L形模型假设对各帧的点云数据进行拟合并从中提取出对车辆形态和位置的量测，并分别用于更新车辆形态参数和运动状态的估计。这类方法的潜在问题是由于车辆上的后视镜产生的点云数据并不能与L形模型很好拟合，导致拟合效果对这些离群点非常敏感，所提取出的车辆形态和位置量测可能存在较大偏差。另一方面，在后续对车辆形态参数及运动状态估计的更新一般是独立进行的。而在正常行驶中，车辆的朝向与速度方向是一致的。若能有效利用车辆形态参数与运动状态的相关性，可有效提高估计精度。
专利技术内容
[0004]本专利技术提供一种改进的基于L形模型的激光雷达车辆跟踪方法、系统及介质，有效利用车辆形态参数与运动状态的相关性，提高车辆L形拟合的准确性，进而提高对车辆跟踪的精度。
[0005]为实现上述技术目的，本专利技术采用如下技术方案：
[0006]一种改进的基于L形模型的激光雷达车辆跟踪方法，包括：
[0007]点云聚类：获取当前帧k的激光雷达点云数据，对所有点云数据聚类得到M个点云组；
[0008]航迹预测：根据N个车辆在上一帧k
‑
1的车辆航迹数据，预测各车辆在当前帧k的航迹数据，记为航迹数据预测；其中，航迹数据包括车辆的形态参数和运动状态；
[0009]数据关联：对M个点云组和N个车辆的航迹数据预测进行一对一关联，得到P个关联对；P<＝M，P<＝N；
[0010]基于L形模型拟合与量测提取：(1)对关联到航迹数据的点云组，首先利用关联的车辆航迹数据预测，设定该车辆的方向搜索范围；然后基于该方向搜索范围，对点云组进行拟合得到车辆的L形模型，并从所得L形模型中提取车辆量测；(2)对未关联到航迹数据的点
云组，则直接进行拟合得到车辆的L形模型，并从所得L形模型中提取车辆量测；
[0011]锚点转换：对P个关联对，若车辆航迹数据预测与提取到的车辆量测使用的参考锚点不同，则将车辆航迹数据预测转换为基于车辆量测的参考锚点进行描述；
[0012]航迹管理：对关联对中的航迹数据，维持为有效航迹；对未关联到航迹数据的点云组，利用提取到的车辆量测初始新航迹；对指定长度帧内未关联到点云组的航迹数据，删除处理；
[0013]结果输出：输出当前帧k的所有车辆航迹数据，完成对当前帧k的激光雷达车辆跟踪。
[0014]进一步地，还包括多模型交互滤波：基于运动模型，利用关联对所对应提取的车辆量测，使用多模型交互滤波方法对车辆在当前帧k的车辆航迹数据预测进行修正。
[0015]进一步地，使用多模型交互滤波方法对车辆在当前帧k的车辆航迹数据预测进行修正的方法为：
[0016][0017][0018][0019][0020]式中，为基于车辆航迹数据预测得到的第k帧的车辆量测预测：得到的第k帧的车辆量测预测：其中，为观测矩阵，对应两个联合演变模型分别为H
NCA
＝diag(I2,02×2,I3)，H
CTRV
＝diag(I2,0,1,0,I2)，I
n
为n维单位矩阵；R
k
则为基于L形模型提取的车辆量测协方差矩阵。
[0021]进一步地，采用二维坐标系下基于最远距离的K
‑
D树算法，对采集到的所有激光雷达点云数据进行聚类。
[0022]进一步地，所述航迹预测，基于匀加速运动、匀速转动建立两种车辆航迹数据联合演变模型描述航迹数据演变，分别为：
[0023][0024][0025]式中，上标NCA和CTRV分别表示基于匀加速运动和匀速转动建立的模型，下标k和k+1用于表示相邻两帧，为车辆在帧k时基于匀加速运动的航迹数据，则为车辆在帧k时基于匀速转动的航迹数据；其中，[x
k
,y
k
]T
为车辆在当前帧k所处位置，v
k
为速度，a
k
为加速度，θ
k
为车身朝向，ω
k
为车身转向速度，l
k
为车身长度，w
k
为车身宽度；和为零均值白高斯过程噪声，分别用于建模帧k时车辆受各种因素干扰造成偏离匀加速运动和匀速转动的情况。
[0026]进一步地，所述数据关联，采用二维分配算法对M个点云组和N个车辆的航迹数据进行一对一关联，且量化各点云组与航迹数据关联的似然函数表示为：
[0027][0028]式中，Λ(k,m,n)表示第k帧第m个点云组与第n个车辆航迹关联时的似然函数，μ
k|k
‑
1,i
为第k帧时车辆航迹数据基于第i个模型演变的概率，表示均值为μ协方差为S的高斯分布在x处的似然函数，[x
k,m
,y
k,m
]T
为第m个点云组中距离激光雷达最近的点的位置，则为第n个车辆航迹基于第i个模型的预测位置，为第n个车辆基于第i个模型的位置信息方差。
[0029]进一步地，利用关联的车辆航迹数据预测设定方向搜索范围为：进一步地，利用关联的车辆航迹数据预测设定方向搜索范围为：其中，为第k帧的航迹数据预测中的车身朝向，为对应的方差。
[0030]进一步地，所述锚点转换的表达式为：
[0031][0032]式中，表示第k帧第i个锚点的位置，和分别为第i个锚点向第j个锚点转换时的角度和长度。
[0033]一种改进的基于L形模型的激光雷达车辆跟踪系统，包括存储器及处理器，所述存储器中存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器实现上述任一项所述的一种改进的基于L形模型的激光雷达车辆跟踪方法。
[0034]一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项所述的一种改进的基于L形模型的激光雷达车辆跟踪方法。
[0035]有益效果
[0036]本专利技术利用上一帧的车辆航迹数据对当前帧的本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种改进的基于L形模型的激光雷达车辆跟踪方法，其特征在于，包括：点云聚类：获取当前帧k的激光雷达点云数据，对所有点云数据聚类得到M个点云组；航迹预测：根据N个车辆在上一帧k
‑
1的车辆航迹数据，预测各车辆在当前帧k的航迹数据，记为航迹数据预测；其中，航迹数据包括车辆的形态参数和运动状态；数据关联：对M个点云组和N个车辆的航迹数据预测进行一对一关联，得到P个关联对；P＜＝M，P＜＝N；基于L形模型拟合与量测提取：(1)对关联到航迹数据的点云组，首先利用关联的车辆航迹数据预测，设定该车辆的方向搜索范围；然后基于该方向搜索范围，对点云组进行拟合得到车辆的L形模型，并从所得L形模型中提取车辆量测；(2)对未关联到航迹数据的点云组，则直接进行拟合得到车辆的L形模型，并从所得L形模型中提取车辆量测；锚点转换：对P个关联对，若车辆航迹数据预测与提取到的车辆量测使用的参考锚点不同，则将车辆航迹数据预测转换为基于车辆量测的参考锚点进行描述；航迹管理：对关联对中的航迹数据，维持为有效航迹；对未关联到航迹数据的点云组，利用提取到的车辆量测初始新航迹；对指定长度帧内未关联到点云组的航迹数据，删除处理；结果输出：输出当前帧k的所有车辆航迹数据，完成对当前帧k的激光雷达车辆跟踪。2.根据权利要求1所述的方法，其特征在于，还包括多模型交互滤波：基于运动模型，利用关联对所对应提取的车辆量测，使用多模型交互滤波方法对车辆在当前帧k的车辆航迹数据预测进行修正。3.根据权利要求2所述的方法，其特征在于，使用多模型交互滤波方法对车辆在当前帧k的车辆航迹数据预测进行修正的方法为：k的车辆航迹数据预测进行修正的方法为：k的车辆航迹数据预测进行修正的方法为：k的车辆航迹数据预测进行修正的方法为：式中，为基于车辆航迹数据预测得到的第k帧的车辆量测预测：得到的第k帧的车辆量测预测：其中，为观测矩阵，对应两个联合演变模型分别为H
NCA
＝diag(I2,02×2,I3)，H
CTRV
＝diag(I2,0,1,0,I2)，I
n
为n维单位矩阵；R
k
则为基于L形模型提取的车辆量测协方差矩阵。4.根据权利要求1所述的方法，其特征在于，采用二维坐标系下基于最远距离的K
‑
D树算法，对采集到的所有激光雷达点云数据进行聚类。5.根据权利要求1所述的方法，其特征在...

【专利技术属性】
技术研发人员：宋聃，韩鹍，李国朋，倪林，周怀哲，谢文佳，耿君峰，
申请(专利权)人：中国人民解放军国防科技大学，
类型：发明
国别省市：