【9】lightning_lm项目-阶段4-定位系统
文章目录
- 阶段四：定位系统（Localization）
- 一、定位系统的整体架构
- 二、核心模块 1：LocSystem（ROS 接口层）
- 三、核心模块 2：LidarLoc（核心定位层）
- 阶段四：定位系统（定位+动态地图）
- 一、定位系统的核心亮点
- 二、核心模块 1：TiledMap（分区动态地图）
- 三、核心模块 2：LidarLoc（激光定位）
- 四、核心模块 3：LocSystem（ROS 接口层）
- 五、建图和定位主要区别
- 六、定位系统的完整流程图
- 阶段四总结
阶段四：定位系统（Localization）
现在学习定位系统！这是实际部署时最常用的功能——在建好的地图上实时定位！
一、定位系统的整体架构
定位系统有三层结构：
┌─────────────────────────────────────┐ │ LocSystem (ROS接口层) │ │ - 订阅IMU/LiDAR │ │ - 发布TF │ └─────────────────────────────────────┘ ↓ ┌─────────────────────────────────────┐ │ Localization (协调层) │ │ - 协调LIO/LidarLoc/PGO │ └─────────────────────────────────────┘ ↓ ┌─────────────────────────────────────┐ │ LidarLoc (核心定位层) │ │ - NDT匹配 │ │ - 动态地图更新 │ │ - 位姿图优化 │ └─────────────────────────────────────┘二、核心模块 1：LocSystem（ROS 接口层）
看 loc_system.h 和 loc_system.cc ，这个很简单：
主要功能 ：
- 创建 ROS node
- 订阅 IMU、LiDAR（标准点云和 Livox 点云）
- 调用 loc::Localization 处理数据
- 设置 TF 回调，发布定位结果
三、核心模块 2：LidarLoc（核心定位层）
看 lidar_loc.h ，这个是核心！
3.1 主要成员变量（先看有什么）
structOptions{boolupdate_dynamic_cloud_=true;// 是否更新动态图层boolforce_2d_=true;// 强制在2D空间};std::shared_ptr<TiledMap>map_=nullptr;// 分区地图！NDTType::Ptr pcl_ndt_=nullptr;// NDT配准std::deque<TimedPose>lidar_loc_pose_queue_;// 位姿队列SE3 current_abs_pose_;// 当前绝对定位位姿阶段四：定位系统（定位+动态地图）
这是 Lightning-LM 的一大特色—— 动静态图层分离的定位系统 ！
一、定位系统的核心亮点
二、核心模块 1：TiledMap（分区动态地图）
看 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米）
三、核心模块 2：LidarLoc（激光定位）
看 lidar_loc.h
3.1 定位流程
输入：IMU + LiDAR ↓ LIO 预测相对运动（激光里程计） ↓ LidarLoc：NDT 匹配到地图上（绝对定位） ↓ PGO：位姿图优化，融合LO和绝对定位 ↓ 输出：高精度定位位姿 ↓ （可选）更新动态地图3.2 关键参数
看 Options 结构体：
- force_2d_ ：强制在2D空间定位（大多数场景适用）
- update_dynamic_cloud_ ：是否更新动态地图
- update_kf_dis_ ：每隔多少米更新一次动态地图（5米）
四、核心模块 3：LocSystem（ROS 接口层）
看 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 / 定位结果 │ └─────────────────────────────────────────────────────────────┘阶段四总结
定位系统学完了！主要特色是：
- 分区动态地图 ：大场景下高效加载
- 动静态图层分离 ：静态存环境，动态存变化的物体
- NDT+LIO+PGO ：融合定位，高精度鲁棒