【Motion Plan】路径规划算法小结

0. 路径规划

路径规划是自动驾驶和机器人导航中的一个关键环节，它可以根据对环境信息的掌握程度被划分为两种主要类型：全局路径规划和局部路径规划。这两种规划方式在信息获取、规划策略和应用场景上有所不同。

1. 全局路径规划：

   • 信息获取： 全局路径规划是基于对环境的完全了解，即拥有先验的完整环境信息。
   • 规划特点：这种规划方式 属于静态规划，因为它依赖于已知的环境地图信息，不需要实时更新。
   • 规划方法：全局路径规划通常采用传统算法，这些算法包括：
     • Dijkstra算法：一种基于图搜索的算法，可以找到从起点到终点的最短路径。
     • A*算法：一种启发式搜索算法，它使用启发式函数来估计从当前节点到目标的成本，以找到最短路径。
   • 应用场景：全局路径规划适用于环境结构相对固定，且能够提前获取完整地图信息的情况。

2. 局部路径规划：

   • 信息获取： 局部路径规划依赖于传感器实时采集的环境信息，这些信息可能包括静态或动态的障碍物。
   • 规划特点：这种规划方式 属于动态规划，因为它需要根据实时的环境变化来调整路径。
   • 规划方法：局部路径规划通常采用更灵活的算法，可以是传统的局部规划算法，也可以结合智能算法，如：
     • PSO算法（粒子群优化）：一种模拟鸟群社会行为的优化算法，用于寻找最优解。
     • 遗传算法：一种基于自然选择和遗传学原理的搜索算法，用于解决优化问题。
     • 强化学习：一种通过与环境的交互来学习最优行为策略的方法。
   • 应用场景：局部路径规划适用于环境信息不完全已知，或者环境动态变化较大的情况。

在实际应用中，全局路径规划和局部路径规划往往需要结合使用。全局路径规划可以提供一个大致的路径方向，而局部路径规划则负责在接近目标的过程中，根据实时环境信息进行路径的细化和调整。这种组合使用的方法可以提高导航系统的鲁棒性和适应性。

1. 路径规划算法小结

算法	类别	应用场景	优点	缺点	实际使用情况
A*	全局路径规划	静态或半静态的仓库，全局导航	启发式加速，找到最优路径，计算速度较快	不适用于动态环境，受环境变化影响较大	静态或半静态仓库环境中的全局路径规划
Dijkstra	全局路径规划	小区域的路径优化，已知障碍物位置的环境	找到最短路径，算法简单	计算速度较慢，适合短距离	小规模、特定路径场景中的最优路径计算
DWA	局部路径规划	动态环境中实时避障	计算速度快，适合动态避障	只能局部规划，需结合全局规划	动态仓库环境中与移动障碍物（人、其他机器人）实时避障
RRT	采样基路径规划	障碍物布局复杂的仓库，路径复杂的环境	高效生成路径，适合复杂环境	路径可能不是最优的	复杂、多变的仓库环境中路径快速生成
RRT*	采样基路径规划	高效、安全路径规划的场景，如机器人协作搬运	提供逐渐收敛到最优的路径	计算复杂度较高	高效路径规划任务，尤其是在复杂障碍物环境中
PRM	采样基路径规划	多次重复路径的仓库场景	多次路径查询效率高，适合固定路线	需要预先构建稀疏图，适用性有限	重复任务场景下的路径规划，如分拣机器人重复运输路径
VFH	局部路径规划	障碍物较多的动态环境下局部导航	实时避障效果好，适合复杂环境	容易受障碍物密集度影响，可能出现局部最优	动态环境中的短距离避障任务，局部导航和障碍物绕行

1.2 算法小结总结：

• 全局路径规划：A* 和 Dijkstra 在已知环境中进行路径规划，适合仓库布局固定的场景。RRT* 提供路径优化，适合复杂场景。
• 局部路径规划：DWA 和 VFH 用于仓库中动态环境下的实时避障，适合与全局路径结合使用。
• 采样基算法：RRT 和 PRM 能应对复杂、多变或高维空间的环境，适合复杂障碍物或路径重复的任务。

通过结合全局和局部算法，仓储机器人能够灵活应对各种环境需求，平衡路径规划的效率与实时性。

机器人路径规划算法广泛用于导航和自主移动的任务中，帮助机器人在复杂环境中规划最佳路径，并实时避障。路径规划算法可以分为全局路径规划和局部路径规划。以下是一些经典的路径规划算法：

2. 全局路径规划算法

全局路径规划算法通常在已知环境或静态地图中使用，目的是找到从起点到目标点的最优路径。

2.1 Dijkstra 算法

• 特点：Dijkstra 算法是一种图搜索算法，适用于已知地图中无负权重的路径规划。它通过从起点开始遍历所有节点，找到到目标点的最短路径。
• 实现流程：
  1. 初始化起点的代价为 0，其余节点的代价为无穷大。
  2. 从起点开始，依次访问未访问过的节点，并更新邻居节点的代价。
  3. 继续选择代价最小的节点，直到找到目标点。
• 应用：适用于静态地图，计算速度较慢，但能保证找到最优路径。

2.2 A 算法*

• 特点：A* 是一种启发式搜索算法，它通过结合实际代价和启发式代价找到最优路径。与 Dijkstra 不同，A* 使用启发式函数（如曼哈顿距离或欧几里得距离）来加速搜索过程。
• 实现流程：
  1. 初始化起点，计算起点到目标点的启发式代价。
  2. 每次选择总代价（实际代价 + 启发式代价）最小的节点。
  3. 继续扩展该节点的邻居，直到找到目标点。
• 应用：广泛用于静态环境中的全局路径规划，具有较高的效率和最优性。
  摘要

2.3 D Lite 算法*

• 特点：D* Lite 是一种适用于动态环境的增量路径规划算法，可以在地图发生变化时重新计算路径，而不需要从头开始规划。
• 实现流程：
  1. 基于 A* 算法的路径搜索。
  2. 当环境或地图发生变化时，增量更新路径，仅重新规划受影响的部分。
• 应用：适合动态环境下的路径规划，如移动机器人在未知或变化的环境中工作时。

3. 局部路径规划算法

局部路径规划算法用于实时避障和导航，通常在未知或动态环境中使用。

3.1 动态窗口法（DWA, Dynamic Window Approach）

• 特点：DWA 是一种局部避障算法，基于机器人的运动学约束和传感器数据生成可行轨迹，并选择安全、快速的轨迹。
  动态窗口法（DWA, Dynamic Window Approach） 是一种用于移动机器人局部路径规划和避障的算法，特别适用于动态和未知环境。其核心思想是通过评估机器人在有限时间内的所有可能轨迹，选择一条安全且有效的路径，从而在避障的同时尽快接近目标。

1. 确定机器人运动模型

DWA的前提是了解机器人的运动学模型，即机器人在给定的速度和时间下能达到哪些运动状态。常见的差速驱动模型包括：

• 线速度 $v$（前进/后退）
• 角速度 $\omega$（转向）
  运动学约束定义了机器人的最大速度、加速度和旋转速度等参数，这些约束用于限制可能的运动轨迹。

2. 计算动态窗口

机器人当前的速度、加速度以及系统的运动学约束决定了其下一时刻可能的速度范围，称为 动态窗口。动态窗口定义了在下一时间步内机器人可能的速度范围：

• 运动学窗口：基于机器人速度和加速度约束，确定当前时间内可能的速度范围 $[v_{min},v_{max}]$和 $[{\omega }_{min},{\omega }_{max}]$。
• 可达窗口：在动态窗口内进一步约束可能的轨迹，确保机器人不会超出自身物理极限，如突然加速或减速太快。

计算公式：
$$\begin{gathered} v_{min}=v-a_{max}\cdot dt \\ {v}_{max}=v+a_{max}\cdot dt \end{gathered}$$
类似地，角速度 $\omega$ 也会被计算。

3. 轨迹生成

在动态窗口内，算法会为每个可能的速度对 $(v,\omega )$ 生成轨迹。每条轨迹通过积分机器人的运动模型，预测机器人在未来一段时间内的位置和方向。常用的时间窗口为1-2秒，即生成这段时间内的所有可能路径。

4. 轨迹评估

生成的轨迹根据多个标准进行评分，通常包括：

• 安全性：轨迹是否与障碍物发生碰撞。如果某条轨迹靠近障碍物，则其安全性评分较低。
• 距离目标的接近程度：轨迹最终到达的位置与目标点的距离，距离越近，评分越高。
• 速度：选择速度较高的轨迹，尽可能使机器人快速移动，评分越高。

评价函数通常为多个因素的加权和形式，例如：
$$G(v,\omega )=\alpha \cdot heading(v,\omega )+\beta \cdot dist(v,\omega )+\gamma \cdot velocity(v,\omega )$$
其中：

• $heading(v,\omega )$表示与目标方向的对齐程度；
• $dist(v,\omega )$ 表示与障碍物的距离；
• $velocity(v,\omega )$ 表示轨迹速度；
• $\alpha ,\beta ,\gamma$为权重参数。

5. 选择最佳轨迹

根据评分函数的结果，选择得分最高的轨迹。该轨迹同时确保了机器人能够安全避开障碍物、快速朝向目标运动。

6. 执行控制指令

选定的轨迹对应于某个具体的速度 v 和角速度 $\omega$。这些速度作为控制指令发送到机器人的驱动器，机器人沿着该轨迹运动。

7. 重复循环

该算法在每个时间步都进行以上步骤的重新计算，以便实时调整机器人路径应对环境中的变化（例如动态障碍物的出现或目标位置的变化）。

3.1.2 DWA的特点

• 实时性：DWA可以在动态环境中进行实时计算，并根据环境变化做出反应。
• 局部最优：DWA每次只考虑局部范围内的最优路径，而不关心全局路径规划。这种局部路径规划适合动态避障，但在复杂环境中可能陷入局部最优解。
• 基于运动学约束：DWA充分考虑了机器人的运动学约束，生成的路径是物理可行的。
  

3.1.3 DWA的应用

• DWA通常用于移动机器人在复杂、动态环境中的局部避障，例如：
  • 仓储机器人：在仓库中，机器人需要在动态障碍物（如其他机器人或人）中快速导航和避障。
  • 服务机器人：在服务环境中，机器人需实时应对移动的人或动态物体，同时保持向目标移动。

3.2 VFH (Vector Field Histogram)

• 特点：VFH 是一种基于直方图的局部路径规划算法，分析机器人周围的障碍物分布，通过直方图生成导航方向。
• 实现流程：
  1. 使用传感器数据生成环境的障碍物直方图。
  2. 计算各个方向的可通过性，选择一个最佳方向。
  3. 调整机器人的运动方向，避开障碍物。
• 应用：适用于动态避障，特别是在复杂环境中进行局部规划。

3.3 人工势场法（Artificial Potential Field, APF）

• 特点：人工势场法通过将目标点设置为吸引力源，障碍物设置为排斥力源，机器人在这些力的作用下运动。
• 实现流程：
  1. 为目标点生成吸引力场，机器人在吸引力作用下向目标移动。
  2. 为障碍物生成排斥力场，机器人在排斥力作用下避开障碍。
  3. 合成吸引力和排斥力，计算机器人的运动方向。
• 应用：适合简单环境中的局部路径规划，但在复杂环境下可能出现局部最小值问题。

##4# 4. 采样基路径规划算法
采样基算法通过在环境中随机采样节点，连接生成路径，适合高维空间的路径规划问题。

4.1 RRT（Rapidly-exploring Random Tree）

• 特点：RRT 是一种基于随机采样的路径规划算法，适用于高维和复杂环境。通过在空间中随机生成树结构，快速探索可行路径。
• 实现流程：
  1. 从起点开始随机生成树节点，并尝试连接到已有树结构上。
  2. 不断扩展树，直到找到目标点。
  3. 提供一条可行路径，不一定是最优路径。
• 应用：适用于大规模、复杂环境的路径规划，如机器人手臂、无人机的路径规划。

4.2 RRT（优化版 RRT）*

• 特点：RRT* 是对 RRT 的改进，通过在采样过程中不断优化路径，逐渐收敛到最优路径。
• 实现流程：
  1. 在随机采样生成树节点后，尝试通过重新连接减少路径长度。
  2. 继续扩展树，同时优化路径质量。
  3. 提供逐渐接近最优的路径。
• 应用：适合高维复杂环境，特别是对路径质量有要求的场景。

3.3 PRM（Probabilistic Roadmap）

• 特点：PRM 也是基于随机采样的路径规划算法，适用于多次查询的场景。通过生成一个稀疏图来连接自由空间中的可行路径。
• 实现流程：
  1. 随机采样自由空间中的节点，并尝试连接邻近的节点，构建稀疏图。
  2. 当需要规划路径时，在图中搜索起点和终点间的最短路径。
• 应用：适合机器人在静态环境中的多次路径查询。

##5# 5. 栅格化方法
栅格化路径规划算法将环境离散为一个个栅格，适用于机器人在二维平面上的路径规划。

5.1 栅格搜索算法（Grid-based Search）

• 特点：基于栅格图的路径规划方法，例如 A* 和 Dijkstra 都可以在栅格地图上运行。栅格图将环境离散为网格，每个网格表示自由空间或障碍物。
• 实现流程：
  1. 将环境映射为栅格图。
  2. 在栅格图上运行路径搜索算法，如 A*。
• 应用：适用于机器人在平面环境中的路径规划任务。

---

5.2 总结

机器人路径规划算法有多种选择，具体的选择依赖于任务需求和环境的复杂度：

• 全局路径规划：A*、Dijkstra、D* Lite 适用于已知环境中的全局导航。
• 局部路径规划：DWA、VFH、APF 等适合在动态和未知环境中的局部避障与导航。
• 高维空间规划：RRT 和 PRM 等采样基算法适用于复杂、高维空间的路径规划任务。

在仓储机器人、自动驾驶车辆和无人机等领域，通常将全局规划与局部规划结合使用，以实现高效且灵活的导航系统。