AMCL 自适应蒙特卡洛定位
实验 3 里小车导航前"大概知道自己在哪"（默认 0,0,0）。但真实场景下小车可能被搬动、地图和现实对不上。这节课解决"我到底在地图哪里"——用 AMCL 粒子滤波，让小车拿雷达扫一扫，就自己猜出精确位置。
01问题引入：里程计漂移与"被绑架"的车
两个真实场景把定位问题逼到台前：
02原理通俗讲解：粒子滤波三步
| AMCL 参数 | 本项目值 | 通俗解释 |
|---|---|---|
| min_particles / max_particles | 500 / 2000 | 撒多少个"假想我"（KLD 自适应增减：迷茫时多撒、笃定时少撒） |
| alpha1 ~ alpha5 | 0.2 | 运动有多"善变"：打滑严重的车调大，让假想我敢散开 |
| laser_model_type | likelihood_field | 似然场打分：先给地图算好"离墙距离场"，扫一扫按光斑重合度打分，比 beam 模型快得多 |
| update_min_a / update_min_d | 0.15 rad / 0.15 m | 每转 8.6° 或每走 15 cm 才重新打分一次——省算力，别每帧都算 |
| laser_max_range / min_range | 8.0 / 0.10 m | 只信这个距离窗口内的扫描点（比雷达物理量程 12 m 更保守），太近的反光噪点直接弃 |
| set_initial_pose / initial_pose | true / (0,0,0) | 启动时直接按 initial_pose 撒粒子；设为 false 则等外部发 /initialpose 再开始 |
| recovery_alpha_fast / slow | 0.0 / 0.0 | 随机粒子注入（=0 关闭）：开成如 0.001 后，粒子会周期性"炸开重撒"，用来救被绑架的车 |
分布复杂（走廊对称、多房间）时自动加粒子，简单场景自动减——"迷茫就多猜几个"，上下限由 min/max_particles 卡住。
粒子云可视化 = 团的松紧直接反映"信心"；amcl_pose 是带协方差的最终位姿。
长走廊两头长得一样，粒子可能在"错误的那头"抱团——收敛 ≠ 收敛对了，换特征明显的房间再验。
它把"纠正量"写进 map→odom 这根 TF，odom→base_footprint 仍由 EKF 平滑输出——所以定位校正时车不会抽搐。
03关联知识点
里程计"记得自己走了多远"但会漂；雷达"看得见墙在哪"但只看眼前——两者互补正是滤波的思想。
AMCL 负责发 map→odom 的修正量：定位跳变 = 这根 TF 被纠正，odom 链保持平滑小车才不抽搐。
RViz「2D Pose Estimate」手动给（发 /initialpose）；或配置 set_initial_pose: true，让节点启动时就用 initial_pose (0,0,0) 起播。
RViz 按钮发的就是它（带 6×6 协方差的 PoseWithCovarianceStamped）——程序化重定位可直接 publish 到这，协方差写大表示"我很不确定"。
粒子团半径 < 0.3 m、且激光点与地图墙线重合 = 收敛；散成一地 = 还没定位上。
车被搬走后旧信念全部失效：默认 recovery_alpha=0 不会自动重撒 → 手动重给 /initialpose，或把 recovery_alpha_fast 开成 0.001 让算法自己找。
04实验目标
- 说清粒子滤波 预测 → 更新 → 重采样 三步及各自输入输出
- 用 RViz 与命令行 两种方式下发初始位姿，观察 /initialpose 生效
- 观察 /particle_cloud：从散乱 → 聚团（半径 < 0.3 m）的收敛全过程
- 完成一次"绑架测试"：搬动小车后重新定位，记录恢复秒数
- 完成调优三组 A/B/C 对照（粒子数 2000 / 3000、alpha 0.5），写出结论
05实验环境与准备
| 项目 | 要求 |
|---|---|
| 地图 | EXP-02 验收合格的 map_01（对称走廊类场景会撞上"假收敛"，选特征明显房间） |
| 软件 | nav2_bringup（amcl 节点已含在 navigation.launch.py 中）+ RViz Nav2 配置 |
| 参数文件 | amcl 参数段：min/max_particles 500/2000、alpha1~5=0.2、likelihood_field、set_initial_pose true |
| 硬件 | 小车 C1 满电、3 m × 3 m 场地、桌面标记物（作参照）方便核对"真实位置" |
| 对照工具 | 秒表（记录绑架恢复时间）、纸笔（三组调优记录表） |
06动画讲解：粒子云收敛
左：初始位姿不确定，粒子（绿点）撒满一片；随小车移动与扫描比对，粒子在"真值"处越挤越紧，最终聚成一团 —— 团心即定位结果（青色箭头）。
07互动讲解：粒子数 × 传感器噪声实验台
调两个旋钮，看定位表现怎么变——太少粒子覆盖不住真值，噪声太大分数全是干扰：
| 调优组 | 改什么 | 预期现象 |
|---|---|---|
| A 组 | max_particles 2000 → 3000 | 复杂房间收敛更稳，CPU 升高；记录收敛秒数 |
| B 组 | 保持 3000，房间换对称走廊 | 观察假收敛：团很紧但位置错——特征决定上限 |
| C 组 | alpha1 0.2 → 0.5 | 粒子更敢散开，动态/打滑场景恢复更快，代价是平时抖一点 |
08实操步骤：给初始位姿并观察收敛
# ① 启动导航（AMCL 随 localization.launch 起播，参数 set_initial_pose=true） ros2 launch car1_navigation navigation.launch.py map:=map_01 # ② 观察粒子云与位姿话题（另开终端；注意话题名是 particle_cloud，没有下划线连写） ros2 topic echo /particle_cloud --once ros2 topic echo /amcl_pose --once # ③ 命令行下发初始位姿（与 RViz 按钮等价）——朝向四元数 w=1 即 yaw 0 ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \ "{header: {frame_id: 'map'}, pose: {pose: {position: {x: 0.5, y: 0.3}, orientation: {w: 1.0}}}}" # ④ 绑架测试：手动把车搬到 2m 外，重新 pub /initialpose，秒表记录恢复时间 # 对比两种做法：① 手动重给位姿 ② 开 recovery_alpha_fast 0.001 让算法自己找回 # ⑤ 调优后必须整栈重启（Ctrl+C 再 launch），别热改参数
09重点与易错点
| ⚠️ 常见坑 | 现象 → 解法 |
|---|---|
| 初始位姿给反 | 车头朝南箭头却朝北 → 重新按住拖方向；症状是"出发就拐弯撞墙" |
| 粒子一锅粥不收敛 | 团散、散点乱跳 → 地图特征弱/太对称，换个特征丰富处再给位姿 |
| "收敛"了但位置错 | 假收敛（对称走廊）→ 挪到有拐角家具的区域验证；必要时做绑架恢复测试 |
| 搬车后不恢复 | 默认 recovery_alpha_fast/slow = 0.0 不会自动重撒 → 手动重给 /initialpose，或开 recovery_alpha_fast 0.001 |
| 粒子数盲目调大 | 3000+ 未必更好：先确认地图与传感器没问题，KLD 已在 500~2000 间自适应 |
| 改参不生效 | AMCL 参数启动时读取 → Ctrl+C 整栈重启 navigation.launch |
- 粒子云从散乱到 聚团半径 < 0.3 m，激光点与地图墙线重合
- RViz 与命令行 两种初始位姿方式都亲手用过
- 绑架测试 恢复秒数有记录，A/B/C 三组调优表填写完整
- 能解释 likelihood_field 为什么比 beam 模型快（先算好"离墙距离场"，打分只查表）
10总结与思考
两种方式给位姿、观察收敛、完成绑架测试与三组调优对照。
预测/更新/重采样的循环本质；KLD"迷茫多撒、笃定少撒"。
/initialpose 发布、/particle_cloud 判读、协方差与团半径肉眼诊断。
位姿给准 → 等收敛 → 仍不对再查地图特征与 alpha/粒子数。