一、Planner Server 规划服务器

1.1 数据流

输入:

  • 当前位置（来自定位系统）
  • 目标位置（来自 BT Navigator）
  • 全局代价地图（来自 Global Costmap）

处理:

  • 基于 Dijkstra/A* 搜索，应用 tolerance 和 allow_unknown 规则
    输出:

全局路径（点序列）

传递给 Controller Server

工作流程

二、Controller Server 控制服务器

我将详细分析 qmini_nav2 包中 controller_server 的各项参数及其功能。让我基于配置文件为您详细解读。

Controller Server 参数详细分析

🎛️ 一、核心服务器参数

1. 基础配置

1
2
3
4
controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 20.0

  • use_sim_time: False

    • 功能: 使用真实世界时间而非仿真时间
    • 用途: 实际机器人部署时设为 False，Gazebo 仿真时设为 True

  • controller_frequency: 20.0

    • 功能: 控制器运行频率（Hz）
    • 含义: 每秒计算 20 次速度命令
    • 影响: 频率越高，响应越快，但计算负载越大

2. 速度阈值参数

1
2
3
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001

  • min_x_velocity_threshold: 线速度最小阈值（m/s）

    • 低于此值认为机器人在 X 方向静止

  • min_y_velocity_threshold: 侧向速度阈值

    • 对差分驱动机器人通常设为较大值（不支持侧向移动）

  • min_theta_velocity_threshold: 角速度最小阈值（rad/s）

    • 低于此值认为机器人旋转停止

3. 容错参数

1
failure_tolerance: 0.3
  • failure_tolerance: 失败容忍度
    • 功能: 控制器可以连续失败的时间（秒）
    • 含义: 0.3秒内控制失败超过此时间则触发恢复行为

🔌 二、插件系统配置

1
2
3
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"] 
controller_plugins: ["FollowPath"]

这三个参数定义了控制器使用的插件架构：

Progress Checker（进度检查器）

1
2
3
4
progress_checker:
  plugin: "nav2_controller::SimpleProgressChecker"
  required_movement_radius: 0.5
  movement_time_allowance: 10.0

  • plugin: 使用的插件类型

  • required_movement_radius: 0.5

    • 功能: 要求的最小移动半径（米）
    • 作用: 机器人在 10 秒内必须移动至少 0.5 米，否则认为卡住

  • movement_time_allowance: 10.0

    • 功能: 允许的最大静止时间（秒）
    • 作用: 防止机器人长时间原地不动

应用场景: 当机器人被困或陷入局部最小值时，触发恢复行为

Goal Checker（目标检查器）

1
2
3
4
5
general_goal_checker:
  stateful: True
  plugin: "nav2_controller::SimpleGoalChecker"
  xy_goal_tolerance: 0.25
  yaw_goal_tolerance: 0.25

  • stateful: True: 保持状态记忆

  • xy_goal_tolerance: 0.25

    • 功能: 位置容差（米）
    • 含义: 机器人中心距目标点 0.25 米内即认为到达

  • yaw_goal_tolerance: 0.25

    • 功能: 朝向容差（弧度，约 14.3°）
    • 含义: 机器人朝向与目标朝向差值小于 0.25 弧度即认为达标

调优建议:

  • 精确导航任务：降低容差至 0.1-0.15
  • 粗略定位任务：提高容差至 0.3-0.5

🚗 三、DWB 局部路径规划器参数

1. 速度限制参数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
FollowPath:
  plugin: "dwb_core::DWBLocalPlanner"
  
  # 线速度限制
  min_vel_x: 0.0
  max_vel_x: 0.2          # 最大前进速度 0.2 m/s
  min_vel_y: 0.0
  max_vel_y: 0.0          # 差分驱动不支持侧向
  
  # 综合速度限制
  min_speed_xy: 0.0
  max_speed_xy: 0.2       # 平面最大速度
  min_speed_theta: 0.0
  
  # 角速度限制
  max_vel_theta: 1.0      # 最大旋转速度 1.0 rad/s (约 57°/s)

功能解析:

  • max_vel_x: 0.2 → 机器人最快以 20 cm/s 前进（较保守，适合室内）
  • max_vel_theta: 1.0 → 最快旋转约 57°/秒

调优方向:

  • 提高速度：适用于空旷环境，增加效率
  • 降低速度：适用于狭窄/拥挤环境，提高安全性

2. 加速度限制参数

1
2
3
4
5
6
7
acc_lim_x: 1.0          # 线加速度 1.0 m/s²
acc_lim_y: 0.0
acc_lim_theta: 3.2      # 角加速度 3.2 rad/s²

decel_lim_x: -1.0       # 线减速度 -1.0 m/s²
decel_lim_y: 0.0
decel_lim_theta: -3.2   # 角减速度 -3.2 rad/s²

功能解析:

  • 加速度限制: 防止机器人急加速导致打滑或不稳定
  • 减速度限制: 确保平滑减速，避免急刹车

物理意义:

  • acc_lim_x: 1.0 → 从静止加速到 0.2 m/s 需要 0.2 秒
  • acc_lim_theta: 3.2 → 从静止加速到 1.0 rad/s 需要 0.31 秒

3. 轨迹采样参数

1
2
3
4
vx_samples: 20          # 线速度采样点数
vy_samples: 5           # 侧向速度采样（差分驱动通常为5）
vtheta_samples: 20      # 角速度采样点数
sim_time: 1.7           # 轨迹仿真时长（秒）

DWB 工作原理:

  1. 在速度空间采样 20×20 = 400 个可能的速度组合
  2. 对每个速度组合模拟 1.7 秒的轨迹
  3. 根据评分标准选择最优轨迹

性能权衡:

  • 更多采样点 → 更精确但计算量大
  • 更长仿真时间 → 更远视距但实时性下降

4. 轨迹精度参数

1
2
3
linear_granularity: 0.05      # 线性粒度 5cm
angular_granularity: 0.025    # 角度粒度 0.025 rad (约 1.4°)
transform_tolerance: 0.2       # TF 变换容忍度

  • linear_granularity: 轨迹点间距

    • 5cm 间距生成轨迹点

  • angular_granularity: 旋转插值精度

    • 每 1.4° 生成一个采样点

  • transform_tolerance: TF 变换延迟容忍

    • 允许坐标变换有 0.2 秒延迟

5. 目标判定参数

1
2
xy_goal_tolerance: 0.25
trans_stopped_velocity: 0.25
  • xy_goal_tolerance: 与 Goal Checker 配合使用
  • trans_stopped_velocity: 判定停止的速度阈值
    • 速度低于 0.25 m/s 认为接近停止状态

6. 优化参数

1
2
3
short_circuit_trajectory_evaluation: True
stateful: True
debug_trajectory_details: True

  • short_circuit_trajectory_evaluation

    • 功能: 提前终止不合格轨迹评估
    • 优势: 显著提升计算效率

  • stateful: True

    • 功能: 保持状态记忆
    • 作用: 连续规划时保持一致性

  • debug_trajectory_details

    • 功能: 输出详细调试信息
    • 用途: 开发调试阶段使用

🎯 四、代价函数评分系统（Critics）

1
2
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", 
          "GoalAlign", "PathAlign", "PathDist", "GoalDist"]

各评分器详解

评分器 权重参数 功能 优先级
BaseObstacle scale: 0.02 障碍物避让 🔴 最高
RotateToGoal scale: 32.0 到达目标前旋转对准 🟡 高
PathAlign scale: 32.0 跟随全局路径 🟡 高
GoalAlign scale: 24.0 朝向目标对齐 🟢 中
PathDist scale: 32.0 靠近路径 🟢 中
GoalDist scale: 24.0 接近目标 🟢 中
Oscillation - 防止震荡 🔵 辅助

详细参数分析

1. BaseObstacle（障碍物评分器）

1
BaseObstacle.scale: 0.02
  • 功能: 评估轨迹与障碍物的距离
  • 权重低: 但影响最大（会直接拒绝碰撞轨迹）
  • 作用: 安全第一，碰撞轨迹得分为 -∞

2. PathAlign（路径对齐评分器）

1
2
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
  • 功能: 评估机器人朝向与路径方向的一致性
  • forward_point_distance: 前瞻点距离
    • 在机器人前方 0.1 米处取路径点作为对齐目标
  • 高权重: 确保机器人紧密跟随全局路径

3. GoalAlign（目标对齐评分器）

1
2
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
  • 功能: 评估机器人朝向与目标方向的一致性
  • 前瞻距离: 0.1 米
  • 作用: 接近目标时开始调整朝向

4. PathDist（路径距离评分器）

1
PathDist.scale: 32.0
  • 功能: 奖励沿路径前进的轨迹
  • 作用: 鼓励机器人快速沿路径移动
  • 高权重: 保证路径跟随性能

5. GoalDist（目标距离评分器）

1
GoalDist.scale: 24.0
  • 功能: 奖励接近目标的轨迹
  • 作用: 在路径末端引导机器人向目标移动

6. RotateToGoal（旋转到目标）

1
2
3
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
  • 功能: 接近目标时优先旋转对准
  • slowing_factor: 减速因子
    • 接近目标时降低速度，系数越大减速越明显
  • lookahead_time: -1.0: 禁用前瞻
    • 立即响应目标朝向

📊 五、参数协同工作流程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
graph TB
    A[Controller Server 启动] --> B[20Hz 控制循环]
    B --> C[获取当前位置和速度]
    C --> D[DWB 速度空间采样<br/>20×20=400个速度组合]
    D --> E[每个组合模拟1.7秒轨迹]
    E --> F[应用7个评分器]
    
    F --> G{轨迹评分}
    G -->|碰撞| H[得分 = -∞<br/>BaseObstacle拒绝]
    G -->|安全| I[综合评分计算]
    
    I --> J[PathAlign×32 + GoalAlign×24<br/>+ PathDist×32 + GoalDist×24<br/>+ RotateToGoal×32]
    
    J --> K[选择最高分轨迹]
    K --> L[提取速度命令]
    
    L --> M{Progress Check}
    M -->|10秒内移动<0.5m| N[触发恢复行为]
    M -->|正常| O{Goal Check}
    
    O -->|距离<0.25m<br/>角度<0.25rad| P[导航成功]
    O -->|未到达| B
    
    style H fill:#f99
    style P fill:#9f9
    style N fill:#ff9

⚙️ 六、实际应用调优建议

场景 1: 狭窄走廊导航

1
2
3
4
5
max_vel_x: 0.15              # 降低速度
acc_lim_x: 0.8               # 降低加速度
xy_goal_tolerance: 0.15      # 提高精度
BaseObstacle.scale: 0.01     # 增强避障权重
PathAlign.scale: 40.0        # 增强路径跟随

场景 2: 开阔空间快速导航

1
2
3
4
max_vel_x: 0.35              # 提高速度
acc_lim_x: 1.5               # 提高加速度
xy_goal_tolerance: 0.3       # 降低精度要求
vx_samples: 15               # 减少采样降低计算量

场景 3: 精确停靠（充电桩）

1
2
3
4
xy_goal_tolerance: 0.05      # 高精度 5cm
yaw_goal_tolerance: 0.1      # 高精度 5.7°
max_vel_x: 0.1               # 接近时降速
RotateToGoal.scale: 50.0     # 强化朝向对准

🔍 七、常见问题诊断

问题现象 可能原因 调整参数
机器人震荡 评分器权重不平衡 降低 PathAlign 权重
不跟随路径 PathAlign 权重太低 提高至 40-50
撞障碍物 速度过快或采样不足 降低 max_vel_x 或增加 vx_samples
到达不了目标 容差太小 提高 xy_goal_tolerance
卡住不动 Progress Checker 太严格 增加 movement_time_allowance
转弯太急 角速度或角加速度过大 降低 max_vel_theta

📝 总结

controller_server 的参数配置体现了安全性、精确性、效率性的三角平衡：

  1. 安全性: 通过速度/加速度限制和 BaseObstacle 评分器保证
  2. 精确性: 通过 Goal Checker 和 Progress Checker 保证
  3. 效率性: 通过 DWB 采样优化和评分器权重调节实现

当前配置适合室内低速精确导航场景，建议根据实际机器人性能和应用场景进行针对性调优。