机器人路径规划算法大全RRT,APF,DS,RL

一、区分路径规划和轨迹规划

二、区分全局规划和局部规划

三、核心主流算法介绍

[3.1 RRT](#3.1 RRT)

[3.2 APF人工势场](#3.2 APF人工势场)

[3.3 DS动态系统调制方法](#3.3 DS动态系统调制方法)

[3.4 强化学习](#3.4 强化学习)

四、OMPL规划库

在这篇文章里准备捋一下所有有关路径规划相关的内容和知识，并且针对每个算法会单独写一篇文章详细讲解，包括原理和代码。

！！！！！这篇博客里的算法都可以点开学习，希望大家多多点赞收藏，会持续更新，目的就是希望能够为大家提供一个全面的路径规划算法检索界面！！！！！

下面的表格列举了主流的路径规划算法，可以直接点进去跳转到相关的文章学习，里面包含了原理，伪代码，英文期刊论文和相关代码。

一、区分路径规划和轨迹规划

简单来说，路径规划是不包含时间和运动状态 ，轨迹规划是包含时间和运动状态 。两者是机器人运动控制的前后衔接环节（先路径规划，后轨迹规划）

**路径规划：**是从起点到目标点的空间无碰撞路径（仅需满足位置约束，不涉及时间或运动状态）
轨迹规划：是在路径规划的基础上，生成包含时间、速度、加速度的运动指令（需满足机器人动力学约束，如硬件加速度上限）

| 对比维度 | 路径规划（Path Planning） | 轨迹规划（Trajectory Planning） |
| 输出结果 | 空间坐标序列 / 几何曲线（如离散点、折线、贝塞尔曲线），无时间信息 | 随时间变化的「状态序列」（位置、速度、加速度、角速度、力矩等），包含时间维度 |
| 常见方法 | 1. 传统搜索类：Dijkstra 算法、A算法、BFS（广度优先搜索）、DFS（深度优先搜索） 2. 概率采样类：RRT 算法、RRT 算法（RRT 改进版，更优路径）、PRM（概率路标法） 3. 势场类：人工势场法 4. 其他：Voronoi 图法、可见性图法 | 1. 关节空间规划（直接对关节角度规划，避免笛卡尔空间奇点）：三次多项式插值、五次多项式插值、梯形速度曲线 2. 笛卡尔空间规划（对末端执行器位置 / 姿态规划）：直线插补（如 G01）、圆弧插补（如 G02/G03）、贝塞尔曲线、B 样条曲线（平滑性更优） 3. 动力学约束规划（考虑实时动态性能）：LQR 控制（线性二次调节器）、模型预测控制（MPC）、轨迹优化（如通过凸优化求解约束问题） |

示例	从客厅沙发到门口，规划出 "绕开茶几的折线 / 平滑曲线"（仅标注空间坐标点）	基于上述绕开茶几的路径，规划 "0-2 秒从静止加速到 0.5m/s，2-8 秒匀速行驶，8-10 秒减速至静止" 的完整运动过程（每 0.1 秒对应一个位置 + 速度值）

所以我们一般提到的RRT，人工势场法等都是路径规划算法。

二、区分全局规划和局部规划

路径规划算法可以分为全局规划算法和局部规划算法，他俩的核心区别就一点：是否实时获取障碍物信息并实时规划。

全局规划： 是提前知道全部的静态地图信息，然后直接规划出一条起点到终点的路径，之后机器人就啥也不管一股脑按照这条规划好的路径从起点走到终点，如果中途碰到动态障碍物撞过来他也不管。所以全局规划适用于静态、已知环境
局部规划：机器人依赖传感器实时获取局部的动态信息（也就是机器人周围小范围内的障碍物信息），然后实时的规划出下一步该怎么走，看上去比全局规划更智能一点。所以局部规划适用于动态、未知环境。

2.1 全局规划算法

	多项式曲线（Polynomial Curve）	- 通过多项式插值（如五次多项式）连接路径点，保证位置、速度、加速度连续； - 平滑性好，但计算复杂度随多项式阶数增加。	高精度运动控制场景（如无人机精准悬停、工业机器人装配路径）。
[全局规划算法汇总表]

2.2 局部规划算法

模型预测控制类	模型预测控制（Model Predictive Control, MPC）	- 基于机器人动力学模型，预测未来一段时间内的轨迹，通过优化目标函数（如跟踪误差、避障代价）选择最优控制量； - 动态适应性强，但计算复杂度高，依赖精准模型。	高动态环境（如自动驾驶高速避障、无人机规避快速移动的障碍物）。
[局部规划算法汇总表]

三、核心主流算法介绍

3.1 RRT

下面按照RRT改进的思路进行罗列

RRT-Smooth	- 对 RRT 生成的离散路径点进行曲线拟合（如三次 B 样条），消除冗余拐点，提升路径平滑性。	对运动平滑性要求高的场景（如自动驾驶车道保持、工业机器人装配路径）。
[#### 3.1.1 路径优化与渐进最优性]

3.1.2探索效率提升

Bias-RRT	- 以概率 p 将目标点作为随机采样点，增加目标导向性； - p 随环境复杂度动态调整（如复杂环境降低 p 以避免局部搜索）。	简单到中等复杂度的静态环境（如室内机器人导航、游戏 AI 寻路）。

3.1.3动态环境适应性

DWA-RRT	- 引入时间滑动窗口，结合时序一致性约束过滤传感器噪声，提升动态障碍物检测准确性。	传感器噪声较大的动态环境（如室外机器人在雨天或粉尘环境中导航）。

3.1.4运动学 / 动力学约束处理

RRT-Kinodynamic	- 直接在状态空间中采样速度、加速度等动力学参数，确保路径的可执行性。	具有非完整约束的机器人（如轮式移动机器人、无人车）。

3.1.5参数自适应与采样优化

RRT with Local Path Pruning	- 双向剪枝：从起点和终点回溯路径，删除冗余节点，缩短路径长度。	路径拐点多、冗余度高的场景（如 RRT 生成的初始路径优化）。

3.1.6 RRT与其他算法融合

算法名称	融合策略	核心优势
**RRT-A Hybrid***	- 先用 RRT 生成初始路径框架，再用 A算法对路径分段进行精细化优化。 - 结合 RRT 的高维探索能力与 A的启发式搜索优势。	路径长度显著缩短（较 RRT 减少 30% 以上），同时保持 RRT 在复杂环境中的高效性。
**RRT-D Fusion***	- RRT 生成全局路径，D * 算法处理动态障碍物导致的局部路径失效。 - 动态重规划时仅更新受影响区域，减少计算量。	动态环境适应性强，重规划时间比纯 RRT 快 50% 以上。
APF-RRT	- 人工势场（APF）的目标引力引导 RRT 采样方向，RRT 的随机扩展避免 APF 陷入局部极小值。	路径平滑性提升（曲率波动减少 40%），狭窄通道通过率提高至 95% 以上。
RRT-DWA	- RRT 生成全局路径作为参考，动态窗口法（DWA）实时调整速度和转向，实现动态避障。	动态障碍物避让成功率达 98%，路径跟踪误差小于 0.2 米。
RRT-DSM	- RRT 提供全局路径，动态系统调制（DSM）通过微分方程实时调整局部运动指令。 - 例如，结合时间弹性带（TEB）优化轨迹连续性。	运动平滑性与动态适应性平衡，加速度波动减少 30%。
RRT-PRM 串联	- 离线阶段用 PRM 构建自由空间路网，在线阶段用 RRT 在路网基础上快速扩展。	在线查询时间缩短至毫秒级，离线预处理后重复规划效率提升 10 倍以上。
RRT-PRM 并联	- PRM 生成稀疏路径骨架，RRT 在骨架节点间填充详细路径。 - 例如，PRM 确定关键点，RRT 连接关键点形成平滑轨迹。	路径节点数量减少 60%，路径长度接近 PRM 最优解。
RRT-PSO	- 粒子群优化（PSO）动态调整 RRT 采样分布，优先向高价值区域扩展。 - 例如，粒子位置对应采样点，速度对应扩展方向。	采样效率提升 50%，路径规划成功率提高至 99%。
RRT-GA	- 遗传算法（GA）优化 RRT 树结构，通过交叉和变异操作生成更优路径。 - 例如，染色体编码路径节点序列，适应度函数评估路径质量。	路径长度比 RRT * 缩短 15%，收敛速度提升 30%。
RRT-CNN	- 卷积神经网络（CNN）分析环境图像特征，指导 RRT 采样区域选择。 - 例如，CNN 预测障碍物分布，RRT 优先在自由空间扩展。	采样无效区域减少 70%，复杂环境下规划时间缩短至 1 秒内。
RRT-PPO	- 近端策略优化（PPO）学习 RRT 采样策略，通过强化学习最大化路径质量奖励。	动态环境下路径长度比传统 RRT 减少 25%，学习收敛速度提升 40%。
RRT - 帕累托最优	- 多智能体独立运行 RRT 生成路径，通过帕累托协商消解冲突。 - 例如，冲突双方调整路径，确保至少一方受益且无人受损。	多机器人协作任务成功率达 95%，冲突消解时间小于 0.5 秒。
RRT - 分布式搜索	- 多智能体共享 RRT 树信息，分布式扩展路径。 - 例如，每个智能体维护局部树，定期同步全局连接点。	路径规划速度比单智能体提升 3 倍，资源利用率提高 60%。

3.2 APF人工势场

以下是经典的人工势场法（APF）改进方法汇总，表格中包含改进方向、核心思想、关键公式（LaTeX 格式）及解决的核心问题，覆盖了针对 APF 传统缺陷（局部极小值、目标不可达、障碍物振荡、动态环境适应性差等）的主流优化方案：

算法名称	核心改进	适用场景
涡旋人工势场（VAPF）	- 引入切向涡旋力，使机器人在接近障碍物时产生环绕运动趋势，打破引力与斥力的平衡。 - ：障碍物极角 - ：涡旋系数 - 引入切向力打破引力与斥力平衡	密集障碍物或凹形结构环境（如迷宫、工厂车间）。
虚拟目标点法	- 在障碍物外生成临时目标点，引导机器人绕过局部极小区域； - 结合梯度下降修正，当合力为零时随机扰动方向。 - ：临时目标点坐标 -：临时引力系数	目标点被障碍物包围或路径存在狭窄通道的场景（如机器人绕过书架取物）。
斥力场函数重构	- 改进斥力计算方式，引入目标距离因子削弱远场斥力； - 设计分段函数：当机器人接近目标时，斥力随目标距离衰减。 - ：机器人到障碍物距离 - ：斥力作用半径 - ：机器人到目标距离 - k：调节因子（1~2）	目标点与障碍物距离过近导致震荡的场景（如机器人在目标点附近避障）。
[#### 3.2.1局部极小值与目标不可达问题解决方案]

3.2.2动态环境适应性改进

预测模型结合	- 采用神经网络或卡尔曼滤波预测动态障碍物轨迹，提前调整斥力方向。：预测时间间隔	障碍物运动模式复杂且难以建模的场景（如室外机器人避开来回走动的行人）。

3.2.3路径平滑与运动约束处理

动态窗口法（DWA）融合	- APF 生成全局参考路径，DWA 在局部实时调整速度和转向； - 评价函数包含路径距离、避障安全性和运动平滑性。 -：动态速度窗口 - ：权重系数 - 综合评价路径距离、安全性和运动平滑性	动态障碍物密集且需实时响应的场景（如无人船在港口避障）。

3.2.4混合算法与协同控制

多智能体协同 APF	- 构建多智能体斥力场，协调个体间距离； - 采用匈牙利算法分配虚拟目标点，形成紧密围捕阵型。 - ：智能体间距离 - ：协作距离阈值 - 匈牙利算法分配虚拟目标点	无人机蜂群协同攻击、仓储机器人集群搬运等多智能体任务。

3.2.5参数自适应与智能优化

遗传算法改进	- 用遗传算法优化势场函数参数（如）； - 染色体编码为参数向量，适应度函数评估路径质量。	多目标优化场景（如路径最短 + 能耗最低的机器人导航）。

3.3 DS动态系统调制方法

DS（动态系统）调制是一种基于动态系统的反应式调制控制策略，通过构建连续的矩阵值调制函数重塑全局动态系统的流场，以实现机器人在含非凸障碍环境中的导航，且需满足不可穿透性、收敛性、实时性等准则。其优点是能处理复杂非凸障碍、支持实时控制（如 1kHz 控制环速率）且具备不可穿透性与收敛性的理论保证。

3.4 强化学习

下面是强化学习的改进算法

多目标优化	HRL	任务分解、多目标奖励函数设计	工业机器人多任务执行、救援场景

以下是专门针对路径规划核心缺点（如稀疏奖励、局部最优、动态冲突等）进行针对性改进的强化学习（RL）算法

算法名称	针对的路径规划核心缺点	核心改进机制（RL 层面的针对性设计）	适用场景
HER 增强型路径规划算法	稀疏奖励（仅到达终点有奖励，中途无反馈导致学习缓慢）	采用后见之明经验回放（Hindsight Experience Replay, HER）：将未达目标的轨迹，以 "轨迹中某点为虚拟目标" 重新标注奖励，丰富经验池中的有效样本，加速 RL 对 "接近目标" 行为的学习。	室内机器人导航、无人机定点飞行、AGV 点对点运输（目标明确且环境静态 / 低动态）
*A 启发式引导 DQN**	局部最优（陷入墙角、绕障碍物循环，无法探索全局最优路径）	在 DQN 的价值函数更新或动作选择中融入 A算法的启发式信息（如曼哈顿距离、欧氏距离）： 1. 价值函数增加 "与 A 参考路径的偏差惩罚"； 2. 动作选择时优先保留 "向 A * 目标方向" 的候选动作，避免局部陷阱。	栅格地图下的移动机器人（如仓储机器人）、静态障碍物环境（如实验室巡检）
动态环境预测 DDPG	动态环境适应性差（障碍物移动 / 环境变化导致已规划路径失效）	在 DDPG 的状态输入层加入障碍物运动预测模块（如基于 LSTM/Transformer 预测障碍物下一步位置），并将预测结果融入 Actor-Critic 网络：使 RL 能 "前瞻性" 规划路径，避免与移动障碍物碰撞。	动态交通场景无人车、商场服务机器人（人群移动）、多移动障碍物室内环境
MADDPG 多智能体路径算法	多智能体冲突（多机器人 / 无人机碰撞、路径拥堵，缺乏协作机制）	每个智能体的Critic 网络同时接收 "自身状态 - 动作" 和 "其他智能体状态 - 动作" 信息，实现 "全局视角" 的价值评估； Actor 网络通过 "最小化群体冲突惩罚" 优化策略，达成协作避碰。	多 AGV 协同运输（工厂车间）、多无人机编队飞行、密集人群中服务机器人集群
安全约束 PPO（CPPO）	路径安全性低（易碰撞障碍物、违反物理约束，如机器人关节限位 / 速度超限）	在 PPO 的策略更新中加入硬安全约束项（通过拉格朗日乘数法实现）： 1. 奖励函数增加 "碰撞惩罚""安全距离不足惩罚"； 2. 策略更新时强制满足 "与障碍物最小距离≥安全阈值"，避免突破安全边界。	高安全需求场景：手术机器人、核电站巡检机器人、狭窄空间移动（如管道机器人）
分层强化学习：Option-Critic 框架	长距离规划不稳定（长路径下信用分配困难、轨迹发散，学习效率低）	将长距离规划拆分为多层子任务（如 "起点→中间点 A→中间点 B→终点"），通过 Option-Critic 框架学习： - 高层策略：选择 "当前应执行的子任务"； - 低层策略：执行子任务内的动作（如避障、转向），降低长任务的学习复杂度。	大范围室外导航（园区 / 矿区巡检机器人）、自动驾驶长途路径规划、多区域连通场景（机场 / 火车站）
DWA-RL	动态环境中运动学约束与避障决策的冲突（纯 DWA 缺乏全局决策能力，纯 RL 易忽略物理约束导致路径不可行）	融合动态窗口法（DWA）的运动学约束与 RL 的决策能力： 1. 利用 DWA 生成符合机器人速度 / 加速度限制的候选动作集（速度窗口）； 2. RL 基于环境状态（障碍物位置、目标方向）评估候选动作的奖励（如安全距离、目标接近度），选择最优动作； 3. 通过奖励函数强化 "满足运动学约束" 的行为，避免 RL 输出物理不可行的路径。	自动驾驶车辆动态避障、室内服务机器人在人群中导航、移动机器人高速运动场景（需兼顾实时性与物理可行性）

四、OMPL规划库

OMPL（Open Motion Planning Library，开源运动规划库）是机器人运动规划领域的核心开源工具库，由斯坦福大学等机构主导开发，专注于提供高效、灵活的运动规划算法实现，广泛应用于移动机器人、机械臂、无人机、自动驾驶等领域。它的核心定位是 "算法库" 而非 "完整解决方案"------ 不直接包含机器人建模或可视化功能，而是通过模块化设计，方便开发者集成到具体的机器人系统中。

Planner Developer Tools（PDT规划器开发工具）官方网站： https://robotic-esp.com/code/pdt/

OMPL官方网站 ：https://ompl.kavrakilab.org/

下面是一些OMPL的教程

ROS 运动规划 (Motion Planning): MoveIt! 与 OMPL