【学习笔记008】ROS1 Navigation 与 ROS2 Nav2

💡

借着学Planning的机会，补一下ROS的导航包中我模糊的一些细节。

ROS1 Navigation Framework

ROS1 的 Navigation Stack 以 move_base 为核心，用于完成移动机器人的自主导航。它接收目标点，并结合地图、定位结果、传感器数据和里程计信息，完成全局路径规划、局部避障和速度控制。

整体数据流可以概括为：

其中各模块作用如下：

map_server：提供静态地图。

AMCL：结合地图、激光雷达和里程计，估计机器人在地图中的位置。

global_costmap：构建全局代价地图，为全局规划提供环境信息。

Global Planner：根据当前位置、目标点和全局代价地图生成全局路径，常见方法有 Dijkstra、A*。

local_costmap：根据激光雷达等传感器实时更新机器人周围的障碍物。

Local Planner：跟踪全局路径并进行局部避障，最终输出速度指令 /cmd_vel，常见算法如 DWA。

Recovery Behaviors：当规划失败或机器人被困时，执行清除代价地图、旋转等恢复操作。

因此，ROS1 Navigation 的核心流程可以总结为：

需要注意的是，move_base 负责的是导航层控制。它输出的 /cmd_vel 通常只是机器人期望的线速度和角速度，真正的电机 PID、轮速控制等一般位于更底层的底盘控制系统中。

Local planner

输入

然后根据当前环境计算机器人下一时刻应该采用的速度：

也就是类似：

对于差速小车，可以理解为输出：

DWA

DWA（Dynamic Window Approach，动态窗口法）是 ROS1 中非常经典的局部规划算法。它的核心目标是：

在机器人当前允许的速度范围内，寻找一组最合适的线速度 v 和角速度，让机器人既能跟随全局路径，又能避开障碍物。

它不是直接在地图上重新搜索一条完整路径，而是搜索“机器人下一小段时间应该怎么运动”。

DWA 的流程可以概括为：

所谓“动态窗口”，指的是机器人并不能瞬间从当前速度跳到任意速度。假设机器人当前速度为：

同时机器人有最大加速度限制，那么在下一个控制周期中，只允许搜索一小块速度范围：

这块随当前速度动态变化的可行区域，就是 Dynamic Window。

例如 DWA 会采样：

然后对每一组向前模拟：

如果某条轨迹会撞到 local_costmap 中的障碍物，就直接淘汰。

剩余轨迹通常会根据几个因素进行评分，例如：

其中大致可以理解为：

：轨迹是否贴近全局路径

：是否朝目标方向前进

：是否远离障碍物

最终选择综合评分最好的一组：

并作为：

发送给底盘。

所以 DWA 最核心的一句话是：

在当前动力学允许的速度范围内，对多组 \((v,\omega)\) 进行短时间轨迹预测，并根据路径、目标和障碍物进行评分，选出最优速度控制机器人运动。

TEB

TEB 是 ROS1 中常用的一种局部规划算法，全称 Timed Elastic Band。它的目标不是像 DWA 那样直接从很多组中挑选一个，而是先得到一条局部轨迹，然后对这条轨迹进行优化，使机器人能够在避开障碍物的同时平滑、快速地沿全局路径运动。

TEB 的整体过程可以概括为：

1. TEB 的局部轨迹从哪里来？

Global Planner 已经给出了一条从当前位置到目标点的全局路径：

TEB 并不会每次优化整条全局路径，而是根据机器人当前位置，从 Global Plan 中取出机器人附近的一段：

这一段路径就作为局部轨迹的初始参考。

TEB 将它表示为一系列机器人位姿：

其中：

也就是每个点不仅有位置，还包含机器人朝向。

2. 为什么叫 Timed Elastic Band？

普通的 Elastic Band 可以理解成一根由很多点组成的“橡皮筋”：

这些点的位置可以在优化过程中移动。

TEB 在此基础上，还在相邻位姿之间加入时间间隔：

于是轨迹变成：

因此 TEB 实际优化的是两类变量：

以及：

这也是 Timed Elastic Band 名字的来源：既优化“轨迹走哪里”，也优化“多长时间走过去”。

3. TEB 是怎么避障的？

假设 Global Plan 原本经过这里：

但是 Local Costmap 检测到了新的障碍物。

TEB 会把“距离障碍物太近”设置成一个较大的代价，于是在优化过程中调整附近轨迹点：

所以可以形象地理解为：

全局路径是一根橡皮筋，而障碍物会把这根橡皮筋向外“推开”。

4. TEB 优化时考虑什么？

TEB 并不是单纯让轨迹离障碍物越远越好，它同时考虑多个约束，例如：

与障碍物保持安全距离；

不超过最大线速度和角速度；

不超过最大加速度；

满足机器人的运动学约束；

尽量快速到达目标；

轨迹尽量平滑、合理。

因此可以粗略表示成一个优化问题：

优化器不断调整：

以及：

最终得到一条满足机器人运动约束、又能够避开障碍物的局部轨迹。

5. 最后如何得到 /cmd_vel？

优化完成后，TEB 已经知道机器人未来一小段时间应该经过哪些位置，以及每段运动所需要的时间。

例如相邻两个轨迹点之间：

时间为：

那么就可以根据位置和姿态变化计算对应的：

最终生成：

并通过：

发送给机器人底盘。

整个过程会不断重复：

所以 TEB 本质上是一种在线滚动优化的局部轨迹规划方法。

一句话总结：

TEB 从全局路径中截取机器人附近的一段作为初始轨迹，将其表示为一系列带时间间隔的位姿点，然后综合考虑障碍物、速度、加速度和机器人运动学约束，对整条局部轨迹进行优化，最后根据优化结果计算 ，输出 /cmd_vel。

和 DWA 最核心的区别可以记成：DWA 主要在速度空间中采样并选择 ，TEB 则直接优化一整段带时间信息的局部轨迹。

ROS2 Nav2 Framework

ROS2 的 Nav2 是 ROS2 中用于移动机器人自主导航的框架。与 ROS1 以 move_base 为核心不同，Nav2 将导航功能拆分为多个独立的 Server，并由 BT Navigator（Behavior Tree） 负责组织和调度整个导航流程。

整体结构可以概括为：

其中主要模块如下：

Map Server：提供静态地图。

AMCL：结合地图、激光雷达和里程计，估计机器人在地图中的位置。

Global Costmap：描述全局环境中的障碍物和通行代价。

Local Costmap：根据激光雷达等传感器实时更新机器人周围的障碍物。

BT Navigator：通过 Behavior Tree 调度规划、控制和恢复行为。

Planner Server：负责生成全局路径，常见算法包括 NavFn、Smac Planner。

Smoother Server：对规划出的路径进行平滑处理。

Controller Server：负责路径跟踪和局部避障，并输出 /cmd_vel，常见 Controller 有 DWB、RPP、MPPI、TEB 等。

Behavior Server：提供 Spin、BackUp、Wait 等行为，用于导航失败后的恢复或特殊动作。

Lifecycle Manager：统一管理 Nav2 各节点的生命周期状态。

因此 Nav2 的基本流程可以总结为：

Nav2 最核心的特点可以概括为：

Nav2 将 ROS1 中集中在 move_base 内部的导航功能拆分为多个 Server，并使用 Behavior Tree 对这些模块进行灵活调度。

Controller Server

Pure Pursuit（RPP）

RPP 首先在全局路径中删除机器人已经经过的部分，然后根据固定或随速度变化的 Look Ahead Distance，在机器人前方的路径上选择一个 Look Ahead Point。根据该点在机器人坐标系中的位置计算机器人需要跟踪的圆弧曲率，并由 得到角速度。同时，RPP 根据路径曲率、障碍物距离以及碰撞风险等因素调节线速度，最终输出 /cmd_vel。

1. RPP 的整体过程

RPP 的输入主要可以理解为：

其中最关键的就是Look Ahead Point 到底从哪里来？

2. Look Ahead Point生成

假设 Planner Server 已经给出了一条 Path：

RPP 首先会找到机器人在这条路径附近的位置，并把机器人已经走过的路径裁掉。

例如原始路径：

机器人后面的部分已经没用了，于是变成：

Nav2 官方将这个过程称为对 global path 的持续 pruning，然后把 Local Costmap 范围内相关的路径转换到机器人坐标系中。

3. 先确定 Look Ahead Distance

接下来先确定一个前视距离：

例如：

那么可以直观理解为：

我要在机器人前方大约 0.6 m 的 Path 上找一个目标点。

经典 Pure Pursuit 可以使用固定：

但 RPP 还可以使用速度自适应前视距离：

其中 T 就是 lookahead_time。

然后限制在：

例如：

也就是说：

低速看近一点，高速看远一点。

Nav2 对应的配置就是 use_velocity_scaled_lookahead_dist、lookahead_time、min_lookahead_dist 和 max_lookahead_dist。

4. Look Ahead Point 到底怎么选？

这是最重要的部分。

假设：

机器人在：

Path 是：

RPP 要做的是：

沿着机器人前方的 Path，找到距离大约为 的那个位置。

可以把它想象成机器人周围画一个半径为的圆：

最重要的一点是：

Look Ahead Point 在 Path 上，不是在机器人朝向前方随便放一个点。

5. 实际 Path 是离散点，怎么办？

Path 本身通常是：

假设：

显然：

那么 RPP 不一定直接拿 P3。

它可以在：之间做插值，得到：

这个插值得到的：

就是更加准确的 Look Ahead Point。

Nav2 的实现会在路径点之间寻找满足 lookahead 距离的位置并进行插值；当前主线代码的公共工具会沿路径累计距离，在跨过 lookahead distance 的那一段上做线性插值。

因此您可以把整个 Look Ahead Point 的产生过程记成：

这就是它真正的来源。

6. 有了 Look Ahead Point 后怎么办？

现在把坐标系放在机器人身上：

假设 Look Ahead Point 在机器人坐标系中的位置是：

Pure Pursuit 会寻找一条圆弧：

使得机器人沿着这条圆弧最终经过 Look Ahead Point。

其曲率可以计算为：

Nav2 RPP 的代码也是按照这一关系计算 lookahead curvature。

7. 曲率怎么变成？

曲率定义为：

对于差速机器人：

因此：

例如：

那么：

最终：

8. 为什么叫 Regulated Pure Pursuit？

到这里其实只是 Pure Pursuit。

RPP 多出来的 Regulated，主要是在决定 v 时进一步进行限制。

例如前方急转弯：

曲率很大：

RPP 就会：

避免高速冲进急弯。

同时如果机器人靠近障碍物：

也可以进一步降低线速度。

此外 RPP 还会沿当前控制产生的运动圆弧进行前向碰撞检测，从而判断按照当前指令继续运动是否可能发生碰撞。

因此：

MPPI

MPPI 会围绕当前控制方案采样大量未来控制序列，利用机器人模型预测对应轨迹，根据路径跟踪、避障和控制平滑等代价给这些轨迹加权，更新出更优的控制序列，并只执行第一步，随后不断滚动重新优化。

MPPI（Model Predictive Path Integral）是一种基于模型预测 + 随机采样 + 代价加权的控制方法，在 Nav2 中可以作为 Controller Server 的局部控制器使用。

它的目标是：

在未来一小段时间内，尝试大量可能的控制方式，预测机器人会怎么走，然后从中综合得到当前最合适的。

整体流程可以概括为：

1. MPPI 采样的不是一个速度，而是一串未来控制

这是它和 DWB 很重要的区别。

DWB 更像是在当前时刻采样很多组：(v,\omega)

而 MPPI 采样的是一整段未来控制序列：

其中：

比如：

也就是说，MPPI 在问：

“如果未来几秒钟一直按照这一串速度执行，机器人会走成什么样？”

2. 利用机器人模型预测未来轨迹

对于每一组控制序列，MPPI 根据机器人运动模型进行前向仿真：

其中：

：机器人当前状态，例如

：控制输入，例如

于是每组控制序列都会对应一条预测轨迹：

3. 给每条预测轨迹计算代价

然后 MPPI 会评价这些轨迹。

代价通常会综合考虑：

例如：

是否接近 Global Path

是否朝目标前进

是否靠近障碍物

控制是否过于剧烈

是否满足速度和运动学约束

例如：

显然 B 更好。

4. MPPI 并不是简单地只选“最好的一条”

这是 MPPI 很有特点的地方。

它会根据每条轨迹的代价赋予不同权重，大致形式可以理解为：

其中：

：第 k 条轨迹的总代价

：控制加权平滑程度的参数

所以：

例如：

MPPI 会综合这些较优轨迹的信息，更新当前的控制序列，而不是简单地：

5. 最后只执行第一步

假设优化后得到：

MPPI 并不会一次把整串控制执行完。

它只执行：

也就是当前时刻的：

然后输出：

机器人运动一个控制周期以后，再重新获取：

然后再次优化。

所以它属于典型的 Receding Horizon（滚动时域） 控制：

AMCL

下面以常见的差速小车、二维地图和激光雷达为例。

先明确：AMCL 始终维护一组可能的位姿及其可信程度。一个粒子可以写成：

其中，是地图中的位置，是朝向，是权重。所有粒子的权重之和为 1。

它的主要流程是：

1．初始化：先表达“我大概在哪里”

你在 RViz 中点击 2D Pose Estimate，相当于提供一个初始位姿。AMCL 根据这个位姿及其协方差，在附近随机生成一批粒子。

协方差表达的是不确定程度：位置越不确定，粒子撒得越开；朝向越不确定，粒子箭头的方向差异越大。

如果完全不知道位置，也可以通过全局定位初始化，在地图空闲区域中分布粒子。这样需要排除的可能性更多。

2．运动预测：每个粒子都按里程计“走一遍”

AMCL 使用的是两次更新之间的里程计变化量。对于差速小车，可以把一段运动分解为：

不考虑误差时，每个粒子的更新为：

实际计算会给这些运动量加入随机噪声，表示轮子打滑、里程计误差等影响。Nav2 的 alpha1～alpha4 用于控制差速运动模型中的相关噪声。

例如，两个粒子分别认为机器人朝东和朝北，那么同样“向前走 1 米”，它们就分别向东、向北移动。它们共享同一条运动信息，但保留各自对机器人朝向的假设。

3．观测更新：假如这个粒子是对的，激光应该落在哪里？

这是最关键的一步。

以 Nav2 常用的 likelihood_field，即似然场模型为例：

假设机器人位于某个粒子的位姿；

根据该位姿和雷达安装位置，把实测激光点转换到地图中；

查询这些激光点距离地图中最近障碍物有多远。

如果假设正确，打到墙上的激光点转换到地图后，应该也靠近地图中的墙。

设某个激光端点到最近障碍物的距离为 d，其匹配得分的主要部分形如：

所以，d 越小，得分越高；越大，对误差越宽容。

这里有一个容易误解的地方：似然场模型主要检查“实测激光端点离地图障碍物多近”，不必为每个粒子逐束模拟激光射线。地图可以预先存储各网格到最近障碍物的距离，查询就很快。

多束激光的得分会合成为该粒子的扫描匹配得分。具体组合方式取决于模型；Nav2 的 likelihood_field 实现使用单束得分的立方累加，并非简单地将所有概率相乘。

进一步解释

这里最容易卡住的地方是：雷达不知道机器人在地图中的位置，它只知道“从自己出发，某个方向上多远有障碍物”。粒子提供一个假设位置，AMCL 检查这个假设能否解释雷达看到的东西。

例如，雷达测到：

我前方 2 米有障碍物。

这只是相对于雷达的位置。这个障碍物究竟在地图的哪里，取决于雷达自身在地图中的位置和朝向。

因此，AMCL 会对每个粒子分别做一次这样的计算：

假如机器人真的在你这个位置、朝着你这个方向，那么这束长 2 米的激光，其终点会落在地图哪里？

然后检查：这个终点附近，地图上有没有墙或其他障碍物？

下面把问题简化成一条直线：所有粒子都朝右，雷达安装位置就当作机器人中心，地图中只有一面位于 x=5 米的墙。三行代表三个不同的假设：

A 的假设能让实测激光终点落在墙上，因此得分最高；B、C 的终点落在地图空地中，离墙越远，得分越低。

实际雷达一帧有很多束激光，AMCL 会选取其中多束，对每个粒子重复这个比较，再综合成该粒子的得分。某个粒子可能碰巧让一束激光对上墙，但要让多个方向的激光同时对上地图，就更难碰巧做到——这就是它能判断位置和朝向的依据。

4．更新权重：把原来的判断与新证据结合

把粒子 i 的扫描匹配得分记作，权重更新可以写成：

第一步让匹配好的粒子获得更高权重；第二步叫归一化，使权重之和重新等于 1。

这一步通常只改变粒子的权重，不直接把粒子的位置推向墙边。机器人估计位置的纠正，是通过不同位置假设的可信程度变化来实现的。

5．重采样：把计算资源集中到更可信的位置

假设只有三个粒子，归一化后的权重为：

粒子	权重	抽取 10 次时的期望次数
A	0.7	7
B	0.2	2
C	0.1	1

重采样就是按照这些概率，有放回地抽取粒子。实际次数具有随机性，未必刚好是 7、2、1。

高权重粒子可能产生多个副本，低权重粒子可能消失。之后，新粒子的权重通常重置为相等：原来通过“权重大小”表达的可信程度，现在通过“副本多少”表达出来。下一轮运动预测加入噪声后，副本又会逐渐分开。

6．自适应采样：这一轮到底需要多少粒子？

普通 MCL 可以使用固定粒子数量。AMCL 则通过 KLD 采样，根据粒子分布情况动态决定数量。

直观上，它把空间划分成许多小格子，在重采样时统计粒子占据了多少格子，据此估计需要多少样本才能足够准确地表达当前分布，并受到最小、最大粒子数量的限制。

所以，“分布复杂时通常多用粒子，集中时通常少用粒子”是便于理解的描述，实际依据是分布占据的格子和误差约束。它不直接知道真实定位误差是多少。

最后，AMCL 从粒子分布中计算位姿估计。Nav2 会考虑粒子簇，选择权重最大的簇来给出位姿，避免把两个相距很远的位置假设直接平均到中间。

在实际 Nav2 中，也不是每收到一帧雷达都完整执行上述所有步骤：update_min_d、update_min_a 控制通常需要移动多远或转动多少才更新，resample_interval 控制重采样间隔。

你再看 RViz 中粒子的变化，就可以这样对应：运动预测使粒子移动并通常变散，雷达观测改变权重，重采样使可信位置附近的粒子占比增大。但粒子聚拢只说明算法的判断集中；在对称环境或错误初始化下，也可能集中到错误的位置。

cmd_vel到底盘控制

/cmd_vel 是导航层和底盘控制层之间的接口。Nav2 输出期望车体速度 v,ωv,\omega，底盘控制器通过逆运动学将其转换为各轮目标转速，再由轮速 PID / FOC 驱动电机；编码器产生反馈，并进一步形成 /odom 返回导航系统。

可以。/cmd_vel 本质上只是一个速度指令接口，它通常不直接控制电机，而是先经过底盘控制节点，把“机器人期望的线速度和角速度”转换成“各个轮子的目标转速”，最后再由 PID、FOC 或电机驱动器控制电机。

最典型的数据流是：

差速底盘如何处理 /cmd_vel

假设是两轮差速机器人：

Nav 给出的控制量是：

但电机真正需要的是：

假设：

轮间距为L

轮半径为r

那么：

这一步叫做逆运动学。

例如：

底盘控制器就会计算出：

然后分别发送给两个电机控制器。

接下来才是 PID

例如左轮：

PID 计算：

然后：

这里的 u 可能最终变成：

因此：

/cmd_vel 负责告诉底盘“整台机器人应该怎么运动”，PID 负责让真实电机把这个运动真正实现出来。

在真实机器人中通常是这样分层的

例如 ROS 运行在一台工控机或者树莓派上：

所以很多时候你在 ROS 代码里面根本看不到 PID。

因为 PID 可能已经写在：

里面。

ROS 中谁订阅 /cmd_vel

这取决于你的底盘架构。

最简单的自定义机器人可能有一个：

然后做：

如果使用 ros_control / ros2_control，则可能是：

这时 diff_drive_controller 会帮你完成车体速度到轮速的转换。

Encoder 反馈又会回到 ROS

轮子真实运动以后，编码器可以计算：

进一步通过正运动学反算机器人的：

再积分得到里程计：

于是形成一个完整闭环：

所以整个系统其实有两层闭环：

义父，请我喝杯蜜雪冰城吧。