RuyiSDK Board Docs

RISC-V ROS2 机器人操作系统编程技术

源码仓库

ch19 · 局部路径规划

教材编程语言运行环境课程文档实验文档
RISC-VC++17SpacemiT K3 CoM260 Kit / Bianbu 4.0.6 / Humble,配合 x86 Ubuntu 22.04 / Humble / Harmonic 课程容器阅读课程开始实验
x86PythonUbuntu 22.04 / Humble 或 Ubuntu 24.04 / Jazzy阅读课程开始实验

第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_score

19.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: True

19.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.0

19.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.5

19.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 params

19.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.0

19.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

课后练习

  1. 原理题: 说明DWA算法中动态窗口的定义,阐述为什么需要结合速度极限、加速度极限和安全停止距离来生成动态窗口。

  2. 编程题: 实现DWA局部路径规划器,包含速度空间采样、轨迹预测和评分函数,并在仿真环境中测试避障效果。

  3. 分析题: 比较DWB和Regulated Pure Pursuit的异同,分析两种控制器在不同场景下的适用性。

  4. 配置题: 为差速机器人在走廊环境配置DWB控制器参数,要求能够顺利通过0.8m宽的走廊并保持稳定的导航行为。

  5. 操作题: 在仿真环境中分别使用DWB和RPP控制器进行导航,对比两种控制器在路径跟踪精度和安全避障方面的表现。

  6. 设计题: 设计一个自适应控制器切换策略:在宽阔区域使用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:=false
ros2 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 控制链路,不比较不同局部规划器的性能指标。

学习材料: