TrajectoryPlanner源码解析：从轨迹采样到评分函数的局部规划器

从一张异常轨迹图说开去为什么我第一眼就盯上了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和DWADynamic 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/看文件结构一目了然。TrajectoryPlannerROStrajectory_planner_ros.cpp是ROS节点的门面负责参数装载、话题订阅、tf坐标变换、发布可视化消息以及调用TrajectoryPlanner完成实际算法。TrajectoryPlannertrajectory_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::vectorgeometry_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::vectorgeometry_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::vectorbase_local_planner::Trajectory traj_generator_; // 轨迹生成器列表 //... };WorldGrid这里要单独解释一下。它其实就是把二维costmap栅格“压平”成一维数组并提供footprint在任意位姿下是否压到障碍物的检测。TrajectoryPlanner不直接访问Costmap2D的cost函数而是通过WorldGrid做了一层抽象目的是在轨迹生成循环中减少计算量——每次都去遍历足迹多边形和栅格做相交测试太贵了WorldGrid把它优化成了位姿到障碍物的确定性查表。2.3 从computeVelocityCommands到updatePlan的调用链TrajectoryPlannerROS的computeVelocityCommands是外部最常见的入口move_base每帧在这里取速度指令。它的内部流程大致是把robot_pose转换到全局坐标系并在局部代价地图上裁剪出当前规划窗口从全局路径中截取一段“可见路点”投射到局部代价地图坐标系交给TrajectoryPlanner的updatePlan把速度空间采样参数同步给算法层调用TrajectoryPlanner的findBestPath返回打分最高轨迹的起点速度将结果发布到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的实现逻辑近似这样根据机器人的footprint_spec一串多边形顶点相对机器人中心坐标结合当前位姿做旋转平移得到世界坐标系下的多边形在多边形包围盒内遍历所有栅格对每个栅格判断其是否落在多边形内部用射线法或叉积法如果某个栅格的cost inscribed_cost内切圆代价或 lethal_cost则返回该障碍物代价否则返回可通行代价。这个方法在栅格分辨率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::vectordouble x_points_; // 轨迹坐标 std::vectordouble y_points_; std::vectordouble th_points_; void addPoint(double x, double y, double th); // ... };其中min_cost_这个字段我起初没在意后来读评分代码才发现它是震荡oscillation检测的数据基础。TrajectoryPlanner会记录最近若干帧的最优轨迹代价如果连续多次最优轨迹的代价持续走高或者最优轨迹方向频繁反转它就认为机器人陷入震荡进入恢复行为。这个机制配合move_base的recovery_behavior能把“画龙”症状限制在局部而不是发散成全局事故。TrajectoryCost是评分函数的统一传入结构包含了机器人在某一时刻可以使用的速度采样空间和代价权重。TrajectoryPlanner会在每帧把当前速度、目标、路径、代价地图指针封装成TrajectoryCost对象再交给各个评分函数做计算。4.2 影响轨迹质量的核心参数直接给一块参数速查基于源码语义和我的调机经验参数源码位置作用调参建议sim_time采样时长决定预测轨迹“看多远”1.5~2.0s过长容易让机器人“想太多”频繁变向sim_granularity积分步长决定轨迹点密集度0.05~0.1分辨率越细越准但CPU开销越大vx_samps / vth_samps采样密度采样网格密度各8~15足够过高是算力浪费acc_lim_x / acc_lim_th加速度限制约束轨迹的物理可达性必须和底盘标定一致否则理论轨迹和实际偏差大goal_distance_bias目标评分权重机器人对“到达终点”的渴望程度0.6~0.9太高容易忽视避障path_distance_bias路径评分权重机器人对“贴近全局路径”的执着0.4~0.6太高会牺牲局部绕障能力occdist_scale障碍物代价权重对障碍物栅格的惩罚强度0.01~0.05太高会导致狭窄通道无法穿越heading_lookahead目标方向前瞻距离控制“提前转弯”的灵敏度差速底盘建议2~3倍车身长度这里专门讲讲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.4occdist_scale从0.03降到0.02sim_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 读懂了TrajectoryPlannerDWA基本等于白送DWA和TrajectoryPlanner在代码层面最大的差别在于采样空间的定义。TrajectoryPlanner是“先采样速度空间再模拟轨迹”DWA是“先计算动态窗口在窗口内采样速度再模拟轨迹”。把采样空间换成动态窗口评分函数体系几乎一模一样。你只要把TrajectoryPlanner的findBestPath看过一遍再去瞄一眼DWAPlanner的computeVelocityCommands会发现它的主循环一模一样连变量名都是从TrajectoryPlanner里复制过来的。所以如果时间不允许全读我给你的建议是TrajectoryPlanner的源码从头到尾读一篇DWA的源码只读_dwaparams.yaml参数说明和两个核心评分函数obstacle_costs、goal_costs其他统统忽略。6.2 Teb与TrajectoryPlanner的“世界观差异”TebTimed 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的评分函数细节和性能优化实战那个话题会更具体也会牵扯到更多和机器人模型强相关的调参经验。留个位坑大家随时跟。