(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210671705.0 (22)申请日 2022.06.14 (71)申请人 中国北方车辆 研究所 地址 100072 北京市丰台区槐树岭四号院 (72)发明人 安旭阳　苏治宝　崔星　宋威龙　 余雪玮　闫曈　 (74)专利代理 机构 北京艾纬铂知识产权代理有 限公司 16101 专利代理师 高会允 (51)Int.Cl. G01S 13/72(2006.01) G01S 13/86(2006.01) G01S 13/931(2020.01) G01S 17/66(2006.01) G01S 17/86(2020.01)G01S 17/931(2020.01) G06T 7/194(2017.01) G06V 10/62(2022.01) G06V 10/762(2022.01) (54)发明名称 一种基于多元数据融合的地面无人平台跟 踪方法 (57)摘要 本发明属于地面无人平台环境感知技术领 域， 具体涉及一种基于多元数据融合的地面无人 平台跟踪方法。 构建目标运动学模型， 以期望跟 踪目标的位置、 速度、 角度和角速度作为状态向 量， 根据运动特征和过程噪声， 构建非线性状态 模型； 选择激光雷达和毫米波雷达作为物理传感 器， 根据地面无人平台的定位定向数据对激光雷 达的数据进行畸变校正， 利用体素滤波、 直通滤 波、 最小分割、 欧式聚类等实现感兴趣区域的提 取； 基于非线性的状态方程， 选择无迹卡尔曼滤 波对激光雷达和毫米波雷达进行数据融合； 选择 目标的位置作为地面无人平台的任务路径点， 通 过局部路径规划、 速度剖面生成、 纯跟踪控制等 实现地面无人平台的横向行驶和纵向行驶。 权利要求书6页 说明书13页 附图3页 CN 115166716 A 2022.10.11 CN 115166716 A 1.一种基于多元 数据融合的地 面无人平台跟踪方法， 其特 征在于该 方法的步骤 包括： 第一步， 采集地面无人平台的运动信息， 并将采集到的运动信息从全局坐标系转化到 激光雷达坐标系； 第二步， 实时获取待跟踪目标的激光雷达点云数据和毫米波雷达数据； 第三步， 使用第一步采集到的地面无人平台的运动信 息对第二步的激光雷达点云数据 进行畸变补偿， 得到补偿后的点云数据； 第四步， 对第三 步得到的补偿后的点云数据进行降采样处 理， 并对噪声点进行剔除； 第五步， 对待跟踪目标的位置进行初始化， 然后根据待跟踪目标的中心点坐标和半径， 利用最小分割算法对输入点云的前 景和背景进行分割， 通过欧式聚类提取目标中心位置； 第六步， 构建待跟踪目标的运动学模型； 第七步， 通过 无迹卡尔曼 滤波算法对待跟踪目标位置进行 预测； 第八步， 根据激光雷达和毫米波雷达数据融合结果， 计算后验状态量和后验估计协方 差矩阵， 对目标的位置进行 更新。 2.根据权利要求1所述的一种基于多元数据融合的地面无人平台跟踪方法， 其特征在 于： 该方法的步骤还包括根据待跟踪目标的位置和地面无人平台的自身定位， 在线生成一 簇备选路径， 对每条备选路径成本、 最大可允许曲率和与全局路径的偏置距离进行加权计 算， 获得每条备选路径的评价值， 选择最小的评价值作为最优路径， 最后通过速度剖面生成 算法、 纯跟踪算法完成地 面无人平台跟踪任务。 3.根据权利要求1或2所述的一种基于多元数据融合的地面无人平台跟踪方法， 其特征 在于： 所述的第一步中， 采集地面无人平台的运动信 息包括地面无人平台的线速度和地面无 人平台的角速度； 所述的第四步中， 对噪声点进行剔除的方法为： 首先创建一个尺寸为0.02m*0.02m* 0.02m的三 维体素网络， 对补偿后的点云数据进行网格划分， 利用每个网格内所有点云的重 心近似代替网格内其 他的点， 每 个网格的点云重心计算公式表述 为： 式中， h为每个网格内点云的数量； (xi， yi， zi)为每个网格第i个点云的坐标； (xcenter， ycenter， zcenter)为每个网格内点云的重心坐标。 4.根据权利要求3所述的一种基于多元数据融合的地面无人平台跟踪方法， 其特征在权　利　要　求　书 1/6 页 2 CN 115166716 A 2于： 所述的第五步中， 采用最小分割算法对输入点云的前景和背景进行分割， 进行分割的 方法为： (1)在每帧点云数据中构建点与点之间的拓扑关系图， 每个顶点通过边与源点和汇聚 点相连， 并且也与自身邻域范围内的顶点相连； (2)根据点与点之间的欧式距离构造 权值smoothCost为： 式中， dist为 点间距离， 距离越远， 边被切割的可能性 就越大； σ 为权函数， 取0.2m； (3)设置前景和背景惩罚函数， 用于缩小目标范围。 前景为顶点与源点相连边的权值， 设置为常量0.5， 背景为顶点与汇聚点相连边的权值backgroundPenalty由公式定义 为： 式中， radius为物体水平半径， 取0.5m； (x,y)为点云坐标； (CenterX,CenterY)为目标 中心坐标； distanceToCenter为 点到目标中心的距离； (4)基于点间的权值， 对点云中的前景和背景进行分割， 通过欧式聚类对前景点云进行 聚类， 获得目标的中心位置 。 5.根据权利要求4所述的一种基于多元数据融合的地面无人平台跟踪方法， 其特征在 于： 所述的第六步中， 构建待跟踪目标的运动学模型的具体步骤为： 根据待跟踪目标的位置、 速度、 角度和角速度构建5维的状态向量x； 式中， (px， py)为目标在x、 y方向上的位置； v为目标的速度； ψ为目标的朝向； 为目标的 角速度； 对状态量进行求 导和积分， 得到状态方程 为： 式中， Δt为时间差； xk、 xk+1分别为k、 k+1时刻的状态向量；权　利　要　求　书 2/6 页 3 CN 115166716 A 3