机器人路径规划优化：从A*到DWA的ROS2 Nav2实战指南

做机器人路径规划优化这个项目，最初是人工智能课程的结课大作业。我本以为核心是算法，一口气在仿真环境里加了动态避障、多目标点、实时重规划，结果小车疯狂撞墙、原地打转、路径乱飘，整套系统连一次完整的到达任务都完不成。后来复盘才发现，问题根本不在某个算法不够好，而是全局规划、局部规划、运动控制这三个层级被我混成了一锅粥。

这篇东西就围绕机器人路径规划优化这条主线，记录一次从需求拆解、算法选型、ROS2 Nav2框架搭建，到具体调参、踩坑、性能对比的完整过程。适用于正在做人工智能大作业、搞ROS2机器人导航，或者想系统理解A*/RRT*/DWA几类规划算法实际怎么落地的人。不保证看完你能造出一台无人驾驶车，但至少能把这几类算法在什么场景下用、参数怎么调、坑怎么救，一次性说清楚。

1. 整体设计思路：路径规划问题到底在优化什么

1.1 先拆三层，别一股脑塞算法

做路径规划之前，我先把问题拆成了三层：全局规划、局部规划、运动控制。很多初学者（包括当时的我）一上来就写A*，但机器人不是地图上的一个点，它有体积、有转向约束，还要面对实时出现的动态障碍物。单靠一个静态的A*算法，根本无法覆盖整个导航问题。

三层各自的边界是这样的：全局规划负责在已知地图上找一条从起点到终点的可行路径，频率低、尺度大，关注“走哪条路”；局部规划根据当前传感器数据，在全局路径附近实时规划短距离的轨迹，频率高、尺度小，关注“下一秒怎么走”；运动控制负责把速度指令真正下发给底盘，让电机响应用得上。

我最初把三个层级混在一起，结果任何一个环节出现异常，整个系统就崩。比如局部规划发现障碍物要绕行，但全局规划给的路径没有更新，两者就会“打架”；运动控制响应慢，局部规划算好的轨迹又跟不上。所以正式写代码前，建议第一件事就是画清楚这三层的职责边界，想清楚每一层各自负责什么、输入输出是什么、异常时怎么降级。这个设计做到位，后面所有问题都好排查。

1.2 为什么选ROS2 + Nav2这套组合

如果纯手写一套路径规划系统，至少需要处理地图表示、栅格更新、A*实现、传感器数据融合、底盘驱动、TF坐标树、任务状态机等一大堆组件。等写完这些，课程也结束了。ROS2生态里的Nav2几乎把这些都封装好了，而且采用的是插件式设计，全局规划器、局部规划器、代价地图层都可以单独替换。

Nav2不是单个节点，而是一组服务节点的集合：planner_server负责全局规划，controller_server执行局部规划与轨迹跟踪，costmap_2d负责构建和维护代价地图，BT Navigator管理整个任务流转。选择ROS2而不是ROS1，核心原因是ROS2在实时性、多机通信、生命周期管理上更适合现代机器人开发。虽然学习曲线比ROS1陡一点，但从长远看，这套技能栈迁移性更强。

结合“人工智能学习”这个背景，我建议不要从零手搓算法来交作业，而是在Nav2框架下做两轮优化：第一轮是参数级优化，把导航栈的膨胀半径、DWA权重、规划频率等标定好；第二轮是逻辑级优化，针对场景痛点在后处理上做文章。既能快速做出效果，又能展示对核心原理的理解，性价比远高于纯造轮子。

1.3 优化目标要有量化指标，否则改半天不知道好坏

路径规划优化最大的坑是“凭感觉调参”。我一开始也是这样，调完觉得“哎，路径好像顺了一点”，但具体顺了多少、快了多少，完全说不出来。后来我给自己定了一套量化评估指标，任何改动都必须落到数据上：

• 路径长度：全局规划生成的路径总长，单位m。越短说明基础效率越高。
• 规划耗时：从发出目标点到规划器返回路径的时间，单位ms。体现算法计算效率。
• 平滑度：用路径相邻线段夹角变化量的累计值衡量，值越小说明路径越平滑，方便下游跟踪。
• 动态避障成功率：在随机移动障碍物场景下，从起点到终点的无碰撞完成率。
• 局部规划稳定性：运动过程中速度指令震荡的次数，反映参数收敛程度。

在开始调参前，先跑通一套baseline记录原始数据。后面每次改动都对照同一张地图、同一组起终点来评估。没有对照组的数据，等于白做实验。我在项目里就把默认A的数据记成了baseline，后面优化A、RRT*、DWA改动全部跟它比，每一步提升都有据可查。

2. 全局与局部规划的核心细节

2.1 全局规划器选型：A还是RRT

全局规划是路径规划的“战略层”，负责把整条大路径先找出来。我在项目里重点对比了A和RRT两个方向。

A是一种基于栅格地图的启发式搜索算法，维护open和closed两个集合，用f(n) = g(n) + h(n)作为扩展顺序的依据。g(n)是起点到当前节点的实际代价，h(n)是当前节点到目标的预估代价，常用的有欧氏距离和曼哈顿距离。只要启发函数h(n)满足可采纳性（admissible），A找出的就是最优解。地图栅格化后，路径精度固定在一个栅格粒度上，但胜在稳定、可解释、调试直观，是工业界用得最多的全局方案。

RRT*则是采样类算法，在地图上随机采样节点，不断生长一棵树，并通过重新选择父节点（choose parent）和重布线（rewire）操作来逼近最优解。它的优势是能处理高维空间和复杂约束，比如机械臂的关节角限制、无人机的动力学约束等。缺点是结果随机性强，单次规划质量不稳定，耗时波动大，而且需要大量采样才能收敛到接近最优的解。

我在项目中做的是室内地面移动机器人，底盘基本满足“可原地旋转、低速运动”的条件，不涉及复杂动力学约束，所以最终选择A作为全局规划器主方案。RRT留作对照组，验证一下不同算法族在同样场景下的表现差异。这个选型逻辑可以总结为一句：问题有什么约束，就选匹配这个约束的算法，而不是选看起来最“高级”的算法。

2.2 代价地图与膨胀层：为什么机器人总是贴墙走

全局规划不是直接在占据栅格地图上搜，而是在代价地图（costmap）上搜。代价地图在占据栅格的基础上，把障碍物栅格向外膨胀，形成带梯度的代价区域，相当于告诉规划器“这里虽然没被占，但离墙太近，不安全”。

膨胀半径是整个优化里最敏感的全局参数之一。设置太小，路径离障碍物太近，甚至贴着墙走，机器人稍有控制误差就会蹭墙；设置太大，稍微窄一点的走廊就无法通过，规划器会直接报错“找不到路径”。在10m×10m的仿真地图里，我用0.25m的膨胀半径就能正常通行，但真机上底盘宽度再大一点就会卡在走廊。

在Nav2里，膨胀层由costmap_2d的inflation_layer实现，核心参数包括inflation_radius和cost_scaling_factor。cost_scaling_factor越小，代价值衰减越慢，路径会倾向离障碍物更远；cost_scaling_factor越大，代价值衰减很快，只有紧贴障碍物的地方才显示高代价。对于动态避障场景，costmap还需要同时维护obstacle_layer（障碍层）和static_layer（静态层），由传感器持续更新障碍层内容。

除了膨胀参数，我还做了一步额外优化：在全局路径点序列上，每隔0.5m检查一次路径点周围的障碍物距离，如果低于安全距离阈值，就在代价图上施加一个临时代价场，把路径“推”出去。这不改变A*主逻辑，纯粹是后处理，但能有效降低贴墙率和碰撞率。这个思路在调不通膨胀参数的时候尤其有用——与其跟一个参数死磕，不如加一道防御。

2.3 局部规划器对比：DWA的实时避障逻辑

局部规划器是路径规划的“战术层”，负责让机器人根据实时传感器数据，安全地跟踪全局路径。我用了DWA（Dynamic Window Approach）作为主局部规划器，同时对照了TEB。

DWA的原理可以归纳成三步：

1. 根据当前机器人运动模型，在速度空间里采样一组由线速度v和角速度w组成的候选速度对。
2. 对每个候选速度对模拟生成未来一小段时间的运动轨迹。
3. 用评价函数对每条轨迹打分，选出得分最高的轨迹对应的速度并执行。

DWA的评价函数通常包含三部分：方位角评价，衡量轨迹终点朝向与目标方向的夹角，越小越好；障碍距离评价，衡量轨迹距离最近障碍物的最小值，越大越好；速度评价，鼓励机器人保持较快的移动速度，避免龟速。

实际调参中最关键的两个参数是最大加速度和sim_time（模拟时间窗口）。sim_time太短，DWA看不到前方较远的障碍物，速度还没减下来就撞上了；sim_time太长，计算量增大，反应滞后。我用的经验公式是：sim_time约等于最大线速度的2到3倍，比如最大线速度0.5m/s，sim_time设1.5s左右，这样大约能预测前方0.75m的轨迹，留足刹车距离。当然这个值还要结合雷达最大探测距离来校准，雷达量程只有1m的话，sim_time再长也只能瞎猜。

3. 实操过程：从仿真到真机的完整实现

3.1 环境搭建与基础配置

开发环境我用的是Ubuntu 22.04 + ROS2 Humble + Nav2，仿真器用的Gazebo。如果只是为了做课程大作业或算法验证，用这套环境跑通完全够，不需要真机。有真机条件的话，重点是把底盘驱动和里程计接入ROS2，并且校准好雷达外参，这部分工作往往比算法本身更耗时。

跑导航之前，第一件事是检查TF树。路径规划对坐标系极其敏感，常见的坐标系包括map（全局地图坐标系）、odom（里程计坐标系）、base_link（机器人本体坐标系）、base_scan（雷达坐标系）。TF树有问题，最典型的表现是：建图的时候一切正常，一跑导航路径就乱飞，或者机器人在原地打转。

用slam_toolbox建图时，我踩过一个很典型的坑：推车速度太快，激光扫描数据变形，出来的地图走廊部分严重扭曲。后来把速度放慢，尤其在拐角和走廊位置来回扫了好几遍，地图质量才合格。地图质量直接决定后续所有规划效果，这一步值得多花时间。

3.2 Nav2参数配置：从默认参数开始优化

Nav2的参数文件很长，命名空间也多，初期很容易看晕。我的建议是不要从头学所有参数，而是专注两个核心模块：planner_server和controller_server。

全局规划器我用的是NavFn插件，这个插件底层就是A*的一种实现。参数上我优化了几个方向：

• tolerance：目标点附近允许的误差。设太大会导致机器人提前停车，离目标还有一段距离就认为到了；设太小则可能因为栅格离散化找不到合法路径。我最终设了0.2m。
• 启用了A*模式而不是默认的Dijkstra，后者虽然能保证最优，但搜索范围大，耗时长。
• 启发函数权重做了一个微调：把h(n)乘了一个略大于1的系数，让搜索更“贪心”一点。权重越大路径越直，但有可能陷入局部拐角；权重越小越接近Dijkstra的遍历式搜索，耗时长。这个系数我最终定在1.1左右，既保证规划速度快，路径质量又不至于下降太多。

controller_server这边用DWA插件，核心参数我刚才提到了，最大加速度、sim_time、三个评价项的权重。权重里我最终把障碍距离评价的权重调得比较高，宁可路径绕一点也要保证安全，这对动态避障场景很关键。

3.3 算法对比实验与数据记录

我在同一个仿真地图上做了对照实验。地图大小10m×10m，里面有12个不同尺寸的障碍物，起点在左上角，终点在右下角。每组方案跑20次，取平均数据。

调整后得到的实验记录如下：

参数优化后的A在路径长度和耗时上都有明显改善，碰撞失败次数也降下来了。RRT在这张10m×10m的栅格地图上并不占优，主要问题是采样耗时高、结果不稳定，它的优势场景还是应该选在高维连续空间。

这个对比证明了我在1.3节强调的观点：不要脱离场景谈算法优劣。A在栅格地图上就是比RRT更适合，哪怕后者听起来更“先进”。

3.4 动态避障场景测试

动态场景里，我在仿真环境中加入了一个随机移动的障碍物模型，模拟人来人往的走廊环境。全局路径被动态障碍物阻断时，DWA应该能实时绕开并回到全局路径上继续走。

实测下来，DWA能应对速度较慢的障碍物，大概低于机器人线速度一半时都还稳。但如果动态障碍物移动速度过快，DWA就会来不及绕行，最后触发急停。这个结果引发了一个更深的反思：动态避障不只是局部规划器的任务，还需要感知模块更早地发现障碍物，并且最好能在全局规划层面做重规划。只靠局部规划器“临场发挥”，永远是被动的。

所以我在后续版本里加了一个逻辑：当局部规划器绕行幅度超过一个阈值时，触发全局重规划，让全局路径也适应新的障碍物布局。这样局部规划和全局规划形成了协作关系，而不是各管各的。实测的动态避障成功率从71%提升到了89%，提升非常明显。

4. 常见问题与排查技巧

4.1 规划失败：明明地图能走，却报路径不存在

这个问题我遇到不下五次。排查下来，原因通常不外乎三个：

1. 目标点落在膨胀层内部。在rviz里点目标的时候，如果目标点附近有很红的代价区域，那路径基本必报失败。解决办法是挪一下目标点，或者调小膨胀半径。
2. 机器人当前位置在代价地图里被判定为“被困住”。建图的时候如果遗留了脏数据，或者传感器短暂遮挡，机器人起点附近会被判成不可通行区域。解决办法是清理地图范围，或者重启costmap。
3. 全局规划器的tolerance设置太小。目标点附近找不到合法栅格时，程序会直接放弃。把tolerance调到0.2m以上，大多数情况能缓解。

排查这类问题，我的固定套路是先在rviz里打开代价地图图层，用肉眼看一眼红色区域的分布。大多数路径规划问题和算法本身无关，而是地图和代价参数不匹配。

4.2 小车路线抖动，开起来像喝醉了

DWA参数不合理时，最典型的表现是机器人左右摇摆，走出的路径做S形震荡。原因通常是最大加速度设置过大——加速度一大，速度突变频繁，轨迹预测跟不上。另外，sim_time太短也会导致机器人走一步看一步，没有前瞻性。

解决办法按优先级排查：先把最大加速度降下来，给机器人“踩油门前先想一秒”的空间；再把sim_time调大一些，让轨迹预测覆盖更远的前方；最后调高障碍距离评价函数的权重，减少靠近障碍物时的急转。

4.3 明明局部路径空了，车还在原地转圈

这个问题初见时我很懵：rviz里看，全局路径是通畅的，局部代价地图也没有障碍，但小车就是原地打转不走。后来定位到问题在BT Navigator的任务流转机制上——当局部规划器连续多次无法跟踪全局路径时，BT Navigator会触发重规划，但重规划之间的等待时间如果过长，机器人就会在“尝试跟路、失败、等重规划、再尝试跟路”的循环里空转。

解决办法是把bt_navigator里面planning_retries的值适当调大，同时把controller_frequency提高一点，让局部规划器更频繁地刷新指令。这两个参数配合好，原地打转的问题基本能解决。

4.4 真机与仿真的差距：雷达噪点导致的误判

仿真环境里雷达数据很干净，一到真机就全变了。真机雷达在遇到玻璃、反光面、细腿椅子时会产生大量噪点，这些噪点在代价地图里变成“幽灵障碍物”，导致明明空旷的地方被判定为不可通行，机器人绕出奇怪的大弧线。

解决办法是在障码层的observation配置里，对激光数据做范围过滤和降采样。比如把最大探测距离设成5m，只关心近距离障碍物；打开inf_is_valid参数判定，把无效测量值过滤掉；还可以在costmap层里把obstacle_range和raytrace_range调到一个合理的值，别让远处噪点影响近处规划。

前面分享的这些坑，每条都是我真金白银踩出来的。最后再聊一点个人心得：做机器人路径规划优化，最大的障碍往往不是算法本身，而是坐标系、参数耦合、层级关系这些看着不起眼的工程细节。在Debug这件事上，80%的时间都花在了检查TF树、看代价地图、调参数上，真正写算法的时间不到两成。

如果你也在做类似的课程项目或毕业设计，我的三条建议是：先跑通一套最简单完整的流水线，再谈优化，不要一开始就追求多复杂的算法；每改一个参数就记录一次实验数据，否则你根本不知道是哪个改动让效果变好的；遇到诡异问题先怀疑TF和坐标，再怀疑算法，方向排查错了会浪费大量时间。

目前这个项目我还在继续往两个方向扩展：一个是把全局规划器换成SMAC这类支持非完整约束的规划器，让小车能以更平滑的曲率走完全程；另一个是引入基于学习的方法，让局部决策不再完全依赖手工调出来的规则权重，而是从数据里学出来。等这两个方向跑通了，再回来分享一轮新的数据和踩坑经验。