TrajectoryPlanner源码解析：局部规划器的打分机制与采样循环

如果你已经跑通了 move_base但对“局部规划器到底在干什么”的理解还停留在黑盒层面——把全局路径塞进去cmd_vel 吐出来——那这篇笔记大概能帮你把那层黑盒打开一半。我读 TrajectoryPlanner 源码的契机是某轮式平台上出现的一个诡异现象走廊里直行没有任何问题一到门口就开始左右摆头全局路径明明很干净局部规划就是不沿着走。当时我把三个“distance_bias”参数来回调了一整晚毫无改善。最后老老实实翻开 navigation 仓库里 base_local_planner 的 TrajectoryPlanner 源码才发现问题根本不在权重比例而在打分项对目标点方向过度敏感。这篇文章是系列的第一篇目标是先把 TrajectoryPlanner 的整体框架讲清楚它在导航栈里的位置、数据流、源码文件组织、打分机制以及最核心的采样主循环。搞懂这些之后第二篇再深入轨迹生成和碰撞检测的源码细节就会轻松很多。适合正在调局部规划参数却总觉得“差点意思”的人也适合准备基于这套代码做二次开发的读者。1. 先搞清楚TrajectoryPlanner在导航栈中的定位与数据流1.1 它不是独立节点而是被move_base加载的一个插件很多人以为局部规划器是一个独立运行的ROS节点其实不是。在navigation框架里move_base是一个大节点内部通过pluginlib机制动态加载全局规划器和局部规划器。配置文件中 base_local_planner 字段写的是base_local_planner/TrajectoryPlannerROSmove_base 启动时就会创建这个插件实例并在每个控制周期里调用它。这里的“Base”不是中文里“基础版”那种谦逊说法而是 navigation 框架最初那代局部规划器在代码包层面的名字。base_local_planner 包里除了 TrajectoryPlanner还有后来被更多人使用的 DWAPlanner在 dwa_local_planner 包里。很多教程里说的“DWA”其实默认指的已经是后者。TrajectoryPlanner 虽然名气不如 DWA但它是理解 ROS 局部规划器全貌最好的起点原因很简单它麻雀虽小五脏俱全。插件化意味着什么意味着你可以在不修改 move_base 主体代码的情况下把局部规划器换成任意实现了 BaseLocalPlanner 接口的类。这个接口的核心方法只有两个setPlan和computeVelocityCommands。前者把全局路径交给规划器后者基于当前位姿计算输出速度。记住这两个方法名后面讲调用链的时候会反复出现。1.2 吃进去的是路径吐出来的是速度从外部看TrajectoryPlanner 做的事情可以压缩成一句话接收当前位姿和全局路径输出一个线速度和角速度。但这句话背后有几个容易被忽略的细节。首先是坐标系。全局规划器给出来的路径在 map 系或者你配置的 global_frame而局部规划器做采样和积分的坐标系是 odom 系。所以在 computeVelocityCommands 真正执行之前封装层 trajectory_planner_ros.cpp 会先把全局路径通过 transformGlobalPlan 变换到 odom 系并按机器人的当前位置裁剪。裁剪逻辑不是取路径的前 N 个点而是计算每个路径点离当前位姿的直线距离只保留前方一定范围同时把已经走过的路径点丢掉。这个裁剪范围很关键。它决定了局部规划器“看多远”。如果裁得太短机器人走两步就得停下来重新等路径刷新如果裁得太长远方路径的曲率变化又容易干扰近处的采样结果。实际代码里路径的保留范围通常和局部代价地图的尺寸有关一般取地图宽度的 1.0~1.5 倍作为前视窗口但这个值是可以改的。我最初调试时以为机器人摆头是全局路径的问题后来把裁剪后的路径打印出来才发现全局路径没问题是局部规划器看的路段太远目标点方向变化过快导致打分振荡。然后是时间步。move_base 的局部规划器不是连续运行的而是按 control_frequency 设置的频率周期调用常见配置是 10Hz 或者 20Hz。每来一个周期TrajectoryPlanner 要做的事情包括读取最新 odom 得到位姿、检查当前是否已经到达目标、更新局部路径、在速度空间里采样、模拟轨迹、打分、选最优、发布 cmd_vel。整个过程必须在一个控制周期内完成所以“实时性”是它所有设计的第一约束。这也是为什么采样密度vx_samples、vtheta_samples不能无限加大后面会详说。主循环的最前面还有一个容易被忽略的早退逻辑如果当前位姿离目标点已经很近或者轨迹已经执行完最后一段TrajectoryPlanner 会直接返回零速不再做任何采样和打分。这个逻辑在源码里就是一个简单的距离判断但它在实际导航里非常重要——否则机器人到了目标点还会因为打分噪声在原地试探性运动。2. 源码文件地图与插件生命周期2.1 三个核心文件各管哪一段打开源码仓库路径一般是 navigation/base_local_planner。这个包里的文件不少但读 TrajectoryPlanner 只需要抓住三个核心文件其余大多为工具类。第一是trajectory_planner_ros.h/cpp它是最外层的 ROS 封装。负责参数加载、话题订阅发布、TF 变换、全局路径的裁剪和变换以及对外暴露 move_base 要调用的 computeVelocityCommands 入口。这一层会依赖很多 ROS 特有类型比如 nav_msgs/Odometry、geometry_msgs/PoseStamped。它把 ROS 世界的“消息”翻译成内部算法类能理解的“数据结构”然后调用算法层。第二是trajectory_planner.h/cpp这是真正的算法核心。类的名字就叫 TrajectoryPlanner它不依赖 ROS 消息类型输入输出都是纯 C 结构。computeVelocityCommands 接受的是位姿、速度、以及速度限制这些基本类型返回的是一个 Trajectory 对象包含所有模拟出来的轨迹点和对应的最优速度。这一层不关心你的位姿是来自 TF 还是来自别的传感器所以逻辑非常干净也方便单独做单元测试。第三是simple_trajectory_generator.h/cpp。名字很直白就是根据一个速度对线速度角速度生成未来一段时间内的轨迹点序列。生成逻辑本身很简单按时间步长离散积分但它的输出直接决定了打分环节能看到多少信息。除这三个文件外读打分逻辑时还会碰到 base_local_planner 里的 costmap_model、world_model 这些工具类。它们的作用是把机器人位姿映射到代价地图栅格上查障碍物开销。2.2 从插件注册到computeVelocityCommands的调用路径先看插件注册。TrajectoryPlannerROS 在文件末尾有一行宏PLUGINLIB_EXPORT_CLASS(base_local_planner::TrajectoryPlannerROS, nav_core::BaseLocalPlanner)。这行宏把 TrajectoryPlannerROS 注册到 pluginlib 里并在 move_base 启动时通过参数 base_local_planner 指定的插件名加载。init() 是生命周期里的第一站做的事情非常机械但非常重要读取所有参数、创建 Costmap2DROS、订阅 odom 话题通常是 odom 话题、把参数从 ROS 的 param server 逐个塞给内部算法对象。这里常见的一个问题是很多人调参时改了 param 文件却忘了检查参数名是否拼写正确。TrajectoryPlanner 的参数名称一堆下划线比如 max_vel_x、max_vel_theta、acc_lim_x、sim_time、sim_granularity还有 vx_samples、vtheta_samples。任何一个参数拼错ROS 都不会报错只是静默保留默认值表现得像“改了没效果”。init 之后move_base 会在每次收到新全局路径时调用 setPlan在每次控制周期里调用 computeVelocityCommands。TrajectoryPlanner 的内部函数调用顺序我读代码时是这样理清的收到新路径 - trajectory_planner_ros.cpp 里的 updatePlan() - 把 map 系全局路径变换到 odom 系并裁剪 - 调 algorithm-setPlan()控制周期 - move_base 调 computeVelocityCommands() - 内部再调 algorithm-computeVelocityCommands() - 返回最优轨迹和速度 - 封装成 Twist 发布这里有个值得注意的地方封装层和算法层的类名都叫 computeVelocityCommands但做的事情完全不同。外面那层要做坐标变换、裁剪、早退判断里面那层才做真正的采样和打分。第一次读源码的人很容易在这两层之间绕晕分不清谁是真正干活的。这一节还有一个容易被忽略的关闭逻辑TrajectoryPlannerROS 的析构函数里会做一些清场动作。如果你在动态切换局部规划器插件时遇到诡异崩溃多半跟插件生命周期管理有关而不是算法本身。3. 打分机制拆解Score和Trajectory背后的代价函数设计3.1 一条轨迹是怎么被“打分”的TrajectoryPlanner 对每条模拟轨迹的处理不是直接给一个分数而是先算出一个包含多个维度的 Score 结构体再按权重合成总分。这个设计我特别喜欢因为它把“这个轨迹怎么样”和“我们最终关心什么”解耦了。Score 里常见的维度有path轨迹末端点与局部全局路径的偏离程度越小越好goal轨迹末端点与目标点的距离越小越好dist轨迹与障碍物的距离开销由代价地图数值决定occ轨迹上所有采样点触碰过的栅格开销总和heading与前方路径方向的夹角相关用于惩罚走回头路或原地转动。最终的总分不是简单相加而是加权求和。源码里的核心计算可以理解成这个形式score path_distance_bias * path_cost goal_distance_bias * goal_cost occdist_scale * occ_cost看到这个公式就能明白为什么网上流传的调参口诀总是围绕三个参数path_distance_bias、goal_distance_bias、occdist_scale。这三个参数控制的是“贴路、奔目标、躲障碍”三者的拉扯强度而其他打分项是固定计算的一部分。3.2 path、goal、occ各自在算什么先说 path_cost。TrajectoryPlanner 的做法是取轨迹的末端点然后在裁剪后的全局路径上找到离这个末端点最近的路径点计算两者的横向距离差或者欧氏距离差。它关心的是“走完这段轨迹后你在不在全局路径附近”而不是“你一路上有没有贴着路径”。这个区别很重要一条轨迹可能起点贴在路径上但末端点因为转弯半径太大偏出了路径path_cost 就会很高。goal_cost 和 path_cost 类似只不过最近点换成了目标点。goal_cost 高说明这条轨迹的末端离目标还远。在全局路径还比较长的时候goal_cost 会稳定地“拉”机器人往目标方向走但当全局路径本身绕路时过于追求 goal_cost 会让机器人试图抄近路反而偏离全局路径。这也是为什么 path 和 goal 两个权重要配合调不能只调一个。occ_cost 则是所有采样点对应栅格 cost 的汇总。costmap 中每个栅格有一个 0~254 的代价数值255 通常表示致命障碍接近 253~254 的是膨胀区。TrajectoryPlanner 对每个轨迹点查询 costmap把相应的代价累加或取最大值用于惩罚。如果一个点落在致命障碍栅格里整条轨迹会直接失效连打分资格都没有这就是碰撞检测的最朴素实现。3.3 “权重陷阱”你以为调的百分比其实调的是相对强度初学调参时很容易掉进一个陷阱以为把 path_distance_bias 调到 2.0就比 0.6 更“贴路”。实际不是这样。最终决策只取决于三个被加权后的项的相对大小。你把 path_distance_bias 和 goal_distance_bias 同时放大两倍对选择结果毫无影响真正起作用的是两者的比值以及它们与 occ_cost 的比值。举例来说如果机器人频繁往障碍方向跑很多人会把 occdist_scale 从 0.01 调到 0.5发现效果不明显或者矫枉过正。这是因为 occ_cost 的绝对值量级和 path_cost 差很多。在 costmap 中膨胀区的数值通常在几十而路径偏离的像素距离可能只有零点几到几个栅格。没有把不同维度的数值量级拉齐之前调权重就是在盲目试错。正确做法是先观察每个打分项的量级用 rosbag 回放把每帧最优轨迹的 Score 打印出来再决定调哪个。我在实际项目里见过最典型的问题就是走廊门口的左右摆头全局路径是干净的直线但 goal_cost 项对末端点方向变化非常敏感每次采样时终点稍微偏一点goal_cost 就大幅波动导致机器人频繁调整角速度。后来我把 goal_distance_bias 相对减小同时给 heading 相关评分加了平滑问题才消失。这类问题只看行为现象完全无法定位把 Score 打出来一眼就能看到是哪一项在剧烈变化。4. 核心采样循环从一个采样点到一条轨迹的诞生4.1 速度空间是怎么划出来的TrajectoryPlanner 的采样不是在整个连续速度空间里撒点而是先生成一个离散的候选速度组合集合。线速度在 [min_vel_x, max_vel_x] 区间上按 vx_samples 等分为若干档角速度在 [min_vel_theta, max_vel_theta] 区间上按 vtheta_samples 等分为若干档。两个维度组合起来就是 vx_samples * vtheta_samples 个速度对。这里有一个重要的细节采样范围不是直接取硬边界而是会结合当前实际速度做一个“动态窗口”。动态窗口的含义是以当前速度为中心加减加速度限制乘以上一个控制周期时间得到一个在当前动力学约束下真正可达的速度范围。如果启用了 DWA 相关的开关最终采样空间就是硬边界和动态窗口的交集。这就是为什么即便你设了 max_vel_x 为 1.0机器人在静止状态下也不会直接采出 1.0m/s 的速度去执行——它会先采出加速能力允许范围内的速度防止执行器跟不上。采样密度是一个典型的权衡问题。vx_samples 取 10vtheta_samples 取 20组合出来就是 200 个速度对。每个速度对都要模拟一条轨迹、对每个轨迹点查一次 costmap计算量已经不小。如果你把 vx_samples 调到 20vtheta_samples 调到 40就是 800 个速度对控制周期如果还是 20HzCPU 占用会明显上升甚至拖垮整机算力。反过来说采样密度太低候选速度组合覆盖不到最优解机器人的运动会显得僵硬、抖动。一般从默认值出发按实机表现一点点加直到行为平滑且控制周期不超时为止。4.2 从速度对到轨迹点序列的离散积分选定一个速度对之后generateTrajectory 开始干活。它的逻辑是从当前位姿 (x, y, theta) 出发以 (vx, vtheta) 在 sim_time 时间内做离散积分。源码里循环的大致结构可以写成下面这样x start_x; y start_y; theta start_theta; for (t 0; t sim_time; t sim_granularity) { x vx * cos(theta) * sim_granularity; y vx * sin(theta) * sim_granularity; theta vtheta * sim_granularity; trajectory.pts.push_back({x, y, theta}); }这个循环看着简单但有三个点值得说一下。第一sim_granularity 是模拟步长不是时间切片。它的单位是秒决定了积分分辨率。步长太大转角时轨迹呈折线碰撞检测容易漏检步长太小轨迹点太多查 costmap 的开销上升。第二vx 是线速度但很多底盘还有单独的横向速度 vyTrajectoryPlanner 的采样空间默认把 vy 当作 0 处理因为大多数差速底盘没有横向自由度。全向底盘的横向速度支持是后续 DWA 和 TEB 这类规划器才做得更好的。第三这段积分是纯运动学模拟完全没有考虑摩擦、加减速曲线等动力学细节。模拟出来的轨迹是“假设机器人立刻就能到达目标速度”的近似轨迹。实际执行时底盘制造商的控制周期和电机响应会滞后这也是为什么仿真里效果好、实机上还要额外调加速度限制的原因之一。4.3 碰撞检测不是“撞到才算”而是看代价地图开销每条模拟轨迹生成完成后TrajectoryPlanner 会拿它去做障碍物检测。做法是遍历轨迹点把每个点通常按机器人 footprint 半径做膨胀映射到 costmap 栅格坐标查栅格代价看是否落在致命障碍区。如果任何一点 cost 超过阈值整条轨迹直接返回无效不再进入打分环节。这里的阈值和膨胀处理都是可以配置的。inscribed_radius 对应的膨胀层会让机器人“以为”自己比实际更大一点从而留出安全间隙。这就是为什么即使全局路径规划得很好局部代价地图膨胀半径设得过小机器人依然会贴着墙走因为它的安全间隙不够。碰撞检测还有一个很多人没注意到的点它检查的是离散的点不是连续扫过的区域。如果 sim_granularity 太大机器人高速转弯时两个采样点之间可能“跳”过一个障碍物栅格。这是漏检的经典来源所以 sim_granularity 和最大采样速度要配套考虑跑得快就要适当缩小步长。4.4 从两百条轨迹里挑一条最优的所有有效轨迹算完 Score 之后TrajectoryPlanner 会选 Score 最小的那一条作为最优轨迹并把这条轨迹里的第一个速度发布给底盘。如果所有候选轨迹都因为碰撞无效就返回一个安全的制动速度通常是 0。这个“全无效就停”的策略是 TrajectoryPlanner 设计里比较保守的一环。它保证了安全但也意味着在狭窄通道里如果候选速度空间恰好被障碍物全部堵死机器人会直接停下而不是尝试倒车。后面的 DWA 和 TEB 在这一点上做了各种改进但我个人认为TrajectoryPlanner 的保守策略在某些工程场景反而是优点至少不会乱窜。选轨迹时还有个细节如果某条轨迹的 Score 和前一条差距极小TrajectoryPlanner 不会硬挑最低那一个避免抖动而是会加一个很小的容差。这个容差也是可以调的具体数值可以在源码里搜到。调参时如果发现机器人频繁在两个行为之间来回“犹豫”大概率就是 Score 差异落在容差范围里。5. 调参与二次开发必须知道的几个坑5.1 窄通道里老是准备撞墙问题可能不在权重有些场景下TrajectoryPlanner 在走廊或门口的表现非常差明明直行就能过去它却偏向旁边甚至原地停下。很多人第一反应是调 occdist_scale但多数情况下真正的问题是 sim_time 太短。sim_time 决定了每条轨迹往后“看”多久。如果 sim_time 只有 1.5 秒机器人在高速状态下只能看到前面一两米的路当障碍物在 3 米开外时它根本没有采样到任何能绕过障碍的轨迹于是表现为路径规划失败。调试方法很简单把 sim_time 一点一点加长观察行为变化。代价是每条轨迹的积分循环更长CPU 占用上升。不要盲目加太长找到一个“刚好能覆盖下一段关键障碍”的值就够了。5.2 odom质量会放大打分抖动TrajectoryPlanner 的所有位姿输入都来自 odom它的准确性和时序直接影响采样基准。odom 消息延迟、数据跳变、甚至话题频率不稳定都会导致同一时刻的采样空间和打分结果发生跳变。常见现象是机器人原地轻微抖动即使全局路径是直线。这种时候先检查 odom 发布频率和延迟不要急着动打分参数。我踩过的一次坑是底盘驱动里 odom 消息偶尔丢帧导致局部规划器以为机器人瞬间静止又瞬间运动采样窗口反复变化表现就是低速下不停点头。5.3 该换规划器的时候别硬调TrajectoryPlanner 适合差速底盘、中低速、结构化程度较高的场景。如果你的项目是阿克曼底盘或者需要在狭窄空间频繁倒车、原地转向TrajectoryPlanner 的采样方式会非常吃力。阿克曼底盘的转向能力体现在角速度和线速度的耦合关系上TrajectoryPlanner 默认把 vx 和 vtheta 当独立变量采样模拟出来的轨迹可能执行器根本无法复现。遇到这种情况我建议直接换 TEB 或者至少换 DWA而不是花大量时间在参数上强行适应。选型建议可以这样判断差速底盘、路径较平滑、算力受限——TrajectoryPlanner 够用复杂停车场、窄道揉库、需要时间最优——直接 TEB想让参数更直观、社区支持和文档更丰富——用 DWA它的实现和 TrajectoryPlanner 非常接近迁移成本不高。5.4 改打分函数时的最小验证闭环如果你打算在 TrajectoryPlanner 上做二次开发比如加一条“远离某个区域”的惩罚项建议遵循一个最小闭环先在 scoreTrajectory 函数里加一行打印确认你的惩罚项确实被计算了然后用 rosbag 录一段固定场景回放时对比最优轨迹的 Score 变化最后再上真机。不要直接在真机上边调边改因为打分项的量级和相互作用很难靠感觉判断。还有一个工程细节改源码重新编译 navigation 包时记得清掉旧插件缓存。pluginlib 对修改后的插件加载偶发缓存问题表现为改完代码重启后行为没变化浪费大半天时间排查。最后再分享一个小经验我读这套源码的过程中最大的收获其实不是某个打分公式而是意识到“局部规划器的一切行为异常最终都能追溯到采样空间、打分函数、参数量级这三者之一”。顺着这个思路排查比盯着机器人看半天有效得多。这篇先写到这里下一期我会把 simple_trajectory_generator 的轨迹生成细节和 costmap 碰撞检测的完整路径单独拆开来讲。如果你手头正好在调 TrajectoryPlanner不妨先把文中提到的 Score 打印和 rosbag 回放这两步做起来。