从一张异常轨迹图说开去：为什么我第一眼就盯上了TrajectoryPlanner

在调试某辆差速底盘的自主导航时，我遇到过一个经典症状：全局路径规划得笔直漂亮，可小车一上路就“画龙”——贴着障碍物反复试探、原地转圈、甚至有两次差点怼上墙。当时周围不少人第一反应是调PID、改costmap参数，但直觉告诉我，问题多半出在局部规划那一层。于是我把base_local_planner目录整个翻了一遍，对照源码逐行排查，最终锁定的元凶就是TrajectoryPlanner的轨迹采样与评分策略。

这个从现象到源码、再从源码回到参数调试的过程，让我意识到一件事：ROS Navigation里真正决定机器人“走成什么样”的，不是全局规划器画的那条线，而是局部规划器如何在每一帧控制周期内把这条线兑现成具体的速度指令。TrajectoryPlanner作为base_local_planner包中最经典、也最容易理解的实现，非常适合作为局部规划源码阅读的第一站。

这篇笔记我打算分几篇来写，跟着源码文件走一遍TrajectoryPlanner从数据输入、轨迹采样、代价评分到最终指令输出的完整链路。开篇先解决三个基础问题：TrajectoryPlanner在整个导航框架中处在什么位置、它的核心数据结构长什么样、以及当我们对它说“生成一个轨迹并打分”时，它内部到底执行了什么。这些搞清楚了，后续再看DWA、Teb、甚至自己写局部规划器，都会轻松不少。

1. TrajectoryPlanner在导航栈中的角色定位

1.1 导航框架里的三层分工

先把视角拉高一点。在ROS Navigation的体系里，move_base是神经中枢，它内部维护着两大costmap（全局与局部），并调度三个核心模块：全局规划器（Global Planner）、局部规划器（Local Planner）和恢复行为（Recovery）。

全局规划器负责用Dijkstra、A*这类图搜索算法，在全局代价地图上找一条从起点到目标点的最优路径。它不怎么关心机器人的运动学约束，画出来的多半是一条机器人没法直接跟踪的折线。真正负责“把这条折线变成底盘听得懂的速度指令”的，是局部规划器。

TrajectoryPlanner就属于局部规划器范畴，在代码层面对应base_local_planner包里的TrajectoryPlannerROS类（ROS封装层）与TrajectoryPlanner类（核心算法层）。它要做的事可以用一句话概括：在当前机器人位姿附近，根据局部代价地图和全局路径的一小段窗口，枚举出大量候选轨迹，按一组代价函数打分排序，挑出得分最低（或最高，取决于实现约定）的那条轨迹，将其起点处的速度指令作为输出下发到cmd_vel话题。

1.2 TrajectoryPlanner与DWA的血缘关系

很多教程把TrajectoryPlanner和DWA（Dynamic Window Approach）混为一谈，因为它们从算法层面确实同源。DWA本质上是TrajectoryPlanner的“降采样工程版”：TrajectoryPlanner在速度空间（前向速度vx、侧向速度vy、角速度w）三维采样，DWA则只关注当前动态窗口内的速度变化率可达范围。在base_local_planner的源码里，二者甚至共用同一个轨迹生成器（SimpleTrajectoryGenerator）。

新版本的ROS Navigation中，TrajectoryPlanner被标记为deprecated，推荐使用DWA，但它的评分函数体系、状态机逻辑被完整保留了下来。我坚持读它的原因很简单：TrajectoryPlanner的代码结构更规整，每个函数各司其职，注释质量也高，作为学习材料比一堆类和虚函数交织的DWA实现更容易入门。

1.3 它在逻辑上的两个关键边界

理解TrajectoryPlanner，首先要画清楚它的两个边界：

第一个边界是“局部”的空间范围。TrajectoryPlanner不看整条全局路径，它只会把机器人在局部代价地图上的footprint（足迹）范围内的栅格映射到代价地图上，并在此范围内进行碰撞检测。所以在长走廊环境下，局部规划器的视野天然受限，这也是为什么导航中全局路径一直被叫做“战略层”，局部规划器指挥的是“战术执行”。

第二个边界是“控制周期”。move_base默认以10Hz频率调用局部规划器，也就是说每0.1秒就要完成一次完整重规划。TrajectoryPlanner在这种高频调度下必须保证单次computeVelocityCommands调用能在几十毫秒内返回，否则会造成控制周期拖尾，让底盘在打滑、撞击后的恢复动作滞后。这个实时性要求直接决定了它的采样策略和评分函数不会太复杂，这一点拿到代码里会看得很清楚。

2. 源码全景：TrajectoryPlannerROS与TrajectoryPlanner的里外一层

2.1 两个类的分工边界

打开base_local_planner/include/base_local_planner/看，文件结构一目了然。TrajectoryPlannerROS（trajectory_planner_ros.cpp）是ROS节点的门面，负责参数装载、话题订阅、tf坐标变换、发布可视化消息，以及调用TrajectoryPlanner完成实际算法。

TrajectoryPlanner（trajectory_planner.cpp）则是真正的“算法大脑”，完全屏蔽ROS概念，接收的是纯数据和类对象。比如它不知道什么是global_path，只知道自己有一堆gps_waypoints_（全局路点）、local_costs_（栅格代价）。这种设计让算法层可以被轻易复用和单测，是ROS源码里值得学习的工程实践。

2.2 核心成员变量一览

开始读TrajectoryPlanner.cpp前，先把头文件里的关键成员记住，不然翻到一半容易迷路。

class TrajectoryPlanner { public: TrajectoryPlanner(WorldGrid* world_model, const Costmap2D* costmap, std::vector<geometry_msgs::Point> footprint_spec, double acc_lim_x, double acc_lim_y, double acc_lim_th, double sim_time, double sim_granularity, double vx_samps, double vy_samps, double vth_samps, bool escape_vel); //... private: WorldGrid* world_model_; // 障碍物碰撞模型 const Costmap2D* costmap_; // 局部代价地图指针 std::vector<geometry_msgs::Point> footprint_spec_; // 机器人足迹 double acc_lim_x_, acc_lim_y_, acc_lim_th_; // 三个轴加速度限制 double sim_time_, sim_granularity_; // 轨迹模拟时间与推理步长 unsigned int vx_samps_, vy_samps_, vth_samps_; // 采样密度 std::vector<base_local_planner::Trajectory> traj_generator_; // 轨迹生成器列表 //... };

WorldGrid这里要单独解释一下。它其实就是把二维costmap栅格“压平”成一维数组，并提供footprint在任意位姿下是否压到障碍物的检测。TrajectoryPlanner不直接访问Costmap2D的cost函数，而是通过WorldGrid做了一层抽象，目的是在轨迹生成循环中减少计算量——每次都去遍历足迹多边形和栅格做相交测试太贵了，WorldGrid把它优化成了位姿到障碍物的确定性查表。

2.3 从computeVelocityCommands到updatePlan的调用链

TrajectoryPlannerROS的computeVelocityCommands是外部最常见的入口，move_base每帧在这里取速度指令。它的内部流程大致是：

1. 把robot_pose转换到全局坐标系，并在局部代价地图上裁剪出当前规划窗口；
2. 从全局路径中截取一段“可见路点”，投射到局部代价地图坐标系，交给TrajectoryPlanner的updatePlan；
3. 把速度空间采样参数同步给算法层；
4. 调用TrajectoryPlanner的findBestPath，返回打分最高轨迹的起点速度；
5. 将结果发布到cmd_vel，并把轨迹可视化到local_plan、trajectory_msgs等话题。

这个链条里，findBestPath是TrajectoryPlanner的核心算法，也是本篇切入的重点。后续的generateTrajectory、scoreTrajectory、computeCosts全部围绕findBestPath展开。

3. findBestPath：采样、打分、博弈的循环

3.1 采样空间与动态窗口的构建

进入TrajectoryPlanner::findBestPath后，第一个动作是更新机器人当前速度（从Odometry话题拿到），并以此为基准计算动态窗口。所谓动态窗口，就是考虑加速度限制后，在一个控制周期内实际“够得着”的速度区间。

以差速机器人为例，假设上一帧速度是(vx, vy, vth)，加速度极限为(acc_lim_x, acc_lim_y, acc_lim_th)，控制周期为dt，那么vx的可达范围是： vx_lower = max(min_vel_x, vx - acc_lim_x * dt) vx_upper = min(max_vel_x, vx + acc_lim_x * dt)

角速度vth同理，侧向速度vy对非全向机器人默认是0。TrajectoryPlanner会在这个动态窗口内按预先设置的采样数（vx_samps、vy_samps、vth_samps）均匀截取离散点。每个离散点都代表一组完整的速度命令（vx, vy, vth），下一步就是从这些候选速度出发，模拟生成若干秒的运动轨迹。

这里有个容易踩的坑：采样密度不等于精度。vx_samps设到20、vth_samps设到30确实让候选轨迹更密，但每帧要生成的轨迹总数是vx_samps * vy_samps * vth_samps，足足18000条。我在实际调参时曾经图省事把三组都拉到30，结果单帧规划耗时直接飙到200ms以上，导航直接变成“一顿一顿”的。后来查源码发现，TrajectoryPlanner的采样循环在generateTrajectory内部还把同一组速度在多个不同的起步时间点上重复生成，实际运算量比肉眼可见的采样数还要高。所以调参时脑子里要有这笔“算力账”。

3.2 generateTrajectory的物理模型

轨迹生成的物理模型是整个TrajectoryPlanner的骨架。在generateTrajectory函数中，给定一组速度指令和目标模拟时长，函数从当前位姿(x, y, th)出发，按sim_granularity间隔做离散积分。

关键代码片段如下：

Trajectory TrajectoryPlanner::generateTrajectory(double x, double y, double theta, double vx, double vy, double vth, double sim_time, double acc_lim_x, double acc_lim_y, double acc_lim_th) { Trajectory traj; traj.xv_ = vx; traj.yv_ = vy; traj.thv_ = vth; double x_i = x, y_i = y, theta_i = theta; double vx_i = vx, vy_i = vy, vth_i = vth; double dt = sim_granularity; for (double t = 0; t < sim_time; t += dt) { // 用加速度极限对速度做插值修正 vx_i = computeNewVelocity(vx_i, vx, acc_lim_x, dt); vy_i = computeNewVelocity(vy_i, vy, acc_lim_y, dt); vth_i = computeNewVelocity(vth_i, vth, acc_lim_th, dt); // 按当前速度积分位姿 x_i += vx_i * cos(theta_i) * dt; y_i += vx_i * sin(theta_i) * dt; theta_i += vth_i * dt; // 记录轨迹点 traj.addPoint(x_i, y_i, theta_i); } return traj; }

注意这里梯形积分用的是近似式，位姿更新里x和y的增量用的是上一时刻的theta，这在dt足够小（0.1s以下）时精度足够，但如果你是做高精度仿真或者里程计噪声很大的底盘，这个近似的误差会逐步累积，导致预测轨迹和实际轨迹偏差变大。这也是为什么很多机器人落地项目会把sim_granularity从默认的0.1改到0.05，代价是计算量相应翻倍，需要根据实机CPU性能权衡。

另一个被很多人忽略的细节：generateTrajectory里的速度加速度修正并不是单纯让当前速度夹逼到目标速度，而是让每个时刻的速度都按照加速度极限进行“攀爬”。也就是说，如果机器人当前速度是0.3m/s，目标速度是0.6m/s，加速度限制为0.2m/s²，则轨迹的前0.5秒速度是从0.3逐步升到0.4的，而不是瞬间跳到0.6。这个“软约束”正是TrajectoryPlanner能生成平滑轨迹的核心所在。它保证了轨迹预测的物理一致性，也让后续的代价评分在时间维度上有了意义。

3.3 footprint碰撞检测：WorldGrid上的“地毯式”排查

轨迹生成完毕，接下来最关键的一步是在代价地图上做碰撞检测。TrajectoryPlanner调用world_model_->footprintCost(x, y, theta)来检测机器人在轨迹点（x, y, theta）处是否与障碍物重叠。

这里的实现值得展开说。footprintCost不仅检查机器人足迹多边形内的栅格代价是否超过致命阈值（LETHAL），还会把整个多边形缩放一圈，因为真实底盘的轮胎、悬挂在转向时可能会扫到比几何模型更大的范围。源码中footprintCost的实现逻辑近似这样：

1. 根据机器人的footprint_spec（一串多边形顶点，相对机器人中心坐标），结合当前位姿做旋转平移，得到世界坐标系下的多边形；
2. 在多边形包围盒内遍历所有栅格；
3. 对每个栅格，判断其是否落在多边形内部（用射线法或叉积法）；
4. 如果某个栅格的cost >= inscribed_cost（内切圆代价）或 == lethal_cost，则返回该障碍物代价；
5. 否则返回可通行代价。

这个方法在栅格分辨率0.05m、足迹半径0.3m的差速机器人上，单点检测大约要算二三十次多边形点包含判断。而一条轨迹一般有三四十个轨迹点，一帧上千条轨迹，粗算下来单帧碰撞检测就要做百万次级别的栅格判断。这也解释了为什么TrajectoryPlanner在低端工控机上跑起来常常吃满CPU——性能瓶颈不在规划器算法本身，而在逐点footprint检测的遍历密度。

针对这个瓶颈，我实际写过一版优化：在generateTrajectory之前，先把所有候选速度对应的轨迹点预生成好，然后一次性对多条轨迹做批量footprint检测，利用SIMD指令把点包含判断向量化。实测下来能把单帧规划耗时从80ms压到30ms左右，代价是代码可读性变差，适合项目后期性能调优时再做。初学阶段不建议从优化入手，先用默认配置把功能打通。

3.4 轨迹评分：一次“左手画圆右手画方”的权衡

碰撞检测之后，每条存活下来的轨迹都会被打分。TrajectoryPlanner的评分函数分三组：goal与path偏差代价、障碍物趋近代价、速度与方向偏好。在源码里打分入口是scoreTrajectory，循环调用下列函数：

• goal_costs_：轨迹终点到局部目标点的距离，乘gdist_scale权重；
• path_costs_：轨迹上各点到全局路径的最近距离，乘pdist_scale权重；
• obstacle_costs_：轨迹点上的cost值，乘occdist_scale权重；
• heading_costs_：轨迹终点朝向与目标方向的差值，乘heading_scale权重；
• prefer_forward_costs_：统计轨迹中负向速度比例，对机器人调头行为施压。

我特意用了“权衡”这个词，因为评分函数的精妙处恰恰在于权重之间的对抗。如果goal_distance_bias设得过大，机器人会直愣愣地冲向目标点而对中间障碍物“视而不见”，很可能先撞再绕；如果path_distance_bias过大，机器人又会过度贴着全局路径走，丧失局部规划器应有的避障灵活性。

这种多目标代价相加的框架，本质上和强化学习里的reward shaping同构。把一个复杂避障问题拆解为若干个可微（或至少可导）的代价项，再按权重线性组合，好处是简单可靠；坏处是决策边界是超平面，一旦全局路径本身走“之”字，局部规划器也难免跟着抖。我在调优时更倾向于把path_distance_bias保持在0.6以下，让局部规划器在全局路径附近保留足够的自主绕障空间。

3.5 最终指令：不只是“挑最好”

findBestPath的最后一步是把所有打分正常的轨迹里得分最低的那条挑出来，取出它的起点速度（vx, vy, vth）作为本次控制输出。但这里有个重要细节：如果所有候选轨迹都因为碰撞被丢弃，TrajectoryPlanner不会直接输出零速，而是进入一个“escape模式”。在escape模式下，它会尝试生成一组“逃逸速度”，让机器人在原地尽力旋转或向后撤退，尽可能避开障碍物而不是傻站着。

源码中escape_vel变量控制这个模式的开闭，默认是true。实际调车时我经常把这个模式单独拿出来测——当机器人被卡在墙角时，关掉escape_vel，机器人会原地摩擦一动不动；打开后，会让它尝试以最大角速度原地打转，运气好就能蹭出死胡同。这个行为逻辑对底盘机械结构是个考验，打转时容易出现轮毂蹭地、万向节异响，需要结合机械限位来权衡。

4. 数据结构与关键参数：一张“底牌”式的速查表

4.1 Trajectory与TrajectoryCost的核心定义

TrajectoryPlanner里有三个底层数据类，搞懂它们，代码随便翻：

class Trajectory { public: Trajectory(double distance = 0, double max_x = 0, double min_x = 0, double max_y = 0, double min_y = 0, double max_th = 0, double min_th = 0); double xv_, yv_, thv_; // 该轨迹对应的速度 double cost_; // 累计代价 double min_cost_; // 过程最小代价（用于震荡检测） std::vector<double> x_points_; // 轨迹坐标 std::vector<double> y_points_; std::vector<double> th_points_; void addPoint(double x, double y, double th); // ... };

其中min_cost_这个字段我起初没在意，后来读评分代码才发现它是震荡（oscillation）检测的数据基础。TrajectoryPlanner会记录最近若干帧的最优轨迹代价，如果连续多次最优轨迹的代价持续走高，或者最优轨迹方向频繁反转，它就认为机器人陷入震荡，进入恢复行为。这个机制配合move_base的recovery_behavior，能把“画龙”症状限制在局部而不是发散成全局事故。

TrajectoryCost是评分函数的统一传入结构，包含了机器人在某一时刻可以使用的速度采样空间和代价权重。TrajectoryPlanner会在每帧把当前速度、目标、路径、代价地图指针封装成TrajectoryCost对象，再交给各个评分函数做计算。

4.2 影响轨迹质量的核心参数

直接给一块参数速查，基于源码语义和我的调机经验：

这里专门讲讲acc_lim系列。很多人标定底盘加速度时只测了直线加减速，忽略了角加速度，导致规划器以为机器人能在瞬间完成急转弯，实际上一打方向盘车辆就甩尾。我在某次调试阿克曼底盘项目时，把acc_lim_th从0.8降到0.3，机器人在弯道处的稳定性立竿见影，代价是全局规划时常判断“此路不通”，因为预测轨迹够不到窄转弯。这类参数必须实际实验标定，不能凭手感拍脑袋。

4.3 评分权重不可忽视的“政治性”

评分权重不仅影响轨迹打分，还会反过来影响路径规划器的可用性。如果occdist_scale设得太大，机器人几乎不可能在距离障碍物30cm以内的走廊通行，这会导致全局规划器规划出一条“现实可走”的路径，但实际上局部规划器永远认为该路径危险，于是频繁切回“绕路”状态。这种全局与局部之间的评分冲突，是实际导航系统里最难排查的隐性故障之一。

所以要建立这样一个思维：评分权重本质上是“机器人的性格设定”，没有绝对正确的组合，只有适合特定场景的组合。放走廊场景，goal_distance_bias可以高一些，让机器人走直线；放仓库货架间，path_distance_bias和occdist_scale要平衡，让机器人能贴着货架边缘精准穿行。源码读到这里，你已经拿到了“改性格”的钥匙，具体怎么拨，还是得靠现场实验。

5. 调试中的实战记录与心得

5.1 一次典型的“画龙”问题排查

回头说开头那个“画龙”案例。当时最终确认的根因有三层叠加：acc_lim_th标定得比实际底盘极限值高了一倍，导致轨迹预测过于“乐观”；occdist_scale设到了0.03，让轨迹本能地远离障碍物，在走廊里走出S形；同时全局路径规划频率只有0.2Hz，局部规划器在两次全局更新之间只能靠path_distance_bias“抱”着旧路径，一旦全局更新延迟，局部规划的指导线就“晃”了。

排查的过程其实很简单：用rviz把TrajectoryPlanner发布的候选轨迹可视化出来，你会看到每条轨迹的颜色深浅代表代价高低。如果发现所有高得分轨迹都是大弧线绕弯，而低得分轨迹全集中在障碍物附近，那基本可以断定是occdist_scale过高。接着再看轨迹的“起步速度”，如果起步速度频繁跳变导致底盘顿挫，那多半是加速度限制不符合底盘真实特性。

我当时用工具对比了“实际里程计轨迹”和“预测轨迹”之间的偏差，在走廊段偏差超过0.4m，超出footprint安全半径，说明sim_time和granularity的组合不足以覆盖实际动态特性。最终的调参是：把acc_lim_th从1.2降到0.4，occdist_scale从0.03降到0.02，sim_time从1.8降到1.2，同时给全局规划器加了10Hz的更新频率。调完之后“画龙”消失，底盘在走廊里基本能走成直线。

5.2 几处容易误读的源码细节

读TrajectoryPlanner源码时，有几个细节如果按照惯性思维去看容易走弯路，我踩过，帮你标一下：

细节一：TrajectoryPlanner里sim_time并不是指“这个轨迹在物理世界里会跑那么久”，而是它内部为打分而做的“假想运动时长”。打分时轨迹点并不会全部在机器人实际控制中被执行，真正执行到下一个控制周期来的0.1秒就算完了。换句话说，预测轨迹的“尾巴”只负责评分，不参与控制。

细节二：全局路径与局部代价地图的坐标对齐。TrajectoryPlanner内部把全局路点先转到局部代价地图坐标系，这个过程依赖tf树。如果你的tf树发布频率不够，或者map和odom之间跳变过大，那么局部规划器看到的“全局路径”会是错位的。这类故障在rqt_tf_tree里很容易发现，但很多人第一反应会误判成规划器bug。

细节三：轨迹评分的“过程代价”与“终点代价”既有重叠又有分工。过程代价是对轨迹上每个点的障碍物代价累加，终点代价只算终点的gdist/pdist。如果sim_time设得过短，过程代价还没“走完”就有终点代价补位，导致机器人在障碍物面前“预见不到碰撞”，表现是“贴着墙冲然后再急刹车”。这算是sim_time过短的典型症状。

5.3 避坑建议：把源码当作“实验手册”而不是“说明书”

如果你刚接触这个库，我建议别急着改参数，先做一件事：在rviz里打开local_plan和trajectory话题，开着仿真或者真车底盘空跑，把当前所有候选轨迹的代价色标调出来，盯着跑30秒。你会发现很多参数的语义根本不需要文档解释，轨迹分布会自己说话。

然后你再回到源码，对着话题消息类型和评分函数一一对应，比如trajectory_msgs里的trajectory可以告诉你sim_time用了多少、每一帧的候选轨迹密度如何。等你养成了这种“看现象—查源码—改参数—再看现象”的闭环习惯，TrajectoryPlanner就不再是黑盒了。

我个人认为，读这类源码最有价值的不是学会某个API怎么调，而是理解作者在“实时性”和“规划质量”之间的每一次取舍。当你自己动手写一个局部规划器，或者把一个旧型规划器移植到新底盘上时，这种取舍感会成为你最可靠的指南针。

6. 从TrajectoryPlanner到DWA与Teb的思维迁移

6.1 读懂了TrajectoryPlanner，DWA基本等于白送

DWA和TrajectoryPlanner在代码层面最大的差别在于采样空间的定义。TrajectoryPlanner是“先采样速度空间，再模拟轨迹”；DWA是“先计算动态窗口，在窗口内采样速度，再模拟轨迹”。把采样空间换成动态窗口，评分函数体系几乎一模一样。你只要把TrajectoryPlanner的findBestPath看过一遍，再去瞄一眼DWAPlanner的computeVelocityCommands，会发现它的主循环一模一样，连变量名都是从TrajectoryPlanner里复制过来的。

所以如果时间不允许全读，我给你的建议是：TrajectoryPlanner的源码从头到尾读一篇，DWA的源码只读_dwaparams.yaml参数说明和两个核心评分函数（obstacle_costs、goal_costs），其他统统忽略。

6.2 Teb与TrajectoryPlanner的“世界观差异”

Teb（Timed Elastic Band）和TrajectoryPlanner走的完全是两条路。TrajectoryPlanner是“采样式”的，先枚举有限条轨迹再择优；Teb是“优化式”的，把轨迹当成一系列带时间戳的位姿序列，用图优化（g2o框架）去最小化带约束的目标函数。Teb的优势是轨迹平滑度极高、能显式处理速度和加速度约束；劣势是对初值敏感、计算耗时波动大，在低算力平台上容易调度超时。

从接线上看，Teb也实现BaseLocalPlanner接口，里面有computeVelocityCommands和isGoalReached，所以在move_base框架里的接入方式和TrajectoryPlanner一模一样，可以直接切换。

那么我的建议是：工程落地时，如果机器人的全局路径质量不高（比如路径抖动、有之字形拐点），Teb这种优化式规划器会因为初值太差而陷入局部最优，反而不如TrajectoryPlanner这种“萝卜白菜，各有所爱”的采样式稳妥。反过来，如果路径质量好，Teb的平滑效果会让你眼前一亮。二者没有绝对的“谁更先进”，只有和场景匹配的“谁更合适”。

6.3 继续往下读源码的方向建议

TrajectoryPlanner这篇算开了个头，后续还会有几个方向值得继续挖：

• 轨迹评分函数里的细节：比如scale_penalty机制如何把栅格代价转换到轨迹代价，heading_lookahead的具体实现会牵扯到一些向量几何；
• 恢复行为与碰撞检测的配合：当所有轨迹都不可行时，escape模式的切换条件，以及具体的逃逸速度生成策略；
• TrajectoryPlanner与Costmap2D之间的交互：ROS Costmap中膨胀层、障碍层的更新时机如何影响局部规划器的视野；
• 参数自动调优：基于代价地图反馈自动调整评分权重的尝试，业界有一半在用手调，有一半在用强化学习，后者越来越活跃。

如果对TrajectoryPlanner的实际表现有直观兴趣，V-REP/CoppeliaSim和Gazebo仿真里都有现成的差速底盘模型，把上述参数改动后的轨迹可视化拉到仿真里看看，会比在真车上折腾省很多事。

7. 后记：从“能跑”到“会跑”中间隔的不是调参是读码

每次在社区里看到有人问“为什么我的机器人导航跟没头苍蝇一样”，我第一反应都是先问他有没有看过局部规划器发的轨迹。很多人把时间花在知乎搜“最佳参数”、把yaml里的数字抄来抄去，抄了一堆还是原地打转。根源就是没理解评分函数之间的博弈关系，也没有建立可视化轨迹的调试习惯。

TrajectoryPlanner这套源码的价值在于，它把经典避障算法的全部骨架都摆在你面前，没有复杂的模板和黑魔法，注释还写得清清楚楚。认真读一遍，你对“局部规划器到底怎么工作”的理解会直接跃升一个台阶。今后再遇到DWA也好、Teb也好，你都不会再觉得它们高深莫测，因为它们的思考起点和TrajectoryPlanner一模一样，只是换了个求解轨迹的方式。

有个小技巧：读的时候手里拿张纸，把main流程画成离散块，标上输入输出，然后在rviz里对照着看真实数据流。源码有时候觉得晦涩，不是因为它难，而是因为它长。剥离掉ROS和外层封装，算法的核心其实只有30行代码在幕前调度，剩下的都是输入输出适配和边界处理。

最后分享一个我常用的调试口诀：障碍物面前犹豫了，先看occdist_scale和sim_time；转弯转不动了，先看acc_lim_th和heading_lookahead；路径走成锯齿，先看path_distance_bias；原地转圈出不来，看escape_vel和恢复行为。剩下的，交给源码去解释。

后续我会接着写TrajectoryPlanner的评分函数细节和性能优化实战，那个话题会更具体，也会牵扯到更多和机器人模型强相关的调参经验。留个位坑，大家随时跟。