简介：这份资源是Kinodynamic RRT算法的MATLAB实现，对应Dustin J. Webb与Jur van den Berg的论文《Kinodynamic RRT: Optimal Motion Planning for Systems with Linear Differential Constraints》，面向从事机器人运动规划、最优控制与自动驾驶路径生成方向的研究生、科研人员及工程师。它解决的是带线性微分约束系统在动力学限制下的最优运动规划问题，适合已具备RRT与最优控制基础、希望复现论文算法或将其迁移到四旋翼、双积分器等平台的中高级学习者。压缩包共17个文件，约14KB，以13个.m脚本为主体，辅以2个.mat数据文件、1个md说明与1个txt许可文件；脚本覆盖rrtstar主流程、double_integrator与rrt_double_int_2d动力学建模、quad系列四旋翼状态采样与可视化、is_positive_definite等矩阵校验工具，数据文件则提供障碍物与路点场景。已有1296人学习下载。读者可据此获得可直接运行的算法骨架、论文核心公式的代码映射、双积分器与四旋翼两类实验场景，以及状态采样、碰撞检测与场函数绘制的完整实现思路。

1. 从一份 MATLAB 源码说起：为什么运动动力学 RRT* 值得你花时间跑一遍

如果你做过移动机器人或者机械臂的路径规划，大概率写过或跑过 RRT、RRT*。但很多人第一次把 RRT* 用到真实平台上时会发现一个尴尬的事：规划出来的路径全是折线，机器人根本走不了——要么转弯半径不够，要么加速度超限，要么在狭窄通道里来回震荡。问题出在标准 RRT* 只考虑几何可行性，完全忽略了机器人的运动学与动力学约束。这份kinodynamic_rrt_star的 MATLAB 源码，解决的正是这个痛点：它在 RRT* 的采样与扩展框架里引入了状态空间和动力学模型，让规划出的轨迹天然满足速度、加速度甚至曲率约束。整套代码用 MATLAB 写成，不依赖任何工具箱，适合做算法验证、课程设计，也适合作为二次开发的起点。下面我按“能干什么 → 怎么跑 → 怎么改”的顺序，把这份源码拆开讲清楚。

2. 项目定位与技术选型：为什么用 MATLAB 写运动动力学规划

2.1 这个项目到底解决什么问题

标准 RRT* 的节点是二维或三维空间中的点，扩展时直接在两节点之间连直线，然后做碰撞检测。这种做法在几何层面没问题，但一旦机器人有非完整约束（比如差速小车不能横移）或者二阶动力学（加速度有限），直线连接就不可行了。kinodynamic_rrt_star的核心思路是把每个节点从“位置点”升级为“状态点”，状态里包含位置和速度（或更多维度），扩展时不再连直线，而是用一组控制输入在固定时间步长内做前向积分，生成一条满足动力学方程的轨迹段。

这样做的好处很直接：规划出来的每一段轨迹都是机器人实际能执行的，不需要后处理再做平滑。代价是状态空间维度变高，采样效率下降，所以代码里通常会配合启发式采样或目标偏置来加速收敛。这份 MATLAB 实现把整个流程拆成了几个独立函数，方便你替换动力学模型或调整采样策略。

2.2 技术栈与文件组织

项目是纯 MATLAB 实现，没有用到 Simulink 或任何工具箱，这意味着你只要有基础 MATLAB 环境就能跑。常见的文件组织方式是这样的：

kinodynamic_rrt_star/ ├── main.m % 入口脚本，设置参数并调用规划器 ├── kinodynamic_rrt_star.m % 核心规划函数 ├── steer.m % 控制输入采样与前向积分 ├── nearest_node.m % 最近邻搜索 ├── near_nodes.m % 近邻节点集合 ├── collision_check.m % 碰撞检测 ├── cost.m % 代价计算 └── draw_result.m % 可视化

这种拆法在 MATLAB 项目里很常见，每个函数职责单一，方便你单独替换。比如你想换成自行车模型，只需要改steer.m里的动力学方程，其他部分不用动。

2.3 状态空间与动力学模型的选择

代码默认使用的动力学模型通常是二阶积分器（double integrator），状态向量为[x, y, vx, vy]，控制输入为[ax, ay]。这个模型虽然简单，但足以验证算法框架。如果你要做差速小车，可以把状态改成[x, y, theta, v, omega]，控制输入改成[a, alpha]。关键是要保证steer.m里的积分步长和main.m里的时间参数一致，否则轨迹会断裂。

提示：MATLAB 的ode45也可以用来做前向积分，但在这个场景下没必要，固定步长的欧拉积分足够快，而且更容易控制轨迹段长度。

3. 核心模块拆解：从采样到轨迹生成的完整链路

3.1 主循环：采样、最近邻、扩展、重连

kinodynamic_rrt_star.m是整个项目的心脏，它的结构和标准 RRT* 类似，但每个环节都做了适配。下面这段代码是主循环的骨架：

% 初始化树 tree.nodes = start_state; tree.edges = []; tree.costs = 0; for iter = 1:max_iter % 1. 采样：带目标偏置的随机采样 if rand() < goal_bias x_rand = goal_state; else x_rand = sample_state(space_limits); end % 2. 最近邻：在状态空间里找距离最近的节点 idx_near = nearest_node(tree, x_rand, distance_metric); % 3. 扩展：从最近邻出发，采样控制输入并前向积分 [x_new, u_new, valid] = steer(tree.nodes(idx_near), x_rand, dt, dyn_model); if ~valid continue; end % 4. 碰撞检测：对整条轨迹段做检查 if collision_check(tree.nodes(idx_near), x_new, u_new, obstacles) continue; end % 5. 近邻重连：在半径 r 内找更优父节点 near_idxs = near_nodes(tree, x_new, radius); [parent_idx, cost_new] = choose_parent(tree, near_idxs, x_new, u_new); % 6. 插入新节点 tree = insert_node(tree, x_new, parent_idx, u_new, cost_new); % 7. 重连：用新节点优化近邻节点的代价 tree = rewire(tree, near_idxs, x_new); % 8. 检查是否到达目标 if norm(x_new(1:2) - goal_state(1:2)) < goal_tolerance break; end end

这段代码里有几个关键点值得展开。第一，采样函数sample_state不是简单地在位置空间均匀采样，而是在整个状态空间里采样，包括速度维度。这意味着速度分量的范围需要根据机器人实际能力设置，否则会采出大量不可行的状态。第二，steer函数返回的valid标志很重要，因为有些控制输入会导致积分后的状态超出物理限制，比如速度超过最大值，这时候应该直接丢弃这次扩展。第三，碰撞检测不是只检查端点，而是要对整条轨迹段做离散化检查，否则会漏掉中间穿墙的情况。

3.2 steer 函数：控制输入采样与前向积分

steer.m是运动动力学 RRT* 区别于标准 RRT* 的核心。标准 RRT* 的 steer 就是连直线，而这里需要采样控制输入并做积分。下面是一个典型的实现：

function [x_new, u, valid] = steer(x_near, x_rand, dt, dyn_model) % 采样控制输入：在允许范围内随机取 u = sample_control(dyn_model.u_min, dyn_model.u_max); % 前向积分：用欧拉法或 RK4 x_new = x_near; for t = 1:dt:dt x_new = x_new + dyn_model.f(x_new, u) * dt; end % 检查状态是否在允许范围内 valid = check_state_limits(x_new, dyn_model.state_min, dyn_model.state_max); % 可选：如果离目标太远，可以调整控制输入方向 % 这里不做，保持随机性 end

这段代码的逻辑是：从最近邻节点出发，随机采一个控制输入，在固定时间dt内做前向积分，得到新状态。dyn_model.f是动力学方程，对于二阶积分器就是[vx; vy; ax; ay]。dt的选择很关键，太大则轨迹粗糙，太小则树增长慢。我一般会取0.1到0.5秒之间，具体看机器人速度。

注意：如果dt固定，而控制输入随机，那么新状态的速度分量可能变化很大。有些实现会限制控制输入的采样范围，让速度变化更平滑，这样规划出的轨迹更适合实际执行。

3.3 最近邻与近邻搜索：距离度量怎么选

在状态空间里做最近邻搜索，距离度量不能只用欧氏距离。因为状态里包含位置和速度，量纲不同，直接算欧氏距离会导致速度分量主导。常见的做法是加权欧氏距离：

function d = distance_metric(x1, x2, weights) diff = x1 - x2; d = sqrt(sum((weights .* diff).^2)); end

weights是一个向量，位置分量权重通常设为 1，速度分量权重设为 0.1 到 0.5。这样位置差异对距离的影响更大，符合规划直觉。近邻搜索near_nodes则是找半径r内的所有节点，r可以固定，也可以随迭代次数递减。固定半径实现简单，但后期可能找不到足够的近邻；递减半径更接近标准 RRT* 的理论，但需要调参。

3.4 碰撞检测：离散化检查与障碍物表示

碰撞检测在运动动力学规划里比几何规划更麻烦，因为轨迹是曲线，不是直线。常见做法是把轨迹按时间步离散成若干点，然后逐点检查。障碍物可以用圆形、矩形或多边形表示，检查函数根据障碍物类型不同而不同。下面是一个圆形障碍物的检查示例：

function collision = collision_check(x_start, x_end, u, obstacles, dt) collision = false; x = x_start; for t = 1:dt:dt x = x + dyn_model.f(x, u) * dt; for i = 1:length(obstacles) if norm(x(1:2) - obstacles(i).center) < obstacles(i).radius + robot_radius collision = true; return; end end end end

这段代码把轨迹段按dt离散，每个离散点都检查是否与障碍物相交。robot_radius是机器人外接圆半径，用来做膨胀处理。如果障碍物是矩形，可以用inpolygon或者自己写点在矩形内的判断。

4. 跑起来：参数配置、运行步骤与结果验证

4.1 环境准备与入口脚本

这份代码不需要额外工具箱，但需要 MATLAB R2016b 或更高版本，因为用到了string类型和某些函数句柄特性。把整个文件夹放到 MATLAB 当前路径下，直接运行main.m即可。main.m里通常包含以下配置：

% 状态空间限制 space_limits = [-5, 5; -5, 5; -1, 1; -1, 1]; % [xmin xmax; ymin ymax; vxmin vxmax; vymin vymax] % 起点和终点 start_state = [0; 0; 0; 0]; goal_state = [4; 4; 0; 0]; % 动力学模型参数 dyn_model.u_min = [-1; -1]; dyn_model.u_max = [1; 1]; dyn_model.f = @(x, u) [x(3); x(4); u(1); u(2)]; % 算法参数 max_iter = 5000; goal_bias = 0.1; dt = 0.2; goal_tolerance = 0.2;

这些参数直接决定了规划效果。space_limits里的速度范围要和机器人实际能力匹配，设太大则采样效率低，设太小则可能找不到可行解。goal_bias是目标偏置概率，一般取 0.05 到 0.2，太大容易陷入局部最优，太小则收敛慢。

4.2 运行步骤与中间输出

运行main.m后，代码会进入主循环，每轮迭代都会在图上更新树结构。如果你想看中间过程，可以在循环里加draw_result调用，但会拖慢速度。建议先跑完再画图。跑完后，draw_result会画出树结构、最终路径和障碍物。最终路径是从目标节点回溯到根节点得到的，每条边对应一段轨迹。

如果跑完发现没有找到路径，先检查max_iter是否够大，再检查goal_tolerance是否太小。有时候目标点附近有障碍物，导致永远无法精确到达，这时候可以适当放宽容差。

4.3 结果验证：路径是否满足动力学约束

规划出的路径不能只看是否到达终点，还要验证每一段是否满足动力学约束。最简单的方法是检查每段轨迹的速度和加速度是否在允许范围内。可以在draw_result里加一段验证代码：

% 验证路径可行性 path = extract_path(tree, goal_idx); for i = 1:length(path)-1 x1 = path(i).state; x2 = path(i+1).state; u = path(i).control; % 检查速度限制 if any(abs(x2(3:4)) > dyn_model.state_max(3:4)) warning('速度超限 at segment %d', i); end % 检查加速度限制 if any(abs(u) > dyn_model.u_max) warning('加速度超限 at segment %d', i); end end

这段代码遍历路径的每一段，检查末端速度和加速度。如果发现超限，说明steer函数里的控制采样范围设大了，或者积分步长dt不合适。

5. 避坑与常见问题：跑不通时先看这几条

5.1 现象：树增长极慢，迭代几千次还没到目标

原因通常是状态空间维度太高，或者速度范围设得太大，导致采样点大量落在不可行区域。解决方法是缩小速度范围，或者改用启发式采样，比如在目标方向附近增加采样概率。另一个可能是dt太小，每段轨迹太短，树扩展慢。可以适当增大dt，但不要超过 0.5 秒，否则轨迹太粗糙。

5.2 现象：规划出的路径抖动严重，机器人无法跟踪

这是因为控制输入采样完全随机，相邻轨迹段之间没有平滑性约束。解决方法是限制控制输入的采样范围，让相邻段的控制输入变化不要太大。可以在steer里加一个参数，让新控制输入与上一段控制输入的差值不超过某个阈值。另一个方法是后处理做轨迹平滑，但这会破坏 RRT* 的最优性保证。

5.3 现象：碰撞检测漏检，路径穿过障碍物

原因通常是离散化步长太大，轨迹段中间的点被跳过了。解决方法是减小碰撞检测的离散步长，至少比dt小一个数量级。如果障碍物很薄，还需要做连续碰撞检测，但这在 MATLAB 里实现起来比较麻烦，一般用更细的离散化就够了。

5.4 现象：重连后代价反而变大

这是 RRT* 实现里的经典坑。重连时要用新节点作为父节点，重新计算近邻节点的代价，但如果代价计算函数没有考虑动力学约束，或者代价传播没有正确更新，就会出现代价震荡。解决方法是确保cost.m里的代价计算和rewire里的更新逻辑一致，并且每次重连后都要更新整棵子树的代价。

5.5 现象：MATLAB 报错“索引超出矩阵维度”

这种错误通常出现在near_nodes返回空集，或者choose_parent里没有正确处理空集。解决方法是加判断：如果近邻集合为空，直接用最近邻作为父节点。另外，tree.nodes如果是矩阵，插入新节点时要注意维度匹配，建议用结构体数组或者元胞数组存储节点。

6. 二次开发：换动力学模型、加约束、提效率的实操思路

6.1 替换动力学模型：从二阶积分器到自行车模型

如果你想把这套代码用到差速小车上，最直接的方法是改steer.m里的动力学方程。二阶积分器的状态是[x, y, vx, vy]，自行车模型的状态是[x, y, theta, v]，控制输入是[a, omega]。动力学方程变成：

dyn_model.f = @(x, u) [x(4)*cos(x(3)); x(4)*sin(x(3)); u(2); u(1)];

这里x(3)是朝向角，x(4)是线速度，u(1)是加速度，u(2)是角速度。改完方程后，状态限制、距离度量、碰撞检测都要相应调整。距离度量里朝向角的差值要处理成[-pi, pi]范围，否则会出现跳变。

6.2 加入曲率约束与速度约束

自行车模型天然有曲率约束，因为角速度和线速度的比值决定了转弯半径。如果你想显式限制曲率，可以在steer里加判断：如果abs(u(2)/x(4)) > max_curvature，就丢弃这次采样。速度约束则通过状态限制实现，在check_state_limits里检查x(4)是否超过最大值。

6.3 提升规划效率：启发式采样与并行化

MATLAB 的循环效率不高，如果max_iter设到几万，跑起来会很慢。提升效率的几个方向：一是用启发式采样，比如在目标点附近增加采样概率，或者用informed RRT*的思路，在椭圆区域内采样；二是把碰撞检测向量化，一次检查多个障碍物；三是用parfor并行化多个独立采样，但 RRT* 的树结构是共享的，并行化需要小心处理。

6.4 验证改造效果：一套可复用的测试流程

每次改完动力学模型或参数，建议按以下流程验证：先用小规模场景（无障碍物、起点终点距离近）跑通，确认能到达目标；再加一个障碍物，检查避障是否正常；最后加多个障碍物和狭窄通道，检查规划成功率和路径质量。路径质量可以用代价、平滑度、规划时间三个指标衡量。我一般会跑 20 次取平均值，避免单次随机性影响判断。

提示：MATLAB 的rng函数可以固定随机种子，方便复现结果。调试时固定种子，对比不同参数的效果会更直观。

6.5 一个容易忽略的细节：控制输入的连续性

标准 RRT* 不关心控制输入的连续性，但实际机器人跟踪轨迹时，控制输入突变会导致执行器饱和或震荡。一个简单的改进是在steer里让新控制输入与父节点的控制输入做加权平均，权重可以设为 0.7 和 0.3。这样轨迹会更平滑，但可能牺牲一点最优性。这个改动很小，但效果很明显，我每次做运动动力学规划都会加上。

从那以后我每次改动力学模型，都会先固定随机种子跑 20 次，确认成功率没掉再继续调参。希望这份拆解能帮你少走点弯路，把这份 MATLAB 源码真正跑起来、改起来。

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