做机器人路径规划优化这个项目最初是人工智能课程的结课大作业。我本以为核心是算法一口气在仿真环境里加了动态避障、多目标点、实时重规划结果小车疯狂撞墙、原地打转、路径乱飘整套系统连一次完整的到达任务都完不成。后来复盘才发现问题根本不在某个算法不够好而是全局规划、局部规划、运动控制这三个层级被我混成了一锅粥。这篇东西就围绕机器人路径规划优化这条主线记录一次从需求拆解、算法选型、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)满足可采纳性admissibleA找出的就是最优解。地图栅格化后路径精度固定在一个栅格粒度上但胜在稳定、可解释、调试直观是工业界用得最多的全局方案。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的实时避障逻辑局部规划器是路径规划的“战术层”负责让机器人根据实时传感器数据安全地跟踪全局路径。我用了DWADynamic Window Approach作为主局部规划器同时对照了TEB。DWA的原理可以归纳成三步根据当前机器人运动模型在速度空间里采样一组由线速度v和角速度w组成的候选速度对。对每个候选速度对模拟生成未来一小段时间的运动轨迹。用评价函数对每条轨迹打分选出得分最高的轨迹对应的速度并执行。DWA的评价函数通常包含三部分方位角评价衡量轨迹终点朝向与目标方向的夹角越小越好障碍距离评价衡量轨迹距离最近障碍物的最小值越大越好速度评价鼓励机器人保持较快的移动速度避免龟速。实际调参中最关键的两个参数是最大加速度和sim_time模拟时间窗口。sim_time太短DWA看不到前方较远的障碍物速度还没减下来就撞上了sim_time太长计算量增大反应滞后。我用的经验公式是sim_time约等于最大线速度的2到3倍比如最大线速度0.5m/ssim_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次取平均数据。调整后得到的实验记录如下方案平均路径长度m平均规划耗时ms碰撞或失败次数默认A*14.6623参数优化后A*13.2361RRT*14.92105参数优化后的A在路径长度和耗时上都有明显改善碰撞失败次数也降下来了。RRT在这张10m×10m的栅格地图上并不占优主要问题是采样耗时高、结果不稳定它的优势场景还是应该选在高维连续空间。这个对比证明了我在1.3节强调的观点不要脱离场景谈算法优劣。A在栅格地图上就是比RRT更适合哪怕后者听起来更“先进”。3.4 动态避障场景测试动态场景里我在仿真环境中加入了一个随机移动的障碍物模型模拟人来人往的走廊环境。全局路径被动态障碍物阻断时DWA应该能实时绕开并回到全局路径上继续走。实测下来DWA能应对速度较慢的障碍物大概低于机器人线速度一半时都还稳。但如果动态障碍物移动速度过快DWA就会来不及绕行最后触发急停。这个结果引发了一个更深的反思动态避障不只是局部规划器的任务还需要感知模块更早地发现障碍物并且最好能在全局规划层面做重规划。只靠局部规划器“临场发挥”永远是被动的。所以我在后续版本里加了一个逻辑当局部规划器绕行幅度超过一个阈值时触发全局重规划让全局路径也适应新的障碍物布局。这样局部规划和全局规划形成了协作关系而不是各管各的。实测的动态避障成功率从71%提升到了89%提升非常明显。4. 常见问题与排查技巧4.1 规划失败明明地图能走却报路径不存在这个问题我遇到不下五次。排查下来原因通常不外乎三个目标点落在膨胀层内部。在rviz里点目标的时候如果目标点附近有很红的代价区域那路径基本必报失败。解决办法是挪一下目标点或者调小膨胀半径。机器人当前位置在代价地图里被判定为“被困住”。建图的时候如果遗留了脏数据或者传感器短暂遮挡机器人起点附近会被判成不可通行区域。解决办法是清理地图范围或者重启costmap。全局规划器的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这类支持非完整约束的规划器让小车能以更平滑的曲率走完全程另一个是引入基于学习的方法让局部决策不再完全依赖手工调出来的规则权重而是从数据里学出来。等这两个方向跑通了再回来分享一轮新的数据和踩坑经验。