📚 课程目录第4章定位与导航基础

4. 定位与导航基础

移动操作机器人要完成“走过去并抓住物体”，第一步不是抓，而是知道自己在哪里、目标在哪里、怎么走。定位与导航就是这条链路的地基：里程计提供短时高频的相对运动估计，卡尔曼滤波与粒子滤波负责把多传感器信息融合成稳定位姿，AMCL 在已知地图中给出全局定位，全局路径规划负责“从 A 到 B 选哪条路”，局部路径规划负责“边走边避障”。本节把这条链路拆开讲清楚。

一、里程计：短时可靠，长时漂移

轮式里程计通过编码器测量左右轮转角，结合轮距和轮半径推算机器人位姿变化。它更新频率高、短时精度好，但存在系统误差（轮径标定不准、轮距误差）和非系统误差（打滑、地面不平、碰撞）。因此里程计适合做“相对运动预测”，不适合单独做长时间全局定位。

# 差速轮里程计递推（离散）
d_left  = k * ticks_left
d_right = k * ticks_right
d_center = (d_left + d_right) / 2
d_theta  = (d_right - d_left) / wheel_base

x     += d_center * cos(theta + d_theta/2)
y     += d_center * sin(theta + d_theta/2)
theta += d_theta

关键参数是轮半径、轮距、编码器分辨率。标定不准时，机器人走直线会画弧，转 90° 会偏成 85° 或 95°，后续抓取必然对不准。

二、卡尔曼滤波：高斯假设下的最优融合

卡尔曼滤波（KF）用“预测—更新”两步循环，把里程计预测与传感器观测融合。它假设状态和噪声都服从高斯分布，适合连续、近似线性的系统。EKF 通过一阶泰勒展开处理非线性运动模型和观测模型，是激光/IMU/轮式里程计融合的常用方案。

预测步：用运动模型把上一时刻位姿推到当前时刻，协方差随之增大。
更新步：用观测（激光匹配、UWB、视觉路标）修正预测，协方差减小。
卡尔曼增益：决定“更信预测还是更信观测”，由两者协方差自动权衡。

KF/EKF 的核心价值：不是让某一传感器变准，而是让融合后的位姿比任何单一来源都更稳、更连续。

三、粒子滤波与 AMCL：多峰、非高斯的全局定位

当机器人不知道初始位置，或环境存在对称走廊、相似房间时，位姿分布可能是多峰的，高斯假设失效。粒子滤波用一组带权重的粒子表示位姿分布，通过“采样—预测—权重—重采样”迭代逼近真实后验。AMCL（Adaptive Monte Carlo Localization）是粒子滤波在 2D 栅格地图上的工程实现，广泛用于 ROS 导航栈。

初始化：在自由空间撒粒子；若已知初始位姿，可集中撒布。
运动更新：按里程计增量给每个粒子加噪声，模拟运动不确定性。
观测更新：用激光扫描与地图匹配，给每个粒子打权重。
重采样：按权重复制高权重粒子，淘汰低权重粒子。
自适应：根据粒子权重分散程度动态增减粒子数，兼顾精度与算力。

AMCL 的典型参数包括粒子数范围、激光模型类型、更新阈值、初始位姿协方差。若机器人“定位跳变”或“卡在错误房间”，通常是粒子数不足、激光模型不匹配或地图质量差。

四、全局路径规划：A*、Dijkstra、RRT

全局规划在已知地图上寻找从起点到目标点的可行路径。Dijkstra 以起点为中心向外扩展，保证最短路径，但无方向性、搜索范围大；A* 引入启发函数（如欧氏距离或曼哈顿距离），在保证最优性的同时大幅减少扩展节点；RRT 通过随机采样构建树，适合高维空间、狭窄通道和运动学约束强的场景，但路径通常不最优、需后处理平滑。

算法	核心思想	优点	局限	典型场景
Dijkstra	广度优先 + 代价最小	保证最短路径	搜索慢、无启发	小规模栅格、教学
A*	代价 + 启发函数	快且最优	启发函数需可采纳	室内导航、游戏寻路
RRT	随机采样扩展树	高维、复杂约束	路径不最优、随机	机械臂、狭窄通道
DWA	速度空间采样评价	实时避障、平滑	易陷局部最优	差速底盘局部导航

五、局部路径规划：DWA 与实时避障

全局路径只给出“大致怎么走”，局部规划负责在动态环境中生成安全、平滑的速度指令。DWA（Dynamic Window Approach）在机器人当前速度约束下采样多组线速度与角速度，前向模拟短时轨迹，用“目标朝向、障碍距离、速度大小”等评价函数打分，选最优组合下发底盘。它的优点是计算量小、实时性好；缺点是评价函数权重难调，容易在 U 型障碍前陷入局部最优。

# DWA 评价函数示例
score = w_heading * heading_score
      + w_clearance * obstacle_score
      + w_velocity * velocity_score

# 速度采样受动态窗口约束
v_min <= v <= v_max
w_min <= w <= w_max
v_dot <= a_max * dt

工程建议：全局规划用 A* 或 RRT 生成参考路径，局部规划用 DWA 跟踪并避障；若机器人频繁“原地打转”，优先检查局部代价地图膨胀半径和 DWA 权重。

六、案例与常见误区

场景：机器人在走廊中前往目标房间抓取杯子。里程计提供高频预测，AMCL 用激光匹配修正全局位姿，A* 在栅格地图上规划到房间门口，DWA 沿路径前进并绕开行人。若 AMCL 粒子收敛到隔壁相似房间，机器人会“自信地走错”；若 DWA 只看局部，可能被临时箱子困住。

常见误区：① 只信里程计不做全局定位，长时间必漂；② 粒子数设得过小，AMCL 在对称环境跳变；③ 全局路径未做膨胀，局部规划贴墙刮擦；④ DWA 权重只调速度不调避障，导致碰撞风险；⑤ 地图与真实环境不一致，定位和规划同时失效。

复盘要点：定位链路要保证“预测—观测—修正”闭环；全局规划解决可达性，局部规划解决安全性与平滑性；参数标定、地图质量、传感器时间同步是三大隐性前提。只有把里程计、滤波、AMCL、全局与局部规划串成一条稳定链路，移动操作机器人才能可靠地走到抓取位姿前。