1 全局规划器
全局规划器（planner_server）配置，全局规划负责从起点到目标点生成一条全局参考路径。
1.1 NavfnPlanner
Navfn 老规划器，现在新项目很多换成 SmacPlanner（Smac Hybrid A*），支持非完整约束，对差速轮、阿克曼底盘效果更好，Navfn 默认是按全向机器人模型算路径。
#### yaml
planner_server:
ros__parameters:
# 预期全局规划运行频率，单位 Hz。
# - 含义：期望每秒尝试重新规划全局路径 20 次；
# - 注意：不是一定 20 次，只是期望值。当环境变化、障碍物挡住原路径时，
# 规划器会重新计算全局路径；如果路径通畅，很多时候不会反复重算；
# - 调参：移动机器人常用 1~10Hz 居多，20Hz 算比较高，CPU 负载会更高；
# 低速底盘可以保留，嵌入式板（树莓派等）建议降到 5~10。
expected_planner_frequency: 20.0
use_sim_time: False
# 加载的全局规划插件列表，这里只启用名字叫 `GridBased` 的规划器，
# 可以同时加载多个规划器，导航时切换
planner_plugins: ["GridBased"]
# Navfn 是 Nav2 默认的**势场法全局规划器**，不是 A*；
# 是 Dijkstra 变种，计算代价场，然后沿着梯度生成路径。
GridBased:
plugin: "nav2_navfn_planner::NavfnPlanner"
# 目标点容差，单位m，全局规划时，如果目标点本身落在障碍物里，
# 会在目标周边半径 0.5m 范围内找一个可行的替代终点**。
# - 不是机器人到达判断！到达判断是 `goal_checker`
#（general_goal_checker）负责，不要搞混；
# - 调参：狭窄环境缩小（0.1~0.2），宽松环境 0.5~1.0。
tolerance: 0.5
# 是否切换成 A * 算法，默认 false。
# - `false`：Navfn 原生 Dijkstra 势场规划，路径平滑，适合大多数场景；
# 缺点：整张地图计算，大地图开销大；
# - `true`：底层换成 A*，搜索更快，大地图友好，但路径更容易出现折线、尖角。
use_astar: false
# 是否允许路径穿过地图上「未知区域（unknown，灰色区域）」
# - `true`：规划器认为未知区域可以通行，机器人会往未知区域走、去探索；
# 适合 SLAM 建图、自主探索；
# - `false`：未知区域直接当成障碍物，路径只在已经扫描到的已知空闲区域（白色）里规划；
# 适合已经建好、完全确定的静态地图，防止机器人冲进没扫描的地方。
allow_unknown: true1.2 SmacPlanner2D
SmacPlanner2D适合差速轮机器人。
### yaml
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: False
planner_plugins: ["GridBased"]
GridBased:
# 2D A*，适合**差速轮、全向轮圆形机器人，会考虑机器人姿态，输出路径更贴合机器人运动约束，
# 不像 Navfn 只算势场，经常出现机器人转不过来的尖角路径
plugin: "nav2_smac_planner::SmacPlanner2D"
# 和 Navfn 含义一致：目标点在障碍物内时，在 0.5m 半径内找可行替代终点（不是到达判定）
tolerance: 0.5
# true：允许穿过地图灰色未知区，适合 SLAM 建图导航；
# 静态建好的地图改成 `false`，防止冲进未知区域
allow_unknown: true
# 是否降采样代价图加速规划。算力弱（树莓派）可以开 `true` + factor=2，
# 地图分辨率减半，速度更快，但路径精度下降
downsample_costmap: false
downsampling_factor: 1
# A* 最大扩展节点数，防止死循环搜很久，-1 = 不限制
max_iterations: 1000000
# 已经进入目标容差范围内后，额外尝试搜索的次数
max_on_approach_iterations: 1000
# 单次规划最大耗时（单位 s），超时直接规划失败返回。
# 嵌入式板建议 1.0~2.0，PC 上可以放宽到 3.0
max_planning_time: 2.0
# 代价权重，越大路径越偏向走通道中间、远离障碍物，代价更高，搜索略慢
cost_travel_multiplier: 2.0
# false：严格使用目标点给的 theta 姿态；
# true：路径最后一段朝向作为终点姿态，适合只关心 (x,y)、不关心朝向的场景Nav2
use_final_approach_orientation: false
# Smac 自带平滑，把 A* 折线路径顺滑化
# `w_smooth`越大路径越平滑，
# `refinement_num`是平滑迭代次数，默认 2，想要更顺滑改成 4
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
do_refinement: true
refinement_num: 21.3 SmacPlannerHybrid
适合阿克曼车、有最小转弯半径的车，不能原地自转，比如遥控车、巡检小车。针对非完整约束车辆的全局规划器，底层 Hybrid A* + Reeds-Shepp/Dubins 曲线。
说明：
a.Hybrid A* 规划出来的路径自带航向约束，必须搭配适合阿克曼的局部控制器（比如 TEB、MPPI；DWB 也能用，但 DWB 原生对阿克曼支持一般）；
b.它的搜索是 (x,y,theta)，CPU 开销远大于 Navfn、Smac2D；
c.这个规划器不支持原地转向，本身就是为 "不能原地自转" 的车设计的；
d.没有内置 smoother 参数！Hybrid A* 的解析曲线本身就是光滑 Reeds-Shepp 曲线，不像 Smac2D 带单独平滑器。
planner_server:
ros__parameters:
# 单位 Hz，期望全局规划器重新规划路径的频率。
# - 阿克曼车规划计算量更大，一般不建议 20Hz，10Hz 比较稳妥；
# - 含义：导航框架尝试以 10Hz 触发全局重规划；不是强制每次都算出新路，
# 路径没有被挡住时会复用旧路径；
# - 算力弱的板子（树莓派）建议降到 2~5Hz。
expected_planner_frequency: 10.0
use_sim_time: False
planner_plugins: ["GridBased"]
GridBased:
# Hybrid A* 和普通 A * 区别：搜索节点是 (x,y,theta) 三维（位置 + 航向角），
# 考虑车辆转弯约束，规划出来的路径车辆可以直接走，不会出现原地掉头的非法路径
plugin: "nav2_smac_planner::SmacPlannerHybrid"
# 如果设置的目标坐标落在障碍物里面，规划器会在目标点半径 0.5m 范围内寻找一个可行、
# 无碰撞的替代终点。窄通道场景改成 0.2；开阔场地 0.5~1.0
tolerance: 0.5
allow_unknown: false # 与smacplanner2d相同
downsample_costmap: false # 与smacplanner2d相同
downsampling_factor: 1
max_iterations: 1000000 # 与smacplanner2d相同
max_planning_time: 3.0 # 与smacplanner2d相同
# 搜索阶段使用的运动曲线模型，二选一：
# -`REEDS_SHEPP`：支持**前进 + 后退**，可以倒车，适合室内、需要掉头的场景；
# - `DUBIN`：只支持单向前进，不能倒车，适合公路单向行驶的小车。
# 注意：这只是搜索的启发式模型，不是控制器的速度指令。
motion_model_for_search: "REEDS_SHEPP"
# 航向角的离散分箱数量
# 一圈 360°，72 份 → 每份 5°。
# - 数值越大：角度离散越精细，路径姿态过渡顺滑，但是节点数量暴涨、规划更慢；
# - 数值越小：搜索更快，但角度很粗糙，路径姿态跳跃大；
# 常用区间：36（10°）~72（5°），不建议超过 144。
angle_quantization_bins: 72
# 解析扩展（Analytic Expansion）触发阈值，Hybrid A*核心参数！
# Hybrid A* 分两部分：
# 1.栅格搜索扩展；2.每隔一段时间尝试直接用 Reeds-Shepp 曲线直接尝试一条直达
# 目标的光滑曲线(解析扩展，快速收敛）
# ratio 含义：当启发值 `h` 乘以这个系数 小于 当前代价 g，就触发解析直达尝试。
# - 数值大：更早、更频繁尝试直接曲线直达，规划更快；
# 但更容易碰到障碍物导致解析曲线碰撞失败；
# - 数值小：更少触发直达，更多依赖栅格搜索，路径更稳妥，速度慢。
# 推荐区间：2.0 ~ 4.0，3.5 是比较均衡的默认值。
analytic_expansion_ratio: 3.5
# 解析直达曲线的最大允许长度，单位 m。
# 当 Reeds-Shepp 直达曲线长度超过 3m 时，不再尝试解析直达，
# 避免长距离曲线穿过一堆障碍物。
# 开阔场地可以放大到 5.0；室内狭小场景缩小到 2.0~3.0
analytic_expansion_max_length: 3.0
# 阿克曼底盘**最重要参数**！单位 m
# 小车物理上最小转弯半径，必须和真实底盘保持一致。
# - 比如前轮转向小车，最小转弯半径实测 0.4m，这里就填 0.4；
# - 如果填的比真实值偏小：规划出转不过去的路径，DWB 跟踪失败、卡在弯道；
# - 如果填的比真实值偏大：路径保守，绕很远，空间利用率差
minimum_turning_radius: 0.4调参建议：
| 现象 | 推荐调整 |
|---|---|
| 规划很慢、超时 | 降低 angle_quantization_bins，开启 downsample_costmap |
| 规划出来的弯道车子转不过去 | 增大 minimum_turning_radius（填真实更大的值） |
| 明明有路，解析直达总撞障碍物 | 降低 analytic_expansion_ratio |
| 想让它更快找到终点 | 调高 analytic_expansion_ratio、放大 analytic_expansion_max_length |
1.4 其他全局规划器
| 插件类全名 | 算法 | 适用底盘 | 特点 |
|---|---|---|---|
nav2_navfn_planner::NavfnPlanner |
Navfn（势场 / Dijkstra，可选 A*） | 差速轮、全向轮（圆形机器人） | 默认老规划器，速度快，不考虑姿态约束，容易出尖角路径 |
nav2_smac_planner::SmacPlanner2D |
2D A* | 差速轮、全向轮（圆形） | 带自带平滑器，路径质量比 Navfn 好 |
nav2_smac_planner::SmacPlannerHybrid |
Hybrid A*（Reeds-Shepp/Dubins） | 阿克曼小车（有最小转弯半径，不能原地转） | SE2 三维搜索 (x,y,θ)，符合车辆运动学 |
nav2_smac_planner::SmacPlannerLattice |
状态格点规划 State Lattice | 任意底盘：非圆形差速、阿克曼、自定义构型机器人 | 预先生成控制原语，适合异形、高速机器人，配置复杂 |
nav2_theta_star_planner::ThetaStarPlanner |
Theta*（任意角度 A*） | 差速、全向轮 | 善于生成直线、斜向路径，减少锯齿折线 |
选型速记：
- 圆形差速轮 / 全向轮室内导航，新手、算力弱：Navfn
- 圆形差速轮，想要顺滑路径，推荐主流：SmacPlanner2D
- 圆形差速轮，想要少拐弯、尽量走直线：ThetaStar
- 阿克曼小车（车模、巡检车，有最小转弯半径）：SmacPlannerHybrid
- 长方形异形底盘、自定义运动学：SmacPlannerLattice
补充：第三方生态还有一些小众插件（RRT、RRT * 等），不属于官方内置包，需要单独编译安装，稳定性不如上面这几个
2 局部规划器
2.1 DWB(dynamic window approach)
Nav2 默认局部控制器（局部轨迹规划器） ，ROS1 时代 DWA 的插件化重构升级版，插件全名：dwb_core::DWBLocalPlanner，运行在 controller_server 里面，负责跟踪全局路径 + 实时动态避障。 适用底盘，优先：差速轮（差分驱动 differential）、全向轮（omni） 阿克曼小车不推荐原生 DWB，原生采样不自带最小转弯半径约束，容易输出走不过去的曲线；阿克曼优先 MPPI / TEB / RPP（Regulated Pure Pursuit）。
controller_server:
ros__parameters:
use_sim_time: False
# 局部规划控制频率
controller_frequency: 20.0
# **里程计速度滤波死区阈值（单位 m/s）**：
# controller_server 在把 odom 送给局部控制器插件之前，先对 odom 里的速度做过滤；
# **速度绝对值小于该阈值 → 强制置 0**，用来滤编码器 / 里程计微小噪声，避免误判机器人在移动
# 注：当前时差速轮，所以min_y_velocity_threshold很大，如果是麦轮全向车，
# 这个参数一般要改成很小（比如 0.001），不然侧向运动直接被吃掉，无法横移
# min_x_velocity_threshold过滤前进 / 后退方向微小抖动噪声；
# 差速轮、阿克曼机器人一般都很小（默认 0.0001）
min_x_velocity_threshold: 0.001
# 差速轮机器人本身理论上不能侧向移动，里程计出来的 vy 全部都是噪声；
# 设一个很大阈值，直接把所有正常情况下的侧向速度全部清零，防止导航误以为
# 机器人在横向漂移、导致进度检查 /goal checker 异常、误报卡住、失败
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
# ***制器插件（FollowPath 对应的控制器，如 RegulatedPurePursuit、MPPI 等）的
# `computeVelocityCommands()` 抛出异常、计算失败时，
# 允许连续失败的最长时间是 0.3s：
# 在 0.3s 以内：只是本次算不出速度指令，**不会直接让 FollowPath 动作失败**，继续重试；
# 连续失败超过 0.3s：判定 FollowPath 失败，向上通知 BT，
# 触发恢复行为（spin、backup 等）或者直接导航失败
# `0.0`：无容错，控制器只要一次异常立刻失败
# `-1.0`：无限容错，控制器就算一直报错也永不判定 FollowPath 失败
# - 狭小、障碍物密集场景：可以给到 0.5～1.0；
# - 高速、安全要求高的场景：缩小到 0.1～0.2，更快报故障；
failure_tolerance: 0.3
# 指定**进度检查器插件别名**，下面必须有一个同名 `progress_checker{ plugin: xxx }` 块
# 职责：判断**机器人有没有在往前走、有没有卡住**
# - 核心参数：`required_movement_radius`、`movement_time_allowance`
# - 逻辑：给你一段时间，如果机器人移动距离达不到半径 → 判定卡住，FollowPath 失败，
# BT 进入恢复动作（spin/backup）
# jazzy已经换成数组写法：["progress_checker"]，但是向下兼容
progress_checker_plugin: "progress_checker"
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0 #10s内必须走0.05m，否则判定为卡住
# 加载**终点到达判定插件列表**，别名 `general_goal_checker`，下面对应
# `general_goal_checker{ plugin: ... }`
# - 两个常用插件（注释里的 precise_goal_checker 是备选）
# - **general_goal_checker（就是 SimpleGoalChecker）**：只判断位姿是否在容差内
# `xy_goal_tolerance / yaw_goal_tolerance`，**不检查机器人是否静止**，
# 到位就算到达；差速车最常用
# - **precise_goal_checker（StoppedGoalChecker）**：到位 +
# **速度小于阈值（机器人停稳）** 两个条件同时满足才算到达；适合对接机械臂、取放料场景，
# 防止冲过目标
# - 可以同时加载多个，在 BT 的 FollowPath 节点里通过 `goal_checker_id` 切换使用哪一个
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
# ***************DWB parameters******************
# DWB 核心原理：在速度空间采样一堆 (vx,vy,w) 速度候选 →
# 前向仿真 sim_time 时间得到多条轨迹 → 各个 critic（代价评价器）打分 →
# 选总分最小的速度发给底盘**
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "dwb_core::DWBLocalPlanner" # 使用DWB局部规划器，还有RPP、MPPI，更改时必须更改参数段
debug_trajectory_details: True # 在 topic 输出采样轨迹、每条轨迹各个 critic 的分数，用于 rviz 看轨迹、定位哪个代价项主导
min_vel_x: -1.0 # x最小速度，后退最大速度 -1.0 m/s
min_vel_y: 0.0 # 侧向最小速度，差速车固定0
max_vel_x: 1.0 # 前进最大速度 1.0 m/s
max_vel_y: 0.0 # 侧向最大速度，差速车固定0
max_vel_theta: 0.5 # 最大角速度 0.5 rad/s
min_speed_xy: 0.0 # xy合速度下限，允许0（可以停）
max_speed_xy: 1.0 # xy合速度上限，和max_vel_x对齐
min_speed_theta: 0.0 # 角速度下限，可以0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 0.5 # x方向最大加速度 0.5 m/s²
acc_lim_y: 0.0 # y侧向加速度，差速车0
acc_lim_theta: 1.0 # 角加速度 1.0 rad/s²
decel_lim_x: -0.5 # x最大减速度 -0.5 m/s²（负代表减速，避障急停靠这个）
decel_lim_y: 0.0
decel_lim_theta: -1.0 # 角减速度
vx_samples: 20 # vx一共采样20个档位
vy_samples: 0 # 不采样vy，差速车固定填0
vtheta_samples: 20 # ω采样20个档位
sim_time: 1.7 # 每条轨迹向前仿真多久：1.7s
linear_granularity: 0.05 # 仿真轨迹上，相邻两个位姿点之间的步长(m)，越长，提前看更远，适合高速；太短容易临近障碍物才反应
angular_granularity: 0.025 # 仿真点角度步长(rad)
transform_tolerance: 0.2 # 等待tf变换的超时时间(s)，tf延迟超过0.2s就报错
xy_goal_tolerance: 0.25 # DWB内部终点xy容差（但是：最终是否到达，由上层goal_checker插件说了算！这个只是DWB自己参考）
trans_stopped_velocity: 0.25 # 判定机器人"已经停下"的平移速度阈值(m/s)
short_circuit_trajectory_evaluation: True #一旦某条轨迹被障碍物代价直接判成无穷大，直接跳过剩下 critic 打分，节省算力；推荐开；
stateful: True #有状态DWB，采样会以上一帧输出速度为中心采样（推荐）；False 是纯全局均匀采样，容易震荡
# 代价评价器列表 critics，DWB 会对每一条采样轨迹，依次跑下面这些评价项，加权求和，总分越小越好
# BaseObstacle 障碍物代价：轨迹离障碍物越近，分越高（惩罚碰撞）
# PathDist 离全局参考路径横向距离，偏离路径就罚分
# GoalDist 这条轨迹仿真终点离目标点的距离，越远越罚
# PathAlign 轨迹朝向和全局路径切线方向是否一致
# GoalAlign 轨迹朝向是否对准最终目标点
# RotateToGoal 靠近终点时，优先对齐目标航向 yaw
# Oscillation 震荡惩罚：来回加减速、左右反复转向，会被扣分，抑制抖动
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
# 评价权重：这套权重特点：
# `PathAlign、PathDist` 权重极高 → **严格贴全局路径走，不喜欢抄近路**；
# BaseObstacle 权重很低 → 遇到障碍物容易卡在路径上不动，适合环境开阔、障碍物少的场景；
# 如果场景障碍物多、容易卡死，需要提高 BaseObstacle.scale**（比如调到 0.4~2.0），降低 PathAlign/PathDist 权重。
BaseObstacle.scale: 0.04 # 障碍物权重，偏低，说明这套配置优先跟踪路径，不是激进避障
PathAlign.scale: 50.0 # 【很高】强约束：必须沿着全局路径的方向走
GoalAlign.scale: 10.0
PathAlign.forward_point_distance: 0.1
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 50.0 # 【很高】强烈惩罚偏离全局路径，贴线走
GoalDist.scale: 10.0
RotateToGoal.scale: 32.0 # 终点附近优先对准目标角度
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0 # -1=自动，不用手动设置前视时间常见评价权重调参方向参考（差速车）
-
跟踪路径歪、容易跑偏：加大 `PathDist / PathAlign.scale`
-
遇到障碍物不避让、直接贴上去卡死：提高 `BaseObstacle.scale`
-
到达终点前冲过头：调大 `RotateToGoal.slowing_factor`、降低 max_vel_x
-
导航震荡、来回拐：加大 Oscillation.scale，或者降低 vx_samples、减小 sim_time
-
算力不够、延迟高：减少 vx_samples/vtheta_samples，关掉 debug_trajectory_details
2.2 RPP(Regulated Pure Pursuit)
在原版 PP、自适应 PP（APP）之上增加了 3 大**速度调节（Regulated）**规则，这就是名字里 Regulated 的来源：
- 曲率调速：弯道越急、曲率越大 → 自动降速，防止过冲、甩尾
- 障碍物距离调速：离障碍物越近 → 自动减速，提升安全性
- 终点接近调速：靠近目标点时平滑减速，平稳停稳
- 还支持自适应前视距离 ：速度越快，前视距离
lookahead_dist拉得越长，高速更稳；低速前视变短，转弯更精准ROS。 另外可选开启 DWPP（Dynamic Window Pure Pursuit），加入加减速物理约束校验，输出速度不会超出底盘极限。
适合底盘：差速轮、阿克曼、足式机器人都支持；不适合全向轮 omni（没法利用横向移动能力） ROS，重点：RPP 非常适合搭配 SmacPlannerHybrid / SmacLattice （运动学可行的全局路径），因为 RPP 强在严格贴路径，本身避障能力弱 ，假设给它的全局路径就是可通行的。要精准沿路线跑、环境简单静态 选 RPP， 环境复杂、人来回走动，需要主动绕障碍物 选 DWB / MPPI。
controller_server:
ros__parameters:
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
# 期望最大线速度（m/s），机器人正常直行时的目标速度；弯道、靠近障碍物、靠近终点时会自动降速
desired_linear_vel: 0.5
# 最大线加速度（m/s²），从静止加速到目标速度的上限，防止打滑
max_linear_accel: 0.8
# 最大线减速度（m/s²），刹车上限；紧急减速、弯道减速都受这个限制
max_linear_decel: 0.8
# 最大角速度/加速度（rad/s ，rad/s2），转向角速度上限
max_angular_vel: 1.0
max_angular_accel: 1.0
# 是否开启**速度自适应前视距离**
# true：`lookahead_dist = v * lookahead_time`，越快看越远，高速走线顺滑
# false：使用固定 `lookahead_dist`，低速高精度场景用
use_velocity_scaled_lookahead_dist: true
# 前视时间（s），只有上面开关 true 生效
# 前视距离 = 当前速度 × lookahead_time
# 数值大：轨迹顺滑，跟踪误差变大；数值小：贴线很紧，容易震荡
lookahead_time: 0.6
# 自适应模式下，前视距离的**下限**。速度极低时，前视距离不会比这个更小，防止控制器抖动
# 如果关闭自适应，就需要写固定参数：`lookahead_dist: 0.6`
min_lookahead_dist: 0.3
# 自适应模式下，前视距离**上限**。速度再快，前视也不会超过这个值，防止离路径太远
max_lookahead_dist: 1.2
# 曲率调速（RPP核心 regulated 功能）
# 开启**曲率调速**，弯道曲率越大（转弯半径越小），自动降速。阿克曼小车强烈建议打开
use_regulated_linear_velocity_scaling: true
# 触发曲率降速的临界转弯半径 (m)。
# - 实际转弯半径 < 0.9m → 开始降速；
# - 转弯半径很大（接近直行）→ 不减速。
regulated_linear_scaling_min_radius: 0.9
# 曲率调速模式下，允许降到的最低速度，防止弯道直接停死
regulated_linear_scaling_min_speed: 0.25
# 代价地图障碍物减速
# 开启代价地图调速：离障碍物越近，代价越高，速度越低
# RPP**不会偏离路径绕开障碍物**，只会减速；障碍物挡死路径后直接堵停，交给行为树恢复动作
use_cost_regulated_linear_velocity_scaling: true
cost_scaling_dist: 0.6 # 距离障碍物多少米以内，开始减速。
cost_scaling_gain: 1.0 # 减速强度系数，越大，靠近障碍物时速度下降越猛
# 终点减速
approach_velocity_scaling_dist: 0.6 # 距离终点还有 0.6m 时，开始平滑减速
min_approach_linear_velocity: 0.1 # 靠近终点时最低行驶速度，保证机器人有前进能力，防止卡在终点前不动
# 对准朝向
use_rotate_to_heading: true # 到达终点附近后，单独原地旋转对准目标姿态（goal 的 theta）
rotate_to_heading_angular_vel: 0.4 # 对准姿态时原地旋转的角速度（rad/s）
# 角度误差阈值（rad，0.785=45°）：
# 当机器人朝向和目标角度差值大于这个值，就触发原地旋转对准；小于该值直接认为朝向合格
rotate_to_heading_min_angle: 0.785
# 底盘运动约束
# 机器人物理最小转弯半径 (m)。
# - 阿克曼：**必须填真实底盘最小转弯半径，和 SmacPlannerHybrid 保持一致**；
# - 差速轮原地可转，填很小值比如 0.01。
# 控制器会拒绝输出小于该半径的曲率，防止跟踪规划不出来的急转弯
min_turning_radius: 0.4
# 是否允许倒车
# - false：只允许前进；
# - true：允许向后跟踪路径（配合 Reeds-Shepp 倒车路径）
allow_reversing: false
# 碰撞检查
# 开启前向碰撞时间检测，预判多久会撞上障碍物
use_collision_detection: true
# 到前视胡萝卜点的预估碰撞时间，如果小于 1s，直接减速 / 停止
max_allowed_time_to_collision_up_to_carrot: 1.0
# TF与搜索
# TF 坐标变换容忍时间（s），TF 延迟超过这个值，控制器报错。实物一般 0.1~0.2
transform_tolerance: 0.2
# 沿着全局路径，从机器人位置向前最多搜索多远来找最近路径点，长路径给大一点
max_robot_pose_search_dist: 10.0调参口诀：
- 走线贴太紧、来回抖：加大 lookahead_time
- 过弯甩尾、冲出路径：减小 regulated_linear_scaling_min_radius、降低 desired_linear_vel
- 离障碍物很远就刹车：减小 cost_scaling_gain、cost_scaling_dist
- 到终点不转角度对准：确认
use_rotate_to_heading: true，调小 rotate_to_heading_min_angle - 阿克曼跟踪 Hybrid A * 路径：min_turning_radius 和规划器保持一模一样
搭配推荐：SmacPlannerHybrid + RPP，适合阿克曼巡检车、AGV 固定路线导航 ；不适合：人流多、动态障碍物需要主动绕开的场景（改用 DWB / MPPI）
2.3 MPPI（Model Predictive Path Integral）
MPPI：基于采样的模型预测路径积分控制器 ，是阿克曼小车主流选择，和 RPP 对比： MPPI：自带主动绕障、预测未来一段轨迹、优化控制序列 ；计算量大，动态环境强 RPP：几何纯跟踪，只贴线，仅障碍物减速，不会绕障；计算极轻，静态固定路线稳.
controller_server:
ros__parameters:
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
# ========= 预测窗口 & 采样核心参数 =========
time_steps: 40
model_dt: 0.05
batch_size: 1200
iteration_count: 1
temperature: 0.3
gamma: 0.015
prune_distance: 2.5
# ========= 速度边界（阿克曼 vy=0，不能横向平移） =========
vx_max: 0.8
vx_min: -0.4
vy_max: 0.0
wz_max: 1.2
# ========= 采样噪声标准差（探索范围） =========
vx_std: 0.25
vy_std: 0.0
wz_std: 0.35
# ========= 运动模型：阿克曼 =========
motion_model: "ackermann"
ackermann:
plugin: "mppi::AckermannMotionModel"
min_turning_r: 0.4
# ========= 评价器Critic列表（核心代价项） =========
critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic",
"PathFollowCritic", "PathAlignCritic", "PathAngleCritic", "PreferForwardCritic"]
# 约束代价：禁止超出运动学、速度极限
ConstraintCritic:
enabled: true
cost_weight: 4.0
cost_power: 1
# 障碍物代价（主动避障核心！MPPI最强项）
ObstaclesCritic:
enabled: true
cost_weight: 8.0
inflation_radius: 0.35
cost_power: 1
# 距离终点代价
GoalCritic:
enabled: true
cost_weight: 3.0
cost_power: 1
# 终点姿态对准代价
GoalAngleCritic:
enabled: true
cost_weight: 2.5
cost_power: 1
threshold_to_consider: 0.5
# 跟踪全局路径距离代价（贴近路径）
PathFollowCritic:
enabled: true
cost_weight: 5.0
cost_power: 1
# 轨迹朝向和路径走向对齐
PathAlignCritic:
enabled: true
cost_weight: 3.0
cost_power: 1
# 路径角度平滑代价，抑制急转弯抖动
PathAngleCritic:
enabled: true
cost_weight: 2.0
cost_power: 1
# 优先向前行驶，惩罚后退
PreferForwardCritic:
enabled: true
cost_weight: 6.0
cost_power: 1
# ========= 调试、TF、可视化 =========
transform_tolerance: 0.15
visualize: true
publish_optimal_trajectory: true
TrajectoryVisualizer:
trajectory_step: 4
time_step: 2顶层标识：
- plugin: "nav2_mppi_controller::MPPIController" 指定加载 MPPI 控制器插件。
预测窗口 & 采样核心（MPPI 灵魂）
- time_steps: 40 预测轨迹总步数；预测总时长 =
time_steps × model_dt40 × 0.05 = 2s 预测窗口，控制器向前看未来 2 秒的轨迹。
- 更大：预判更远，提前避障；但计算量暴涨
- 更小：反应灵敏，适合高速，容易短视
- model_dt: 0.05 每一步仿真时间间隔，单位 s。0.05=50Hz 仿真步长。
- batch_size: 1200 每一轮迭代随机采样多少条候选轨迹。
- 越多：搜索空间大，更容易找到优质避障轨迹，CPU 压力大；
- 越少：跑得更快，但容易陷入次优、绕障能力下降；
PC 工控机 1000~2000；树莓派等嵌入式建议 400~800。
- iteration_count: 1 MPPI 内循环优化迭代次数，官方推荐默认 1，优先加大 batch_size 而不是迭代次数。
- temperature: 0.3 温度系数，控制代价权重选择的锐利程度：
- 越小：只选代价最低的最优轨迹，选择很保守；
- 越大：很多轨迹都会被加权平均，平滑但是容易差；一般 0.2~0.5。
- gamma: 0.015 控制控制量更新平滑性，抑制控制指令跳变、抖动；一般不动。
- prune_distance: 2.5 沿着全局路径，控制器只关心机器人前方 2.5m 内的路径，远处路径截断，减少计算。
速度边界约束（阿克曼）
- vx_max: 0.8 最大前进线速度 m/s
- vx_min: -0.4 最大倒车速度 m/s；不想倒车改成 0
- vy_max: 0.0 阿克曼不能横向移动，固定 0
- wz_max: 1.2 最大角速度 rad/s
采样噪声（探索幅度）
- vx_std / vy_std / wz_std 采样时速度的高斯标准差，代表探索范围
- std 大：敢于尝试大速度、大转向，绕障更激进；
- std 小：轨迹比较保守平滑，变化小。
阿克曼运动模型：
motion_model: "ackermann"
ackermann:
plugin: "mppi::AckermannMotionModel"
min_turning_r: 0.4- min_turning_r: 0.4 阿克曼最小转弯半径！必须和 SmacPlannerHybrid 里面的 minimum_turning_radius 完全一致，否则规划出的路径控制器跟踪失败。
Critics 评价器组（打分插件，可按需开关、调权重）
cost_weight：权重，越大该项代价越优先；cost_power：代价幂次，一般保持 1
- ConstraintCritic 硬约束：惩罚超出速度、运动学极限的非法轨迹，权重建议高一点，保证输出指令底盘可执行。
- ObstaclesCritic【MPPI 核心】 障碍物代价，MPPI 和 RPP 最大区别就在这里 ： MPPI 会主动搜索一条绕开障碍物的轨迹；RPP 只会减速，不会偏离路径绕开。
inflation_radius：障碍物膨胀半径，比 costmap 的 inflation 略大一点，留安全距离。 - GoalCritic：到目标点距离代价，引导向终点前进。
- GoalAngleCritic ：靠近终点时对齐目标姿态；
threshold_to_consider：距离终点小于 0.5m 才开始约束角度。 - PathFollowCritic：轨迹到全局路径的横向距离代价，保证不离全局路径太远。
- PathAlignCritic：轨迹前进方向和路径走向对齐，防止横着切路径。
- PathAngleCritic：轨迹曲率平滑惩罚，抑制频繁急打方向、车身抖动。
- PreferForwardCritic：强烈惩罚后退轨迹，适合室内优先前进场景；需要频繁倒车掉头（Reeds-Shepp）可以降低权重。
调试 & TF
- transform_tolerance:0.15 TF 延迟容忍时间
- visualize: true Rviz 可视化所有采样轨迹（调试时打开，上线可关闭省性能）
- publish_optimal_trajectory: true 发布最优轨迹话题，可视化、记录调试
- TrajectoryVisualizer：可视化抽稀参数，不用每条采样点都画，防止 RViz 卡顿
MPPI VS RPP 阿克曼对比：
| 项目 | MPPI | RPP（Regulated Pure Pursuit） |
|---|---|---|
| 算法 | 采样型模型预测 MPC | 几何纯跟踪（胡萝卜点） |
| 避障能力 | ✅ 强，主动绕开障碍物 | ❌ 弱，仅靠近障碍物减速，不会绕障 |
| 跟踪特性 | 可以适度偏离全局路径找可行轨迹 | 严格贴着全局路径，偏离代价很高 |
| 算力开销 | 高（batch 越大越吃 CPU） | 极低，几乎无压力 |
| 动态环境（人、移动障碍） | 首选 | 不适合，容易卡死 |
| 静态固定巡检路线 | 可用，但是浪费算力 | 首选，走线丝滑稳定 |
| 对全局路径依赖 | 有容错，路径轻微不合理也能救 | 强依赖，全局路径必须完全可通行 |
| 调参工作量 | 大（一堆 critic 权重、采样参数） | 简单，前视、曲率调速几个关键参数 |
调参经验口诀：
- 绕障保守、不敢靠近障碍物：提高 ObstaclesCritic 权重，或者减小 inflation_radius
- 老是脱离全局路径乱跑：加大 PathFollowCritic 权重，降低温度 temperature、vx_std/wz_std
- 控制指令抖动、转向频繁跳变：提高 gamma，降低 wz_std、PathAngleCritic 权重
- CPU 负载太高、卡顿：降低 batch_size，减小 time_steps，关闭 visualize
- 弯道甩尾、跟踪不上 Hybrid A * 路径：核对两边 min_turning_r 保持一致，降低 vx_max
推荐组合：
- 室内有人走动、动态障碍物、需要灵活避障：SmacPlannerHybrid + MPPI
- 工厂固定巡检路线、静态环境、追求走线稳定、低算力：SmacPlannerHybrid + RPP