第19章 局部路径规划
课程:ROS2 Python 编程
章节:第19章
课时:2 课时(90 分钟)
教学方式:讲授 + 演示
学习目标
本章学习目标包括:理解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 Python实现
import numpy as np
from enum import Enum
class DWAPlanner:
"""DWA动态窗口法规划器"""
def __init__(self, config: dict = None):
# 运动学约束
self.max_speed = config.get('max_speed', 0.5) # m/s
self.min_speed = config.get('min_speed', 0.0)
self.max_yaw_rate = config.get('max_yaw_rate', 1.0) # rad/s
self.max_accel = config.get('max_accel', 2.0) # m/s²
self.max_delta_yaw_rate = config.get('max_delta_yaw_rate', 3.0) # rad/s²
# 仿真参数
self.dt = config.get('dt', 0.1) # 时间步长(s)
self.predict_time = config.get('predict_time', 2.0) # 预测时长(s)
# 速度分辨率
self.v_resolution = config.get('v_resolution', 0.05)
self.w_resolution = config.get('w_resolution', 0.1)
# 评分权重
self.alpha_heading = config.get('alpha_heading', 1.0) # 目标方向
self.alpha_dist = config.get('alpha_dist', 0.5) # 障碍物距离
self.alpha_velocity = config.get('alpha_velocity', 0.3) # 速度
self.alpha_path = config.get('alpha_path', 2.0) # 路径跟踪
def plan(self, state: np.ndarray, goal: np.ndarray,
obstacles: np.ndarray, global_path: np.ndarray = None) -> np.ndarray:
"""
执行DWA规划
参数:
state: [x, y, theta, v, omega] 当前状态
goal: [x, y] 局部目标点
obstacles: (N, 2) 障碍物坐标
global_path: (M, 2) 全局路径
返回:
[v, omega] 最优速度指令
"""
# 1. 生成动态窗口
v_min, v_max, w_min, w_max = self._calc_dynamic_window(state)
# 2. 速度空间采样
best_traj = None
best_score = -float('inf')
best_cmd = np.array([0.0, 0.0])
for v in np.arange(v_min, v_max, self.v_resolution):
for w in np.arange(w_min, w_max, self.w_resolution):
# 3. 轨迹预测
traj = self._predict_trajectory(state, v, w)
# 4. 评分
score = self._evaluate_trajectory(
traj, goal, obstacles, global_path, v, w
)
if score > best_score:
best_score = score
best_cmd = np.array([v, w])
best_traj = traj
return best_cmd, best_traj
def _calc_dynamic_window(self, state: np.ndarray) -> tuple:
"""
计算动态窗口
考虑:
1. 速度极限: [v_min, v_max], [w_min, w_max]
2. 加速度极限: 在dt内可达到的速度范围
3. 安全停止: 能及时停下的速度
"""
v = state[3]
w = state[4]
# 运动学极限
vs = [self.min_speed, self.max_speed,
-self.max_yaw_rate, self.max_yaw_rate]
# 加速度极限
vd = [v - self.max_accel * self.dt,
v + self.max_accel * self.dt,
w - self.max_delta_yaw_rate * self.dt,
w + self.max_delta_yaw_rate * self.dt]
# 取交集
v_min = max(vs[0], vd[0])
v_max = min(vs[1], vd[1])
w_min = max(vs[2], vd[2])
w_max = min(vs[3], vd[3])
return v_min, v_max, w_min, w_max
def _predict_trajectory(self, state: np.ndarray, v: float,
w: float) -> np.ndarray:
"""
前向模拟轨迹
参数:
state: [x, y, theta, v, omega]
v: 线速度
w: 角速度
返回:
traj: (N, 3) 轨迹点 [x, y, theta]
"""
x, y, theta = state[0], state[1], state[2]
traj = [(x, y, theta)]
time = 0
while time < self.predict_time:
x += v * np.cos(theta) * self.dt
y += v * np.sin(theta) * self.dt
theta += w * self.dt
time += self.dt
traj.append((x, y, theta))
return np.array(traj)
def _evaluate_trajectory(self, traj: np.ndarray, goal: np.ndarray,
obstacles: np.ndarray,
global_path: np.ndarray,
v: float, w: float) -> float:
"""
轨迹评分
评分项:
1. 目标方向评分: 轨迹终点与目标方向的对齐程度
2. 障碍物距离评分: 轨迹与最近障碍物的距离
3. 速度评分: 鼓励高速运动
4. 路径跟踪评分: 与全局路径的贴合度
"""
final_x, final_y, final_theta = traj[-1]
# 1. 目标方向评分
angle_to_goal = np.arctan2(
goal[1] - final_y, goal[0] - final_x
)
heading_diff = abs(final_theta - angle_to_goal)
heading_diff = min(heading_diff, 2 * np.pi - heading_diff)
heading_score = 1.0 - heading_diff / np.pi
# 2. 障碍物距离评分
min_obstacle_dist = float('inf')
for traj_point in traj:
for obs in obstacles:
dist = np.linalg.norm(traj_point[:2] - obs)
min_obstacle_dist = min(min_obstacle_dist, dist)
if min_obstacle_dist < 0.2: # 碰撞
return -float('inf')
obstacle_score = min(1.0, min_obstacle_dist / 0.5)
# 3. 速度评分
speed_score = v / self.max_speed
# 4. 路径跟踪评分
path_score = 0
if global_path is not None and len(global_path) > 0:
for traj_point in traj:
min_dist = min(
np.linalg.norm(traj_point[:2] - gp)
for gp in global_path
)
path_score += 1.0 / (1.0 + min_dist)
path_score /= len(traj)
# 加权综合评分
total_score = (
self.alpha_heading * heading_score +
self.alpha_dist * obstacle_score +
self.alpha_velocity * speed_score +
self.alpha_path * path_score
)
return total_score19.2.3 DWA评分机制详解
class DWAScorer:
"""DWA评分器 - 各评分项详解"""
def __init__(self):
pass
def heading_score(self, robot_pose, goal, trajectory):
"""
目标方向评分
评估轨迹终点方向与目标方向的偏差
"""
# 轨迹终点
end_pose = trajectory[-1]
theta = end_pose[2]
# 到目标的向量
dx = goal[0] - end_pose[0]
dy = goal[1] - end_pose[1]
target_angle = np.arctan2(dy, dx)
# 角度差
error = abs(theta - target_angle)
error = min(error, 2*np.pi - error)
# 归一化
return 1.0 - error / np.pi
def obstacle_score(self, trajectory, obstacles,
robot_radius=0.22):
"""
障碍物距离评分
评估轨迹中所有点到障碍物的最小距离
"""
min_dist = float('inf')
for point in trajectory:
px, py = point[0], point[1]
for ox, oy in obstacles:
dist = np.sqrt((px-ox)**2 + (py-oy)**2) - robot_radius
min_dist = min(min_dist, dist)
if min_dist < 0:
# 碰撞:给予极低分数
return -float('inf')
elif min_dist < 0.2:
# 危险区域:线性比例
return min_dist / 0.2
else:
# 安全区域
return 1.0
def velocity_score(self, v, omega, max_v, max_w):
"""
速度评分
鼓励机器人以较高速度行驶
"""
v_score = abs(v) / max_v
w_penalty = abs(omega) / max_w
# 鼓励直线高速,惩罚过度旋转
return v_score * (1.0 - 0.3 * w_penalty)
def path_alignment_score(self, trajectory, global_path):
"""
路径对齐评分
评估轨迹与全局路径的贴合程度
"""
if global_path is None or len(global_path) == 0:
return 0.5
total_dist = 0
for point in trajectory:
px, py = point[0], point[1]
dists = [np.sqrt((px-gx)**2 + (py-gy)**2)
for gx, gy, *_ in global_path]
total_dist += min(dists) if dists else float('inf')
avg_dist = total_dist / len(trajectory)
return 1.0 / (1.0 + avg_dist)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参数配置
# DWB控制器配置
controller_server:
ros__parameters:
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
# 速度限制
min_vel_x: 0.0
max_vel_x: 0.5
min_vel_y: 0.0
max_vel_y: 0.0 # 差速机器人,无侧向运动
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.5
min_speed_theta: 0.0
# 加速度限制
acc_lim_x: 2.5
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_theta: -3.2
# 仿真参数
vx_samples: 20 # 线速度采样数
vtheta_samples: 40 # 角速度采样数
sim_time: 1.5 # 仿真时长(s)
sim_granularity: 0.025 # 仿真步长(m)
# 评分器列表
critics: [
"RotateToGoal",
"Oscillation",
"BaseObstacle",
"GoalAlign",
"PathAlign",
"PathDist",
"GoalDist"
]
# 各评分器权重
RotateToGoal.scale: 32.0
Oscillation.scale: 10.0
BaseObstacle.scale: 32.0
GoalAlign.scale: 24.0
PathAlign.scale: 32.0
PathDist.scale: 32.0
GoalDist.scale: 24.0
# 目标检查器
goal_checker_plugin: "goal_checker"
goal_checker:
plugin: "dwb_core::SimpleGoalChecker"
xy_goal_tolerance: 0.1 # 位置容忍度(m)
yaw_goal_tolerance: 0.1 # 角度容忍度(rad)
stateful: True19.3.3 评分器详解
class DWBCritics:
"""DWB评分器集合"""
@staticmethod
def BaseObstacle(trajectory, costmap):
"""
基础障碍物评分
评估轨迹上各栅格的代价地图值
碰撞=最大惩罚
"""
total_cost = 0
for point in trajectory:
gx = int(point[1] / 0.05)
gy = int(point[0] / 0.05)
if 0 <= gx < costmap.shape[0] and 0 <= gy < costmap.shape[1]:
cost = costmap[gx, gy]
if cost >= 254: # 致命障碍
return float('inf')
total_cost += cost
return total_cost / len(trajectory)
@staticmethod
def PathAlign(trajectory, global_path):
"""
路径对齐评分
轨迹与全局路径的对齐程度
"""
if len(global_path) < 2:
return float('inf')
total_dist = 0
for point in trajectory:
px, py = point[0], point[1]
# 找到全局路径上最近的点
min_dist = min(np.sqrt((px-gx)**2 + (py-gy)**2)
for gx, gy in global_path)
total_dist += min_dist
return total_dist / len(trajectory)
@staticmethod
def GoalDist(trajectory, goal):
"""
目标距离评分
轨迹终点到目标点的距离
"""
end_pt = trajectory[-1]
dist = np.sqrt((end_pt[0]-goal[0])**2 + (end_pt[1]-goal[1])**2)
return dist
@staticmethod
def RotateToGoal(robot_pose, goal):
"""
转向目标评分
机器人当前朝向与目标方向的偏差
"""
dx = goal[0] - robot_pose[0]
dy = goal[1] - robot_pose[1]
target_angle = np.arctan2(dy, dx)
angle_diff = abs(robot_pose[2] - target_angle)
angle_diff = min(angle_diff, 2*np.pi - angle_diff)
return angle_diff
@staticmethod
def Oscillation(history):
"""
振荡检测评分
检测机器人是否在前进-后退间来回摆动
"""
if len(history) < 5:
return 0
# 检查最近的速度指令是否有方向交替
recent_v = [cmd[0] for cmd in history[-5:]]
sign_changes = sum(1 for i in range(len(recent_v)-1)
if recent_v[i]*recent_v[i+1] < 0)
# 方向变换过多则惩罚
return sign_changes * 100.019.3.4 官方要点——DWB 评分体系与 critics 插件的组合调优
DWB 官方页面说明它是动态窗口法的插件化实现:在每个周期对速度空间(由 min_vel_x/max_vel_x、vx_samples、sim_time、sim_granularity 等参数定义)采样,沿候选轨迹用一组 critics 评分。官方内置约 20 个 critic,常用组合包括 BaseObstacle(碰撞拒绝)、PathAlign/PathDist(贴路径)、GoalAlign/GoalDist(靠近目标)、RotateToGoal(终点对准)与 Oscillation(抑制抖动)。官方调优指南的顺序是:先只开启碰撞与目标类 critic 保证基本可达,再逐个加入精细 critic 并调整权重,避免多目标互相抵消——这正是练习第 4 题走廊参数配置的方法论。
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实现
import numpy as np
class RegulatedPurePursuit:
"""调节纯追踪控制器"""
def __init__(self, config: dict = None):
# 基本参数
self.desired_linear_vel = config.get('desired_linear_vel', 0.3)
self.max_linear_vel = config.get('max_linear_vel', 0.5)
self.min_linear_vel = config.get('min_linear_vel', 0.05)
self.max_angular_vel = config.get('max_angular_vel', 1.0)
# 前瞻参数
self.lookahead_dist = config.get('lookahead_dist', 1.0)
self.min_lookahead_dist = config.get('min_lookahead_dist', 0.3)
self.max_lookahead_dist = config.get('max_lookahead_dist', 2.0)
self.use_velocity_scaled_lookahead = config.get(
'use_velocity_scaled_lookahead', True)
# 调节参数
self.use_regulated_linear_velocity = config.get(
'use_regulated_linear_velocity', True)
self.use_cost_regulated_linear_velocity = config.get(
'use_cost_regulated_linear_velocity', True)
self.cost_scaling_dist = config.get('cost_scaling_dist', 0.3)
self.cost_scaling_gain = config.get('cost_scaling_gain', 1.0)
# 曲率调节
self.max_allowed_curvature = config.get('max_allowed_curvature', 0.5)
self.curvature_weight = config.get('curvature_weight', 0.3)
def compute_velocity_commands(self, robot_pose: np.ndarray,
global_path: np.ndarray,
costmap: np.ndarray = None) -> np.ndarray:
"""
计算速度指令
参数:
robot_pose: [x, y, theta] 机器人当前位姿
global_path: (N, 2) 全局路径点
costmap: 代价地图(可选,用于速度调节)
返回:
[linear_vel, angular_vel]
"""
if len(global_path) < 2:
return np.array([0.0, 0.0])
# 1. 寻找前瞻点
lookahead_point, lookahead_idx = self._find_lookahead_point(
robot_pose, global_path
)
if lookahead_point is None:
return np.array([0.0, 0.0])
# 2. 计算曲率
curvature = self._compute_curvature(robot_pose, lookahead_point)
# 3. 计算线速度
linear_vel = self._compute_linear_velocity(
robot_pose, lookahead_point, curvature, costmap
)
# 4. 计算角速度
angular_vel = linear_vel * curvature
# 限幅
angular_vel = np.clip(angular_vel, -self.max_angular_vel,
self.max_angular_vel)
linear_vel = np.clip(linear_vel, self.min_linear_vel,
self.max_linear_vel)
return np.array([linear_vel, angular_vel])
def _find_lookahead_point(self, robot_pose: np.ndarray,
global_path: np.ndarray) -> tuple:
"""寻找前瞻点"""
# 计算机器人到路径上各点的距离
dx = global_path[:, 0] - robot_pose[0]
dy = global_path[:, 1] - robot_pose[1]
distances = np.sqrt(dx**2 + dy**2)
# 计算前瞻距离
if self.use_velocity_scaled_lookahead:
# 速度自适应前瞻:速度越快,前瞻越远
lookahead = (self.lookahead_dist +
self.desired_linear_vel * 0.5)
lookahead = np.clip(lookahead, self.min_lookahead_dist,
self.max_lookahead_dist)
else:
lookahead = self.lookahead_dist
# 找第一个超过前瞻距离的点
for i, dist in enumerate(distances):
if dist >= lookahead:
return global_path[i], i
# 如果都小于前瞻距离,返回最后一个点
return global_path[-1], len(global_path) - 1
def _compute_curvature(self, robot_pose: np.ndarray,
lookahead_point: np.ndarray) -> float:
"""计算曲率"""
dx = lookahead_point[0] - robot_pose[0]
dy = lookahead_point[1] - robot_pose[1]
# 机器人到前瞻点的距离
L = np.sqrt(dx**2 + dy**2)
if L < 0.001:
return 0.0
# 机器人朝向与目标方向的夹角
alpha = np.arctan2(dy, dx) - robot_pose[2]
alpha = np.arctan2(np.sin(alpha), np.cos(alpha))
# 曲率 = 2 * sin(alpha) / L
curvature = 2.0 * np.sin(alpha) / L
# 限幅
return np.clip(curvature, -self.max_allowed_curvature,
self.max_allowed_curvature)
def _compute_linear_velocity(self, robot_pose: np.ndarray,
lookahead_point: np.ndarray,
curvature: float,
costmap: np.ndarray = None) -> float:
"""计算线速度(带调节机制)"""
velocity = self.desired_linear_vel
if self.use_regulated_linear_velocity:
# 曲率调节:曲率大则减速
curvature_factor = 1.0 - abs(curvature) * self.curvature_weight
velocity *= max(curvature_factor, 0.3)
if self.use_cost_regulated_linear_velocity and costmap is not None:
# 代价调节:靠近障碍物则减速
gx = int(robot_pose[1] / 0.05)
gy = int(robot_pose[0] / 0.05)
if 0 <= gx < costmap.shape[0] and 0 <= gy < costmap.shape[1]:
cost = costmap[gx, gy]
if cost > 0:
cost_factor = 1.0 - (cost / 254.0) * self.cost_scaling_gain
velocity *= max(cost_factor, 0.1)
return max(velocity, self.min_linear_vel)19.4.3 RPP配置
# Regulated Pure Pursuit 参数配置
controller_server:
ros__parameters:
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::\
RegulatedPurePursuitController"
# 速度参数
desired_linear_vel: 0.3
max_linear_vel: 0.5
min_linear_vel: 0.05
max_angular_vel: 1.0
# 前瞻参数
lookahead_dist: 1.0
min_lookahead_dist: 0.3
max_lookahead_dist: 2.0
use_velocity_scaled_lookahead: true
# 调节参数
use_regulated_linear_velocity: true
use_cost_regulated_linear_velocity: true
cost_scaling_dist: 0.3
cost_scaling_gain: 1.0
# 曲率限制
max_allowed_curvature: 0.5
curvature_weight: 0.3
# 碰撞防护
use_collision_detection: true
collision_check_distance: 0.5
max_linear_deceleration: -0.519.4.4 DWB vs RPP对比
def compare_controllers():
"""对比DWB和RPP控制器的特点"""
comparison = """
┌───────────────┬───────────────────────┬───────────────────────┐
│ 特性 │ DWB │ RPP │
├───────────────┼───────────────────────┼───────────────────────┤
│ 算法基础 │ 动态窗口法+多评分器 │ 纯追踪+速度调节 │
│ 计算复杂度 │ 高(评分器多) │ 低(几何计算) │
│ 参数数量 │ 多(大量评分器参数) │ 少(简单直观) │
│ 平滑性 │ 好 │ 好 │
│ 狭窄通道 │ 好 │ 中(需要调前瞻参数) │
│ 动态环境 │ 好(实时避障) │ 中(被动反应) │
│ 调试难度 │ 难(参数多) │ 易(参数少) │
│ 适用场景 │ 通用 │ 简单可靠场景 │
└───────────────┴───────────────────────┴───────────────────────┘
"""
print(comparison)19.4.5 官方要点——Regulated Pure Pursuit 的曲率与障碍双调节
RPP 官方文档指出,它在经典 Pure Pursuit 的"预瞄点跟踪"之上增加了两种 regulation:按路径曲率限制线速度(曲率越大速度越低),以及按障碍物接近程度减速(inflation_dist/curvature_dist/regulation_scaling_distance)。预瞄距离 lookahead_distance 可设为按速度缩放的区间值,使机器人高速时看得更远、低速时转弯更紧。文档特别提醒:RPP 不做速度采样,本身不具备绕障决策,依赖规划器提供避障路径;它支持差速与阿克曼底盘,并可通过 collision_detection 参数开启前向碰撞检查。这些细节与练习第 5 题的对比实验高度对应。
19.5 MPPI控制器
19.5.1 MPPI原理
MPPI (Model Predictive Path Integral) 是一种基于采样的模型预测控制方法:
**核心思想:**对控制序列进行多次采样(加入噪声),前向模拟每条采样序列的未来状态,根据代价函数计算每条序列的权重,加权平均生成最优控制序列,然后执行第一个控制指令并重复。
19.5.2 MPPI优势
MPPI可处理非线性动力学,自然支持并行计算(GPU加速),对模型误差鲁棒,适合复杂动态环境。
controller_server:
ros__parameters:
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
# 采样参数
samples: 1000 # 采样数
time_steps: 56 # 预测步数
dt: 0.1 # 时间步长
# 噪声参数
vx_std: 0.2 # 线速度噪声标准差
vy_std: 0.0 # 侧向速度噪声
wz_std: 0.4 # 角速度噪声标准差
# 代价权重
path_cost_weight: 5.0 # 路径跟踪代价
goal_cost_weight: 5.0 # 目标到达代价
obstacle_cost_weight: 10.0 # 避障代价
smoothness_cost_weight: 1.0 # 平滑性代价19.5.3 官方要点——控制器选型与插件生态
Nav2 官方与 navigation2 仓库的插件列表显示,除 DWB 和 RPP 外还有:MPPI(基于序列模型预测采样的最优控制,Jazzy 版本起成为默认控制器)、TEB(时间弹性带,支持倒车与阿克曼)、Rotation Shim(先原地转向再交给下层控制器的组合插件)与 Graceful Motion 等。官方选型建议可归纳为:狭窄密集环境优先采样类控制器(DWB/MPPI),强调轨迹平滑与跟踪精度时选 RPP,非完整约束底盘考虑 TEB/MPPI。Robotics Back-End 的教程用同一套地图对比了 DWB 与 RPP 的穿越成功率与跟踪误差,可直接作为本章练习的对照实验设计模板。
19.6 局部规划参数调优
19.6.1 速度参数调优
def recommend_speed_params(robot_type: str,
environment: str) -> dict:
"""根据场景推荐速度参数"""
params = {}
if robot_type == 'differential':
# 差速机器人
params['max_linear_vel'] = 0.5
params['max_angular_vel'] = 1.0
params['acc_lim_x'] = 2.5
elif robot_type == 'ackermann':
# 阿克曼机器人
params['max_linear_vel'] = 0.8
params['max_angular_vel'] = 0.5
params['acc_lim_x'] = 1.5
if environment == 'narrow':
# 狭窄环境:低速高转向
params['max_linear_vel'] *= 0.6
params['acc_lim_x'] *= 0.7
elif environment == 'open':
# 开阔环境:高速
params['max_linear_vel'] *= 1.2
return params19.6.2 前瞻距离调优
前瞻距离是RPP最关键参数:
def compute_optimal_lookahead(linear_vel: float,
control_frequency: float,
robot_length: float) -> float:
"""
计算最优前瞻距离
原则:
- 速度越快,前瞻越远
- 控制频率越高,前瞻可适当缩短
- 机器人越长,前瞻越远
"""
base_lookahead = linear_vel * 3.0 # 3倍速度
frequency_factor = 10.0 / control_frequency
length_factor = robot_length * 2.0
lookahead = (base_lookahead * frequency_factor + length_factor) / 2.0
return np.clip(lookahead, 0.3, 3.0)19.6.3 评分权重调优指南
# 不同场景的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 调试命令
# 查看当前控制器参数
ros2 param describe /controller_server FollowPath
# 动态调整速度
ros2 param set /controller_server FollowPath.max_vel_x 0.3
# 查看轨迹细节
ros2 topic echo /controller_server/trajectory_details
# 查看速度指令
ros2 topic echo /cmd_vel
# RViz2中可视化
rviz2
# 添加: Local Plan, Trajectories课后练习
-
原理题: 说明DWA算法中动态窗口的定义,阐述为什么需要结合速度极限、加速度极限和安全停止距离来生成动态窗口。
-
编程题: 实现DWA局部路径规划器,包含速度空间采样、轨迹预测和评分函数,并在仿真环境中测试避障效果。
-
分析题: 比较DWB和Regulated Pure Pursuit的异同,分析两种控制器在不同场景下的适用性。
-
配置题: 为差速机器人在走廊环境配置DWB控制器参数,要求能够顺利通过0.8m宽的走廊并保持稳定的导航行为。
-
操作题: 在仿真环境中分别使用DWB和RPP控制器进行导航,对比两种控制器在路径跟踪精度和安全避障方面的表现。
-
设计题: 设计一个自适应控制器切换策略:在宽阔区域使用RPP提高效率,在狭窄通道切换到DWB提高通过能力,并给出切换条件和参数过渡方案。
仿真结合实例(当前仓库):控制器跟踪路径并输出速度
目标与知识点对应
在 Nav2 仿真中观察 DWB 控制器、局部代价地图和 /cmd_vel 的关系,理解局部规划器如何将全局路径和实时激光障碍转换为速度指令。
运行步骤
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch navigation_sim_demo_ros2 nav2_demo.launch.py \
use_gazebo:=true use_rviz:=true gz_headless:=falseros2 topic echo /cmd_vel --once
ros2 topic echo /odom --once
ros2 topic info /scan观察结果
发送导航目标后,控制器应根据路径输出速度;RViz 可同时显示 Local Costmap 和路径。若没有 odom → base_link TF,控制器会等待变换,需先检查 Gazebo 桥和 TF。
源码与边界
控制器参数位于 src/navigation_sim_demo_ros2/params/nav2_params.yaml,仿真桥配置位于 src/robot_sim_demo/config/gazebo2_bridge.yaml,目标节点脚本位于 src/navigation_sim_demo_ros2/navigation_sim_demo_ros2/nav_goal_runner.py。
本实例验证 Nav2 控制链路,不比较不同局部规划器的性能指标。
学习材料:
- 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