在现代移动机器人（AMR/AGV）的产研体系中，定位（Localization）、导航（Navigation）和轨迹跟踪（Trajectory Tracking）构成了经典的机器人自主运动闭环。简单来说，定位解决“我在哪”，导航解决“我去哪”，轨迹跟踪解决“我怎么精准且安全地开过去”。

梳理这三大核心算法集群的技术架构、主流算法选型以及工业级落地实践：

---

一、 定位算法（Localization）：解决“我在哪”

定位是所有运动的前提。移动机器人主要依赖激光SLAM（Simultaneous Localization and Mapping）和视觉SLAM，并结合轮式里程计（Odom）及惯导（IMU）进行多传感器融合。

1. 经典算法选型

• 激光 SLAM（工业绝对主流）：
  • 建图（Mapping）：经典算法如 Cartographer（谷歌开源，基于图优化）、Gmapping（基于粒子滤波）。
  • 纯定位模式（Localization Only）：在已知静态地图中，采用 AMCL（自适应蒙特卡洛定位） 算法。它通过粒子群的收敛与发散，比对实时激光雷达数据与先验地图，实现厘米级定位。在高动态、长廊等特征缺失场景，通常切换为基于正态分布变换的 NDT 匹配算法。
• 视觉 SLAM（V-SLAM）：
  • 主要算法如 ORB-SLAM3、VINS-Mono。通过摄像头捕捉环境中的特征点（如稀疏特征、稠密光流），建立三维点云地图。由于其易受光照变化影响且算力消耗大，工业界通常将其作为激光定位的辅助融合项。

2. 多传感器融合（滤波与优化）

单一定位源都有死角（如轮子打滑导致里程计漂移，长廊场景激光匹配失效）。

• 工业级方案使用 扩展卡尔曼滤波（EKF） 或 无迹卡尔曼滤波（UKF）（如 ROS 官方的 robot_localization 功能包），将高频的 IMU 数据、里程计数据与低频的激光/视觉匹配结果进行时空融合，输出平滑、无跳跃的连续位姿状态。

---

二、 导航与路径规划算法（Navigation）：解决“我去哪”

导航层负责在定位信息的基础上，避开障碍物，规划出一条从 A 点到 B 点的最优路径。为了平衡计算速度和安全性，工业界普遍采用分层规划架构：

[全局地图 + 起终点] ──> [全局路径规划 (Global Planner)] ──> 生成全局拓扑轨迹
                                                                 │
                                                                 ▼
[底盘运动执行] <── [局部避障规划 (Local Planner)] <── [代价地图 (Costmap 2D/3D)]

1. 全局路径规划（Global Planner）

不考虑动态障碍物，只在静态全局地图中寻找宏观最优解。

• 核心算法：A* 算法（网格地图寻路首选）、Dijkstra 算法、Hybrid A*（阿克曼转向车辆首选，考虑了车辆最大转弯半径约束）。
• 工业变体：在多车协同场景，常升级为时间窗 A*（Time-Space A*）或 CBS（冲突搜索算法），在空间的基础上引入时间轴，防止多车在路口对撞死锁。

2. 局部避障规划（Local Planner / Timed-Elastic-Band）

在全局路径的指导下，实时采集激光雷达数据更新局部代价地图（Local Costmap），动态绕行突然出现的行人、临时叉车。

• 核心算法：
  • TEB（Timed-Elastic-Band）：将路径视为一根有弹性的橡皮筋，障碍物施加排斥力，目标点施加吸引力。它通过在时域上求解多约束非线性优化问题，能完美支持两轮差速和阿克曼车型的时空同步规划，过弯极其丝滑，但极耗 CPU。
  • DWA（动态窗口法）：在机器人的速度和角速度物理空间中进行采样，前向模拟多条轨迹，选择得分最高（离目标近、离障碍物远、速度快）的一条速度指令发布给底盘。计算极快，适合算力低下的单片机或树莓派平台。

---

三、 轨迹跟踪算法（Trajectory Tracking）：解决“怎么精准开过去”

局部规划器输出的往往是一串带时间戳的坐标点（轨迹），而底盘电机无法直接识别坐标。轨迹跟踪算法负责将这些点转化为底盘电机能执行的线速度（Linear Velocity）和角速度（Angular Velocity）。

1. 经典算法选型

前文提到的算法在此处各司其职：

• Pure Pursuit（纯路径跟踪）：将机器人后轴作为参考，向前截取固定距离（Look-ahead）获取目标点，驱动车辆走圆弧过渡。
• Nav2 Regulated Pure Pursuit（RPP - 现代工业标准）：ROS 2 官方目前的明星插件。在传统 Pure Pursuit 基础上引入了弯道自动减速、靠近障碍物自动减速、大角度原地自转对齐（Rotate-to-Heading）等强安全规则，是目前两轮差速 AGV/扫地机落地最多的跟踪器。
• Stanley（前轮反馈控制）：参考前轴中心，将横向距离误差与航向角度误差通过 $\arctan$ 进行动态加权结合，纠偏极快，适合园区自动驾驶物流车。
• MPC（模型预测控制）：基于车辆动力学或运动学状态空间矩阵，在时域内进行在线滚动优化求解（QP）。性能最高、控制最精准平顺，能完美卡死最大转向角等硬性执行器约束，但需要车载电脑（工控机）具备较强的计算能力。

---

四、 工业产研落地：ROS 2 Nav2 导航流水线配置

在目前的工业级多无人车、AMR 产研团队中，绝大多数企业不会从零手写这些算法，而是基于 ROS 2 Navigation2 (Nav2) 框架进行组件化拼装与参数调优：

$$\begin{aligned} \text{AMCL (定位插件)} &\longrightarrow \text{Smac/NavFn (全局规划插件)} \\ &\longrightarrow \text{Costmap2D (动态代价地图)} \\ &\longrightarrow \text{RPP / TEB (局部跟踪控制器)} \\ &\longrightarrow \text{Behavior Trees (行为树状态机调度)} \end{aligned}$$ 

通过编写一个统一的 nav2_params.yaml 配置文件，即可将 AMCL（定位）、Smac Planner（全局规划）、Regulated Pure Pursuit（轨迹跟踪）无缝串联。同时，顶层由行为树（Behavior Trees, BT）进行状态控制：当定位漂移时触发原地打旋（Spin），当被障碍物卡死时触发清理代价地图（Clear Costmap），实现了系统级的功能安全和高容错。