接手一个移动机器人的导航项目时我最早花了很多时间只调 DWA认为局部避障做好了车就能跑起来。后来发现在几十米范围的仓库场景里光靠 DWA 根本走不到目标点——它只能看到周围一米左右的环境遇到长的障碍物带就绕不过去常常原地左右摇摆。搭上 A* 之后全局路径有了跑起来顺了很多再后来又遇到巷道边缘堆着临时托盘、行人乱走的状况A* 的栅格路径在动态变化的环境里不够稳于是把 RRT 也拉进来做全局备选。从“只会一个算法”到“三个算法分工配合”这个过程踩了不少坑也把每种算法的脾气摸清了。这篇内容算是自己在这个项目里的完整梳理适合正在做移动底盘导航、或者刚入门路径规划的开发者参考。我手里的测试平台是一台差速底盘的轮式机器人配单线激光雷达建图用的是常规栅格地图地图分辨率 0.05m整车尺寸 0.6m×0.7m最大速度 1.2m/s最大角速度 1.5rad/s。场景是一个 40m×50m 的模拟仓储库房有三条 1.8m 宽的巷道还有两处直角弯和一处狭窄门洞。后面讲到的参数、代码、数据都是从这套环境里来的你可以按自己的平台适当缩放。1. 路径规划系统里三个算法到底各自扛什么活1.1 项目背景与“单算法”的隐患先说结论A*、RRT、DWA 这三个东西不是同一个层级上的同类算法硬拿来对比谁好谁坏没有意义。它们解决的是不同尺度、不同约束下的问题。DWA 是局部规划器负责在当前时刻输出车的线速度和角速度A* 是全局规划器负责在已知静态地图里算出一条从起点到终点的大致路径RRT 也是全局规划器但它更适合高维空间、非结构化环境和快速避让动态变化区域。最初我接到这个任务时只用 DWA 做导航。DWA 本质上是一个速度空间采样算法它从当前速度出发枚举一小段时间内所有可能的“线速度角速度”组合用运动模型推演出未来一段轨迹再根据评分选择最优的一组速度去执行。听起来很合理实际一跑就发现问题DWA 的规划视野取决于速度采样时的模拟时间predict_time我一开始设成 1.5s最大速度 1.2m/s也就是它最多能看到未来 1.8m 左右的轨迹。而仓库里一堵墙可能有 8m 长车到了墙面前DWA 检测到前方障碍横向的候选速度又被减掉只能反复切换“左转/右转”在两个方向上摇摆最后停在原地报“无可行轨迹”。这不是参数没调好而是原理决定的局部规划器只看得到局部永远做不出“从墙的尽头绕过去”这种全局决策。要让车在真实场景里从 A 点移动到 B 点必须有一个更高层的规划器先画出一条完整路线DWA 再沿着这条路线做局部避障。这个认知花了两个礼拜才彻底想明白也解释了为什么业内大多数导航框架都是“全局规划 局部规划”分层架构。A* 在这个架构里承担全局选路的任务。它工作在栅格地图上把地图看成网格用广度优先搜索的思路从起点开始扩展节点但通过一个启发函数指导扩展方向。它保证在给定栅格地图上找到最短路径如果启发函数满足一致性要求这也是它在工程中受欢迎的原因之一——路径质量稳定、可重复、可解释。RRT 则是我在后期处理复杂场景时才加进来的。它不需要把地图网格化而是直接在连续空间里随机撒点然后从起点出发不断把新采样点连接到已有随机树上。它的实现简单且非常适合高维状态空间在地图分区不规则、包含动态障碍物或需要快速重新规划的场合RRT 比 A* 更快找到一条可行通道但路径长度通常不是最优的。它不是用来替代 A*而是作为全局规划层的另一条候选路径来源在某些场景下比 A* 更可靠。1.2 全局规划与局部规划的职责边界工程实现上我会把整个导航逻辑拆成三层任务层接收目标点维护当前状态机如待命、前往目标、任务完成全局规划层负责在栅格地图上计算全局路径每隔一段时间或检测到地图变化时重新规划局部规划层DWA以全局路径作为参考输出实时控制指令。全局规划和局部规划之间靠一个关键机制对接全局路径上的“局部目标点”。DWA 不是机械地追全局路径上的每一个点而是从当前机器人位置出发沿全局路径找一个距离机器人一定距离比如 2~3m的前瞻点把这个点作为当前局部目标。这样做的原因是全局路径通常很密集A* 在 0.05m 分辨率地图上算出的一小段路径可能包含几十上百个点如果 DWA 每个周期都试图精确逼近当前最近点会出现两个问题一是路径点之间的小角度变化会让 DWA 频繁加减速车走起来一顿一顿的二是如果全局路径在某个转角处有轻微锯齿DWA 可能在转角处反复对点导致轻微摆动。用前瞻点之后DWA 只关心“怎么在避开障碍的前提下到达前方那个点”全局路径的锯齿被天然过滤掉了。这个设计也符合人的开车间距意识——不是每一步都盯着眼前的车道线而是看着前方一个合适距离的参考点。这个分层结构还有一个好处如果全局规划算法要替换或者新增比如我要用 RRT 做备选路径只需要改全局规划层DWA 完全不需要动。我最初给全局规划层设计了一个统一接口输入是当前坐标、目标坐标、地图输出是PathPoint[]后续不管是 A* 还是 RRT 都实现同一个接口切换成本很低。这个项目经验让我养成了一个习惯做算法集成时先定义好数据接口再往上堆算法否则后期每加一个算法都要重写局部规划器工程量会成倍增加。2. 动态窗口法 DWA本地执行层的安全底线2.1 DWA 是怎么在速度空间里“找窗口”的DWA 全称 Dynamic Window Approach中文常译作“动态窗口法”核心思想非常直白每一时刻机器人能执行的速度并不是一个无限集合而是受到自身加速度和最大速度限制的“动态窗口”约束。在这个窗口内采一组离散速度模拟未来一段时间的轨迹然后按评分挑选最优速度执行。整个过程在实时控制周期内完成通常一轮计算耗时在 10ms 级完全满足控制频率要求。具体来说速度空间由三个约束裁剪机器人自身速度极限线速度不能超过v_max角速度不能超过w_max电机加减速能力当前速度v_cur在时间间隔dt内最多能变化v_acc * dt角速度同理这形成了速度值的动态下界和上界安全制动距离以当前候选速度模拟刹车如果在碰到障碍之前能停下来该速度才被允许否则速度组被剔除。经过这三层约束后剩下的合法速度组就叫“动态窗口”。接下来在这个窗口里按照一定分辨率通常线速度 0.01m/s、角速度 0.05rad/s 左右均匀采样。然后对每一组候选速度推演轨迹。对于一个差速底盘可以用下面的运动模型近似推演x_{k1} x_k v * cos(theta_k) * dt y_{k1} y_k v * sin(theta_k) * dt theta_{k1} theta_k w * dt这里v是线速度w是角速度theta是航向角。把这一步重复n次n predict_time / dt就得到一条未来轨迹。然后把轨迹上的每个点与障碍物地图做碰撞检测对可通行轨迹计算评分。评分函数是 DWA 最有意思的部分基本上每个工程落地时都要针对场景调整权重。最经典的评分公式是score alpha * heading beta * clearance gamma * velocityheading轨迹末端方向与目标方向的角度差越小分越高clearance轨迹离最近障碍物的距离距离越大分越高velocity鼓励保持较大线速度减少到达时间。但实际工程里我很少直接套用这三个权重。比如在一个窄巷道环境里如果beta障碍距离权重设置相对过大机器人会走到巷道正中间碰到只有一侧有障碍物的区域时轨迹会偏向另一侧空地上导致大幅绕行走出来的路径严重偏离全局路径。所以我更倾向于在评分前用全局路径做“目标点”约束用前面提到的前瞻点替代heading的单一目标方向。如果追求更平滑的路径还可以在评分项里加上“与上一时刻速度变化量”的惩罚项减少急加速急减速。这个项类似 PID 里的微分项score -lambda * (abs(v_candidate - v_current) abs(w_candidate - w_current))加了这项之后机器人实际控制指令更加平滑底盘电机的电流波动也小很多。代价是到达时间会长一点但这个代价在仓储环境下完全值得。2.2 自己实现 DWA 时的参数调参经验DWA 参数算不上多但每一项都很敏感。我自己在模拟仓库环境里调出来的参数可以供参考参数数值作用说明最大线速度1.2 m/s由底盘硬件决定最大角速度1.5 rad/s受限于转向能力线加速度0.8 m/s²电机加减速能力角加速度1.2 rad/s²电机加减速能力predict_time2.0 s规划视野时长模拟时间步长0.1 s轨迹推演步长线速度采样间隔0.02 m/s候选密度角速度采样间隔0.06 rad/s候选密度前瞻点距离2.5 m全局路径截取距离predict_time 的选择很关键。设置太小机器人视野短速度刚起来就发现前方障碍频繁急刹设置太大轨迹推演的点数增多计算量变大而且远离机器人的预测轨迹可信度低地图误差和动力学误差都会累积。我的经验是predict_time ≈ 2 * 前瞻距离 / 最大速度是一个不错的出发点比如前瞻 2.5m、最大速度 1.2m/s算出来约 2.1s取 2.0s 正好。v_max不能想当然地设成底盘标称最大速度。在一个 1.8m 宽的巷道里如果最大速度 1.2m/s直角转弯处 DWA 为了让轨迹安全通过会频繁选择低速转向实际平均速度可能只有 0.4m/s。这种情况下适当降速反而提升通过效率我后来在直角弯区域把限速降到了 0.6m/s整体任务时间反而缩短了。还有一点DWA 给出的线速度角速度指令需要低通滤波后再发给底盘否则底盘执行器响应很快会出现轻微的抖动。我在指令侧加了一个一阶低通滤波时间常数约 0.2s实测轨迹跟踪平滑性提升很明显。但要注意滤波时间常数不能太大否则会导致真实速度反馈滞后DWA 的碰撞检测防线会失效这在动态障碍多的场景里很危险。3. A* 与 RRT 的选型和组合细节3.1 A* 在栅格地图上的实现与性能优化A* 在栅格地图上的实现本质上是在节点图上做带启发式的最短路径搜索。我采用的栅格地图是 0.05m 分辨率40m×50m 的场景就是 800×1000 个栅格总共 80 万个节点。暴力搜索全部节点不现实所以 A* 的性能优化非常重要。A* 的核心公式是f(n) g(n) h(n)其中g(n)是起点走到当前节点n的实际代价h(n)是从当前节点到目标点的启发估计。启发函数的选择直接影响搜索速度和路径质量。我前前后后试过三种启发欧几里得距离sqrt((dx)^2 (dy)^2)计算代价略高路径方向性弱一些曼哈顿距离|dx| |dy|要求只能横竖移动在允许斜向移动的情况下会低估实际距离搜索范围偏大八方向距离切比雪夫距离max(|dx|, |dy|)适合允许八邻域移动的栅格图我最终选了这个搜索节点数量比曼哈顿少约 30%路径也更自然。A* 的性能瓶颈通常不在启发函数而在开放列表的维护。我用的是二叉堆priority queue每次从开放列表取出f值最小的节点这一步是 O(logN)。还有一个关键优化提前判断节点能否进入开放列表。常规做法是把当前节点能到达的邻居都先塞进去再由堆排序去筛选。但栅格地图里很多邻居会重复进入浪费大量时间和内存。我的做法是对每个节点维护一个best_g值只有当新的g比之前的best_g更小时才允许入堆否则直接丢弃。这个“懒删除”思路能大幅减少无用节点数量。实测下来在 80 万栅格的地图里从头到尾规划一条横穿全场的路径平均耗时从 280ms 降到了 90ms 左右。还有工程上很实用的两个小技巧。第一栅格地图在做碰撞检测时一定要先把机器人半径考虑进去对地图做膨胀处理。机器人半径 0.35m加上 0.1m 的安全余量就是 0.45m膨胀半径按 9 个栅格0.45 / 0.05处理。不做膨胀的后果是 A* 算出来的路径贴着墙走DWA 执行时发现障碍物距离不足频繁给“刹车”指令根本跑不起来。第二A* 的路径点密度极高在 0.05m 分辨率下1m 路径就有 20 个点。直接传给 DWA 会让局部规划器对每个点都做一次目标点匹配开销很大而且容易发生抖动。我在 A* 输出端做了抽稀处理保留路径方向变化大于 15° 的点和关键转折点这样一条 30m 的全局路径从 600 个点压到大概 80 个点DWA 的匹配压力小了很多。3.2 RRT 面向复杂巷道和变化环境的用法RRT 的全称是 Rapidly-exploring Random Tree中文常叫快速扩展随机树。它的原理比 A* 原始得多从起点开始建立一棵树每次在地图的可通行区域内随机采样一个点然后从树上找距离这个采样点最近的节点沿着两点连线方向扩展一段固定距离步长如果这段路径没有碰到障碍就把新节点加入树中。不断重复这个过程直到某个节点靠近目标点一定距离时回溯出一条从起点到目标的路径。我最早觉得 RRT 就是个“碰运气”的算法路径不可预测而且不够最优。直到遇到这种场景仓库里有几排货架货架之间形成窄巷道如果其中一个巷道被临时停放的托盘堵住了A* 在静态栅格地图上仍然会规划穿过这个巷道的路径因为地图上根本没有托盘。而使用 RRT 时如果我们在每次传感器发现新障碍后重新采样RRT 的随机性反而变成了优势——它会尝试各种其他通道哪怕它们不在原始 A* 路径上。RRT 的步长参数非常关键。步长越大每次扩展距离越远收敛越快但路径越粗糙甚至在窄通道采样时新节点会直接跨过走廊、嵌入对侧障碍物里。步长越小路径越精细但节点数量爆炸收敛时间变长。在 1.8m 宽的巷道环境里我用的步长是 0.3m这个值大约是巷道宽度的 1/6既能保证通过窄通道时不会“跨栏”又能控制扩展次数。另一个参数是提前停止的判断当树中某节点与目标点的距离小于goal_tolerance时就认为路径已找到。我设的goal_tolerance是 0.3m这防止了算法为了精确到达目标点而无限扩展。如果地图里目标点周围有障碍物还要检查最后一段路径是否真的无碰撞。RRT 生成路径后一般要做平滑。因为这个算法生成的路径由大量短线段组成转折角可能很大直接交给 DWA 跟踪会出现频繁减速。我用的平滑方式是对 RRT 路径做“剪枝”凡是两点之间的连线不碰障碍就跳过中间节点将这两点直接相连。重复这个过程直到无法再缩短然后再做一次三次样条平滑。这样处理之后RRT 路径的可跟踪性跟 A* 路径差别不大了。3.3 两者怎么选怎么互补A* 和 RRT 同属全局规划但在架构里承担的角色其实可以共存。我采用的方式是双全局路径源A* 是默认主路径因为它在静态地图上给出的是确定性的最短路径便于预测任务时间、避开已知大障碍。RRT 作为备选路径源在以下场合启用全局路径的终点附近被新障碍物阻断A* 重新规划后仍然走不通地图部分区域因为动态障碍出现明显变化需要快速生成一条替代路线目标点在人无法进入的狭窄空间A* 因网格离散化可能找不到路径RRT 的连续采样更擅长这种场景。在实际系统中我保留两条全局路径由上层逻辑根据代价选择。代价函数包括路径长度、预估通行耗时、以及路径与最近障碍物的距离。RRT 路径通常在长度上吃亏但在“绕开变化区域”这一点上更有价值。我把 A* 看成“稳妥的保守派”把 RRT 看成“灵活的冒险派”两者并存时绝大多数场景都能兜住。用表格简单总结一下它们的对比对比维度A*RRT工作空间栅格离散图连续或任意空间最优性栅格分辨率下最优不保证最优计算复杂度与栅格数量相关与迭代次数相关窄通道表现稳定但耗时长可能快速通过动态变化适应性需重新规划天然适合快速重规划路径平滑度较好仍有锯齿差需要后处理从实现角度看A* 是先算后走RRT 是边走边算快速重规划。两者的本质差异在于A* 依赖一张准确的全局代价图RRT 更依赖实时采样和碰撞检测器的速度。这在它们与 DWA 融合时会体现出截然不同的工程调性。4. 三算法融合落地全局规划 局部避障的工程方案4.1 融合框架从全局路径到局部目标点的传递现在说最核心的部分——怎么把三个算法安全地组合在一个系统里跑。一个最常见的错误是“主程序里先调 A*再在每一步循环里调 DWA完全不管中间状态”这样两个算法各做各的A* 的路径只是一根“看得见摸不着”的线DWA 走到一半发现路径被动态障碍挡住也不会重新请求 A* 规划结果就是机器人停在障碍前发呆。我的做法是引入一个“全局重规划条件”和“局部目标点管理模块”。整体控制循环可以抽象成这样的伪代码while task_active: if need_global_replan(): global_path choose_global_path(astar_path, rrt_path) path_smooth smooth_global_path(global_path) lookahead_point get_lookahead_point(robot_pose, path_smooth, lookahead_dist) v_cmd, w_cmd dwa_planner(robot_pose, lookahead_point, local_costmap) send_velocity(v_cmd, w_cmd)choose_global_path(astar_path, rrt_path)就是融合决策模块。我给它设定的规则很简单优先采用 A* 路径如果 A* 路径被动态障碍阻断再重新规划一次若重新规划后仍然阻断调用 RRT 生成新路径并在下一次控制周期里评估两条路径的代价选择可行且总代价较低的一条如果 RRT 找不到可行路径则上报“全局路径失败”请求操作员介入。get_lookahead_point是连接全局规划和 DWA 的纽带。这个函数接收机器人当前位置和完整全局路径返回一个距离当前位置指定距离的点。这里的距离不是欧几里得距离而是沿路径的累计弧长。因为如果直接用欧几里得距离选点在经过 U 形弯时会选到弯道对侧的路径点导致 DWA 目标点穿越障碍区。我用的是沿路径点逐个累计距离找到第一个累计弧长大于lookahead_dist的点。DWA 拿到这个局部目标点后它的作用就变得非常单纯在局部代价地图里车载激光雷达实时扫描数据与全局地图叠加算出避障轨迹并把车导向这个前瞻点。只要前瞻点选择合理DWA 的避障就和全局方向天然一致不会出现在错误的障碍侧绕行。工程上还有一个细节很值得写DWA 的代价地图与 A* 的全局地图的坐标系必须完全同步。设备启动时我们做了一次坐标标定把激光雷达坐标系、底盘坐标系、里程计坐标系全部转换到地图坐标系。之后每次全局重规划或 DWA 更新局部代价地图都从同一个坐标基准读取这省掉了很多在融合阶段极其隐蔽的“坐标系错位”bug。早期项目里DWA 局部地图漂移了 0.2m车在窄巷道里老是贴着墙跑排查了两天才发现是坐标变换多乘了一次车辆的偏航角。4.2 路径平滑为目的的可操作步骤全局路径直接给 DWA 用还是不够。A* 的路径有锯齿RRT 的路径更是瘸腿的折线。直接跟踪这种路径DWA 为了贴合每次的转向点会频繁减速严重时可能把平均速度从 1.0m/s 拉到 0.35m/s。所以我的融合层里有一个独立的“路径平滑”步骤。当初我尝试过很多平滑算法简单有效的方案是“梯度下降 三次样条”的组合。大致步骤这样对全局路径点做抽稀把路径点数量减少到原来的 1/5 左右对抽稀路径做碰撞检测如果某段抽稀后的连线穿过障碍物保留中途的关键节点对所有路径点做带障碍物约束的平滑迭代对平滑结果做三次样条插值得到稠密的、曲率连续的路径。平滑迭代的核心代码如下它其实就是一个带约束的梯度下降# 参数 iters 300 alpha 0.3 obs_cost 0.15 for it in range(iters): for i in range(1, len(path) - 1): prev_p path[i - 1] next_p path[i 1] cur_p path[i] # 向两点中点收敛 mid (prev_p next_p) / 2 grad (mid - cur_p) * alpha # 避开障碍物 if is_obstacle(cur_p): grad (cur_p - nearest_obstacle(cur_p)) * obs_cost path[i] grad这里有一个重要细节平滑迭代前必须先把路径抽稀否则连续相邻的路径点会因为彼此距离太近导致步长和障碍代价相互干扰平滑结果反而变得扭曲。我抽稀后相邻点之间的最小距离保持在 0.2m 左右迭代收敛很稳定。迭代次数不是越多越好。400 次以后路径基本定型再多迭代也不会明显改善反而可能因为反复拉扯而出现轻微过冲导致路径压到障碍物边缘。所以在迭代过程中我会持续检查平滑后路径与障碍物的最近距离一旦小于安全阈值就停止迭代并回退到上一版路径——这个保护机制在实际调试中救过我很多次。平滑完成后再用三次样条插值把路径均匀加密到每 0.1m 一个点。DWA 拿到的是这样一条“顺滑的、连续的、可跟踪的”路径。从 A*/RRT 粗糙输出到 DWA 可用输入质量提升非常明显实测平均跟踪误差从 0.18m 降到 0.06m。4.3 融合实测效果与关键对比数据我在模拟仓储环境里做了一组对照实验用同一台底盘、同一张地图、同一组任务点分别跑了以下几种配置配置平均速度任务完成时间碰到障碍次数全局重规划次数仅 DWA0.28 m/s未完成多次0A* DWA0.72 m/s128s08次RRT DWA0.65 m/s142s017次A* RRT DWA0.78 m/s115s05次数据说明几个问题。仅 DWA 跑不完全程因为它在长墙前反复摇摆。A* DWA 配置下平均速度已经可以接受但重规划次数偏多——原因是 A* 规划出的最短路径要经过一个很窄的门洞而门洞附近经常有临时堆放的箱子DWA 判断无法通过于是频繁触发全局重规划。这个场景正是 RRT 发挥作用的地方加进来之后遇到障碍物时 RRT 快速生成另一条绕行路径全局重规划次数反而下降到 5 次而且被选中路径并不总是 A* 的路径——有时候是通过另一个巷道多绕了 6m 但是更稳妥的那条。融合配置的任务时间从 128s 降到 115s主要收益来自减少“无效重规划 原地等待”。A* 每次重规划耗时大约 90msRRT 每次约 40ms等待本身并不算长但每次重规划后 DWA 都要重新确定前瞻点、重新加速这个“重新建立运动状态”的过程才是时间损耗的大头。从这个角度看全局规划层的冗余反而提升了系统整体效率。还有一个值得注意的指标碰撞次数。三种配置下都是 0 次这说明 DWA 在面对未知障碍物时确实可靠。DWA 的职责边界很清晰——它不负责“绕远路”它只负责“别撞上”。全局规划层给了它一个绕行的机会DWA 只需要在局部做精细避让两者协作后系统的安全底线和通行效率都有保障。5. 实战排坑与避坑经验5.1 常见问题速查表调这三个算法时目标行为怪异但出错点往往不在算法本身而在输入数据和参数之间。我在自己的项目里总结了一个问题速查表基本覆盖了遇到的大多数情况。现象最可能原因解决方案DWA 原地摇摆全局路径缺失或前瞻点太近检查 lookahead_dist上调到 2.5~3.0mA* 规划极慢开放列表重复节点过多加 best_g 防止重复入堆A* 路径贴墙没有进行地图膨胀按机器人半径安全余量膨胀地图RRT 迟迟找不到路径步长过大窄通道内采样失效缩短步长至巷道宽度 1/6 左右RRT 路径过弯太大节点回溯后未做剪枝和平滑增加剪枝和三次样条平滑融合后车突然急刹前瞻点跨过障碍区用累计弧长选点而非欧几里得距离局部地图漂移坐标变换额外乘了偏航角检查 tf 树和坐标系变换DWA 在转弯处减速明显速度权重太低或最大速度过大适当降低该区域限速降低速度权重A* 路径被废弃RRT 频繁接管动态障碍在 A* 路径附近检查地图刷新频率加大局部代价地图更新频率车绕大圈走远路平滑迭代把路径扯变形抑制过冲增加碰撞检测保护这个表格里每个问题都是我实际调过、修过的不是网上随便抄来的。比如“DWA 原地摇摆”这个现象最有迷惑性第一眼看着像参数问题实际上往往不是 DWA 的问题——你去看 DWA 日志会发现它每个周期都在选“当前最好”的速度只是那个“最好”一直在左右横跳。真正原因是全局路径在墙前面断了或者前瞻点选到了障碍物上。把这几行排查逻辑记住能省下大量调参时间。5.2 我踩过三次以上的坑第一个大概率会踩的坑忽略模拟器与真机的动力学差异。我在模拟仓库里把 DWA 的加速度设得很大因为仿真模型的响应是完美的顺利跑通。到真机上底盘电机的响应延迟和摩擦非线性导致实际加速度明显低于设定值结果 DWA 模拟出来的轨迹与真实轨迹不一致碰撞检测形同虚设。后来我在每个速度周期里加入了真实底盘速度反馈的闭环补偿DWA 模拟所用起始速度不再取“上次指令速度”而是取“当前里程计反馈速度”这从原理上保证了运动模型和真实状态同步。真机效果瞬间正常了。第二个反复踩的坑是“地图刷新频率与全局重规划之间的时间竞赛”。初版系统里激光雷达以 10Hz 更新局部代价地图全局规划层每 5s 才检查一次是否需要重规划。当动态障碍突然出现在全局路径上时DWA 第一个发现并避让但全局规划层不知道路径已经失效还在用旧路径引导。这导致一个很别扭的状态DWA 明明绕开了障碍全局路径却还在障碍上下次前瞻点又会把车拉回去车像“画圈”似的来回绕。解决办法是把“DWA 连续 N 次检测到前方无障碍但无法沿全局路径前进”作为全局重规划的触发条件而不是单纯按时间轮询。这个改动让整个系统在动态场景下的表现稳定了很多。第三个坑属于工程素养层面的不要试图在一个功能里同时改三个算法。我在早期融合时每次遇到问题就顺手调一下 A* 的启发代价、再改一改 RRT 的步长、再动一下 DWA 的权重结果根本分不清是哪个改动导致的出了新问题也无法定位。后来我给自己定了一个死规矩每次只改一个参数改完跑一遍基准测试记录行为变化再改下一个。虽然过程慢但排查效率非常高很多之前积累的参数选择经验都是靠这个习惯沉淀下来的。做路径规划算法融合真正的难点不在算法本身而在“把三个算法各自的状态、触发条件、参数影响边界梳理清楚”这个系统性工程。如果让我给新人一个直接的建议先把 DWA 单独调到能在一个简单的矩形场地里稳定绕开临时障碍再把 A* 或 RRT 单独调好最后才考虑融合融合时也要先从最简单的一个全局路径源 DWA 的组合入手跑通了再叠加冗余路径源。这个顺序能避开 80% 的坑。最后再分享一个小技巧每个规划周期里把当前选择的速度、轨迹、全局路径、前方障碍物距离这些关键数据全部可视化出来。你不需要画多精致的图只要能同时看到“全局路径”和“DWA 当前候选轨迹”这两条线融合逻辑里大部分诡异行为都会一目了然。我调试过程中有一半的问题都是靠盯着一张实时轨迹图瞬间看明白的比看任何日志都快。 移动机器人路径规划：A*、RRT、DWA三种算法融合实战解析
接手一个移动机器人的导航项目时我最早花了很多时间只调 DWA认为局部避障做好了车就能跑起来。后来发现在几十米范围的仓库场景里光靠 DWA 根本走不到目标点——它只能看到周围一米左右的环境遇到长的障碍物带就绕不过去常常原地左右摇摆。搭上 A* 之后全局路径有了跑起来顺了很多再后来又遇到巷道边缘堆着临时托盘、行人乱走的状况A* 的栅格路径在动态变化的环境里不够稳于是把 RRT 也拉进来做全局备选。从“只会一个算法”到“三个算法分工配合”这个过程踩了不少坑也把每种算法的脾气摸清了。这篇内容算是自己在这个项目里的完整梳理适合正在做移动底盘导航、或者刚入门路径规划的开发者参考。我手里的测试平台是一台差速底盘的轮式机器人配单线激光雷达建图用的是常规栅格地图地图分辨率 0.05m整车尺寸 0.6m×0.7m最大速度 1.2m/s最大角速度 1.5rad/s。场景是一个 40m×50m 的模拟仓储库房有三条 1.8m 宽的巷道还有两处直角弯和一处狭窄门洞。后面讲到的参数、代码、数据都是从这套环境里来的你可以按自己的平台适当缩放。1. 路径规划系统里三个算法到底各自扛什么活1.1 项目背景与“单算法”的隐患先说结论A*、RRT、DWA 这三个东西不是同一个层级上的同类算法硬拿来对比谁好谁坏没有意义。它们解决的是不同尺度、不同约束下的问题。DWA 是局部规划器负责在当前时刻输出车的线速度和角速度A* 是全局规划器负责在已知静态地图里算出一条从起点到终点的大致路径RRT 也是全局规划器但它更适合高维空间、非结构化环境和快速避让动态变化区域。最初我接到这个任务时只用 DWA 做导航。DWA 本质上是一个速度空间采样算法它从当前速度出发枚举一小段时间内所有可能的“线速度角速度”组合用运动模型推演出未来一段轨迹再根据评分选择最优的一组速度去执行。听起来很合理实际一跑就发现问题DWA 的规划视野取决于速度采样时的模拟时间predict_time我一开始设成 1.5s最大速度 1.2m/s也就是它最多能看到未来 1.8m 左右的轨迹。而仓库里一堵墙可能有 8m 长车到了墙面前DWA 检测到前方障碍横向的候选速度又被减掉只能反复切换“左转/右转”在两个方向上摇摆最后停在原地报“无可行轨迹”。这不是参数没调好而是原理决定的局部规划器只看得到局部永远做不出“从墙的尽头绕过去”这种全局决策。要让车在真实场景里从 A 点移动到 B 点必须有一个更高层的规划器先画出一条完整路线DWA 再沿着这条路线做局部避障。这个认知花了两个礼拜才彻底想明白也解释了为什么业内大多数导航框架都是“全局规划 局部规划”分层架构。A* 在这个架构里承担全局选路的任务。它工作在栅格地图上把地图看成网格用广度优先搜索的思路从起点开始扩展节点但通过一个启发函数指导扩展方向。它保证在给定栅格地图上找到最短路径如果启发函数满足一致性要求这也是它在工程中受欢迎的原因之一——路径质量稳定、可重复、可解释。RRT 则是我在后期处理复杂场景时才加进来的。它不需要把地图网格化而是直接在连续空间里随机撒点然后从起点出发不断把新采样点连接到已有随机树上。它的实现简单且非常适合高维状态空间在地图分区不规则、包含动态障碍物或需要快速重新规划的场合RRT 比 A* 更快找到一条可行通道但路径长度通常不是最优的。它不是用来替代 A*而是作为全局规划层的另一条候选路径来源在某些场景下比 A* 更可靠。1.2 全局规划与局部规划的职责边界工程实现上我会把整个导航逻辑拆成三层任务层接收目标点维护当前状态机如待命、前往目标、任务完成全局规划层负责在栅格地图上计算全局路径每隔一段时间或检测到地图变化时重新规划局部规划层DWA以全局路径作为参考输出实时控制指令。全局规划和局部规划之间靠一个关键机制对接全局路径上的“局部目标点”。DWA 不是机械地追全局路径上的每一个点而是从当前机器人位置出发沿全局路径找一个距离机器人一定距离比如 2~3m的前瞻点把这个点作为当前局部目标。这样做的原因是全局路径通常很密集A* 在 0.05m 分辨率地图上算出的一小段路径可能包含几十上百个点如果 DWA 每个周期都试图精确逼近当前最近点会出现两个问题一是路径点之间的小角度变化会让 DWA 频繁加减速车走起来一顿一顿的二是如果全局路径在某个转角处有轻微锯齿DWA 可能在转角处反复对点导致轻微摆动。用前瞻点之后DWA 只关心“怎么在避开障碍的前提下到达前方那个点”全局路径的锯齿被天然过滤掉了。这个设计也符合人的开车间距意识——不是每一步都盯着眼前的车道线而是看着前方一个合适距离的参考点。这个分层结构还有一个好处如果全局规划算法要替换或者新增比如我要用 RRT 做备选路径只需要改全局规划层DWA 完全不需要动。我最初给全局规划层设计了一个统一接口输入是当前坐标、目标坐标、地图输出是PathPoint[]后续不管是 A* 还是 RRT 都实现同一个接口切换成本很低。这个项目经验让我养成了一个习惯做算法集成时先定义好数据接口再往上堆算法否则后期每加一个算法都要重写局部规划器工程量会成倍增加。2. 动态窗口法 DWA本地执行层的安全底线2.1 DWA 是怎么在速度空间里“找窗口”的DWA 全称 Dynamic Window Approach中文常译作“动态窗口法”核心思想非常直白每一时刻机器人能执行的速度并不是一个无限集合而是受到自身加速度和最大速度限制的“动态窗口”约束。在这个窗口内采一组离散速度模拟未来一段时间的轨迹然后按评分挑选最优速度执行。整个过程在实时控制周期内完成通常一轮计算耗时在 10ms 级完全满足控制频率要求。具体来说速度空间由三个约束裁剪机器人自身速度极限线速度不能超过v_max角速度不能超过w_max电机加减速能力当前速度v_cur在时间间隔dt内最多能变化v_acc * dt角速度同理这形成了速度值的动态下界和上界安全制动距离以当前候选速度模拟刹车如果在碰到障碍之前能停下来该速度才被允许否则速度组被剔除。经过这三层约束后剩下的合法速度组就叫“动态窗口”。接下来在这个窗口里按照一定分辨率通常线速度 0.01m/s、角速度 0.05rad/s 左右均匀采样。然后对每一组候选速度推演轨迹。对于一个差速底盘可以用下面的运动模型近似推演x_{k1} x_k v * cos(theta_k) * dt y_{k1} y_k v * sin(theta_k) * dt theta_{k1} theta_k w * dt这里v是线速度w是角速度theta是航向角。把这一步重复n次n predict_time / dt就得到一条未来轨迹。然后把轨迹上的每个点与障碍物地图做碰撞检测对可通行轨迹计算评分。评分函数是 DWA 最有意思的部分基本上每个工程落地时都要针对场景调整权重。最经典的评分公式是score alpha * heading beta * clearance gamma * velocityheading轨迹末端方向与目标方向的角度差越小分越高clearance轨迹离最近障碍物的距离距离越大分越高velocity鼓励保持较大线速度减少到达时间。但实际工程里我很少直接套用这三个权重。比如在一个窄巷道环境里如果beta障碍距离权重设置相对过大机器人会走到巷道正中间碰到只有一侧有障碍物的区域时轨迹会偏向另一侧空地上导致大幅绕行走出来的路径严重偏离全局路径。所以我更倾向于在评分前用全局路径做“目标点”约束用前面提到的前瞻点替代heading的单一目标方向。如果追求更平滑的路径还可以在评分项里加上“与上一时刻速度变化量”的惩罚项减少急加速急减速。这个项类似 PID 里的微分项score -lambda * (abs(v_candidate - v_current) abs(w_candidate - w_current))加了这项之后机器人实际控制指令更加平滑底盘电机的电流波动也小很多。代价是到达时间会长一点但这个代价在仓储环境下完全值得。2.2 自己实现 DWA 时的参数调参经验DWA 参数算不上多但每一项都很敏感。我自己在模拟仓库环境里调出来的参数可以供参考参数数值作用说明最大线速度1.2 m/s由底盘硬件决定最大角速度1.5 rad/s受限于转向能力线加速度0.8 m/s²电机加减速能力角加速度1.2 rad/s²电机加减速能力predict_time2.0 s规划视野时长模拟时间步长0.1 s轨迹推演步长线速度采样间隔0.02 m/s候选密度角速度采样间隔0.06 rad/s候选密度前瞻点距离2.5 m全局路径截取距离predict_time 的选择很关键。设置太小机器人视野短速度刚起来就发现前方障碍频繁急刹设置太大轨迹推演的点数增多计算量变大而且远离机器人的预测轨迹可信度低地图误差和动力学误差都会累积。我的经验是predict_time ≈ 2 * 前瞻距离 / 最大速度是一个不错的出发点比如前瞻 2.5m、最大速度 1.2m/s算出来约 2.1s取 2.0s 正好。v_max不能想当然地设成底盘标称最大速度。在一个 1.8m 宽的巷道里如果最大速度 1.2m/s直角转弯处 DWA 为了让轨迹安全通过会频繁选择低速转向实际平均速度可能只有 0.4m/s。这种情况下适当降速反而提升通过效率我后来在直角弯区域把限速降到了 0.6m/s整体任务时间反而缩短了。还有一点DWA 给出的线速度角速度指令需要低通滤波后再发给底盘否则底盘执行器响应很快会出现轻微的抖动。我在指令侧加了一个一阶低通滤波时间常数约 0.2s实测轨迹跟踪平滑性提升很明显。但要注意滤波时间常数不能太大否则会导致真实速度反馈滞后DWA 的碰撞检测防线会失效这在动态障碍多的场景里很危险。3. A* 与 RRT 的选型和组合细节3.1 A* 在栅格地图上的实现与性能优化A* 在栅格地图上的实现本质上是在节点图上做带启发式的最短路径搜索。我采用的栅格地图是 0.05m 分辨率40m×50m 的场景就是 800×1000 个栅格总共 80 万个节点。暴力搜索全部节点不现实所以 A* 的性能优化非常重要。A* 的核心公式是f(n) g(n) h(n)其中g(n)是起点走到当前节点n的实际代价h(n)是从当前节点到目标点的启发估计。启发函数的选择直接影响搜索速度和路径质量。我前前后后试过三种启发欧几里得距离sqrt((dx)^2 (dy)^2)计算代价略高路径方向性弱一些曼哈顿距离|dx| |dy|要求只能横竖移动在允许斜向移动的情况下会低估实际距离搜索范围偏大八方向距离切比雪夫距离max(|dx|, |dy|)适合允许八邻域移动的栅格图我最终选了这个搜索节点数量比曼哈顿少约 30%路径也更自然。A* 的性能瓶颈通常不在启发函数而在开放列表的维护。我用的是二叉堆priority queue每次从开放列表取出f值最小的节点这一步是 O(logN)。还有一个关键优化提前判断节点能否进入开放列表。常规做法是把当前节点能到达的邻居都先塞进去再由堆排序去筛选。但栅格地图里很多邻居会重复进入浪费大量时间和内存。我的做法是对每个节点维护一个best_g值只有当新的g比之前的best_g更小时才允许入堆否则直接丢弃。这个“懒删除”思路能大幅减少无用节点数量。实测下来在 80 万栅格的地图里从头到尾规划一条横穿全场的路径平均耗时从 280ms 降到了 90ms 左右。还有工程上很实用的两个小技巧。第一栅格地图在做碰撞检测时一定要先把机器人半径考虑进去对地图做膨胀处理。机器人半径 0.35m加上 0.1m 的安全余量就是 0.45m膨胀半径按 9 个栅格0.45 / 0.05处理。不做膨胀的后果是 A* 算出来的路径贴着墙走DWA 执行时发现障碍物距离不足频繁给“刹车”指令根本跑不起来。第二A* 的路径点密度极高在 0.05m 分辨率下1m 路径就有 20 个点。直接传给 DWA 会让局部规划器对每个点都做一次目标点匹配开销很大而且容易发生抖动。我在 A* 输出端做了抽稀处理保留路径方向变化大于 15° 的点和关键转折点这样一条 30m 的全局路径从 600 个点压到大概 80 个点DWA 的匹配压力小了很多。3.2 RRT 面向复杂巷道和变化环境的用法RRT 的全称是 Rapidly-exploring Random Tree中文常叫快速扩展随机树。它的原理比 A* 原始得多从起点开始建立一棵树每次在地图的可通行区域内随机采样一个点然后从树上找距离这个采样点最近的节点沿着两点连线方向扩展一段固定距离步长如果这段路径没有碰到障碍就把新节点加入树中。不断重复这个过程直到某个节点靠近目标点一定距离时回溯出一条从起点到目标的路径。我最早觉得 RRT 就是个“碰运气”的算法路径不可预测而且不够最优。直到遇到这种场景仓库里有几排货架货架之间形成窄巷道如果其中一个巷道被临时停放的托盘堵住了A* 在静态栅格地图上仍然会规划穿过这个巷道的路径因为地图上根本没有托盘。而使用 RRT 时如果我们在每次传感器发现新障碍后重新采样RRT 的随机性反而变成了优势——它会尝试各种其他通道哪怕它们不在原始 A* 路径上。RRT 的步长参数非常关键。步长越大每次扩展距离越远收敛越快但路径越粗糙甚至在窄通道采样时新节点会直接跨过走廊、嵌入对侧障碍物里。步长越小路径越精细但节点数量爆炸收敛时间变长。在 1.8m 宽的巷道环境里我用的步长是 0.3m这个值大约是巷道宽度的 1/6既能保证通过窄通道时不会“跨栏”又能控制扩展次数。另一个参数是提前停止的判断当树中某节点与目标点的距离小于goal_tolerance时就认为路径已找到。我设的goal_tolerance是 0.3m这防止了算法为了精确到达目标点而无限扩展。如果地图里目标点周围有障碍物还要检查最后一段路径是否真的无碰撞。RRT 生成路径后一般要做平滑。因为这个算法生成的路径由大量短线段组成转折角可能很大直接交给 DWA 跟踪会出现频繁减速。我用的平滑方式是对 RRT 路径做“剪枝”凡是两点之间的连线不碰障碍就跳过中间节点将这两点直接相连。重复这个过程直到无法再缩短然后再做一次三次样条平滑。这样处理之后RRT 路径的可跟踪性跟 A* 路径差别不大了。3.3 两者怎么选怎么互补A* 和 RRT 同属全局规划但在架构里承担的角色其实可以共存。我采用的方式是双全局路径源A* 是默认主路径因为它在静态地图上给出的是确定性的最短路径便于预测任务时间、避开已知大障碍。RRT 作为备选路径源在以下场合启用全局路径的终点附近被新障碍物阻断A* 重新规划后仍然走不通地图部分区域因为动态障碍出现明显变化需要快速生成一条替代路线目标点在人无法进入的狭窄空间A* 因网格离散化可能找不到路径RRT 的连续采样更擅长这种场景。在实际系统中我保留两条全局路径由上层逻辑根据代价选择。代价函数包括路径长度、预估通行耗时、以及路径与最近障碍物的距离。RRT 路径通常在长度上吃亏但在“绕开变化区域”这一点上更有价值。我把 A* 看成“稳妥的保守派”把 RRT 看成“灵活的冒险派”两者并存时绝大多数场景都能兜住。用表格简单总结一下它们的对比对比维度A*RRT工作空间栅格离散图连续或任意空间最优性栅格分辨率下最优不保证最优计算复杂度与栅格数量相关与迭代次数相关窄通道表现稳定但耗时长可能快速通过动态变化适应性需重新规划天然适合快速重规划路径平滑度较好仍有锯齿差需要后处理从实现角度看A* 是先算后走RRT 是边走边算快速重规划。两者的本质差异在于A* 依赖一张准确的全局代价图RRT 更依赖实时采样和碰撞检测器的速度。这在它们与 DWA 融合时会体现出截然不同的工程调性。4. 三算法融合落地全局规划 局部避障的工程方案4.1 融合框架从全局路径到局部目标点的传递现在说最核心的部分——怎么把三个算法安全地组合在一个系统里跑。一个最常见的错误是“主程序里先调 A*再在每一步循环里调 DWA完全不管中间状态”这样两个算法各做各的A* 的路径只是一根“看得见摸不着”的线DWA 走到一半发现路径被动态障碍挡住也不会重新请求 A* 规划结果就是机器人停在障碍前发呆。我的做法是引入一个“全局重规划条件”和“局部目标点管理模块”。整体控制循环可以抽象成这样的伪代码while task_active: if need_global_replan(): global_path choose_global_path(astar_path, rrt_path) path_smooth smooth_global_path(global_path) lookahead_point get_lookahead_point(robot_pose, path_smooth, lookahead_dist) v_cmd, w_cmd dwa_planner(robot_pose, lookahead_point, local_costmap) send_velocity(v_cmd, w_cmd)choose_global_path(astar_path, rrt_path)就是融合决策模块。我给它设定的规则很简单优先采用 A* 路径如果 A* 路径被动态障碍阻断再重新规划一次若重新规划后仍然阻断调用 RRT 生成新路径并在下一次控制周期里评估两条路径的代价选择可行且总代价较低的一条如果 RRT 找不到可行路径则上报“全局路径失败”请求操作员介入。get_lookahead_point是连接全局规划和 DWA 的纽带。这个函数接收机器人当前位置和完整全局路径返回一个距离当前位置指定距离的点。这里的距离不是欧几里得距离而是沿路径的累计弧长。因为如果直接用欧几里得距离选点在经过 U 形弯时会选到弯道对侧的路径点导致 DWA 目标点穿越障碍区。我用的是沿路径点逐个累计距离找到第一个累计弧长大于lookahead_dist的点。DWA 拿到这个局部目标点后它的作用就变得非常单纯在局部代价地图里车载激光雷达实时扫描数据与全局地图叠加算出避障轨迹并把车导向这个前瞻点。只要前瞻点选择合理DWA 的避障就和全局方向天然一致不会出现在错误的障碍侧绕行。工程上还有一个细节很值得写DWA 的代价地图与 A* 的全局地图的坐标系必须完全同步。设备启动时我们做了一次坐标标定把激光雷达坐标系、底盘坐标系、里程计坐标系全部转换到地图坐标系。之后每次全局重规划或 DWA 更新局部代价地图都从同一个坐标基准读取这省掉了很多在融合阶段极其隐蔽的“坐标系错位”bug。早期项目里DWA 局部地图漂移了 0.2m车在窄巷道里老是贴着墙跑排查了两天才发现是坐标变换多乘了一次车辆的偏航角。4.2 路径平滑为目的的可操作步骤全局路径直接给 DWA 用还是不够。A* 的路径有锯齿RRT 的路径更是瘸腿的折线。直接跟踪这种路径DWA 为了贴合每次的转向点会频繁减速严重时可能把平均速度从 1.0m/s 拉到 0.35m/s。所以我的融合层里有一个独立的“路径平滑”步骤。当初我尝试过很多平滑算法简单有效的方案是“梯度下降 三次样条”的组合。大致步骤这样对全局路径点做抽稀把路径点数量减少到原来的 1/5 左右对抽稀路径做碰撞检测如果某段抽稀后的连线穿过障碍物保留中途的关键节点对所有路径点做带障碍物约束的平滑迭代对平滑结果做三次样条插值得到稠密的、曲率连续的路径。平滑迭代的核心代码如下它其实就是一个带约束的梯度下降# 参数 iters 300 alpha 0.3 obs_cost 0.15 for it in range(iters): for i in range(1, len(path) - 1): prev_p path[i - 1] next_p path[i 1] cur_p path[i] # 向两点中点收敛 mid (prev_p next_p) / 2 grad (mid - cur_p) * alpha # 避开障碍物 if is_obstacle(cur_p): grad (cur_p - nearest_obstacle(cur_p)) * obs_cost path[i] grad这里有一个重要细节平滑迭代前必须先把路径抽稀否则连续相邻的路径点会因为彼此距离太近导致步长和障碍代价相互干扰平滑结果反而变得扭曲。我抽稀后相邻点之间的最小距离保持在 0.2m 左右迭代收敛很稳定。迭代次数不是越多越好。400 次以后路径基本定型再多迭代也不会明显改善反而可能因为反复拉扯而出现轻微过冲导致路径压到障碍物边缘。所以在迭代过程中我会持续检查平滑后路径与障碍物的最近距离一旦小于安全阈值就停止迭代并回退到上一版路径——这个保护机制在实际调试中救过我很多次。平滑完成后再用三次样条插值把路径均匀加密到每 0.1m 一个点。DWA 拿到的是这样一条“顺滑的、连续的、可跟踪的”路径。从 A*/RRT 粗糙输出到 DWA 可用输入质量提升非常明显实测平均跟踪误差从 0.18m 降到 0.06m。4.3 融合实测效果与关键对比数据我在模拟仓储环境里做了一组对照实验用同一台底盘、同一张地图、同一组任务点分别跑了以下几种配置配置平均速度任务完成时间碰到障碍次数全局重规划次数仅 DWA0.28 m/s未完成多次0A* DWA0.72 m/s128s08次RRT DWA0.65 m/s142s017次A* RRT DWA0.78 m/s115s05次数据说明几个问题。仅 DWA 跑不完全程因为它在长墙前反复摇摆。A* DWA 配置下平均速度已经可以接受但重规划次数偏多——原因是 A* 规划出的最短路径要经过一个很窄的门洞而门洞附近经常有临时堆放的箱子DWA 判断无法通过于是频繁触发全局重规划。这个场景正是 RRT 发挥作用的地方加进来之后遇到障碍物时 RRT 快速生成另一条绕行路径全局重规划次数反而下降到 5 次而且被选中路径并不总是 A* 的路径——有时候是通过另一个巷道多绕了 6m 但是更稳妥的那条。融合配置的任务时间从 128s 降到 115s主要收益来自减少“无效重规划 原地等待”。A* 每次重规划耗时大约 90msRRT 每次约 40ms等待本身并不算长但每次重规划后 DWA 都要重新确定前瞻点、重新加速这个“重新建立运动状态”的过程才是时间损耗的大头。从这个角度看全局规划层的冗余反而提升了系统整体效率。还有一个值得注意的指标碰撞次数。三种配置下都是 0 次这说明 DWA 在面对未知障碍物时确实可靠。DWA 的职责边界很清晰——它不负责“绕远路”它只负责“别撞上”。全局规划层给了它一个绕行的机会DWA 只需要在局部做精细避让两者协作后系统的安全底线和通行效率都有保障。5. 实战排坑与避坑经验5.1 常见问题速查表调这三个算法时目标行为怪异但出错点往往不在算法本身而在输入数据和参数之间。我在自己的项目里总结了一个问题速查表基本覆盖了遇到的大多数情况。现象最可能原因解决方案DWA 原地摇摆全局路径缺失或前瞻点太近检查 lookahead_dist上调到 2.5~3.0mA* 规划极慢开放列表重复节点过多加 best_g 防止重复入堆A* 路径贴墙没有进行地图膨胀按机器人半径安全余量膨胀地图RRT 迟迟找不到路径步长过大窄通道内采样失效缩短步长至巷道宽度 1/6 左右RRT 路径过弯太大节点回溯后未做剪枝和平滑增加剪枝和三次样条平滑融合后车突然急刹前瞻点跨过障碍区用累计弧长选点而非欧几里得距离局部地图漂移坐标变换额外乘了偏航角检查 tf 树和坐标系变换DWA 在转弯处减速明显速度权重太低或最大速度过大适当降低该区域限速降低速度权重A* 路径被废弃RRT 频繁接管动态障碍在 A* 路径附近检查地图刷新频率加大局部代价地图更新频率车绕大圈走远路平滑迭代把路径扯变形抑制过冲增加碰撞检测保护这个表格里每个问题都是我实际调过、修过的不是网上随便抄来的。比如“DWA 原地摇摆”这个现象最有迷惑性第一眼看着像参数问题实际上往往不是 DWA 的问题——你去看 DWA 日志会发现它每个周期都在选“当前最好”的速度只是那个“最好”一直在左右横跳。真正原因是全局路径在墙前面断了或者前瞻点选到了障碍物上。把这几行排查逻辑记住能省下大量调参时间。5.2 我踩过三次以上的坑第一个大概率会踩的坑忽略模拟器与真机的动力学差异。我在模拟仓库里把 DWA 的加速度设得很大因为仿真模型的响应是完美的顺利跑通。到真机上底盘电机的响应延迟和摩擦非线性导致实际加速度明显低于设定值结果 DWA 模拟出来的轨迹与真实轨迹不一致碰撞检测形同虚设。后来我在每个速度周期里加入了真实底盘速度反馈的闭环补偿DWA 模拟所用起始速度不再取“上次指令速度”而是取“当前里程计反馈速度”这从原理上保证了运动模型和真实状态同步。真机效果瞬间正常了。第二个反复踩的坑是“地图刷新频率与全局重规划之间的时间竞赛”。初版系统里激光雷达以 10Hz 更新局部代价地图全局规划层每 5s 才检查一次是否需要重规划。当动态障碍突然出现在全局路径上时DWA 第一个发现并避让但全局规划层不知道路径已经失效还在用旧路径引导。这导致一个很别扭的状态DWA 明明绕开了障碍全局路径却还在障碍上下次前瞻点又会把车拉回去车像“画圈”似的来回绕。解决办法是把“DWA 连续 N 次检测到前方无障碍但无法沿全局路径前进”作为全局重规划的触发条件而不是单纯按时间轮询。这个改动让整个系统在动态场景下的表现稳定了很多。第三个坑属于工程素养层面的不要试图在一个功能里同时改三个算法。我在早期融合时每次遇到问题就顺手调一下 A* 的启发代价、再改一改 RRT 的步长、再动一下 DWA 的权重结果根本分不清是哪个改动导致的出了新问题也无法定位。后来我给自己定了一个死规矩每次只改一个参数改完跑一遍基准测试记录行为变化再改下一个。虽然过程慢但排查效率非常高很多之前积累的参数选择经验都是靠这个习惯沉淀下来的。做路径规划算法融合真正的难点不在算法本身而在“把三个算法各自的状态、触发条件、参数影响边界梳理清楚”这个系统性工程。如果让我给新人一个直接的建议先把 DWA 单独调到能在一个简单的矩形场地里稳定绕开临时障碍再把 A* 或 RRT 单独调好最后才考虑融合融合时也要先从最简单的一个全局路径源 DWA 的组合入手跑通了再叠加冗余路径源。这个顺序能避开 80% 的坑。最后再分享一个小技巧每个规划周期里把当前选择的速度、轨迹、全局路径、前方障碍物距离这些关键数据全部可视化出来。你不需要画多精致的图只要能同时看到“全局路径”和“DWA 当前候选轨迹”这两条线融合逻辑里大部分诡异行为都会一目了然。我调试过程中有一半的问题都是靠盯着一张实时轨迹图瞬间看明白的比看任何日志都快。