(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210534960.0 (22)申请日 2022.05.17 (71)申请人 中国人民解 放军国防科技大 学 地址 410073 湖南省长 沙市开福区砚瓦池 正街47号 (72)发明人 宋聃　韩鹍　李国朋　倪林　 周怀哲　谢文佳　耿君峰　 (74)专利代理 机构 长沙市融智专利事务所(普 通合伙) 43114 专利代理师 熊开兰 (51)Int.Cl. G01S 17/66(2006.01) G01S 17/58(2006.01) G06V 10/762(2022.01) G06V 20/58(2022.01) (54)发明名称 改进的基于L形模型的激光雷达车辆跟踪 方 法、 系统及 介质 (57)摘要 本发明公开了一种改进的基于L形模 型的激 光雷达车辆跟踪方法、 系统及介质， 包括： 获取当 前帧的激光雷达点云数据， 聚类得到若干点云 组； 基于车辆形态参数及运动状态联合演变模型 和各车辆上一帧的航迹数据估计预测当前帧的 航迹数据； 对点云组和车辆的航迹数据预测关联 得到若干关联对； 利用航迹数据预测设定关联的 点云组L形模型拟合方向搜索区间， 对点云组进 行拟合， 并从中提取车辆量测； 使用提取的车辆 量测与多模型交互滤波方法对车辆航迹数据预 测进行修正； 对车辆航迹进行管理， 并输出当前 帧的所有车辆航迹数据， 完成对当前帧的激光雷 达车辆跟踪。 本发明 改善了激光雷达车辆跟踪的 精度和稳定性。 权利要求书3页 说明书7页 附图2页 CN 114924285 A 2022.08.19 CN 114924285 A 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帧的车辆量测预测： 其中， 为观测矩阵， 对应两个联合演变模型分别为HNCA＝diag(I2,02×2,I3)， HCTRV＝diag(I2,0,1,0,I2)， In为n维单位矩阵； Rk则为基于 L形模型提取的车辆量测协方差矩 阵。 4.根据权利要求1所述的方法， 其特征在于， 采用二维坐标系下基于最远距离的K ‑D树 算法， 对采集到的所有激光雷达点云数据进行聚类。 5.根据权利要求1所述的方法， 其特征在于， 所述航迹预测， 基于匀加速运动、 匀速转动 建立两种车辆航迹数据联合演变模型描述 航迹数据演变， 分别为：权　利　要　求　书 1/3 页 2 CN 114924285 A 2式中， 上标NCA和CTRV分别表示基于匀加速运动和匀速转动建立的模型， 下标k和k+1用 于表示相邻两帧， 为车辆在帧k时基于匀加速运动的航迹数 据， 则为车辆在帧k时基于匀速转动的航迹数据； 其中， [xk,yk]T为车辆在当前帧k所处位置， vk为速度， ak为加速度， θk为车身朝向， ωk为车身转向 速度， lk为车身长度， wk为车身宽度； 和 为零均值白高斯过程噪声， 分别用于建模 帧k时车辆受各种因素干扰造成偏离匀 加速运动和 匀速转动的情况。 6.根据权利要求1所述的方法， 其特征在于， 所述数据关联， 采用二维分配算法对M个点 云组和N个车辆的航迹数据进行一对一关联， 且量化各点云组与航迹数据关联的似然函数 表示为： 式中， Λ(k,m,n)表示第k帧第m个点 云组与第n个 车辆航迹关联时的似然函数， μk|k‑1,i为 第k帧时车辆航迹数据基于第i个模型演变的概率， 表示均值为 μ协方差为S的高斯 分布在x处的似然函数， [xk,m,yk,m]T为第m个点云组中距离激光雷达最近的点的位置， 则为第n个车辆航迹基于第i个模型的预测位置， 为第n个车辆基于第 i个模型的位置信息方差 。 7.根据权利要求1所述的方法， 其特征在于， 利用关联的车辆航迹数据 预测设定方向搜 索范围为： 其中， 为第k帧的航迹数据预测中的车 身朝向， 为对应的方差 。 8.根据权利要求1所述的方法， 其特 征在于， 所述锚点 转换的表达式为：权　利　要　求　书 2/3 页 3 CN 114924285 A 3