【9】lightning_lm项目-阶段4-定位系统

文章目录阶段四定位系统Localization一、定位系统的整体架构二、核心模块 1LocSystemROS 接口层三、核心模块 2LidarLoc核心定位层阶段四定位系统定位动态地图一、定位系统的核心亮点二、核心模块 1TiledMap分区动态地图三、核心模块 2LidarLoc激光定位四、核心模块 3LocSystemROS 接口层五、建图和定位主要区别六、定位系统的完整流程图阶段四总结阶段四定位系统Localization现在学习定位系统这是实际部署时最常用的功能——在建好的地图上实时定位一、定位系统的整体架构定位系统有三层结构┌─────────────────────────────────────┐ │ LocSystem (ROS接口层) │ │ - 订阅IMU/LiDAR │ │ - 发布TF │ └─────────────────────────────────────┘ ↓ ┌─────────────────────────────────────┐ │ Localization (协调层) │ │ - 协调LIO/LidarLoc/PGO │ └─────────────────────────────────────┘ ↓ ┌─────────────────────────────────────┐ │ LidarLoc (核心定位层) │ │ - NDT匹配 │ │ - 动态地图更新 │ │ - 位姿图优化 │ └─────────────────────────────────────┘二、核心模块 1LocSystemROS 接口层看 loc_system.h 和 loc_system.cc 这个很简单主要功能 创建 ROS node订阅 IMU、LiDAR标准点云和 Livox 点云调用 loc::Localization 处理数据设置 TF 回调发布定位结果三、核心模块 2LidarLoc核心定位层看 lidar_loc.h 这个是核心3.1 主要成员变量先看有什么structOptions{boolupdate_dynamic_cloud_true;// 是否更新动态图层boolforce_2d_true;// 强制在2D空间};std::shared_ptrTiledMapmap_nullptr;// 分区地图NDTType::Ptr pcl_ndt_nullptr;// NDT配准std::dequeTimedPoselidar_loc_pose_queue_;// 位姿队列SE3 current_abs_pose_;// 当前绝对定位位姿阶段四定位系统定位动态地图这是 Lightning-LM 的一大特色—— 动静态图层分离的定位系统 一、定位系统的核心亮点二、核心模块 1TiledMap分区动态地图看 tiled_map.h 这个是核心2.1 地图结构TiledMap ├── static_chunks_ (静态地图块) │ ├── chunk (100m x 100m) │ ├── chunk (100m x 100m) │ └── ... └── dynamic_chunks_ (动态地图块) ├── chunk ├── chunk └── ...2.2 动态地图策略看 DynamicCloudPolicy 枚举2.3 动态地图更新看 UpdateDynamicCloud() 接口用定位后的点云更新动态图层只在一定范围内更新 update_kf_dis_ 5米三、核心模块 2LidarLoc激光定位看 lidar_loc.h3.1 定位流程输入IMU LiDAR ↓ LIO 预测相对运动激光里程计 ↓ LidarLocNDT 匹配到地图上绝对定位 ↓ PGO位姿图优化融合LO和绝对定位 ↓ 输出高精度定位位姿 ↓ 可选更新动态地图3.2 关键参数看 Options 结构体force_2d_ 强制在2D空间定位大多数场景适用update_dynamic_cloud_ 是否更新动态地图update_kf_dis_ 每隔多少米更新一次动态地图5米四、核心模块 3LocSystemROS 接口层看 loc_system.h这个很简单就是封装 ROS 接口订阅 IMU 话题订阅 LiDAR 话题标准点云和 Livox调用定位模块发布 TF世界坐标系 - 机器人坐标系五、建图和定位主要区别建图阶段需要 回环、关键帧保存定位阶段只需 加载地图、高频匹配模式主体节点功能建图 (run_slam_online)LIO LoopClosing G2P5实时构建地图、优化位姿、输出全局 PCD定位 (run_loc_online)LIO Localization在已有地图上做 scan-to-map 匹配输出位姿两者在参数上存在差异放在一个文件里会互相干扰模块建图模式定位模式地图加载初始化空地图读取 map_path 下保存地图回环检测开启关闭栅格生成g2p5在线生成通常关闭动态层策略新建仅更新/加载关键帧存储每隔一段时间保存只用作定位参考优化线程全量图优化局部匹配修正六、定位系统的完整流程图┌─────────────────────────────────────────────────────────────┐ │ 输入IMU LiDAR │ └─────────────────────────────────────────────────────────────┘ ↓ ┌─────────────────────────────────────────────────────────────┐ │ 1. LIO 处理 (激光里程计) → 相对运动预测 │ └─────────────────────────────────────────────────────────────┘ ↓ ┌─────────────────────────────────────────────────────────────┐ │ 2. TiledMap 加载 → 只加载车辆附近的地图块 │ │ (load_nearby_chunks_ 8) │ └─────────────────────────────────────────────────────────────┘ ↓ ┌─────────────────────────────────────────────────────────────┐ │ 3. LidarLoc::Localize() → NDT 配准 │ │ - 粗分辨率匹配 (10.0) │ │ - 精分辨率匹配 (2.0 / 1.0) │ └─────────────────────────────────────────────────────────────┘ ↓ ┌─────────────────────────────────────────────────────────────┐ │ 4. PGO 优化 → 融合 LIO 和绝对定位 │ └─────────────────────────────────────────────────────────────┘ ↓ ┌─────────────────────────────────────────────────────────────┐ │ 5. 更新动态地图 (可选) │ │ - 用定位后的点云更新动态图层 │ └─────────────────────────────────────────────────────────────┘ ↓ ┌─────────────────────────────────────────────────────────────┐ │ 6. 输出 → 发布 TF / 定位结果 │ └─────────────────────────────────────────────────────────────┘阶段四总结定位系统学完了主要特色是分区动态地图 大场景下高效加载动静态图层分离 静态存环境动态存变化的物体NDTLIOPGO 融合定位高精度鲁棒