第19章 局部路径规划
课程:ROS 2 C++17 编程(COM260 Kit / Humble)
章节:第19章
课时:2 课时(90 分钟)
教学方式:讲授 + 演示
先完成教案示例运行说明的双端准备。C++ 示例在 COM260 上运行,Gazebo / Nav2 / RViz 在 x86 上运行。
学习目标
本章学习目标包括:理解DWA(动态窗口法)算法的原理和实现,掌握Regulated Pure Pursuit控制器的工作机制,熟悉局部路径规划的轨迹评价方法,能够在Nav2中配置DWB和RPP控制器,掌握局部规划参数调优方法。
19.1 局部路径规划概述
19.1.1 问题定义
局部路径规划是在全局路径的引导下,根据传感器实时信息生成安全的速度指令,实现动态避障和路径跟踪。
与全局规划的区别:
| 特性 | 全局路径规划 | 局部路径规划 |
|---|---|---|
| 规划范围 | 整个地图 | 机器人周围(数米) |
| 信息源 | 已知地图 | 实时传感器 |
| 更新频率 | 低 (1-2 Hz) | 高 (10-50 Hz) |
| 规划目标 | 最短路径 | 路径跟踪+避障 |
| 输出 | 路径点序列 | 速度指令 (v, ω) |
**局部规划的核心挑战:**包括实时性(需要在毫秒级生成控制指令)、安全性(必须避开动态障碍物)、平滑性(速度变化不能太剧烈)和可达性(满足机器人运动学约束)。
19.1.2 Nav2局部控制器
Nav2支持多种局部控制器插件:
| 控制器 | 算法 | 特点 | 适用场景 |
|---|---|---|---|
| DWB | 动态窗口法 + 评分 | 功能最全,可定制 | 通用场景 |
| Regulated Pure Pursuit | 调节纯追踪 | 简单可靠 | 差速/阿克曼 |
| MPPI | 模型预测路径积分 | 采样+并行优化 | 高端动态场景 |
| TEB | 时间弹性带 | 考虑时间最优 | 复杂环境 |
19.1.3 官方要点——controller_server 官方文档的插件化局部控制框架
docs.nav2.org 将 controller_server 描述为一个 FollowPath Action 服务器:全局规划器只负责给出路径,实际的速度指令由通过插件加载的控制器以 controller_frequency(默认 20 Hz)频率在最新代价地图上计算。官方强调这一解耦的价值——全局规划可以低频运行以节省算力,而局部控制必须高频响应动态障碍,两者互不阻塞。文档还指出控制器在计算前会检查新目标位姿与 TF 是否就绪,未就绪时直接返回失败并由行为树接管恢复,这解释了本章 19.2 中"控制器报错先查上游"的排障顺序。
19.2 DWA动态窗口法
19.2.1 DWA算法原理
DWA (Dynamic Window Approach) 在速度空间中采样,预测每条轨迹的未来状态,通过评分选择最优速度指令。
算法流程:
1. 生成速度采样窗口
- 考虑最大/最小速度限制
- 考虑加速度限制(动态窗口)
- 考虑安全停止距离
2. 对每组速度 (v, ω) 模拟轨迹
- 使用运动模型前向模拟
- 预测未来N步的位姿
3. 评估每条轨迹
- 方向评分(与目标对齐)
- 障碍物评分(最小距离)
- 速度评分(鼓励高速)
- 路径评分(与全局路径对齐)
4. 选择最优速度
- 加权求和各评分
- 选择综合得分最高的速度
19.2.2 C++ 实现
#pragma once
#include <array>
#include <grid_path_planner/search.hpp>
struct Motion { double x, y, yaw, v, w; };
struct Velocity { double v = 0, w = 0; };
struct DWAConfig {
double max_speed = 0.5, min_speed = 0, max_yaw_rate = 1;
double max_accel = 2, max_yaw_accel = 3;
double dt = 0.1, predict_time = 2, v_step = 0.05, w_step = 0.1;
double radius = 0.22;
};
struct TrajectoryScore { double heading = 0, obstacle = 0, velocity = 0, path = 0; };
struct DWAResult {
Velocity command;
std::vector<Motion> trajectory;
double score = -std::numeric_limits<double>::infinity();
};
inline std::array<double, 4> dynamic_window(Motion state, const DWAConfig & c) {
return {std::max(c.min_speed, state.v - c.max_accel * c.dt),
std::min(c.max_speed, state.v + c.max_accel * c.dt),
std::max(-c.max_yaw_rate, state.w - c.max_yaw_accel * c.dt),
std::min(c.max_yaw_rate, state.w + c.max_yaw_accel * c.dt)};
}
inline std::vector<Motion> predict_trajectory(Motion state, Velocity command, const DWAConfig & c) {
std::vector<Motion> trajectory{state};
for (int step = 0; step < static_cast<int>(std::ceil(c.predict_time / c.dt)); ++step) {
state.x += command.v * std::cos(state.yaw) * c.dt;
state.y += command.v * std::sin(state.yaw) * c.dt;
state.yaw += command.w * c.dt;
state.v = command.v; state.w = command.w;
trajectory.push_back(state);
}
return trajectory;
}
inline double point_path_distance(Point point, const Path & path) {
double nearest = std::hypot(point.first - path.front().first, point.second - path.front().second);
for (std::size_t i = 1; i < path.size(); ++i) {
const auto [ax, ay] = path[i - 1];
const double dx = path[i].first - ax, dy = path[i].second - ay;
const double squared = dx * dx + dy * dy;
const double t = squared == 0 ? 0 : std::clamp(
((point.first - ax) * dx + (point.second - ay) * dy) / squared, 0.0, 1.0);
nearest = std::min(nearest, std::hypot(point.first - ax - t * dx, point.second - ay - t * dy));
}
return nearest;
}
inline double score_trajectory(const std::vector<Motion> & trajectory, Point goal,
const Path & obstacles, const Path & path,
const DWAConfig & c, TrajectoryScore & scores) {
const auto end = trajectory.back();
const double angle = end.yaw - std::atan2(goal.second - end.y, goal.first - end.x);
scores.heading = 1 - std::abs(std::atan2(std::sin(angle), std::cos(angle))) / std::acos(-1);
double clearance = std::numeric_limits<double>::infinity();
scores.path = 0;
for (auto state : trajectory) {
for (auto [x, y] : obstacles) { clearance = std::min(clearance, std::hypot(state.x - x, state.y - y) - c.radius); }
if (!path.empty()) {
scores.path += 1 / (1 + point_path_distance({state.x, state.y}, path));
}
}
const double braking = end.v * end.v / (2 * c.max_accel);
if (clearance <= braking) { return -std::numeric_limits<double>::infinity(); }
scores.obstacle = std::min(1.0, clearance / 0.5);
scores.velocity = end.v / c.max_speed;
scores.path /= trajectory.size();
return scores.heading + 0.5 * scores.obstacle + 0.3 * scores.velocity + 2 * scores.path;
}
inline DWAResult dwa_plan(Motion state, Point goal, const Path & obstacles,
const Path & path = {}, const DWAConfig & c = {}) {
DWAResult best;
for (double value : {state.x, state.y, state.yaw, state.v, state.w, goal.first, goal.second,
c.max_speed, c.min_speed, c.max_yaw_rate, c.max_accel, c.max_yaw_accel,
c.dt, c.predict_time, c.v_step, c.w_step, c.radius}) {
if (!std::isfinite(value)) { return best; }
}
for (const auto * points : {&obstacles, &path}) {
for (auto [x, y] : *points) { if (!std::isfinite(x) || !std::isfinite(y)) { return best; } }
}
if (c.max_speed <= 0 || c.max_accel <= 0 || c.dt <= 0 || c.predict_time <= 0 ||
c.v_step <= 0 || c.w_step <= 0 || c.radius <= 0 || c.max_yaw_rate <= 0 ||
c.max_yaw_accel <= 0 || c.min_speed < 0 || c.min_speed > c.max_speed) { return best; }
const double remaining = std::hypot(state.x - goal.first, state.y - goal.second);
if (remaining <= 0.1) { return best; }
if (!path.empty()) {
auto nearest = std::min_element(path.begin(), path.end(), [&](Point a, Point b) {
return std::hypot(a.first - state.x, a.second - state.y) <
std::hypot(b.first - state.x, b.second - state.y);
});
double ahead = 0;
goal = *nearest;
for (auto next = nearest + 1; next != path.end() && ahead < 0.6; ++next) {
ahead += std::hypot(next->first - goal.first, next->second - goal.second);
goal = *next;
}
}
auto window = dynamic_window(state, c);
window[1] = std::min(window[1], remaining / (2 * c.predict_time));
if (window[1] < window[0] || window[3] < window[2]) { return best; }
const int nv = std::max(1, static_cast<int>(std::ceil((window[1] - window[0]) / c.v_step)));
const int nw = std::max(1, static_cast<int>(std::ceil((window[3] - window[2]) / c.w_step)));
for (int i = 0; i <= nv; ++i) {
for (int j = -1; j <= nw; ++j) {
const double yaw_rate = j < 0 ? 0 : window[2] + (window[3] - window[2]) * j / nw;
if (yaw_rate < window[2] || yaw_rate > window[3]) { continue; }
Velocity command{window[0] + (window[1] - window[0]) * i / nv,
yaw_rate};
const auto trajectory = predict_trajectory(state, command, c);
TrajectoryScore scores;
const double score = score_trajectory(trajectory, goal, obstacles, path, c, scores);
if (score > best.score) { best = {command, trajectory, score}; }
}
}
return best;
}完整源码与构建说明。算法默认速度上限为 0.5 m/s、加速度为 2 m/s²、角加速度为 3 rad/s²、步长为 0.1 s、预测时长为 2 s;Gazebo 闭环节点限速 0.15 m/s。无有效轨迹、无效输入或到达目标时输出零速度。方向评分使用全局路径上前方约 0.6 m 的局部目标,避免被障碍后方的最终目标吸引而停滞;到达判断仍使用最终目标。接近终点时将预测位移限制在剩余距离的一半内,避免候选轨迹越过终点而导致静止候选长期占优。
19.2.3 DWA评分机制详解
inline double score_trajectory(const std::vector<Motion> & trajectory, Point goal,
const Path & obstacles, const Path & path,
const DWAConfig & c, TrajectoryScore & scores) {
const auto end = trajectory.back();
const double angle = end.yaw - std::atan2(goal.second - end.y, goal.first - end.x);
scores.heading = 1 - std::abs(std::atan2(std::sin(angle), std::cos(angle))) / std::acos(-1);
double clearance = std::numeric_limits<double>::infinity();
scores.path = 0;
for (auto state : trajectory) {
for (auto [x, y] : obstacles) { clearance = std::min(clearance, std::hypot(state.x - x, state.y - y) - c.radius); }
if (!path.empty()) {
scores.path += 1 / (1 + point_path_distance({state.x, state.y}, path));
}
}
const double braking = end.v * end.v / (2 * c.max_accel);
if (clearance <= braking) { return -std::numeric_limits<double>::infinity(); }
scores.obstacle = std::min(1.0, clearance / 0.5);
scores.velocity = end.v / c.max_speed;
scores.path /= trajectory.size();
return scores.heading + 0.5 * scores.obstacle + 0.3 * scores.velocity + 2 * scores.path;
}评分权重为方向 1.0、障碍 0.5、速度 0.3、路径 2.0。净距小于等于 v²/(2a) 的候选轨迹直接拒绝;角差先归一化,路径项取每个预测点到全局路径线段的最近距离,避免稀疏路径点让静止状态获得不合理优势。这个点障碍教学模型不包含传感器误差、网络延迟和障碍运动预测,仿真结论不等于实车安全保证。
19.3 DWB控制器
19.3.1 DWB与DWA的关系
DWB (Dynamic Window Based) 是Nav2中DWA的升级实现,增加了更多可配置的评分器(Critic):
**DWB的核心组件:**Critics(评分器)负责评估轨迹的各项指标,Goal Checker(目标检查器)判断是否到达目标,Oscillation Handler(振荡处理器)防止前进-后退振荡。
19.3.2 DWB参数配置
controller_server:
ros__parameters:
use_sim_time: true
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: progress_checker
goal_checker_plugins:
- general_goal_checker
controller_plugins:
- FollowPath
progress_checker:
plugin: nav2_controller::SimpleProgressChecker
required_movement_radius: 0.1
movement_time_allowance: 10.0
general_goal_checker:
stateful: true
plugin: nav2_controller::SimpleGoalChecker
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.25
FollowPath:
plugin: dwb_core::DWBLocalPlanner
debug_trajectory_details: true
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 0.26
max_vel_y: 0.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.26
min_speed_theta: 0.0
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
vx_samples: 20
vy_samples: 5
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
xy_goal_tolerance: 0.1
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: true
stateful: true
critics:
- RotateToGoal
- Oscillation
- BaseObstacle
- GoalAlign
- PathAlign
- PathDist
- GoalDist
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0配置与本章实际仿真一致。Burger 的线速度上限为 0.26 m/s;原理示例中的 0.5 m/s 不直接写入机器人参数。Humble 的目标检查器位于 controller_server 的 general_goal_checker 命名空间,插件名为 nav2_controller::SimpleGoalChecker。BaseObstacle.scale=0.02 与路径类权重分别作用于不同量纲,不应只比较数值大小。
19.3.3 评分器详解
#pragma once
#include "dwa_planner/dwa.hpp"
// Small scoring examples; Nav2 DWB loads its own critic plugins.
inline double obstacle_cost(const std::vector<Motion> & trajectory, const Costmap2D & map) {
double total = 0;
if (trajectory.empty()) { return std::numeric_limits<double>::infinity(); }
for (auto state : trajectory) {
if (!inside_map(map, {state.x, state.y}) || map.get_cost(state.x, state.y) >= 253) {
return std::numeric_limits<double>::infinity();
}
total += map.get_cost(state.x, state.y);
}
return total / trajectory.size();
}
inline double path_distance(const std::vector<Motion> & trajectory, const Path & path) {
if (trajectory.empty() || path.empty()) { return std::numeric_limits<double>::infinity(); }
double total = 0;
for (auto state : trajectory) {
total += point_path_distance({state.x, state.y}, path);
}
return total / trajectory.size();
}
inline double oscillation_cost(const std::vector<Velocity> & history) {
if (history.size() < 5) { return 0; }
double cost = 0;
for (std::size_t i = history.size() - 4; i < history.size(); ++i) {
if (history[i - 1].v * history[i].v < 0) { cost += 100; }
}
return cost;
}这里是评分含义的数值演示,分数越小越好。GoalDist 可直接用 std::hypot(end.x-goal.first, end.y-goal.second);朝向误差用 atan2(sin(error), cos(error)) 归一化。Nav2 实际加载自己的 critic 插件,RotateToGoal 还包含终点减速/转向阶段,不能与这个简化角度公式视为完全相同。
19.3.4 官方要点——DWB 评分体系与 critics 插件的组合调优
DWB 在每个周期对速度空间采样,沿候选轨迹组合 critic 代价;采样数和预测时长影响计算量。当前配置使用 BaseObstacle、PathAlign、PathDist、GoalAlign、GoalDist、RotateToGoal 和 Oscillation。碰撞拒绝、路径跟踪、目标减速与振荡处理需要一起观察,调权重时记录实际轨迹,避免只提高某一项而破坏其他行为。
19.4 Regulated Pure Pursuit
19.4.1 Pure Pursuit基本原理
Pure Pursuit是一种基于几何追踪的路径跟踪算法,通过追踪前方的一个目标点来引导机器人。
算法原理:
1. 在全局路径上找到一个前瞻点 (lookahead point)
- 距离机器人当前位置为 lookahead_distance
2. 计算机器人到前瞻点的转角
- 使用圆弧拟合,圆弧经过机器人和前瞻点
3. 输出速度指令
- 线速度:根据曲率自适应调节
- 角速度:圆弧曲率
曲率计算公式:
γ = 2 * sin(α) / L
其中:
- α: 机器人朝向与前向量之间的夹角
- L: 前瞻距离
- γ: 曲率
控制量:
- ω = v * γ (角速度 = 线速度 × 曲率)
19.4.2 Regulated Pure Pursuit实现
#pragma once
#include <array>
#include <grid_path_planner/search.hpp>
struct RPPConfig {
double desired_speed = 0.3, max_speed = 0.5, min_speed = 0.05, max_yaw_rate = 1;
double lookahead = 1, min_lookahead = 0.3, max_lookahead = 2;
double max_curvature = 0.5, curvature_weight = 0.3, cost_gain = 1;
};
struct RPPCommand { double v = 0, w = 0, curvature = 0; Point target{}; };
inline RPPCommand pure_pursuit(std::array<double, 3> pose, const Path & path,
const Costmap2D * map = nullptr,
double current_speed = 0.3, const RPPConfig & c = {}) {
RPPCommand command;
for (double value : {pose[0], pose[1], pose[2], current_speed, c.desired_speed, c.max_speed,
c.min_speed, c.max_yaw_rate, c.lookahead, c.min_lookahead, c.max_lookahead,
c.max_curvature, c.curvature_weight, c.cost_gain}) {
if (!std::isfinite(value)) { return command; }
}
for (auto [x, y] : path) { if (!std::isfinite(x) || !std::isfinite(y)) { return command; } }
if (path.size() < 2 || c.desired_speed <= 0 || c.max_speed <= 0 || c.max_yaw_rate <= 0 ||
c.min_speed < 0 || c.min_speed > c.max_speed || c.min_lookahead <= 0 ||
c.max_lookahead < c.min_lookahead || c.max_curvature <= 0) { return command; }
if (map && !inside_map(*map, {pose[0], pose[1]})) { return command; }
const auto goal = path.back();
if (std::hypot(goal.first - pose[0], goal.second - pose[1]) <= 0.1) { return command; }
if (map && map->get_cost(pose[0], pose[1]) >= 253) { return command; }
std::size_t nearest = 0;
double best = std::numeric_limits<double>::infinity();
for (std::size_t i = 0; i < path.size(); ++i) {
double distance = std::hypot(path[i].first - pose[0], path[i].second - pose[1]);
if (distance < best) { best = distance; nearest = i; }
}
const double lookahead = std::clamp(c.lookahead + std::abs(current_speed) * 0.5,
c.min_lookahead, c.max_lookahead);
command.target = goal;
for (std::size_t i = nearest; i < path.size(); ++i) {
if (std::hypot(path[i].first - pose[0], path[i].second - pose[1]) >= lookahead) {
command.target = path[i]; break;
}
}
const double dx = command.target.first - pose[0], dy = command.target.second - pose[1];
const double angle = std::atan2(std::sin(std::atan2(dy, dx) - pose[2]),
std::cos(std::atan2(dy, dx) - pose[2]));
if (std::abs(angle) > std::acos(-1) / 4) {
command.w = std::clamp(angle, -c.max_yaw_rate, c.max_yaw_rate);
return command;
}
command.curvature = std::clamp(2 * std::sin(angle) / std::hypot(dx, dy),
-c.max_curvature, c.max_curvature);
double speed = c.desired_speed * std::max(0.3, 1 - std::abs(command.curvature) * c.curvature_weight);
if (map) { speed *= std::max(0.1, 1 - map->get_cost(pose[0], pose[1]) / 254.0 * c.cost_gain); }
command.v = std::clamp(speed, c.min_speed, c.max_speed);
if (command.curvature != 0) { command.v = std::min(command.v, c.max_yaw_rate / std::abs(command.curvature)); }
command.w = command.v * command.curvature;
if (map) {
auto predicted = pose;
Point previous{pose[0], pose[1]};
for (int i = 0; i < 10; ++i) {
predicted[0] += command.v * std::cos(predicted[2]) * 0.1;
predicted[1] += command.v * std::sin(predicted[2]) * 0.1;
predicted[2] += command.w * 0.1;
Point next{predicted[0], predicted[1]};
if (!clear_segment(*map, previous, next)) { return {}; }
previous = next;
}
}
return command;
}完整源码从最近路径进度向前选择目标,保留曲率、代价和速度调节,并在到达目标或预测碰撞时停止。这是原理示例;下面的 Nav2 RPP 插件使用其自身实现与参数,不会执行这个函数。
19.4.3 RPP配置
controller_server:
ros__parameters:
controller_plugins:
- FollowPath
FollowPath:
plugin: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
desired_linear_vel: 0.26
lookahead_dist: 1.0
min_lookahead_dist: 0.3
max_lookahead_dist: 2.0
lookahead_time: 1.5
use_velocity_scaled_lookahead_dist: true
use_regulated_linear_velocity_scaling: true
use_cost_regulated_linear_velocity_scaling: true
regulated_linear_scaling_min_radius: 0.9
regulated_linear_scaling_min_speed: 0.05
cost_scaling_dist: 0.3
cost_scaling_gain: 1.0
inflation_cost_scaling_factor: 3.0
use_collision_detection: true
max_allowed_time_to_collision_up_to_carrot: 1.0
use_rotate_to_heading: true
rotate_to_heading_angular_vel: 1.0
max_angular_accel: 3.2
transform_tolerance: 0.2本节列出 Humble 支持的参数。max_linear_vel、max_allowed_curvature 等是教学函数参数,不是此 Nav2 RPP 插件的参数;前瞻与调速开关使用完整的 _dist / _scaling 名称。
19.4.4 DWB vs RPP对比
| 特性 | DWB | RPP |
|---|---|---|
| 算法基础 | 动态窗口采样 + 多评分器 | 几何追踪 + 速度调节 |
| 计算量 | 随候选轨迹、预测步数与评分器增长 | 主要为路径检索与前向碰撞检查 |
| 参数 | 较多,需协调 critic 权重 | 重点是速度、前瞻和 regulation |
| 狭窄通道 | 受足迹、采样和权重影响 | 受前瞻距离和路径曲率影响 |
| 障碍物 | 在候选轨迹中选可行速度 | 碰撞检查后减速或停止,依赖上游绕障路径 |
| 平滑性与精度 | 需实测 | 需实测 |
按相同地图、初始位姿、目标和障碍设置比较,记录轨迹和停止反馈;不能由算法名称直接推断穿越成功率。
19.4.5 官方要点——Regulated Pure Pursuit 的曲率与障碍双调节
Humble RPP 使用 use_regulated_linear_velocity_scaling 与 use_cost_regulated_linear_velocity_scaling 开启曲率和障碍调速,距离参数为 cost_scaling_dist;use_velocity_scaled_lookahead_dist 配合 lookahead_time 决定前瞻距离。前向碰撞检查由 use_collision_detection 与 max_allowed_time_to_collision_up_to_carrot 控制。RPP 不做 DWA 式速度采样,绕障路径由上游规划器提供;这些行为需与练习 5 的实际轨迹对照。
19.5 MPPI控制器
19.5.1 MPPI原理
MPPI (Model Predictive Path Integral) 是一种基于采样的模型预测控制方法:
**核心思想:**对控制序列进行多次采样(加入噪声),前向模拟每条采样序列的未来状态,根据代价函数计算每条序列的权重,加权平均生成最优控制序列,然后执行第一个控制指令并重复。
19.5.2 MPPI优势
MPPI可以处理非线性模型,通过多条候选控制序列的代价加权选择控制量。它适合研究复杂场景中的控制优化,但计算成本随采样数和预测时域增加;实时性和模型误差影响需实测。
controller_server:
ros__parameters:
controller_plugins:
- FollowPath
FollowPath:
plugin: nav2_mppi_controller::MPPIController
batch_size: 1000
time_steps: 56
model_dt: 0.1
vx_std: 0.2
vy_std: 0.0
wz_std: 0.4
vx_max: 0.26
vx_min: 0.0
vy_max: 0.0
wz_max: 1.0
motion_model: DiffDrive
critics:
- ConstraintCritic
- ObstaclesCritic
- GoalCritic
- GoalAngleCritic
- PathAlignCritic
- PathFollowCritic
- PathAngleCritic
- PreferForwardCritic
PathFollowCritic.cost_weight: 5.0
GoalCritic.cost_weight: 5.0
ObstaclesCritic.repulsion_weight: 10.0
controller_frequency: 10.0本节列出当前 Humble 实际支持的参数。采样数与步长分别使用 batch_size、model_dt,代价权重设置到对应 critic。控制频率 10 Hz 与 0.1 s 模型步长匹配;Humble 实现使用 CPU 数值运算,本配置没有启用 GPU。
19.5.3 官方要点——控制器选型与插件生态
本课程固定 Humble / Nav2 1.1.20,实际镜像含 DWB、RPP 和 MPPI;默认选择由参数文件决定。Rotation Shim 可在进入路径追踪前先调整朝向,TEB 属于另外集成的插件,不能假定当前镜像已安装。选型时应同时考虑足迹、运动学、地图、CPU 预算和停止行为;本章用同一地图和目标比较 DWB 与 RPP,并单独检查 MPPI 入口。
19.6 局部规划参数调优
19.6.1 速度参数调优
#pragma once
#include <algorithm>
#include <string>
#include <stdexcept>
struct SpeedParameters { double linear, angular, acceleration; };
inline SpeedParameters recommend_speed(const std::string & robot, const std::string & environment) {
SpeedParameters speed;
if (robot == "differential") { speed = {0.5, 1.0, 2.5}; }
else if (robot == "ackermann") { speed = {0.8, 0.5, 1.5}; }
else { throw std::invalid_argument("unknown robot type"); }
if (environment == "narrow") { speed.linear *= 0.6; speed.acceleration *= 0.7; }
else if (environment == "open") { speed.linear *= 1.2; }
return speed;
}这些函数用于调参练习,不直接控制 Burger;最终速度还必须受模型和实际配置上限约束。
19.6.2 前瞻距离调优
前瞻距离是RPP最关键参数:
inline double recommend_lookahead(double speed, double frequency, double length) {
if (frequency <= 0) { throw std::invalid_argument("frequency must be positive"); }
return std::clamp((speed * 3 * 10 / frequency + length * 2) / 2, 0.3, 3.0);
}这是启发式建议,不是优化器给出的全局最优值。Humble RPP 的速度缩放前瞻使用 lookahead_time、min_lookahead_dist 和 max_lookahead_dist,与此练习公式分开。
19.6.3 评分权重调优指南
下列数值用于权重比较练习,不能与不同量纲的 critic 分数直接类比。Nav2 配置见 planning.yaml,运行步骤见教案示例运行说明。
# 不同场景的DWB权重推荐
# 狭窄走廊场景
critics: ["BaseObstacle", "PathAlign", "GoalDist"]
BaseObstacle.scale: 50.0 # 相对同一配置提高障碍代价权重
PathAlign.scale: 50.0 # 强化路径跟踪
GoalDist.scale: 20.0
# 开阔大厅场景
critics: ["BaseObstacle", "GoalAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 30.0 # 弱化避障
GoalAlign.scale: 40.0 # 强化目标方向
PathDist.scale: 20.0
GoalDist.scale: 30.0
# 动态障碍物场景
critics: ["BaseObstacle", "PathAlign", "GoalAlign", "Oscillation"]
BaseObstacle.scale: 60.0 # 强化避障
PathAlign.scale: 20.0
GoalAlign.scale: 20.0
Oscillation.scale: 15.019.6.4 调试命令
先加载本章 COM260 环境,再按当前控制器查询:
ros2 param get /controller_server FollowPath.plugin
ros2 param get /controller_server FollowPath.max_vel_x
ros2 param set /controller_server FollowPath.max_vel_x 0.2
ros2 topic echo /cmd_vel --once
ros2 topic echo /odom --once上述速度参数用于 DWB;RPP 使用 FollowPath.desired_linear_vel。DWB 开启 debug_trajectory_details 后,先用 ros2 topic list 确认实际发布的 evaluation/local_plan 话题,再在 RViz 添加对应显示。ROS 参数按具体名称查询,FollowPath 只是命名空间。
课后练习
-
原理题: 说明DWA算法中动态窗口的定义,阐述为什么需要结合速度极限、加速度极限和安全停止距离来生成动态窗口。
-
编程题: 实现DWA局部路径规划器,包含速度空间采样、轨迹预测和评分函数,并在仿真环境中测试避障效果。
-
分析题: 比较DWB和Regulated Pure Pursuit的异同,分析两种控制器在不同场景下的适用性。
-
配置题: 为差速机器人在走廊环境配置DWB控制器参数,要求能够顺利通过0.8m宽的走廊并保持稳定的导航行为。
-
操作题: 在仿真环境中分别使用DWB和RPP控制器进行导航,对比两种控制器在路径跟踪精度和安全避障方面的表现。
-
设计题: 设计一个自适应控制器切换策略:在宽阔区域使用RPP提高效率,在狭窄通道切换到DWB提高通过能力,并给出切换条件和参数过渡方案。
仿真结合实例(当前仓库):控制器跟踪路径并输出速度
目标与知识点对应
在 Nav2 仿真中观察 DWB 控制器、局部代价地图和 /cmd_vel 的关系,理解局部规划器如何将全局路径和实时激光障碍转换为速度指令。
运行步骤
按教案示例运行说明分别启动 dwb 与 rpp,在 COM260 使用 C++ 入口:
source ~/.config/ros2-course-com260-ch18-ch19/env.bash
ros2 run navigation_client_cpp bt_navigator_client --ros-args \
-p use_sim_time:=true -p x:=1.0 -p y:=0.0
ros2 topic echo /odom --once
ros2 topic info /scan自编 DWA 节点、障碍和 0.8 m 走廊的步骤见教案示例运行说明。
观察结果
发送导航目标后,控制器应根据路径输出速度;RViz 可同时显示 Local Costmap 和路径。若没有 odom → base_link TF,控制器会等待变换,需先检查 Gazebo 桥和 TF。
源码与边界
教学算法与闭环节点位于 dwa_planner、rpp_controller。Nav2 DWB/RPP/MPPI 由固定 Humble 镜像提供,配置见 planning.yaml。
数值示例输出、导航轨迹和停止反馈的参考结果,以及对应截图和视频,见教案示例运行说明及运行证据。
学习材料:
- Nav2 官方文档 —— Controllers、DWB Controller 与 Regulated Pure Pursuit:https://docs.nav2.org/
- docs.ros.org —— nav2_dwb_controller 与 nav2_regulated_pure_pursuit_controller 包文档:https://docs.ros.org/
- navigation2 仓库 —— 控制器插件列表与选型说明:https://github.com/ros-navigation/navigation2
- The Construct —— Nav2 局部规划控制器课程:https://www.theconstructsim.com/
- Robotics Back-End —— Nav2 控制器对比实战教程:https://roboticsbackend.com/
RuyiSDK Board Docs