阶段四：定位系统（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 接口：

1. 订阅 IMU 话题
2. 订阅 LiDAR 话题（标准点云和 Livox）
3. 调用定位模块
4. 发布 TF（世界坐标系 -> 机器人坐标系）

五、建图和定位主要区别

• 建图阶段需要 回环、关键帧保存
• 定位阶段只需 加载地图、高频匹配

两者在参数上存在差异，放在一个文件里会互相干扰

六、定位系统的完整流程图

┌─────────────────────────────────────────────────────────────┐ │ 输入：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 / 定位结果 │ └─────────────────────────────────────────────────────────────┘

阶段四总结

定位系统学完了！主要特色是：

1. 分区动态地图 ：大场景下高效加载
2. 动静态图层分离 ：静态存环境，动态存变化的物体
3. NDT+LIO+PGO ：融合定位，高精度鲁棒