专利原文与附图
工程车辆智能化SLAM定位方法
An Intelligent SLAM Localization Method for Construction Vehicles
该专利利用IMU预积分和里程计完成点云去畸变与位姿初始化，通过深度图和鸟瞰图差异去除动态点，再以静态线面特征配准输出逐帧精确位姿。
专利摘要
本发明适用于车辆定位技术领域，公开了一种工程车辆智能化SLAM定位方法。该方法在IMU模块中建立预积分约束并输出IMU里程计；利用里程计矫正激光点云畸变、初始化当前帧位姿；结合当前帧和全局子地图的深度图、鸟瞰图差异去除动态点；提取静态点云的线面特征并与子地图配准，输出当前帧精确位姿；随后将该位姿作为历史帧位姿，继续实时优化下一帧。
- 工程车辆SLAM
- IMU预积分
- 点云去畸变
- 动态点去除
- 深度图
- 鸟瞰图
- 线面特征
- 静态点云配准
运动畸变与动态目标会共同影响工程车辆点云定位
激光雷达扫描一帧点云期间，车辆运动会使各点的观测原点发生变化，形成点云位置畸变。
工程场景中的车辆、行人等动态目标产生非静态点云，若直接参与配准，会影响位姿估计和全局地图。
图1展示激光雷达扫描车辆和行人时的遮挡场景。
查看高清原图 ↗IMU预积分形成帧间运动约束并输出里程计
预积分利用相邻关键帧之间的多帧IMU数据，计算位移、速度和旋转变化，并将加速度计与陀螺仪零偏纳入状态。
IMU里程计提供扫描期间预测轨迹，用于把激光点补偿到统一时刻并给出当前帧初始位姿。
图5展示相邻点云数据帧之间多帧IMU数据的预积分区间。
查看高清原图 ↗S1—S6从惯性预测推进到下一帧实时优化
当前帧原始点云和IMU里程计进入去畸变与位姿初始化，随后依次执行动态点去除、线面特征提取和静态点云配准。
当前帧精确位姿反馈为历史帧辅助位姿，继续处理下一帧点云。
图3展示IMU预积分、点云预处理、动态点去除、线面特征配准和历史位姿反馈。
查看高清原图 ↗深度图和鸟瞰图差异共同定位动态点
当前帧和全局子地图分别转换为深度图与鸟瞰图，并计算两类差值图。
两种差值结果用于识别当前帧动态点；删除动态点后得到用于配准的静态点云。
图4展示当前帧和子地图经深度图、鸟瞰图差异识别并删除动态点的流程。
查看高清原图 ↗
图9为鸟瞰图转化示意图，保留原图左右对照关系。
查看高清原图 ↗点云局部曲率用于区分线特征点和面特征点
方法根据邻域点集计算局部曲率，对点云排序后划分面特征点和线特征点。
静态特征减少动态目标对后续对应关系和残差计算的干扰。
点到线和点到面的几何残差共同优化当前帧位姿
KD-tree用于在全局子地图中搜索线特征和面特征的邻近对应点，并分别构造点到线距离和点到面距离。
线面残差组成最小二乘问题，迭代求解帧间位姿变化，更新当前帧精确位姿。
当前帧精确位姿作为下一帧的历史状态
优化后的当前帧位姿进入历史帧点云辅助位姿，并与IMU预积分共同支持下一帧初始化。
该递推关系使去畸变、动态点去除、特征配准和位姿输出按帧连续执行。
第二层阅读
需要更快地把握方法链和核心机制？
AI简报不会替代上方的专利内容导读；它只在您主动打开时加载。引用专利
以下格式根据专利文献标识生成；使用前请按目标期刊或机构规范复核。
毕秋实，何海腾，商敬斌，姚宗伟，李学飞，毕延泽. 工程车辆智能化SLAM定位方法: CN 119575397 A[P]. 2025-03-07.
毕秋实，何海腾，商敬斌，姚宗伟，李学飞，毕延泽. (2025). 工程车辆智能化SLAM定位方法 (CN 119575397 A). China National Intellectual Property Administration.
[1] 毕秋实，何海腾，商敬斌，姚宗伟，李学飞，毕延泽, “工程车辆智能化SLAM定位方法,” 中国专利 CN 119575397 A, 2025-03-07.