Nav2（Navigation2）是 ROS2 中用于自主导航的完整软件栈，它继承并改进了 ROS1 中的 move_base 导航框架。本章将深入探讨 Nav2 的核心架构，重点介绍行为树（Behavior Trees）这一革命性的任务编排机制，以及各种路径规划器和控制器的设计原理。通过学习本章，您将掌握如何设计和调优复杂的机器人导航系统，处理动态环境中的各种挑战，并了解工业界在大规模部署中的最佳实践。
Nav2 采用模块化设计，主要包含以下核心组件：
┌─────────────────┐
│ Nav2 BT │
│ Navigator │
└────────┬────────┘
│
┌────────────────────┼────────────────────┐
│ │ │
┌────▼────┐ ┌────▼────┐ ┌────▼────┐
│ Planner │ │Controller│ │Recovery │
│ Server │ │ Server │ │ Server │
└─────────┘ └──────────┘ └─────────┘
│ │ │
┌────▼────┐ ┌────▼────┐ ┌────▼────┐
│ Global │ │ Local │ │ Spin │
│ Costmap │ │ Costmap │ │ Clear │
└─────────┘ └──────────┘ └─────────┘
Nav2 的所有节点都实现了 ROS2 的生命周期接口，支持以下状态转换：
\[\text{Unconfigured} \xrightarrow{\text{configure}} \text{Inactive} \xrightarrow{\text{activate}} \text{Active}\]这种设计允许系统在运行时动态重配置，无需重启整个导航栈。生命周期管理器（Lifecycle Manager）负责协调所有节点的状态转换：
# 生命周期转换序列
lifecycle_sequence = [
'map_server',
'amcl',
'controller_server',
'planner_server',
'recoveries_server',
'bt_navigator'
]
Nav2 遵循 REP-105 坐标系约定：
坐标变换关系： \(T_{map}^{base} = T_{map}^{odom} \cdot T_{odom}^{base}\)
行为树是一种用于描述任务执行流程的层次化结构，相比传统的有限状态机（FSM），具有更好的模块化和可重用性。
基本节点类型：
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="1.0">
<RecoveryNode number_of_retries="1">
<ComputePathToPose goal="{goal}" path="{path}"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely"/>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1">
<FollowPath path="{path}"/>
<ClearEntireCostmap service_name="local_costmap/clear_entirely"/>
</RecoveryNode>
</PipelineSequence>
<SequenceStar name="RecoveryActions">
<ClearEntireCostmap service_name="local_costmap/clear_entirely"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely"/>
<Spin spin_dist="1.57"/>
<Wait wait_duration="5"/>
</SequenceStar>
</RecoveryNode>
</BehaviorTree>
</root>
创建自定义 BT 节点需要继承相应的基类：
class CheckBattery : public BT::ConditionNode {
public:
CheckBattery(const std::string& name,
const BT::NodeConfiguration& config)
: BT::ConditionNode(name, config) {}
static BT::PortsList providedPorts() {
return { BT::InputPort<double>("min_battery") };
}
BT::NodeStatus tick() override {
double min_battery;
getInput("min_battery", min_battery);
if (battery_level_ < min_battery) {
return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::SUCCESS;
}
};
Nav2 提供多种全局路径规划算法：
NavFn Planner（Dijkstra/A*）:
A* 启发式函数： \(f(n) = g(n) + h(n)\) 其中 $g(n)$ 是起点到节点 $n$ 的实际代价，$h(n)$ 是节点 $n$ 到目标的启发式估计。
Smac Planner (State Lattice/Hybrid-A*):
Hybrid-A* 结合了连续空间和离散搜索： \(\text{State} = (x, y, \theta)\)
运动原语基于 Dubins 或 Reeds-Shepp 曲线。
Theta* Planner（任意角度路径）:
Line-of-sight 检查实现任意角度路径： \(\text{parent}(s) = \begin{cases} \text{parent}(s') & \text{if LOS}(\text{parent}(s'), s) \\ s' & \text{otherwise} \end{cases}\)
原始路径通常需要平滑处理以提高可执行性：
Simple Smoother：基于梯度下降的平滑 \(\min \sum_{i} \alpha \|p_i - p_i^{orig}\|^2 + \beta \|p_{i-1} - 2p_i + p_{i+1}\|^2\)
Constrained Smoother：保持障碍物约束的平滑
约束优化问题： \(\begin{aligned} \min_{\{p_i\}} & \sum_{i} w_{smooth} \cdot \text{curvature}(p_i) \\ \text{s.t.} & \quad \text{distance}(p_i, \text{obstacles}) > d_{min} \end{aligned}\)
代价地图是路径规划的基础，将传感器数据转换为规划可用的代价值：
代价值范围：
- 0: 自由空间
- 1-252: 不同程度的代价
- 253: 膨胀半径内
- 254: 致命障碍物（碰撞）
- 255: 未知区域
膨胀函数： \(\text{cost}(d) = \begin{cases} 254 & d \leq r_{inscribed} \\ 253 \cdot e^{-\alpha(d - r_{inscribed})} & r_{inscribed} < d \leq r_{inflation} \\ 0 & d > r_{inflation} \end{cases}\)
Dynamic Window Based (DWB) 控制器基于动态窗口方法，在速度空间中搜索最优控制：
速度搜索空间： \(V_s = \{(v, \omega) | v \in [v_{min}, v_{max}], \omega \in [\omega_{min}, \omega_{max}]\}\)
动态窗口约束： \(V_d = \{(v, \omega) | v \in [v_c - \dot{v} \cdot \Delta t, v_c + \dot{v} \cdot \Delta t]\}\)
评价函数： \(G(v, \omega) = \sigma(\alpha \cdot \text{heading}(v, \omega) + \beta \cdot \text{dist}(v, \omega) + \gamma \cdot \text{vel}(v, \omega))\)
Model Predictive Path Integral (MPPI) 控制器使用采样优化方法：
控制序列优化： \(u^* = \arg\min_{u} \mathbb{E}[\sum_{t=0}^{T} L(x_t, u_t) + \Phi(x_T)]\)
权重更新： \(w_i = \frac{e^{-\frac{1}{\lambda}S_i}}{\sum_j e^{-\frac{1}{\lambda}S_j}}\)
Pure Pursuit 控制器通过跟踪前视点实现路径跟踪：
转向角计算： \(\delta = \arctan\left(\frac{2L\sin\alpha}{l_d}\right)\)
其中 $L$ 是轴距，$\alpha$ 是前视点角度，$l_d$ 是前视距离。
自适应前视距离： \(l_d = k_v \cdot v + l_{d,min}\)
Nav2 提供了多层次的恢复机制来处理导航失败：
恢复决策树：
导航失败
├── 路径规划失败？
│ ├── 清除全局代价地图
│ └── 重新规划
├── 路径跟踪失败？
│ ├── 清除局部代价地图
│ ├── 原地旋转
│ └── 后退重试
└── 多次失败？
└── 请求人工干预
实时监控导航状态，及时检测异常：
振荡检测： \(\text{oscillation} = \frac{1}{T} \int_0^T |\dot{\theta}(t)| dt > \theta_{threshold}\)
卡死检测： \(\text{stuck} = \|p_t - p_{t-\Delta t}\| < \epsilon \quad \forall t \in [t_0, t_0 + T_{stuck}]\)
路径偏离检测： \(d_{lateral} = \min_{p \in \text{path}} \|p_{robot} - p\| > d_{max}\)
class SafetyController {
bool checkCollision(const Trajectory& traj) {
// 层级1：几何碰撞检测
if (geometricCollisionCheck(traj)) return true;
// 层级2：动态障碍物预测
if (predictiveCollisionCheck(traj)) return true;
// 层级3：紧急制动距离
double stop_dist = v * v / (2 * a_max);
if (stop_dist > clearance) return true;
return false;
}
};
使用多传感器融合进行障碍物检测：
传感器融合架构：
LaserScan ──┐
├── Obstacle ──> Tracking ──> Prediction
PointCloud ─┘ Layer
卡尔曼滤波器跟踪动态障碍物：
状态向量： \(x = [p_x, p_y, v_x, v_y]^T\)
预测步骤： \(\begin{aligned} x_{k|k-1} &= F x_{k-1|k-1} \\ P_{k|k-1} &= F P_{k-1|k-1} F^T + Q \end{aligned}\)
更新步骤： \(\begin{aligned} K_k &= P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1} \\ x_{k|k} &= x_{k|k-1} + K_k(z_k - H x_{k|k-1}) \end{aligned}\)
Velocity Obstacles (VO) 用于动态避障：
速度障碍物集合： \(VO_{A|B} = \{v_A | \exists t \geq 0: p_A + tv_A \in B \oplus -A\}\)
可行速度空间： \(V_{feasible} = V_{reachable} \setminus \bigcup_i VO_{A|O_i}\)
在人群中导航时使用社会力模型：
\[F_{total} = F_{goal} + \sum_i F_{obstacle}^i + \sum_j F_{social}^j\]目标吸引力： \(F_{goal} = k_{goal} \cdot \frac{p_{goal} - p_{robot}}{\|p_{goal} - p_{robot}\|}\)
社会排斥力： \(F_{social} = A \cdot e^{(r_{ij} - d_{ij})/B} \cdot \vec{n}_{ij}\)
考虑时间维度的路径规划：
状态空间扩展： \(\text{State} = (x, y, t)\)
时空代价函数： \(C(x, y, t) = C_{static}(x, y) + \sum_i C_{dynamic}^i(x, y, t)\)
动态障碍物预测轨迹： \(p_i(t) = p_i(0) + v_i \cdot t + \frac{1}{2} a_i \cdot t^2\)
Fetch Robotics 为大型电商仓库提供自主移动机器人（AMR）解决方案，其 Freight 系列机器人需要在复杂的仓库环境中实现 24/7 不间断运行。主要挑战包括：
planner_server:
ros__parameters:
planner_plugins: ["GridBased", "Lattice"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
use_astar: true
allow_unknown: false
Lattice:
plugin: "nav2_smac_planner/SmacPlannerLattice"
lattice_filepath: "warehouse_lattice.json"
w_smooth: 0.3
w_data: 0.2
关键性能指标（KPIs）：
| 指标 | 目标值 | 实际达成 | 优化措施 |
|---|---|---|---|
| 平均速度 | 1.2 m/s | 1.15 m/s | 动态速度调节 |
| 定位精度 | ±5 cm | ±3 cm | UWB 辅助定位 |
| 路径效率 | >85% | 89% | A* 启发式优化 |
| 碰撞率 | <0.01% | 0.008% | 多层安全检测 |
| 系统可用性 | >99% | 99.3% | 自动恢复机制 |
优化实践：
// 静态层：仓库布局
static_layer:
map_topic: "warehouse_map"
subscribe_to_updates: false
// 动态层：实时障碍物
obstacle_layer:
observation_sources: laser_scan point_cloud
laser_scan:
clearing: true
marking: true
max_obstacle_height: 1.8
// 膨胀层：安全距离
inflation_layer:
inflation_radius: 0.55
cost_scaling_factor: 3.0
<!-- 仓库特定行为树 -->
<RecoveryNode number_of_retries="3">
<Sequence>
<!-- 检查货架占用 -->
<CheckShelfOccupancy shelf_id="{target_shelf}"/>
<!-- 动态重规划 -->
<RateController hz="2.0">
<ComputePathToPose goal="{goal}" path="{path}"/>
</RateController>
<!-- 精确对接 -->
<PreciseDocking tolerance="0.02"/>
</Sequence>
<ForceSuccess>
<RequestHumanAssistance/>
</ForceSuccess>
</RecoveryNode>
def resolve_deadlock(self, other_robot_id):
priority = self.compute_priority()
if priority < other_priority:
# 后退到最近的避让点
self.navigate_to_passing_zone()
else:
# 等待对方避让
self.wait_for_clearance()
class FleetCoordinator:
def coordinate_paths(self, robot_paths):
# 时空预留表
spacetime_map = SpaceTimeMap()
for robot_id, path in robot_paths.items():
# 检测冲突
conflicts = spacetime_map.check_conflicts(path)
if conflicts:
# 协商解决
resolved_path = self.negotiate_path(
robot_id, path, conflicts
)
spacetime_map.reserve(robot_id, resolved_path)
传统导航将人类视为静态或动态障碍物，而社会导航考虑人类的社会规范和舒适度：
社会代价函数： \(C_{social} = w_1 \cdot C_{proximity} + w_2 \cdot C_{visibility} + w_3 \cdot C_{motion} + w_4 \cdot C_{personal\_space}\)
其中：
Proxemics 理论应用：
Hall’s Proxemics 空间区域：
Social LSTM 模型：
隐藏状态更新考虑社会池化： \(h_i^t = \text{LSTM}(h_i^{t-1}, e_i^t, S_i^t)\)
社会池化层： \(S_i^t = \sum_{j \in \mathcal{N}_i} \phi(h_j^{t-1})\)
Trajectron++ 多模态预测：
条件概率分布： \(p(\mathbf{y}^{t+1:t+T}_i | \mathbf{x}^{t-H:t}) = \sum_k \pi_k \mathcal{N}(\mu_k, \Sigma_k)\)
注视方向估计： \(\vec{g} = R(\theta_{head}) \cdot R(\theta_{gaze}) \cdot \vec{e}_z\)
手势识别状态机：
停止手势 → 减速 → 等待确认
挥手召唤 → 接近模式 → 服务交互
指向手势 → 方向理解 → 路径调整
// CUDA 核函数：批量轨迹预测
__global__ void predictTrajectories(
float* positions, float* velocities,
float* predictions, int num_agents, int horizon
) {
int idx = blockIdx.x * blockDim.x + threadIdx.x;
if (idx < num_agents) {
// 社会力计算
float3 social_force = computeSocialForce(idx, positions);
// 轨迹积分
for (int t = 0; t < horizon; t++) {
predictions[idx * horizon + t] =
integrateMotion(positions[idx],
velocities[idx],
social_force, t);
}
}
}
class EfficientSocialAttention(nn.Module):
def __init__(self, d_model, num_heads):
super().__init__()
self.attention = nn.MultiheadAttention(
d_model, num_heads, batch_first=True
)
# 稀疏注意力模式
self.sparsity_mask = self.create_sparse_mask()
def forward(self, x, neighbors):
# 仅关注最近的 K 个邻居
top_k = torch.topk(distances, k=self.k, largest=False)
sparse_attn = self.attention(
x, neighbors[top_k.indices],
attn_mask=self.sparsity_mask
)
return sparse_attn
# 三层决策架构
high_level:
frequency: 1 Hz
tasks:
- goal_selection
- route_planning
- social_context_analysis
mid_level:
frequency: 10 Hz
tasks:
- local_planning
- crowd_navigation
- gesture_recognition
low_level:
frequency: 50 Hz
tasks:
- collision_avoidance
- trajectory_tracking
- emergency_stop
class SocialNavigator:
def __init__(self):
self.comfort_distance = 1.2 # 米
self.social_force_model = SocialForceModel()
self.gesture_recognizer = GestureRecognizer()
def plan_social_path(self, start, goal, people_tracks):
# 预测人群未来轨迹
predictions = self.predict_crowd_motion(people_tracks)
# 生成社会兼容路径
path = self.astar_social(
start, goal,
static_costmap=self.costmap,
dynamic_costs=self.compute_social_costs(predictions)
)
# 速度调节
velocity_profile = self.adapt_velocity_to_crowd(
path, predictions
)
return path, velocity_profile
def compute_social_costs(self, predictions):
costs = np.zeros_like(self.costmap)
for person_traj in predictions:
# Gaussian 分布个人空间
costs += self.gaussian_personal_space(
person_traj,
sigma=self.comfort_distance
)
return costs
本章深入探讨了 ROS2 导航栈 Nav2 的核心架构和关键技术。主要内容包括：
行为树架构：相比传统状态机，行为树提供了更好的模块化、可重用性和可读性，成为 Nav2 任务编排的核心机制。
恢复机制：多层次恢复行为、异常检测、安全保障
练习 12.1：解释为什么 Nav2 选择行为树而不是有限状态机作为任务编排机制？至少给出三个技术优势。
练习 12.2：在 DWB 控制器中，如果机器人最大速度为 1.0 m/s，最大加速度为 0.5 m/s²，时间步长为 0.1s，计算动态窗口的速度范围。
练习 12.3：给定机器人半径 0.3m，障碍物半径 0.2m，计算代价地图的内切圆半径（inscribed radius）和膨胀半径（inflation radius）。
练习 12.4：设计一个行为树来处理以下导航场景：机器人需要通过一个可能被占用的狭窄通道，如果通道被占用超过 30 秒，需要寻找替代路径。
练习 12.5：实现一个简化的 MPPI 控制器，给定代价函数 $L(x,u) = |x - x_{goal}|^2 + 0.1|u|^2$，采样 100 条轨迹，计算最优控制。
练习 12.6：在社会导航场景中，机器人检测到前方 2m 处有一个人以 0.5 m/s 的速度横穿，设计一个基于速度障碍物（VO）的避让策略。
练习 12.7：设计一个多层恢复策略，处理机器人在电梯门口等待时的各种异常情况。
练习 12.8：实现一个基于社会力模型的人群导航算法，考虑 5 个行人的相互作用。
问题：机器人频繁碰撞或过度保守
# 错误配置
inflation_layer:
inflation_radius: 0.1 # 太小，容易碰撞
cost_scaling_factor: 10.0 # 太大，过度保守
正确配置：
inflation_layer:
inflation_radius: 0.55 # 机器人半径 + 安全裕度
cost_scaling_factor: 3.0 # 平滑代价渐变
问题：恢复行为相互触发导致死锁
<!-- 错误：可能无限循环 -->
<RecoveryNode number_of_retries="-1">
<FollowPath path="{path}"/>
<ClearCostmap/>
</RecoveryNode>
解决：设置重试次数限制和超时
<RecoveryNode number_of_retries="3">
<Timeout msec="30000">
<FollowPath path="{path}"/>
</Timeout>
<ClearCostmap/>
</RecoveryNode>
症状：机器人左右摇摆，无法稳定跟踪路径
原因：
调试方法：
# 记录控制器输出用于分析
def debug_controller_oscillation(self):
history = []
for _ in range(100):
cmd_vel = self.controller.compute_velocity()
history.append({
'linear': cmd_vel.linear.x,
'angular': cmd_vel.angular.z,
'error': self.path_error
})
# 检测振荡
angular_changes = np.diff([h['angular'] for h in history])
oscillation_score = np.sum(np.abs(angular_changes))
if oscillation_score > threshold:
self.reduce_controller_gains()
问题：对快速移动物体反应不及时
解决：提高传感器更新频率和预测范围
obstacle_layer:
observation_sources: laser
laser:
data_type: LaserScan
clearing: true
marking: true
expected_update_rate: 0.0 # 不检查更新率
observation_persistence: 0.0 # 立即清除旧数据
问题：多机器人在狭窄空间死锁
预防措施：
class MultiRobotCoordinator:
def prevent_deadlock(self):
# 1. 优先级分配
self.assign_dynamic_priorities()
# 2. 预留时空槽
self.reserve_spacetime_slots()
# 3. 死锁检测
if self.detect_circular_wait():
self.break_deadlock_by_backoff()