MATLAB路径规划实战：A*、PRM与RRT算法核心原理与工程应用对比-平芜编程栈

简介：本资源是一套面向计算机科学与技术等相关专业本科生的移动机器人路径规划MATLAB实践方案，适用于课程设计、期末大作业及算法综合实训等场景。聚焦A*、PRM与RRT三类经典路径规划算法，分别实现其改进版本——包括启发式优化的A搜索、融合A局部寻优的PRM框架，以及具备目标偏向与重采样机制的增强型RRT，完整覆盖全局规划与随机探索两类核心范式。压缩包共31个文件，含20个核心MATLAB源码（.m）、3个动态演示GIF、5个备份文件（.zbak）及1份README说明文档，总大小6.28MB，结构清晰、模块解耦，便于分步调试与算法对比分析。已有42人学习下载，所有代码均通过多地图测试，注释详尽、接口规范，配套可视化函数支持路径、障碍物、搜索过程的实时渲染，可直接运行复现结果，为算法理解、MATLAB工程实践与后续科研扩展提供扎实支撑。

1. 项目缘起：为什么需要同时搞懂A*、PRM和RRT？

如果你正在做移动机器人、无人机或者自动驾驶相关的项目，路径规划绝对是你绕不开的核心环节。我最早接触这个领域时，和很多人一样，以为路径规划就是找个最短路径，用个Dijkstra或者A算法就万事大吉了。直到真正把机器人放到复杂环境里跑起来，才发现问题远没那么简单：地图稍微大一点，A搜索就慢得让人抓狂；环境里障碍物形状稍微不规则一点，规划出来的路径就贴着障碍物边缘走，机器人根本不敢执行；更别提动态环境了，目标点一动，整个规划就得重来。

正是这些实际开发中遇到的痛点，让我意识到，没有一种算法是“银弹”。A*、PRM（概率路图法）和RRT（快速探索随机树）这三兄弟，各自代表了路径规划中不同维度的经典思路。A*是确定性格点搜索的标杆，PRM是解决高维空间采样的先驱，而RRT则是应对复杂约束和动态环境的利器。只懂其中一个，就像木匠只会用锤子，遇到需要拧螺丝或者刨木头的活儿就傻眼了。

所以，我决定用MATLAB这个强大的仿真验证平台，把这三个算法的核心思想、改进策略以及它们之间的对比，从头到尾实现一遍。这个项目的目的，不是简单地复现几个函数，而是要把每种算法“为什么这么设计”、“在什么场景下会失效”以及“我们该怎么改进它”这些问题讲透。无论你是刚入门的学生，还是需要快速验证算法可行性的工程师，希望这篇结合了代码与思考的总结，能给你带来实实在在的参考价值。

2. A*算法：在确定性的世界里寻找最优解

A*算法可以说是路径规划领域的“基本功”。它的思想非常直观：结合了Dijkstra算法确保找到最短路径的“完备性”，和贪心最佳优先搜索的“启发性”，通过一个评价函数f(n) = g(n) + h(n)来指导搜索方向。其中，g(n)是从起点到当前节点n的实际代价，h(n)是从当前节点n到目标点的预估代价（启发函数）。

2.1 经典A*的MATLAB实现要点与局限

在MATLAB里实现一个基础的A*，数据结构的设计是关键。我们通常需要维护两个列表：开放列表（Open List）和关闭列表（Closed List）。开放列表存放待考察的节点，关闭列表存放已考察过的节点。每次从开放列表中取出f值最小的节点进行扩展。

一个最直接的实现是用网格地图（Grid Map）。每个栅格是一个节点，移动代价通常考虑四连通或八连通。启发函数h(n)最常用的是曼哈顿距离（适用于四连通）或对角距离（适用于八连通）。代码结构大致如下：

初始化：将起点加入开放列表，其g值为0，f值为h(start)。
主循环： a. 如果开放列表为空，则路径不存在，失败退出。 b. 从开放列表中取出f值最小的节点current，将其移入关闭列表。 c. 如果current是目标点，则回溯路径，成功退出。 d. 遍历current的所有邻居节点neighbor： - 如果neighbor不可通过（障碍物）或已在关闭列表中，则跳过。 - 计算从起点经过current到neighbor的临时g值tentative_g。 - 如果neighbor不在开放列表中，或者tentative_g比它原有的g值更小，则更新neighbor的g、h、f值，并将其父节点设为current。如果它原本不在开放列表中，则加入。

这个经典实现虽然能保证找到网格地图上的最短路径，但在实际应用中立刻会暴露出几个问题：

搜索效率与地图尺度：当地图很大时，开放列表的维护（每次找最小值）和节点扩展会成为瓶颈。MATLAB中虽然能用min函数，但数据量大了依然很慢。
路径“不光滑”与“贴边”：由于搜索基于网格，规划出的路径是由一系列栅格中心点连接而成的折线，存在不必要的转折，且容易紧贴障碍物，不符合机器人运动学和控制要求。
启发函数的“陷阱”：如果启发函数h(n)不满足“可采纳性”（即永远不高估实际代价），A*就无法保证最优性。而在非网格地图（如连续空间）中，设计一个既高效又可采纳的启发函数并非易事。

2.2 针对移动机器人的A*改进策略

针对上述问题，我们在MATLAB实现中可以引入几种有效的改进：

2.2.1 数据结构优化：二叉堆提升效率直接使用数组或列表存储开放列表，每次查找最小f值节点是O(n)操作。改用二叉堆（最小堆）数据结构，可以将插入和提取最小值的操作降至O(log n)。在MATLAB中，我们可以自己实现一个简易的二叉堆，或者利用PriorityQueue的思想来管理开放列表，这对于大规模地图的搜索速度提升是立竿见影的。

2.2.2 路径后处理：让折线变平滑A*规划出的是由栅格中心点构成的路径P = {p1, p2, ..., pn}。我们可以通过后处理算法使其更符合机器人运动。

贪心路径简化：从起点开始，依次连接后续点，判断连线是否与障碍物相交。如果从pi到pj的直线无碰撞，则可以直接删除pi+1到pj-1的所有中间点。重复此过程，能得到一条由关键拐点组成的更简洁的折线。
梯度下降平滑：将路径点视为可移动的质点，定义一个包含路径长度和平滑度的代价函数，然后使用梯度下降法迭代调整路径点位置（起点和终点固定），使其在远离障碍物的同时更加平滑。MATLAB的优化工具箱（如fminunc）可以很方便地实现这一过程。

2.2.3 启发函数设计：权衡最优与速度除了曼哈顿距离和对角距离，在连续空间中，欧几里得距离是最直接的。为了加速搜索，有时可以使用略微“过估计”的启发函数（如将欧氏距离乘以一个大于1的系数），这虽然牺牲了最优性保证，但能极大加快搜索速度，这种变体称为“加权A*”。在实际中，这常常是一个有效的折衷。

2.2.4 跳点搜索（JPS）：跳过对称路径在均匀网格地图中，A*会扩展很多对称的、不必要的节点。JPS算法通过识别“跳点”（Jump Point）来跳过这些单调区域，直接向远处探索，能大幅减少扩展的节点数量。在MATLAB中实现JPS需要修改节点扩展规则，当某个方向存在“强迫邻居”或到达目标时，才认为发现了一个跳点。这对于存在大量空旷区域的地图，效率提升非常显著。

注意：A*及其改进算法本质上是“图搜索”算法，它们强依赖于一个离散的、预先定义好的图（如栅格地图）。当环境是高维连续空间（如机械臂关节角空间）或障碍物形状极其复杂时，构建这个图本身就会变得非常困难甚至不可能。这时，我们就需要PRM和RRT这类基于采样的规划方法。

3. PRM算法：为高维空间绘制一张概率路网

当机器人的自由度增加，比如是一个多关节机械臂，它的配置空间（C-Space）维度会变得很高。在这种高维空间中，像A*那样进行全局的、精细的网格划分和搜索，会遭遇“维度灾难”——所需的内存和计算时间呈指数级增长。PRM算法的核心思想非常巧妙：与其详尽地探索整个空间，不如通过随机采样的方式，在这个高维空间中撒下一系列“路标点”，然后尝试将这些点连接起来，形成一张稀疏的“路网”（Roadmap）。规划时，只需将起点和终点连接到这张网上，然后在网上搜索路径即可。

3.1 PRM的两阶段哲学与MATLAB实现

PRM通常分为两个阶段：学习阶段（Learning Phase）和查询阶段（Query Phase）。

3.1.1 学习阶段：构建路图这是PRM的核心，在MATLAB中我们可以这样实现：

随机采样：在机器人的自由配置空间（即无碰撞的区域）内，随机生成N个样本点（配置）。在MATLAB中，这通常意味着调用机器人的正运动学模型和碰撞检测函数，来验证一个随机生成的关节角度向量是否会导致机械臂与障碍物碰撞。
邻居查找与局部规划：对于每一个样本点q，找到它在一定距离r（邻居半径）内的所有其他样本点。然后，尝试用一条简单的局部规划器（最常用的就是直线连接）将q与每个邻居点连接起来。在MATLAB中，这条“直线”需要在配置空间进行碰撞检测，通常是在连线上进行密集采样（如插值10个点），逐一检查每个中间配置是否无碰撞。
构建图：如果q和某个邻居点之间的局部路径是无碰撞的，就在图中添加一条连接这两点的边。最终，我们得到一个无向图G=(V, E)，其中V是所有无碰撞的样本点，E是所有无碰撞的局部路径。

3.1.2 查询阶段：在线路径规划当给定具体的起点q_start和目标点q_goal后：

连接起终点：尝试将q_start和q_goal分别连接到路图G上。方法是找到G中离它们最近的几个节点，然后用局部规划器尝试连接。如果连接成功，就将这两个点临时加入图G。
图搜索：在更新后的图G上，使用图搜索算法（如A*或Dijkstra）寻找从q_start到q_goal的路径。

在MATLAB中，我们可以用graph对象来存储和操作这个路图，用shortestpath函数来进行查询阶段的搜索，非常方便。

3.2 PRM的瓶颈与针对性改进

经典的PRM算法简单有效，但它有几个明显的性能瓶颈，改进也主要围绕这些瓶颈展开：

3.2.1 采样策略：从完全随机到启发式引导完全随机采样在空旷区域效率很低，很多样本点“浪费”在了无关紧要的地方。改进方法包括：

障碍物边界采样：在障碍物附近进行更密集的采样，因为路径的“咽喉要道”通常出现在障碍物之间的狭窄通道处。可以在障碍物表面法线方向进行小范围扰动来生成样本。
高斯采样：以已有采样点为中心，进行高斯分布采样，使得新样本更可能出现在已有样本的邻域，有助于探索局部区域。
桥测试采样：随机生成一对紧挨着的点（一个在障碍物内，一个在障碍物外），取它们的中点。如果这个中点在自由空间，且其邻域内同时包含障碍物和自由空间，那么这个点很可能位于狭窄通道内，是一个高质量的采样点。

在MATLAB中实现这些策略，需要更精细的碰撞检测和几何判断，但能显著提升路图在复杂环境下的连通性。

3.2.2 邻居策略与局部规划器固定的邻居半径r是个难题：设大了，连接尝试的计算量暴增；设小了，图可能无法连通。可以采用k-最近邻策略，即尝试连接每个点的最近k个邻居。此外，局部规划器也不一定非要用直线。对于带有动力学约束的机器人，可以使用更复杂的局部规划器，如基于动力学的轨迹片段，但这会大大增加计算负担。

3.2.3 懒惰PRM：推迟昂贵的碰撞检测碰撞检测是PRM中最耗时的操作。懒惰PRM的核心思想是：在构建路图时，先假设所有随机点和潜在连接都是无碰撞的，快速构建一个完整的图。在查询阶段，当需要为具体的q_start和q_goal寻找路径时，再对候选路径上的边进行碰撞检测。如果某条边发生碰撞，则将其从图中删除，并重新搜索路径。这种方法将计算资源用在了“刀刃”上，特别适合多次查询同一张地图的场景。

实操心得：在MATLAB中实现PRM，碰撞检测函数的效率是绝对的性能关键。对于机械臂，建议预先将障碍物用简单的几何体（如长方体、圆柱体）包络，并利用空间划分数据结构（如AABB树）来加速碰撞查询。直接进行高精度的三角网格碰撞检测在MATLAB中会非常慢，不适合大规模采样。

4. RRT算法：像树根一样向未知空间生长

如果说PRM是“先织网，后找路”，那么RRT（快速探索随机树）则是“一边探索，一边找路”。RRT是一种单查询算法，它特别适合解决带有复杂约束（如非完整约束、动力学约束）的路径规划问题。它的思想是模拟一棵树在配置空间中向未探索区域快速生长的过程。

4.1 基础RRT：一个高效的探索者

基础RRT的MATLAB实现流程非常清晰：

初始化：将起点q_start作为树的根节点。
循环生长： a.随机采样：在整个配置空间中随机生成一个点q_rand。 b.寻找最近邻：在当前树的所有节点中，找到距离q_rand最近的节点q_near。距离度量通常是配置空间中的欧氏距离，但对于有不同量纲的关节空间，可能需要加权。 c.向随机点延伸：从q_near向q_rand方向延伸一个步长step_size，得到一个新点q_new。即q_new = q_near + step_size * (q_rand - q_near) / norm(q_rand - q_near)。 d.碰撞检测与添加节点：检查从q_near到q_new的路径段是否无碰撞。如果无碰撞，则将q_new加入树中，其父节点为q_near。
终止条件：如果q_new进入了目标点q_goal的某个邻域（例如，距离小于某个阈值），则认为规划成功，可以通过回溯父节点得到路径。也可以设置最大迭代次数。

RRT的强大之处在于它的探索能力。由于每次随机采样都引导树向空白区域生长，它能以概率完备的方式（只要迭代次数足够多）探索整个连通的空间。

4.2 RRT的演进与关键变种

基础RRT能找到一条可行路径，但这条路径往往质量不高（曲折、冗长）。因此，诞生了许多改进版本。

4.2.1 RRT-Connect：双向生长，大幅提速这是最著名且最有效的改进之一。其思想是同时从起点q_start和目标点q_goal生长两棵树（T_a和T_b）。在每次迭代中，其中一棵树（如T_a）执行一次标准的RRT扩展尝试，得到q_new。然后，不是就此结束，而是让另一棵树T_b尝试直接向这个q_new进行“贪婪连接”（Connect）：即从T_b的最近邻点开始，以最大步长不断向q_new延伸，直到发生碰撞或到达q_new。如果两棵树成功连接，则路径找到。这种方法极大地加快了树的汇合速度。在MATLAB中实现时，需要小心处理两棵树交替扩展和连接的逻辑。

4.2.2 RRT：渐进最优的奇迹* RRT* 是RRT算法的一个革命性改进，它能在迭代过程中使路径代价（通常是长度）渐进收敛到最优。它与基础RRT的主要区别在于两个关键步骤：

重新选择父节点（Rewiring）：在成功添加q_new后，RRT* 不会简单地将其父节点定为最近的q_near。而是在q_new附近一定半径内的所有节点中，寻找一个节点q_min，使得从起点经过q_min再到q_new的路径总代价最小，然后将q_min设为q_new的新父节点。
重布线（Rewiring）：上一步完成后，RRT* 还会检查q_new的加入，是否能为它邻居节点提供更优的路径。即对于q_new附近的每个邻居节点q_nearby，计算从起点经过q_new再到q_nearby的代价，如果这个代价小于q_nearby原有的代价，则把q_nearby的父节点改为q_new。

这两个步骤使得RRT* 生成的树在不断生长的同时，其内部连接也在不断优化，最终生成的路径会越来越短。在MATLAB中实现RRT*，需要仔细设计邻居半径（这个半径应随着节点数增加而递减），并维护每个节点到起点的代价。

4.2.3 Informed RRT：聚焦于优化* 标准的RRT* 在找到第一条路径后，仍然在全空间进行随机采样，其中很多采样点对优化当前路径没有帮助。Informed RRT* 在找到一条初始路径后，会将随机采样限制在一个“ Informed 子集”内——即一个以起点和终点为焦点的超椭球体内，这个椭球体内的任何点到起终点的路径长度都不会超过当前最优路径长度。这样就使采样集中在有可能改进当前路径的区域，大大提高了收敛到最优解的速度。在MATLAB中，这需要我们在每次更新当前最优路径后，动态调整采样范围。

5. MATLAB实战：对比、调试与可视化技巧

理论讲完了，最终都要落到代码上。在MATLAB中实现并对比这些算法，不仅能加深理解，更是工程应用的预演。下面分享一些关键的实战经验和调试技巧。

5.1 统一测试框架与性能指标设计

为了公平比较，我们需要建立一个统一的测试环境。这包括：

统一的地图表示：对于基于网格的A*，使用二维矩阵（0表示自由，1表示障碍）。对于PRM和RRT，需要编写一个通用的碰撞检测函数，输入一个配置（对于移动机器人是(x, y)），返回布尔值。
统一的起终点与障碍物：在同一张地图上设置相同的起点、终点和障碍物形状（如多边形障碍物）。障碍物的复杂度要分级，从简单空旷到复杂狭窄。
定义性能指标：
- 规划成功率：在固定时间或迭代次数内找到路径的比例。
- 路径长度：最终路径的欧氏距离总和。
- 规划时间：从调用函数到返回路径所消耗的CPU时间（使用tic和toc）。
- 搜索节点数/采样点数：反映算法的“探索成本”。
- 路径平滑度：可以用路径的总转角或曲率来度量。

在MATLAB中，我们可以编写一个测试脚本，循环调用不同算法的函数，并收集这些指标，最后用表格或图表（如bar,plot）进行直观对比。

5.2 算法核心环节的MATLAB编码细节

5.2.1 A*的二叉堆实现MATLAB没有内置的堆数据结构，但我们可以用数组模拟。维护一个节点列表和一个对应的f值列表。每次提取最小值时，用[~, idx] = min(fList)，然后将其与末尾元素交换并删除。插入时，直接加到末尾，然后上浮（与父节点比较交换）。虽然不如真正的堆高效，但比每次在完整列表中找min要快。

5.2.2 PRM的碰撞检测加速对于二维移动机器人，碰撞检测可以简化为判断点是否在多边形内（inpolygon函数）以及线段是否与多边形相交。对于后者，可以计算线段与多边形每条边的交点，但效率较低。一个更高效的方法是使用“分离轴定理”进行粗略判断，或者将障碍物进行膨胀处理（机器人半径），然后将机器人视为质点，只需判断点是否在膨胀后的障碍物内。

5.2.3 RRT的最近邻搜索优化* RRT* 中需要频繁进行两种查询：为随机点q_rand找最近邻，以及为新节点q_new找一定半径内的所有邻居。暴力搜索（遍历所有节点）的复杂度是O(n)，当树很大时不可接受。可以使用空间划分数据结构来加速，如KDTree。MATLAB的统计和机器学习工具箱提供了KDTreeSearcher对象，可以极大地提升最近邻和半径搜索的效率。

% 示例：使用 KDTree 加速 RRT* 的邻居查找 points = [tree.nodes.position]; % 假设 tree.nodes 是包含位置信息的结构体数组 kdtree = KDTreeSearcher(points); % 创建 KDTree [idx, dist] = rangesearch(kdtree, q_new, rewiring_radius); % 查找 q_new 半径内的所有邻居 neighbor_indices = idx{1}; % 获取邻居索引

5.3 强大的可视化：调试与理解的利器

MATLAB的图形能力是算法调试的绝佳助手。我习惯在算法运行的每次关键迭代后都更新图形，这能帮助我直观理解算法的行为。

实时绘制：对于RRT，可以在plot时使用hold on，并在添加新节点和新边时，用plot或line函数实时绘制出来。用不同的颜色区分树、最终路径、起点和终点。
动画记录：使用getframe和VideoWriter可以将规划过程录制成视频，这对于展示算法动态生长过程、对比不同算法行为非常有说服力。
绘制采样点：对于PRM，将所有的随机采样点（包括碰撞的和自由的）用不同颜色点绘制出来，可以清晰看到采样策略的效果。将最终的路图用线条绘制出来，可以直观检查其连通性。
绘制启发函数：对于A*，可以绘制出每个栅格的f值或g值的等高线图或热力图，这能直观展示算法的搜索前沿。

踩坑实录：在实现RRT-Connect时，我曾遇到一个棘手的Bug：两棵树偶尔会“穿过”一个非常薄的障碍物连接成功。原因是我的局部连接器（Connect函数）步长设置过大，且碰撞检测只在每个步长的终点进行，导致“跳过”了障碍物。解决方案是：在Connect过程中，不仅检查终点，还要以更小的分辨率检查整条延伸线段上的中间点。这个教训告诉我，在路径规划中，碰撞检测的“分辨率”必须高于机器人的“步进分辨率”，否则就会产生致命的碰撞风险。

6. 如何为你的项目选择算法？一张决策表

学完了三种算法，面对具体项目该如何选择？没有最好的算法，只有最合适的算法。下面这个基于经验的决策表，可以帮你快速做出初步判断：

考量维度	A* (及改进版)	PRM	RRT (及RRT*, RRT-Connect)
适用空间	低维离散空间（2D/3D网格）	高维连续空间（机械臂C-space）	高维连续空间，尤其适合非完整约束系统
规划类型	全局、静态路径规划	通常为全局、静态，也可用于动态（需重建图）	单次查询，静态/动态皆可（动态需快速重规划）
输出路径性质	最优（在给定启发函数下）	可行路径，非最优，取决于采样和连接	可行路径，基础RRT非最优，RRT*渐进最优
计算特点	搜索前需构建完整图（地图），查询快	预处理（学习阶段）耗时，但一旦建图，多次查询极快	无需预处理，每次查询独立计算，适合单次或环境变化的场景
内存消耗	与地图分辨率成正比（网格数量）	与采样点数成正比，通常远小于精细网格	与树的节点数成正比，通常可控
关键优势	保证最优性，在低维网格中非常成熟高效	能有效解决高维问题，路图可重复利用	强大的探索能力，能处理复杂约束，实时性相对好
主要劣势	维度灾难，难以处理复杂约束	在狭窄通道环境采样困难，图可能不连通	路径随机性大，基础RRT路径质量差，收敛到最优慢
典型应用场景	游戏AI、移动机器人2D导航、已知栅格地图	机械臂运动规划、已知复杂环境下的多任务规划	无人机避障、自动驾驶局部规划、带动力学模型的机器人规划

决策流程建议：

先看空间维度与约束：如果是2D/3D网格地图且无复杂运动约束，优先考虑A*（尤其是JPS）。如果是机械臂（6维以上），直接排除A*，在PRM和RRT间选择。
再看规划需求：如果需要为同一个环境规划成千上万次不同的路径（如仓库机器人调度），PRM的“一次建图，多次查询”优势巨大。如果环境频繁变化或只规划一次，RRT系列更合适。
最后看路径质量要求：如果对路径长度、平滑度有严格要求，A*（最优）或RRT*（渐进最优）是首选。PRM和基础RRT的路径需要后处理优化。
混合策略是王道：在实际复杂系统中，常常混合使用。例如，全局规划用A*或PRM生成一条粗略路径，然后局部规划器（如基于RRT的变种或DWA）负责跟踪这条路径并实时避障。

7. 超越基础：从仿真到现实的思考

在MATLAB里跑通算法，看到漂亮的路径动画，只是第一步。要让算法在真实的机器人上运行，还有大量的工程问题需要解决。

7.1 从连续路径到可执行轨迹规划算法输出的是一个路径点序列。机器人控制器需要的是一个随时间变化的轨迹（Trajectory），它包含了位置、速度、加速度甚至加加速度（Jerk）的信息。你需要进行轨迹生成，常见的方法有：

梯形速度规划：在路径点之间进行简单的匀加速-匀速-匀减速规划。
多项式插值（如三次样条、五次多项式）：可以保证路径点处的位置、速度甚至加速度连续，运动更平滑。
时间最优轨迹规划（TOPP）：考虑机器人的动力学约束（最大速度、加速度），生成时间最短的轨迹。

在MATLAB中，你可以先用规划算法得到路径，再调用优化工具箱（如fmincon）或机器人工具箱（如 Robotics System Toolbox）的轨迹生成函数来创建轨迹。

7.2 感知不确定性带来的挑战仿真环境中的地图是精确已知的。现实中，地图来自SLAM（同步定位与建图），存在噪声和误差。障碍物的位置和形状也可能不确定（如行人、临时摆放的箱子）。这就要求规划算法必须具备一定的鲁棒性。

在规划中引入安全边际：将障碍物进行膨胀（Inflation），膨胀半径为机器人半径加上一个安全裕量。
考虑感知不确定性：如果知道障碍物位置的概率分布，可以采用机会约束规划或基于采样的方法（如将障碍物视为随机区域，在规划时要求碰撞概率低于某个阈值）。
实时重规划：当传感器发现新的障碍物或与地图有较大出入时，需要能够快速重新规划。RRT系列算法由于其单次查询、无需预处理的特点，在重规划方面有天然优势。

7.3 与底层控制的结合规划层和控制器不能脱节。一条数学上最优的路径，如果曲率变化过大，可能超出底层轮式机器人差速控制的能力，导致跟踪误差大甚至失稳。对于非完整机器人（如汽车），还需要满足曲率约束。这就需要在规划阶段就考虑运动学甚至动力学约束。RRT及其变种（如Kinodynamic RRT*）通过直接在状态空间（包含速度、加速度）中采样和扩展，能够自然地生成满足约束的轨迹，这是它相比A*和PRM的一大优势。

在我自己的移动机器人项目里，最终的方案是一个分层架构：顶层使用改进的A*（带跳点搜索和路径平滑）在已知的代价地图上进行全局规划；底层使用一个局部规划器（融合了动态窗口法DWA和滚动优化的思想），结合实时激光雷达数据，跟踪全局路径的同时进行实时避障和速度规划。而MATLAB在这个项目中扮演的角色，就是前期所有算法原型验证、参数调优和性能对比的“数字沙盘”。只有在这个沙盘里把逻辑和边界情况都跑通了，才有信心把代码迁移到ROS（机器人操作系统）或嵌入式系统上。

本文还有配套的精品资源，点击获取