1. 从A到B的智慧：路径规划算法的世界

无论是手机地图App为你规划出避开拥堵的回家路线，还是仓库里穿梭自如的搬运机器人，亦或是游戏里NPC绕过障碍物向你走来，背后都离不开一个核心的技术——路径规划算法。这听起来可能有点学术，但说白了，它就是解决“怎么从这儿到那儿”这个问题的数学与工程智慧。随着自动驾驶、无人机物流、智能仓储的兴起，路径规划算法早已从实验室走进了我们生活的方方面面。今天，我们不谈那些高深莫测的数学公式，而是像同行交流一样，拆解一下这个领域里那些经典和前沿的算法，看看它们到底是怎么“思考”的，以及在实际项目中我们该如何选择和驾驭它们。无论你是刚入行的工程师，还是对机器人、游戏AI感兴趣的技术爱好者，这篇梳理都能帮你建立起一个清晰的认知框架。

2. 全局与局部：路径规划的两大战略视角

在深入具体算法之前，我们必须先建立一个顶层的分类框架。路径规划不是铁板一块，根据对环境的认知程度和规划范围，主要分为全局路径规划和局部路径规划。理解这两者的区别与联系，是正确选用算法的第一步。

2.1 全局路径规划：运筹帷幄的“战略家”

全局路径规划，顾名思义，是在已知或部分已知的全局环境地图中，为移动主体（机器人、车辆等）寻找一条从起点到终点的最优或次优路径。它像一个战略家，在行动前就俯瞰全局，制定好行军路线。

核心特点与适用场景：

• 环境信息已知：规划依赖于一张预先构建好的地图，这张地图包含了静态的障碍物、道路网络等信息。例如，基于高精地图的自动驾驶长途路线规划，或者仓库管理系统为AGV（自动导引运输车）指定的跨区域搬运路线。
• 结果是最优路径：由于掌握了全局信息，算法可以系统地搜索和比较，目标通常是找到代价（如距离、时间、能耗）最小的路径。
• 无法处理动态未知障碍：这是它的局限性。一旦地图上没有标注的临时障碍物（如突然出现的行人、掉落的箱子）出现，预先规划好的全局路径就可能失效。

常见的全局规划算法家族包括：

• 搜索类算法：如Dijkstra算法、A算法及其各种变体（如D、LPA*）。它们将环境离散化为图（Graph）进行搜索。
• 采样类算法：如快速随机树（RRT）及其优化版本（RRT*）。它们通过在构型空间中随机采样来构建路径树，特别适合高维空间。
• 基于图网络的算法：在拥有成熟路网数据的场景（如汽车导航），直接基于道路拓扑进行规划。

2.2 局部路径规划：随机应变的“战术家”

局部路径规划则专注于解决眼前的、局部的导航问题。它不关心完整的起点到终点，而是根据传感器（如激光雷达、摄像头）实时感知到的周围局部环境信息，计算出下一步或短时间内的安全运动指令。它是一位战术家，负责应对突发状况。

核心特点与适用场景：

• 依赖实时感知：不需要完整的先验地图，或者仅将先验地图作为参考，主要依据激光、视觉等传感器的实时数据。
• 反应式与动态性：能够实时躲避未预料到的动态和静态障碍物，适应环境变化。
• 可能陷入局部最优：由于视野有限，可能做出局部最优但全局来看很糟糕的决策，比如在U型障碍前反复震荡。

常见的局部规划算法包括：

• 动态窗口法（DWA）：在速度空间中采样，模拟未来短时间内的运动轨迹，并选择一条最优的（兼顾朝向目标、速度快、远离障碍物）。
• 时间弹性带（TEB）：将路径表示为一系列带时间戳的位姿点，并对其进行优化，使其同时满足动力学约束和避障要求，在自动驾驶和机器人中很常见。
• 人工势场法：将目标和障碍物分别模拟为吸引力和斥力，通过合力引导移动，但容易在复杂环境下产生局部极小点（即“卡住”）。
• 模型预测控制（MPC）：更高级的方法，通过求解一个有限时域内的优化问题来得到控制序列，能显式处理各种约束。

全局与局部的协同：在实际系统中，尤其是自动驾驶和高级机器人中，通常采用“全局规划 + 局部重规划”的框架。全局规划器给出一条粗略的参考路径（比如，沿着车道中心线行驶），而局部规划器则负责跟踪这条参考路径，同时实时避障，平滑轨迹，并满足车辆动力学约束。当局部规划器发现无法跟踪全局路径（如道路被完全堵塞）时，会通知全局规划器重新规划。

3. 经典全局规划算法深度拆解

了解了战略与战术的分工后，我们深入几个最经典、应用最广泛的全局规划算法的内部，看看它们是如何工作的，以及在实际编码和应用中需要注意什么。

3.1 Dijkstra算法：稳健的基石

Dijkstra算法是图搜索中最经典的单一源点最短路径算法。它的思想非常直观：从起点开始，像水波纹一样向外层层扩散，每次从未访问的节点中选取距离起点最近的节点进行访问，并更新其邻居节点的距离，直到终点被访问。

算法核心步骤：

1. 初始化：起点距离为0，其他所有节点距离为无穷大。所有节点标记为未访问。
2. 循环：在所有未访问节点中，选出当前距离起点最小的节点，记为当前节点u，并将其标记为已访问。
3. 松弛操作：遍历当前节点u的所有邻居节点v。计算distance[u] + weight(u, v)（即从起点经u到v的距离）。如果这个值小于v当前记录的距离distance[v]，就更新distance[v]为这个更小的值，并记录v的前驱节点为u。
4. 重复步骤2和3，直到终点被标记为已访问，或所有可达节点都被访问。

实操要点与心得：

• 数据结构是关键：算法的效率很大程度上取决于如何高效地从“未访问节点集合”中取出距离最小的节点。使用优先队列（最小堆）可以将时间复杂度从 O(V²) 优化到 O((V+E) log V)，其中V是节点数，E是边数。这是面试和实际实现时必须掌握的优化。
• “已访问”标记的陷阱：一旦节点被标记为已访问，其最短距离就被确定。这意味着Dijkstra算法不能处理负权边。因为负权边可能导致后续找到一条更短的路径通往一个“已访问”的节点，但算法不会再考虑它，从而导致错误结果。
• 适用场景：当图中边的权重均为非负，且需要找到确切的最短路径时，Dijkstra是可靠的选择。例如，在道路导航中，距离、预估时间作为权重通常都是非负的。

3.2 A*算法：启发式搜索的典范

A*算法是对Dijkstra的智能增强。它在选择下一个要扩展的节点时，不仅考虑从起点到该节点的实际代价g(n)，还加上一个从该节点到终点的预估代价h(n)，即f(n) = g(n) + h(n)。这个预估代价h(n)就是启发式函数。

为什么A*通常更快？因为它通过启发函数h(n)引导搜索方向朝向终点，减少了大量不必要的、背离终点的搜索，从而在大多数情况下比Dijkstra快得多。

启发函数h(n)的设计艺术：h(n)的设计直接影响A*算法的效率和最优性。

• 可采纳性：如果h(n)永远不会高估从节点n到终点的实际代价，那么A*算法保证能找到最短路径。这样的h(n)被称为“可采纳的”。例如，在网格地图中，曼哈顿距离（只允许上下左右移动）或欧几里得距离（允许斜向移动）都是可采纳的启发函数。
• 一致性（或单调性）：如果对于任意节点n及其后继节点n’，满足h(n) ≤ cost(n, n’) + h(n’)，且h(goal)=0，则称h(n)是一致的。一致的启发函数一定是可采纳的，并且能保证每个节点第一次被访问时就是最优路径。欧几里得距离在网格地图中就是一致的。
• 启发函数的强度：在可采纳的前提下，h(n)越接近真实代价，算法扩展的节点就越少，效率越高。但高估的h(n)会破坏最优性。

实现A*的注意事项：

• Open List与Close List：通常用优先队列管理Open List（待扩展节点，按f(n)排序），用哈希表管理Close List（已扩展节点）。当从Open List中取出一个节点时，检查它是否已在Close List中（因为可能被以更差的f值加入过），如果是则跳过。
• 路径重建：每个节点需要记录其父节点。当终点被加入Close List时，通过回溯父节点链即可重建完整路径。
• 变种与优化：针对不同场景有诸多优化，如Jump Point Search用于网格地图跳过大量对称路径；Theta* 用于任何角度路径规划，能生成更平滑的路径。

3.3 RRT与RRT*：应对高维空间的随机采样高手

当规划空间维度很高（如机械臂有6个以上关节）或环境非常复杂时，基于图搜索的方法可能因为状态空间爆炸而失效。这时，基于随机采样的规划器，如快速随机树，就显示出其优势。

RRT基本思想：

1. 初始化：树T只包含根节点（起点）。
2. 随机采样：在自由空间（非障碍物区域）内随机采样一个点q_rand。
3. 寻找最近邻：在树T中找到距离q_rand最近的节点q_near。
4. 扩展新节点：从q_near向q_rand的方向延伸一个步长step_size，得到新点q_new。检查q_near到q_new的连线是否与障碍物碰撞。
5. 添加节点与边：若无碰撞，则将q_new加入树T，并添加边(q_near, q_new)。
6. 重复2-5步，直到q_new进入终点区域，或达到最大迭代次数。

RRT的优缺点：

• 优点：概率完备性（只要解存在，给定无限时间总能找到），适合高维空间，实现相对简单。
• 缺点：找到的路径通常不是最优的，而且路径可能非常曲折、不光滑。

RRT：渐进最优的改进* RRT* 在RRT的基础上增加了“重连接”步骤，使得搜索树能够不断优化，最终收敛到最优路径。

1. 在找到q_near并生成q_new后，RRT* 不仅将q_new连接到q_near。
2. 它会在q_new附近的一个邻域内，寻找所有可能的父节点候选。计算通过每个候选节点到达q_new的代价，选择代价最小的那个作为q_new的真正父节点（重选父节点）。
3. 接着，它还会尝试对邻域内的其他节点进行“重布线”：检查如果以q_new作为父节点，是否能降低这些节点的路径代价。如果能，就改变它们的父节点到q_new（重布线）。

通过这两步，RRT* 的树结构会随着时间的推移不断优化，路径代价逐渐降低，最终达到渐进最优。

实操心得：

• 步长选择：step_size是关键参数。太大可能导致碰撞检查失败率高，扩展效率低；太小则生长缓慢。可以设计自适应步长。
• 偏向目标采样：纯粹随机采样效率较低。可以采用“目标偏向”策略，即以一定概率（如5%）直接将采样点设为终点，能显著加快收敛。
• 碰撞检测效率：这是RRT/RRT* 的性能瓶颈。工业级实现中需要依赖高效的几何碰撞检测库（如FCL, Bullet）。
• 路径后处理：RRT生成的路径通常由线段组成，有棱角。实际应用中需要对路径进行平滑化处理，例如使用样条插值或进行梯度下降优化，使其符合机器人的运动学约束。

4. 局部与融合规划算法实战解析

全局规划给出了“战略方向”，而局部规划负责“战术执行”。尤其在动态环境中，局部规划器的能力直接决定了系统的安全性和流畅性。

4.1 动态窗口法：机器人的实时避障决策

DWA非常直观地模拟了机器人的决策过程：在当前状态下，有哪些可行的速度组合（线速度和角速度）？每个速度组合对应的未来一段轨迹是什么？哪条轨迹最好？

DWA的核心步骤：

1. 速度空间采样：在机器人最大最小线速度[v_min, v_max]和角速度[ω_min, ω_max]定义的矩形区域内进行离散采样，得到一系列(v, ω)对。同时，考虑机器人的加减速能力，从当前速度(v_c, ω_c)出发，在下一个控制周期内能达到的速度窗口是有限的（即“动态窗口”）。
2. 轨迹模拟：对于每一个采样速度(v, ω)，假设机器人以此速度匀速运动一段模拟时间（如3秒），通过运动学模型（通常是差分驱动模型）推演出未来一段轨迹。
3. 轨迹评价：对每一条模拟轨迹进行打分。评价函数G(v, ω)通常是多个子目标的加权和：
   • Heading(v, ω)：轨迹末端朝向与目标点方向的对齐程度。
   • Dist(v, ω)：轨迹上离最近障碍物的距离。距离越近，得分越低，甚至为负（直接剔除）。
   • Velocity(v, ω)：速度大小，鼓励快速移动。
   • G(v, ω) = α*Heading + β*Dist + γ*Velocity
4. 选择最优：选择评价得分最高的(v, ω)作为当前周期发送给机器人底层的控制指令。

参数调优心得：

• 评价函数权重：α, β, γ的调整是门艺术。增大β（障碍物距离权重）会使机器人更保守，远离障碍物；增大α（朝向权重）会使机器人更执着地指向目标；增大γ（速度权重）则鼓励快速运动。需要在实际场景中反复测试平衡。
• 模拟时间与分辨率：模拟时间太长，计算量大，且环境可能已变化；太短则预见性不足。采样分辨率（速度离散化的粒度）也影响精度和计算效率。
• 局限性：DWA本质上是一种局部贪婪算法，容易在复杂狭窄空间（如狭窄走廊、U型陷阱）中失效，因为它只模拟很短的时间，看不到全局困境。

4.2 时间弹性带：融合全局与局部的优化器

TEB算法将路径规划和控制问题统一到了一个优化框架中。它不再将路径视为一系列空间点，而是视为一系列带时间戳的位姿点B_i = (x_i, y_i, θ_i, t_i)，这个序列被称为“时间弹性带”。

TEB的优化思想：TEB通过求解一个非线性优化问题，来同时优化这条“带子”的形态和时间间隔，使其满足多种约束：

1. 目标函数：最小化总时间、与全局参考路径的偏差、加速度/角加速度（使运动平滑）等。
2. 约束条件：
   • 运动学约束：相邻位姿点之间必须满足机器人的运动学模型（如差分驱动、阿克曼转向）。
   • 动力学约束：速度、加速度、角速度、角加速度不能超过机器人的物理极限。
   • 避障约束：机器人的轮廓（可以建模为多个圆形）与障碍物之间的距离必须大于安全阈值。
   • 时间约束：时间间隔必须为正。

实现流程与工具：

1. 初始化：通常以全局规划器生成的路径（忽略时间信息）作为TEB的初始猜想。
2. 构建优化问题：将上述目标和约束全部数学化，构建成一个大规模稀疏的非线性最小二乘问题。
3. 求解：使用专用的稀疏非线性优化求解器（如g2o,Ceres Solver,NLopt）进行求解。这些求解器能高效处理TEB问题特有的稀疏结构。
4. 输出：求解后得到的优化后的位姿-时间序列，可以直接用于生成平滑的控制命令。

TEB的优势与挑战：

• 优势：能直接生成平滑、动态可行的轨迹；显式地处理时间和各种约束；将路径规划和轨迹优化融为一体。
• 挑战：
  • 对初始值敏感：如果初始路径（来自全局规划器）离可行解太远，优化可能失败或陷入局部最优。因此需要一个合理的全局路径。
  • 实时性：优化计算量较大，对处理器有要求。通常需要通过限制优化带宽（位姿点数量）、使用高效求解器来保证实时性。
  • 参数繁多：各类约束的权重参数需要仔细调试。

4.3 模型预测控制：更通用的优化控制框架

MPC是比TEB更一般化的框架。它在每个控制周期内，求解一个有限时域内的开环最优控制问题，但只执行第一个控制指令，到下一周期再根据新的状态重新求解，形成“滚动优化”的闭环。

在路径跟踪与避障中的应用：MPC的优化问题可以设计为：在未来N个时间步内，寻找一系列控制输入（如加速度、前轮转角），使得预测的状态轨迹尽可能好地跟踪参考路径（来自全局规划器），同时满足车辆动力学模型、避免与障碍物碰撞、以及各种状态和输入约束（如速度、加速度、转角限制）。求解这个带约束的优化问题后，取第一个控制指令输出给执行器。

MPC vs TEB：

• 相似性：两者都是基于优化的方法，都处理约束。
• 差异性：TEB优化的是“轨迹”（一系列状态点），而MPC优化的是“控制序列”。MPC更侧重于控制，其模型通常是连续的；TEB可以看作是一种特殊的、离散化的轨迹优化MPC。MPC的理论框架更通用，能处理更复杂的模型和约束，但计算负担通常也更大。

工程实现建议：对于大多数移动机器人或低速自动驾驶场景，DWA因其简单高效，常作为首选的局部规划器。当需要更平滑、动态可行的轨迹时，TEB是一个强大的选择。而对于模型复杂、约束严苛的高性能控制（如赛车、无人机），MPC则是更合适的工具。在实际项目中，我们经常需要根据机器人的算力、对轨迹质量的要求、环境的动态程度来做出权衡。

5. 前沿与特定场景算法掠影

除了上述经典算法，针对特定场景和需求，也涌现出许多重要的算法变体和前沿方向。

5.1 泊车路径规划算法

自动泊车对路径规划提出了特殊挑战：空间极度受限、需要精确的终点位姿（车位内）、且通常是非完整约束（阿克曼转向）。单纯的全局搜索或局部反应方法往往不够。

常用方法组合：

1. 几何分解法：将泊车过程分解为几个标准的几何动作阶段，如“向前切入-倒车入库-调整”。Reeds-Shepp曲线或Dubins曲线常被用来生成连接两个位姿的最短路径（考虑最小转弯半径）。规划器的工作就是选择合适的切换点和动作序列。
2. 基于优化的方法：将车辆和车位建模为多边形，将泊车问题构建为一个带约束的非线性优化问题，直接求解出一条平滑、无碰撞、符合动力学的轨迹。这需要较强的实时计算能力。
3. 搜索与优化结合：先用基于采样的方法（如Hybrid A*）在低分辨率下搜索出一个粗略的、可行的动作序列，再用优化方法（如TEB）对这个粗略轨迹进行精细化和平滑化。

关键考量：

• 碰撞检测精度：必须使用精确的车辆轮廓模型进行碰撞检测，考虑后悬外摆等。
• 终点容差：规划的目标不是一个点，而是一个允许的位姿范围（车位区域）。
• 舒适性：轨迹的曲率变化应平缓，避免急打方向。

5.2 无人机路径规划算法

无人机路径规划除了考虑地面障碍，还需考虑三维空间、能耗、风场等复杂因素。

核心算法扩展：

• 三维A与DLite：将传统的二维网格搜索扩展到三维体素网格。D* Lite 及其变种常用于未知或动态变化的三维环境（如无人机探索）。
• 基于采样的方法：RRT* 在三维空间中同样有效，并且有面向三维空间的变体如RRT-Smart*。
• Minimum Snap轨迹生成：对于多旋翼无人机，一个非常重要的环节是生成光滑的、动力学可行的轨迹。Minimum Snap（最小加加速度）或Minimum Jerk（最小加加速度）轨迹生成方法，通过优化多项式轨迹的系数，使轨迹的某阶导数（如加速度的导数，即加加速度）的积分最小，从而得到极其平滑、适合无人机跟踪的轨迹。这通常与前端路径搜索（如A*）结合使用。

5.3 局部路径规划算法中的QP应用

QP（二次规划）是优化问题的一个子类，其目标函数是二次的，约束是线性的。它在局部路径规划中扮演着“微调”和“约束满足”的关键角色。

典型应用场景：

1. 路径跟踪与偏移：全局路径可能太靠近障碍物，或者不够平滑。我们可以将路径表示为一组离散的路径点，然后构建一个QP问题：目标是最小化路径点相对于原始参考路径的偏移量（二次代价），同时约束每个路径点与最近障碍物的距离必须大于安全值（线性约束）。求解这个QP，就能得到一条既保持原路径形状、又满足安全距离的平滑路径。
2. 速度规划：给定一条空间路径，我们需要规划沿这条路径行驶的速度曲线。这可以构建为一个QP：目标是最小化行驶时间或加速度变化（二次代价），约束包括速度、加速度、加加速度的上下限（线性约束），以及根据路径曲率计算出的向心加速度限制。
3. MPC中的子问题：许多MPC求解器在每一步迭代中，需要求解一个QP问题（例如，使用序列二次规划SQP方法）。

使用心得：

• 求解器选择：有大量高效、成熟的QP求解器库可用，如OSQP（专门用于凸二次规划）、qpOASES（适用于模型预测控制）、CVXOPT等。选择时需考虑问题规模、实时性要求以及许可证。
• 问题构建：如何将实际的物理约束（如障碍物距离）转化为线性的不等式约束，是应用QP的关键。有时需要对非线性约束进行线性化近似。
• 实时性：对于需要高频（如100Hz）运行的局部规划，QP问题的规模必须严格控制（优化变量和约束数量不能太多），以确保能在单个控制周期内求解完毕。

6. 算法选型与工程实践指南

面对琳琅满目的算法，在实际项目中该如何选择？这里没有银弹，只有权衡。

6.1 根据场景与需求选择算法

6.2 常见问题与调试技巧实录

在实际编码和调试路径规划系统时，以下是一些高频问题和解决思路：

问题1：A*算法搜索速度慢，扩展节点太多。

• 排查：首先检查启发函数h(n)。如果h(n) = 0，A* 就退化成了Dijkstra，速度最慢。如果h(n)是可采纳的但很弱（如远低于真实代价），引导性就差。
• 解决：
  • 使用更贴近真实代价的启发函数。在网格地图中，如果允许对角移动，使用对角线距离（切比雪夫距离）或欧几里得距离比曼哈顿距离更好。
  • 考虑使用Weighted A*，即f(n) = g(n) + ε * h(n)，其中ε > 1。这会牺牲最优性（找到的是次优解，但代价不超过最优解的ε倍），以换取更快的搜索速度。这在很多实时应用中是可接受的。
  • 检查地图表示是否过于精细。在不损失必要信息的前提下，适当降低地图分辨率（网格变大）能极大减少搜索节点。

问题2：DWA机器人陷入局部震荡，在障碍物前“左右横跳”。

• 现象：机器人接近障碍物时，向左转觉得右边离障碍物近，向右转又觉得左边离障碍物近，导致在原地左右摇摆。
• 解决：
  • 调整评价函数：大幅提高Dist(v, ω)（障碍物距离）项的权重β，让机器人将安全放在第一位，宁愿慢一点也要远离障碍物。
  • 引入“停滞恢复”机制：检测机器人是否长时间速度接近零且未到达目标。如果陷入停滞，可以临时改变行为，比如让机器人原地旋转一定角度，或者执行一个简单的后退动作，以脱离局部极小点。
  • 改进采样策略：在评价函数中加入对“平滑性”的考量，惩罚相邻周期速度指令的剧烈变化，可以减少振荡。

问题3：RRT/RRT生成的路径非常曲折，不光滑。*

• 解决：RRT系列算法本身只负责找到一条可行的路径，不保证质量。路径后处理是必须的。
  • 路径修剪：遍历路径节点，尝试连接不相邻的节点。如果连线无碰撞，则删除中间的所有节点，从而缩短路径。
  • 路径平滑：使用曲线拟合方法，如三次样条插值或贝塞尔曲线，对修剪后的路径点进行平滑。更高级的方法是使用梯度下降或非线性优化，在保持无碰撞的前提下，直接优化路径点的位置，使其满足曲率约束。

问题4：TEB优化求解失败或耗时过长。

• 排查：
  • 初始值太差：检查输入给TEB的全局初始路径是否合理。如果初始路径穿墙而过，优化很难收敛。
  • 参数过于激进：例如，最大速度/加速度设置得过高，而优化步长（dt）设置得过大，可能导致数值不稳定。
  • 问题规模太大：时间弹性带上的位姿点数量过多。
• 解决：
  • 确保全局规划器提供一条无碰撞的、粗略可行的初始路径。
  • 从保守的参数开始调试：降低最大速度/加速度，增加障碍物安全距离的权重。
  • 减少优化频率或减少位姿点数量。TEB不需要每帧都从头优化，可以设置一个合理的优化周期。
  • 使用性能剖析工具，查看优化求解中哪一步最耗时，针对性优化。

问题5：规划系统整体延迟大，控制不跟手。

• 性能剖析：这是一个系统工程问题。需要测量各个环节耗时：
  1. 感知延迟：从传感器数据采集到生成障碍物地图/点云的时间。
  2. 全局规划延迟：触发全局重规划到计算出新路径的时间。
  3. 局部规划延迟：局部规划器单次计算周期。
  4. 控制与通信延迟：指令下发到底层执行器的时间。
• 优化策略：
  • 异步规划：全局规划与局部规划在不同线程运行。局部规划高频运行（如50-100Hz），全局规划低频运行或在需要时触发。
  • 感知与规划解耦：局部规划器使用一个固定频率更新的、轻量级的局部代价地图，而不是直接处理原始的、庞大的传感器数据。
  • 算法简化：在算力有限的平台上（如嵌入式主板），优先考虑DWA而非TEB/MPC。对A*搜索进行剪枝，使用更粗糙的地图。
  • 预测与缓冲：局部规划器可以简单预测动态障碍物的运动，并在代价地图中预留出空间，避免急刹。

路径规划是一个理论与实践紧密结合的领域。再精巧的算法，也需要在具体的机器人平台、传感器配置和实际环境中反复调试和打磨。我的经验是，从简单的模型和算法开始（比如先在仿真环境中实现一个DWA），确保整个感知-规划-控制的 pipeline 能跑通，然后再逐步引入更复杂的算法和优化。理解每个算法的核心思想、优缺点和适用边界，比单纯追求算法的“高级”更重要。在实际项目中，一个由A*提供全局引导、DWA负责局部避障的朴素组合，往往比一个未经充分调试的复杂优化器更加稳定可靠。