4. 地图构建与定位
移动机器人要完成自主取放与搬运，首先必须回答两个问题：“我在哪里”和“周围环境是什么样”。前者是定位，后者是建图。SLAM（Simultaneous Localization and Mapping，同步定位与建图）把这两个问题放在同一个概率框架中联合求解：机器人一边根据运动估计自身位姿，一边用观测到的环境特征修正位姿并增量式地构建地图。没有地图，定位缺少参照；没有定位，地图无法对齐。二者互为条件，这正是SLAM的核心难点。
一、SLAM基本原理：概率估计与误差闭环
SLAM的数学本质是估计后验概率 p(x_{1:t}, m | z_{1:t}, u_{1:t})，其中 x 为机器人位姿序列，m 为地图，z 为观测，u 为控制输入。工程上通常拆成两个交替步骤：
- 预测（Prediction）：用轮式里程计或IMU积分推算位姿，位姿不确定性随时间累积增大。
- 更新（Correction）：用激光雷达、深度相机等观测与地图匹配，计算观测似然，通过贝叶斯滤波或图优化回缩误差。
按后端处理方式，SLAM可分为滤波类（EKF-SLAM、粒子滤波）与优化类（位姿图、因子图）。滤波类计算量小、适合小场景；优化类通过回环检测构建全局约束，能在大场景中保持一致性，是当前主流。回环检测是抑制漂移的关键：当机器人重新经过已建图区域时，识别出“这里来过”，在因子图中加入一条回环边，一次性拉直整条轨迹。
二、栅格地图与特征地图
地图表示决定了后续定位与导航的接口形式。工程中最常用的是栅格地图与特征地图。
| 对比项 | 栅格地图 | 特征地图 |
|---|---|---|
| 表示方式 | 二维/三维离散网格，每格记录占据概率 | 点、线、面、角点等几何特征集合 |
| 典型格式 | OccupancyGrid、PGM+YAML、八叉树 | 路标点、线段、平面、特征描述子 |
| 优点 | 直观、可直接用于路径规划与避障 | 紧凑、匹配精度高、便于优化 |
| 缺点 | 存储随分辨率增长，动态物体易残留 | 依赖特征提取质量，稀疏区域易退化 |
| 典型用途 | 导航代价地图、AMCL定位 | 视觉SLAM、激光特征匹配、LIO-SAM |
栅格地图用 0~100 的占据概率描述每格状态，通常以 0.05 m 分辨率保存。特征地图则把环境抽象为可重复观测的几何元素，便于在优化中建立精确约束。实际系统常同时维护两者：特征用于前端匹配与后端优化，栅格用于导航与避障。
三、AMCL：已知地图下的粒子滤波定位
AMCL（Adaptive Monte Carlo Localization）是ROS中经典的二维概率定位算法，适用于已有栅格地图的场景。它用一组带权粒子表示位姿分布，流程如下：
- 粒子采样：按运动模型对粒子位姿进行传播，模拟里程计噪声。
- 权重更新：用激光观测与地图做似然场匹配，给每个粒子打分。
- 重采样：淘汰低权粒子，复制高权粒子，抑制退化。
- 自适应粒子数：根据分布散度动态增减粒子，兼顾精度与算力。
典型参数配置如下：
min_particles: 500
max_particles: 5000
laser_model_type: likelihood_field
laser_max_range: 12.0
update_min_d: 0.2
update_min_a: 0.2
resample_interval: 1
四、Cartographer与LIO-SAM：两类主流建图方案
Cartographer是Google开源的二维/三维激光SLAM系统，核心是“局部子图+全局回环”。前端把连续激光帧插入子图（Submap），后端用分支定界快速扫描匹配做回环检测，再用位姿图优化消除累积误差。它支持纯激光、激光+IMU、多雷达输入，适合室内仓储、走廊等结构化环境，输出可直接用于AMCL的栅格地图。
LIO-SAM是激光-惯性紧耦合的因子图优化方案，基于GTSAM构建。它把IMU预积分因子、激光里程计因子、GPS因子和回环因子统一放入因子图，利用IMU高频信息去畸变并约束激光匹配，在室外大场景、颠簸路面和快速运动中表现稳定。其关键因子包括：
- IMU预积分因子：提供高频姿态与速度约束。
- 激光里程计因子：帧间匹配得到相对位姿。
- 回环因子：识别历史位置，拉直全局轨迹。
- GPS因子：室外绝对位置约束，抑制长期漂移。
两者定位不同：Cartographer偏重二维室内建图与栅格输出，LIO-SAM偏重三维室外高精度轨迹与点云地图。选型时应根据传感器配置、场景尺度与导航栈需求决定。
五、典型场景与工程误区
以仓储搬运机器人为例：先用Cartographer建好仓库二维栅格地图，保存为 map.pgm 与 map.yaml；运行时加载地图并启动AMCL，机器人从初始位姿开始粒子收敛；导航栈基于代价地图规划路径。若仓库货架频繁变动，静态地图会与现场不符，此时可结合动态障碍层或定期重跑建图。
复盘时建议检查：里程计标定是否准确、外参是否标定、TF树是否完整、传感器时间戳是否对齐、回环是否被正确触发。只有建图与定位链路稳定，后续的取放与搬运任务才有可靠的位姿基础。