去年调一台室内送餐机器人地图上A规划出来的路径明明很正常可机器人一过门就来回打方向站在旁边看着都替它着急。后来我才意识到问题根本不在A而是我把离散的路径点直接喂给了底层速度控制器。那次之后我把运动规划这条链路从头捋了一遍A负责在地图上找出折线路径RRT负责在连续空间里做同样的搜索DWA负责在局部速度空间里实时避开障碍物Minimum Snap负责把路径拟合成加速度连续的光滑轨迹。这四个算法基本覆盖了从离散搜索到连续优化的全过程也是目前大多数自主移动机器人系统里的核心组合。这篇内容想把它们放在同一张图里讲清楚适合刚接触导航、被“路径规划”和“轨迹规划”两个词绕晕的同学也适合已经能跑通Demo、但想搞清楚每个模块到底该调什么的工程师。1. 先搞清楚路径规划不等于轨迹规划1.1 四个算法在整条链路里的位置很多人第一次接触运动规划时最容易踩的坑就是把这几个算法当成互相替代的关系A* 和 RRT* 都能“找路”DWA 也能“走路”Minimum Snap 看起来不过是在做平滑。但实际上它们解决的问题根本不是一个层面上的。路径是一串不含时间信息的几何点序列比如“从A点经过B、C两点到D点”。轨迹则是在路径之上叠加了时间信息不仅包括“经过哪”还包括“什么时候到、到达时速度是多少、加速度怎么样”。A* 和 RRT* 输出的是路径DWA 输出的是速度指令Minimum Snap 输出的才是真正的轨迹。从算法链路来看它们的定位非常清晰算法搜索空间输入输出典型运行频率A*离散栅格图栅格地图、起点、终点离散路径点0.1~1 HzRRT*连续状态空间障碍物几何、起点、终点折线路径0.1~1 HzDWA速度采样空间局部地图、局部目标线速度、角速度10~20 HzMinimum Snap多项式系数空间航路点序列、时间分配连续光滑轨迹离线/在线均可我在实际项目里内部通常把 A* / RRT* 称为“前端规划器”把 DWA 称为“局部执行器”把 Minimum Snap 称为“后端优化器”。前端走低频后端走高频互相之间的数据通过航路点来衔接。这样分工有个明显好处前端可以慢慢找最优解后端必须每帧都算完谁也拖累不了谁。1.2 为什么采用“前端搜索 后端优化”的分层设计很多人会问直接让 A* 输出一条足够密的路径再让底层控制器沿着点走不就行了这种做法在小车原地转圈、速度极慢的演示场景里可能凑合但一旦速度提上来或者路径里有急转弯问题就非常明显。原因在于A* 走出来的路径是折线段在每一个拐点处路径方向发生突变。对底层控制器来说方向突变意味着期望速度方向瞬间变化但机器人有质量和转动惯量速度不能突变。于是控制器会先减速到接近零再转向再加速表现就是“一顿一顿”的。如果控制周期短、增益高还可能出现振荡甚至超调。所以工程上普遍的做法是分层离散搜索解决“能不能到达”的组合问题连续优化解决“怎么走得稳”的可行性问题。这就像我们人走路时大脑先规划“绕过那个水坑往右边走”这是路径级决策至于每一步迈多大、脚抬多高、重心怎么移这是轨迹级控制大脑皮层和小脑分开干活不在同一个尺度上处理。把这种分层思想落到代码层面就是常见的三层结构全局规划层负责在较大范围内找一条无碰路径允许耗时较长。局部规划层加载全局路径上的某一段作为参考结合实时传感器信息避障输出速度指令。轨迹优化层在局部决策之上做平滑或者直接替代局部规划器输出的原始速度曲线。其中Minimum Snap 可以和 A* 串接也可以放在局部规划之后。比如在无人机场景里路径搜索得到折线路径后必须做Minimum Snap因为飞控需要连续可导的位置输入折线路径在导数上不存在直接使用会对飞控造成极大的冲击。2. A* 与 RRT*两种主流的全局搜索思路2.1 A* 的工程细节启发式搜索为什么能在栅格地图上高效A* 的本质是在一个有向图里做带启发信息的图搜索。它维护两个集合open list 存放待考察节点closed list 存放已确定代价的节点。每个节点有两个值g 代表从起点到当前节点的实际代价h 代表从当前节点到终点的估计代价f g h。每次从 open list 里取 f 最小的节点展开直到到达终点。够基础但工程里真正决定 A* 效率的不是这套框架而是几个细节启发函数必须满足“一致性”才能让 A* 保证最优。在栅格地图上如果允许 8 邻域移动那么欧氏距离是对角距离的近似如果允许 4 邻域移动则曼哈顿距离更合适。我见过不少代码直接用欧氏距离做 4 邻域地图的启发函数结果搜索效率严重降低因为 h 值偏低导致展开的节点远多于必要数量。地图膨胀这一步几乎不能省。机器人不是质点有实际宽度。在把地图交给 A* 之前应该以机器人半径加安全距离作为半径做障碍物膨胀。我踩过的坑是直接用原始栅格地图给 A*规划出的路径贴着墙走局部规划器稍微有点误差就直接撞墙。膨胀半径还要和栅格分辨率匹配膨胀层至少覆盖 3~5 个栅格否则膨胀效果被离散化吃掉。栅格分辨率的选择也很关键。分辨率太细地图内存和搜索时间爆炸太粗路径质量差无法穿过狭窄通道。我做室内轮式机器人时常用 0.05m 分辨率做代价地图但用于全局路径搜索时会把栅格降采样到 0.1~0.2m搜索速度快得多再用后续的轨迹优化把路径拉平。如果对实时性有要求A* 还有一个性价比很高的加速手段是 JPSJump Point Search。核心思想是在可通行区域中剪掉大量“对称路径”只在拐点处扩展节点。在开阔环境下 JPS 可以比普通 A* 快一个数量级而且不损失最优性。在一张 2000×2000 的栅格地图上普通 A* 可能要搜索几十万节点JPS 可能只展开几千个效果非常明显。2.2 RRT*在连续空间里渐进逼近最优路径A* 依赖离散栅格这在二维平面环境里很自然但放到高维就麻烦了。机械臂有六七甚至更多自由度状态空间维数高栅格离散化要么内存爆炸要么分辨率低到不可用无人机在三维空间中飞行障碍物也不能总用栅格模型描述。这时采样类算法是更合理的选择。RRT 的基本流程是随机采样一个状态找到当前树中离它最近的节点然后沿着两点连线方向扩展一段距离如果这段路径没有碰撞就把新节点加入树中。反复迭代直到树连接到终点。RRT* 的改进在于两点choose parent 和 rewire。每次插入新节点时不再直接把最近邻作为父节点而是在新节点附近一个半径内的所有节点中找一个能让路径代价最小的父节点这叫 choose parent随后检查这个新节点是否可以作为附近节点的父节点来缩短它们的路径代价叫 rewire。正是这两个操作让 RRT* 具有渐进最优性。我用一个简化版伪代码来展示核心逻辑方便理解def choose_parent(new_node, near_nodes): best_parent None best_cost float(inf) for node in near_nodes: if not collision_free(node, new_node): continue cost node.cost distance(node, new_node) if cost best_cost: best_parent node best_cost cost return best_parent def rewire(new_node, near_nodes): for node in near_nodes: if not collision_free(new_node, node): continue new_cost new_node.cost distance(new_node, node) if new_cost node.cost: node.parent new_node node.cost new_cost为什么要反复调用这两个函数因为 RRT* 的“最优”不是一次搜索就能保证的它随迭代次数增加逐步收敛。实际工程中我们通常设定最大迭代次数或搜索时间上限比如 1 秒到 5 秒取当前已找到的路径中最短的一条作为输出。RRT* 的参数直接影响路径质量。步长设得太短会造成树增长过慢我一般设为环境特征尺寸的 5%~10%目标偏向概率很关键设得过高容易让搜索退化到贪心算法我常用 5%~10%。对于 rewrie 半径随着节点数增加理论值应该按 \( r \gamma (\log n / n)^{1/d} \) 衰减但实际工程里很多人直接固定一个常数比如 1.5 到 2 倍步长效果也足够好。2.3 A* 和 RRT* 到底该选谁有一种误解是 RRT* 一定比 A* 高级其实不对。A* 在有栅格地图的二维环境里可以给出确定性的最短路径这对扫地机、AGV 这类场景非常重要RRT* 的优势在于不用栅格化、支持高维、支持复杂约束但它的搜索结果有随机性而且在实际使用中要想达到与 A* 相当的路径质量通常需要额外的后处理。我自己做选型时会看三个条件维度、环境表示方式、对最优性的要求。如果环境已经给成栅格地图且维数不超过三维我优先选 A*如果环境是几何体表示、或者维数高于三维我优先选 RRT*如果有全局最优的强需求且算力充足我甚至会把两者结合先用 A* 在粗分辨率格子上找一条初始路径再用 RRT* 在初始路径附近局部优化。3. DWA在速度空间里做局部避障3.1 动态窗口法的搜索空间与轨迹预测DWA 全称 Dynamic Window Approach直译是动态窗口法。它跟前两个算法最大的不同是它不去搜索位置空间而是直接在速度空间里穷举可能的 (v, ω) 组合。为什么叫“动态窗口”因为每一步采样时可行速度不是整个速度区间而是“当前速度在给定时间周期内能到达的速度区间”。假如当前线速度是 0.5m/s最大线加速度是 1m/s²控制周期是 0.1s那么下一时刻可行的线速度就在 [0.4, 0.6] m/s 之间。这个由电机物理性能决定的窗口随当前速度变化而变化所以叫动态窗口。DWA 的采样空间至少要满足三层约束速度边界约束v 和 ω 不能超过机器人硬件允许的最大值。加速度约束速度变化量不能超过电机的加减速能力。安全观测约束以当前速度行驶时必须在碰到障碍物之前能够刹住也就是 \( v^2 / (2a) \) 要小于到障碍物的距离。对每一个采样出来的 (v, ω)DWA 会假设机器人在预测时间内沿圆弧运动生成一条预测轨迹然后检查这条轨迹是否碰撞障碍物最后用评价函数打分选最优的一组速度下发。我常用的预测参数是预测时间 2~3 秒v 采样分辨率 0.01m/sω 采样分辨率 0.1rad/s。这样一轮采样大约几百个组合在嵌入式控制器上也能跑 10Hz 以上。需要注意的是预测时间太长会过度依赖于当前传感器数据容易误判太短又看不到前方障碍物我一般根据机器人最大速度和刹车距离来定。3.2 评价函数的三个权重怎么调DWA 的导航行为完全由评价函数决定一般包含三个项方位角偏差预测轨迹末端朝向与目标方向的夹角越小越好。障碍物距离预测轨迹与障碍物的最小距离越大越好。速度大小总速度越接近期望速度越好鼓励机器人别磨蹭。每个项会先做归一化再乘权重相加。网上常看到的heading0.8, dist0.1, velocity0.1这类参数只能作为起步值实际调试时必须按照机器人的表现来调整。我的经验是如果机器人总在障碍物前犹豫不决、频繁转向说明障碍物距离项权重过高或者安全距离阈值设得太大如果机器人总是冲到障碍物附近才急刹车说明障碍物代价太低如果机器人绕远路、偏离全局路径则需要调高“朝向目标”之外的“全局路径跟踪项”——很多工程实现里会额外加一项“到全局路径最近点的距离代价”这几乎是我解决 DWA 绕路问题的首选手段。还有一个非常实际的坑目标点刚好在机器人正后方时角度误差会从很小的负角度跳变到很大的正角度导致机器人原地摇摆倒退。原因是角度值在 ±π 处不连续计算误差时必须先做 wrap 处理。我在代码里永远先写一个normalize_angle()函数把任意角度映射到 [−π, π]再算误差。DWA 本质上是个局部算法它对全局地图一无所知因此面对 U 形障碍物或者很长的走廊时非常容易陷入局部最小值。工程上不能只靠调权重解决这种问题正确姿势是配合全局路径做参考让局部目标点始终取全局路径上的某个下一点而不是直接指向最终目标这样即使局部路径被障碍物挡住DWA 也会贴着全局路径恢复。4. Minimum Snap把离散路径变成连续光滑的轨迹4.1 为什么要最小化加加速度的导数Minimum Snap 这个名字里的 Snap 指位置的四阶导数也就是“加加速度jerk的导数”。如果大家开车时追求“乘客不晕车”关心的是加速度的变化率也就是 jerk而 Snap 则是 jerk 的变化率它对应到电机执行层面就是力矩的剧烈抖动。为什么要在轨迹优化里最小化它一个很重要的原因是执行器的平滑性需求。对于四旋翼这类高动态平台姿态和推力与轨迹的二阶、三阶、四阶导都有耦合关系不控制高阶导数意味飞行器会一直处于“猛拉”状态不仅能耗高飞行质量也差。对于机械臂最小化 Snap 可以减少关节扭矩波动延长谐波减速器寿命。Minimum Snap 的数学形式很简单我们要求解一条分段多项式轨迹使它的四阶导平方积分最小。如果只要求到三阶导就是 Minimum Jerk很多移动底盘项目用 Minimum Jerk 就已经足够。目标函数写成[ J \int_{0}^{T} \left( p^{(4)}(t) \right)^2 dt ]其中 \( p(t) \) 是位置轨迹。这个目标函数是二次的约束是线性的因此整个问题是一个标准的二次规划 QP求解非常成熟可靠。这也是 Minimum Snap 能在实际系统里大规模使用的根本原因它不光好写好算而且在数学上有可证明的平滑性保证。4.2 从航路点到 QP 问题的构造构造 Minimum Snap 轨迹的完整步骤我在项目里通常走五步。第一步拿到航路点序列。这些点可以来自 A* 输出、人为指定或上层任务规划。拿到后我会先做一次冗余点剔除把共线或距离过近的点合并否则每增加一个航路点就增加一段多项式计算量变大轨迹也可能在无意义的小波动上浪费能量。第二步确定每段轨迹的时间分配。先按各段路径长度比例做一个初始分配再根据执行器速度上限做一次缩放得到一个“大概合理”的 \( T_1, T_2, \ldots, T_M \)。第三步选择多项式阶数。Minimum Snap 一般用 7 阶多项式Minimum Jerk 用 5 阶多项式。阶数不是拍脑袋定的它需要满足所有约束后仍有余量每段多项式有 8 个系数要满足位置、速度、加速度、加加速度的端点约束和段间连续约束7 阶是刚刚有拓扑余量的选择。第四步构造目标函数中的 H 矩阵。每一段轨迹的目标函数是对四阶导平方做积分把每一段的多项式系数向量记为 \( p_k \)目标函数可以写为 \( p_k^T H_k p_k \)其中 \( H_k \) 由时间段 \( T_k \) 和各阶系数积分关系计算得到。把所有段的 H 矩阵拼成一个大 block-diagonal 矩阵这就是 QP 里的代价矩阵。第五步构造线性约束。约束包括每段起点和终点位置固定、段与段连接处速度/加速度/加加速度连续、起点终点速度加速度给定通常为零。这些约束全部可以用形如 \( A_{eq} p b_{eq} \) 的等式方程表示。在实际求解时我并不建议自己手写 QP 求解器。工程中最常用的是 OSQP、qpOASES它们都有 C/C 版本也可以在 ROS 里直接集成。仿真阶段也可以用 Python 的 scipy.optimize.minimize只是这种通用求解器在大规模问题上效率不如专用 QP 求解器。写代码时有一个很小的但坑很多的地方多项式系数在数值上非常病态时间越长系数绝对值越大。解决办法是做时间归一化每段都用 \( \tau t / T_k \) 替换原始时间变量。如果不做归一化即使只用 5 阶多项式T 等于 10 秒时系数也容易差到 8 个数量级常规 QP 求解器会直接报数值错误。4.3 时间分配最容易被忽略的生死线Minimum Snap 里最常被忽略的参数不是多项式阶数而是时间分配。同一个航路点序列时间给得不一样轨迹形态会差很多。如果各段时间给得太短等价于要求机器人极短时间走过很长的距离优化器只能拉高速度随之而来的是加速度和加加速度大幅超限执行器根本跟不上如果时间给得太长轨迹会变得非常平缓效率低下在狭窄空间里反而容易产生诡异的绕弯因为多项式为了满足平滑性宁愿走更远的路。我在项目中实际用过的初始化方法是每一段的时间按路径长度比例分配并假设一个期望平均速度 \( v_{ref} \)比如室内轮式机器人取 0.5m/s飞行器取 3m/s。初值给完之后跑一次 Minimum Snap检查各段的速度峰值和加速度峰值是否超出执行器限制超限就把对应段时间拉长 20%再跑一次。两三轮迭代之后基本能得到一组可执行的时间分配。如果要做更严格的轨迹优化可以把时间分配当作优化变量加入问题但这样目标函数不再是简单的二次型求解难度会明显上升。实际部署中我尽量先尝试固定时间分配的迭代法绝大多数场景都够用。另外一个工程要点是Minimum Snap 只约束航路点附近的位置不约束航路点之间的区域。如果障碍物在航路点之间轨迹完全可能从障碍物内部穿过去。解决办法是构建一个凸的安全走廊Flight Corridor把每一段轨迹的中间位置约束在某个无障碍凸多面体内然后再在 Minimum Snap 的 QP 中加入这些不等式约束。这一步是从“仿真小样”走向“真实部署”的分水岭。5. 工程实战常见问题与排查速查手册5.1 轨迹抖动、过冲、加速度超限这是我在做 Minimum Snap 时遇到最多的一个现象轨迹曲线看起来很平滑但机器人走起来加速过猛甚至在某个点出现明显的“抻一下”的感觉。排查时先从数据入手把每一轴的速度、加速度、加加速度曲线打印出来找出超限的具体位置。常见的几个原因和对应的解决手段我整理成一个表现象可能原因解决手段速度峰值集中在某一段该段时间分配过短拉长时间或重新分配加速度整体偏大平均速度目标设置过高降低参考速度尖峰出现在航路点附近冗余点未剔除合并过近航路点轨迹整体振荡多项式阶数不足或约束多了冲突提高阶数或去掉弱约束求解器报数值错误未做时间归一化使用归一化时间 \( \tau \)还有一类抖动来自规划层与执行层频率不匹配Minimum Snap 输出轨迹有 50Hz但底层控制器只有 20Hz相当于控制器每个周期都在跳目标点。这时不一定要改控制器也可以对轨迹做等间隔重新采样后再下发给控制器效果立竿见影。5.2 DWA 局部规划与全局路径不匹配DWA 常见问题中最影响使用体验的是“规划出来的局部路径和全局路径打架”。比如全局路径明明贴着走廊右侧走DWA 为了避障从左边绕了一下然后就一直回不到右侧结果在较宽走廊里走成 S 型。这种问题有几种处理思路。先检查局部代价地图和全局地图是否对齐尤其要确认 SLAM 的坐标变换没有漂移再检查全局路径本身的参考点选择逻辑每帧都应该取当前机器人位置前方 1~2 米处的路径点作为局部目标而不是取最近的点最后再调评价函数里的全局路径跟踪项权重。我在调车时最常用的一个实操技巧是把 DWA 的障碍物距离项先单独设为 0让机器人明显表现出“直线冲向目标”的行为确认代价地图和坐标变换正确再把障碍物项权重从零开始逐步加回每次只加一点点直到避障行为出现。这种“从极限往回收”的调法比从网上抄一组权重靠得住得多。5.3 四个算法怎么组合选型很多人在项目一开始就想着把四个算法全部接上以为算法越多越可靠。实际上每多一个模块就多一组参数和一串故障源。我更推荐按机器人形态和应用场景决定组合应用场景推荐组合理由扫地机器人/室内轮式服务机器人A* DWA计算量小地图栅格化天然匹配仓储 AGV 产线搬运A* TEBTEB 对路径跟踪的平滑性更强室外地面无人车RRT* DWA/MPC环境无法栅格化连续空间更合适无人机/机械臂RRT* Minimum Snap需要高动态平滑轨迹支持高维空间个人经验是第一次搭建系统时先用“A* DWA”跑通完整闭环至少稳定运行一周没问题再考虑加入 Minimum Snap。如果一开始就把 Minimum Snap 接上遇到问题你很难判断是搜索阶段选点不好还是优化阶段时间分配不对。还有一点关于重规划的冷却时间全局路径规划不建议每帧都重跑既浪费算力又容易因为地图噪声让路径在两次规划之间来回跳动。我在实际代码里会给全局规划设一个冷却时间比如 2~5 秒只有机器人偏离全局路径超过阈值时才触发重规划其余时间沿用旧路径稳定性会显著提升。如果硬要说这几年在运动规划上最深的体会大概是先把“离散路径”和“连续轨迹”这两层彻底分开理解再用最小能跑的方案验证整条闭环最后才是逐步加复杂度。调 DWA 就先看速度波形调 Minimum Snap 就先看加速度曲线别在公式里埋头太久车是要跑起来的多观察数据比多背公式有用得多。我自己第一次把 Minimum Snap 接到 A* 后面时最大的坑就是时间分配后来养成先看速度曲线再调参数的习惯效率翻了不少。希望这篇内容能让你少走几段我当年走过的弯路。