简介基于MATLAB的动力学RRT算法实现面向机器人路径规划研究者和学生解决含动力学约束的运动规划问题。该算法在经典RRT基础上引入速度、加速度等限制使规划结果不仅几何可行也具备动态可行性适用于无人机、移动机器人等场景的仿真与学习。资源共17个文件以13个m脚本为核心覆盖主程序、RRT*主体、动力学模型、距离函数、插值及可视化等模块另有2个mat数据文件提供障碍物与航点配置以及txt与md说明文档压缩包仅14KB结构紧凑清晰。代码包含二维双积分器与四旋翼两种算例便于对比不同动力学模型的规划效果注释和模块划分也适合二次开发。目前已有1754人学习下载通过阅读和运行代码可直观理解状态空间表示、采样、近邻查找、路径优化等完整流程并能快速修改参数验证不同场景适合作为课程设计或科研初期的参考实现。1. kinodynamic RRT*这不是给普通RRT*加了个动力学模型那么简单如果你试过把经典 RRT* 的代码直接拿过来、把节点从 (x, y) 换成 (x, y, vx, vy)大概率会在两个地方翻车一是树怎么都长不大二是好不容易找到一条路径把轨迹丢给仿真器一看加速度曲线早就冲破限幅了。原因在于带动力学约束的运动规划里“状态”不再是一个位置点而是一组包含位置和速度的向量两点之间的“距离”也不再是欧氏距离而是满足动力学可行性的最短时间或最小能量。这份 kinodynamic RRT* 的 Matlab 实现解决的正是这个问题。我把它拆开看过一遍核心代码不复杂但 steer、距离度量、rewire 这三块的处理方式决定了它能不能收敛、轨迹能不能真正被执行。适合正在做运动规划课题、需要把动力学约束写进采样树里的开发者也适合想搞懂 kinodynamic RRT* 和普通 RRT* 本质差异的人。2. 算法架构状态空间、距离度量与局部规划器是三个关键分水岭2.1 状态空间从几何构型到“几何 速度”经典 RRT* 处理的是几何构型空间里的路径规划问题节点是一个位置坐标边是一段直线或圆弧碰撞检测只需要检查这条几何线段是否穿过障碍物。但在真实机器人、无人机、自动驾驶场景里被控对象有质量、有惯性、有执行器饱和控制器输出的是一条随时间变化的控制序列而不是一条几何曲线。这就是 kinodynamic 规划要解决的核心问题。这份代码把状态定义为x [px, py, vx, vy]其中(px, py)是位置(vx, vy)是速度。输入是加速度向量u [ax, ay]满足|ax| amax、|ay| amax的限幅约束。系统模型是标准的双积分模型离散化后写成x_next [px vx * dt; py vy * dt; vx ax * dt; vy ay * dt];这是整个实现里最基础也最重要的一行。它意味着树里的每一条边不再是一条线段而是一段离散时间序列上的状态转移。这条边是否合法要看这段轨迹上每一个时刻的加速度是否在限幅内、速度是否不超过vmax、每个采样点是否都不在障碍物里。正因为状态里带了速度分量采样阶段也需要在状态空间里进行。代码里的sampleState函数做法是以一定概率比如 0.1把采样点偏置到目标位置其余时间在位置空间随机采样速度分量的采样则是在[-vmax, vmax]范围内随机生成。这里有个容易被忽略的点目标偏置采样时位置被设成终点但速度不能被设成零否则树末端的节点全都带着“零到达速度”去接近目标反而会让局部规划器更容易失败。2.2 距离度量欧氏距离在带动力学约束的系统里是坑RRT* 的最近邻查找决定了搜索树的扩展方向。经典 RRT* 用欧氏距离做度量因为几何空间里两点连线的长度就是传播代价的合理估计。但到了 kinodynamic 场景从状态 A 到状态 B 的时间代价不是由直线距离决定的而是由动力学可行轨迹决定的。举个例子假设当前节点速度是 (5, 0)目标节点速度是 (-5, 0)。欧氏距离可能很小因为两个位置挨得很近但要让一个正在向右飞的物体瞬间改为向左飞它必须经历减速、反向加速的过程所花的时间和消耗的能量远大于欧氏距离暗示的数值。如果最近邻查找用欧氏距离搜索树就会频繁地向实际上“动力学可达性很差”的方向扩展导致 steer 函数大量返回失败树生长效率急剧下降。这份代码里实现了一个基于时间估计的启发式距离度量function d distanceMetric(x1, v1, x2, v2, vmax, amax) % 用一维最短时间最优控制的解析结果估算状态间的时间代价 d1 abs(x2(1) - x1(1)) / min(vmax, abs(v1(1)) amax * 0.5); d2 abs(x2(2) - x1(2)) / min(vmax, abs(v1(2)) amax * 0.5); d max(d1, d2); end这个公式不追求精确而是给最近邻搜索一个合理的优先级排序。它粗略估算了在两个轴向上分别完成位置变化所需的最短时间然后取两者中的较大值作为距离。物理含义是如果横向位移比纵向位移更难完成那么总时间由横向位移决定。实际工程里更精确的做法是预计算一张可达域表或者对每对状态调用一次局部规划器再取返回时间但那样计算量太大不适合每轮扩展都调用的最近邻查找。2.3 局部规划器steer 函数是重活所在局部规划器也就是 steer 函数是 kinodynamic RRT* 和几何 RRT* 的分水岭。几何版本只需要算一条线段kinodynamic 版本则要解一个两点边值问题BVP给定起点状态(x0, v0)、终点状态(xf, vf)和总时间 T找一条满足动力学方程和边界条件的轨迹。严格的最优控制解法是用庞特里亚金最小值原理推导出控制量的 bang-bang 结构再通过数值方法求解切换时间。但那种做法在 Matlab 里实现起来比较啰嗦而且对初值敏感。这份代码采用的是一种工程上很实用的替代方案用三次多项式插值构造轨迹然后迭代调整总时间 T直到整条轨迹的加速度不超过限幅。function [traj, feasible] steerBVP(x0, v0, xf, vf, T, dt) % 用三次多项式构造满足边界条件的轨迹加速度超限时增大T重试 traj.x []; traj.v []; traj.feasible false; for attempt 1:20 % 三次Hermite插值系数x轴和y轴分别计算 a2 (3*(xf(1) - x0(1)) - (2*v0(1) vf(1))*T) / T^2; a3 (2*(x0(1) - xf(1)) (v0(1) vf(1))*T) / T^3; b2 (3*(xf(2) - x0(2)) - (2*v0(2) vf(2))*T) / T^2; b3 (2*(x0(2) - xf(2)) (v0(2) vf(2))*T) / T^3; % 离散生成轨迹并检查加速度限幅 t 0:dt:T; pos zeros(length(t), 2); vel zeros(length(t), 2); acc zeros(length(t), 2); for i 1:length(t) ti t(i); pos(i,1) x0(1) v0(1)*ti a2*ti^2 a3*ti^3; pos(i,2) x0(2) v0(2)*ti b2*ti^2 b3*ti^3; vel(i,1) v0(1) 2*a2*ti 3*a3*ti^2; vel(i,2) v0(2) 2*b2*ti 3*b3*ti^2; acc(i,1) 2*a2 6*a3*ti; acc(i,2) 2*b2 6*b3*ti; end max_acc max(max(abs(acc))); max_vel max(max(abs(vel))); if max_acc amax max_vel vmax traj.t t; traj.x pos; traj.v vel; traj.a acc; traj.feasible true; return; end % 加速度超限增大飞行时间重新构造 T T * 1.5; end end逻辑说明三次多项式能严格满足起点和终点的位置、速度边界条件这是数学上保证的。轨迹的加速度是时间的线性函数最大加速度出现在区间端点或极值点因此检查很方便。如果最大加速度超过amax说明给定时间 T 太短、物体来不及完成状态转移此时把 T 放大 1.5 倍重新构造最多迭代 20 次。参数说明T的初值对整个算法影响不小。太小会导致反复迭代甚至最终失败太大则轨迹运动缓慢、树扩展效率低。通常的做法是用 2.3 里的距离度量估算一个初始 T再乘一个 1.2 到 1.5 的安全系数。速度上限vmax的检查容易被忽略但三次多项式在 T 较小时可能会产生速度尖峰所以必须和加速度一起检查。2.4 树结构与重连RRT* 的最优性靠 rewire 保住RRT* 和 RRT 的本质区别在于每插入一个新节点后会尝试用这个新节点去重新连接周围一定半径内的旧节点如果新路径代价更低且动力学可行就把旧节点的父节点改掉。这个过程叫 rewiring也是保证算法渐近最优性的关键机制。在 kinodynamic 版本里rewire 的代价判断用的是累计时间或控制能量。节点结构里除了状态向量还存了cost从起点到该节点的累计时间和parent索引。当新节点插入后算法找出半径 r 内所有节点对每个节点尝试用新节点作为新的父节点连接检查轨迹可行性后比较代价可行就替换。这个过程中最容易犯的错误是在 rewire 时只检查轨迹是否碰障碍物不检查动力学可行性。或者反过来只调用 steer 判断可行性但忘记做碰撞检测。这条路径稍后我会在避坑章节细说。总之rewire 是 RRT* 的灵魂kinodynamic 版本里它的计算开销远大于几何版本因为每次尝试都要调用一次局部规划器所以邻居半径 r 的取值要谨慎——取太大了每轮扩展都在反复解 BVP性能直线下降。3. Matlab 实现拆解模块划分与核心函数逐段过3.1 目录结构与调用关系这份代码按功能拆成了 10 个文件结构很清晰kinodynamic_rrt_star/ ├── main.m # 主入口配置参数、构建环境、循环扩展 ├── initTree.m # 初始化搜索树根节点为起点状态 ├── sampleState.m # 在状态空间采样目标点 ├── findNearest.m # 用时间启发距离找最近邻居 ├── steerBVP.m # 局部规划器生成动力学可行轨迹 ├── checkCollision.m # 碰撞检测轨迹采样点是否碰到障碍物 ├── findNearNodes.m # 找半径内的邻居节点集合 ├── insertNode.m # 把新节点加入树结构 ├── rewire.m # 用新节点尝试重新连接邻居 ├── plotTree.m # 可视化绘制树、障碍物和最终轨迹 └── solveTInitial.m # 估算BVP初始飞行时间调用关系是main.m每轮扩展调用sampleState采样findNearest找最近节点steerBVP生成轨迹checkCollision验证安全性通过后insertNode入库接着findNearNodes取邻居、rewire做重连。可视化在每轮扩展后刷新一次方便观察树的生长过程。3.2 主循环主循环是整套代码的骨架我把它简化成核心逻辑for iter 1:maxIter x_rand sampleState(map, goal, vmax); near_idx findNearest(tree, x_rand, vmax, amax); [traj, feasible] steerBVP(tree(near_idx).x, tree(near_idx).v, ... x_rand(1:2), x_rand(3:4), T_init, dt); if ~feasible || checkCollision(traj, obstacles) continue; end new_node insertNode(tree, near_idx, traj); near_indices findNearNodes(tree, new_node, r, vmax, amax); tree rewire(tree, new_node, near_indices, obstacles, dt); if isGoalReached(new_node, goal, vmax) path extractPath(tree, new_node); plotTree(tree, path, obstacles, map); break; end end逻辑说明循环的第一步是在状态空间采样一个随机目标点第二步找最近邻居第三步调用局部规划器生成可行轨迹。如果轨迹不可行或撞上障碍物直接跳过这一轮继续采样。轨迹通过后新节点插入树中然后在半径 r 内查找邻居节点并尝试 rewire。当某个新节点离目标足够近且速度足够小时就认为路径已经找到回溯父节点链输出轨迹。参数说明maxIter控制最大迭代次数一般设 3000 到 5000。找到路径后立即跳出循环这个设计在实际使用中比跑满迭代次数再搜索更高效。isGoalReached的判断条件要同时检查位置误差和速度大小位置误差小于某个阈值如 1.0速度大小小于vmax的十分之一才算真正到达目标。如果只检查位置可能出现节点“飞过”目标点的情况轨迹末端速度依然很大产生明显急停。3.3 采样与最近邻采样函数的设计直接决定了树的探索方向function x_rand sampleState(goal, vmax, goalBias) % 以goalBias概率偏向目标否则全局随机采样 if rand() goalBias x_rand [goal(1); goal(2); 0; 0]; else x_rand [rand()*mapWidth; rand()*mapHeight; ... (rand()-0.5)*2*vmax; (rand()-0.5)*2*vmax]; end end注意目标偏置时速度设为 0。刚接触 kinodynamic 规划的人经常在这里犯错——把速度也随机采样导致大量偏置样本虽然位置正确但速度方向和大小与当前树节点根本不匹配steer 函数试几十次才能成功一次。最近邻搜索用的不是普通欧氏距离而是第 2.2 节的时间启发度量。这个函数在节点数量少的时候可以直接遍历整棵树节点超过几百个以后就要改成 kd-tree 或者网格索引Matlab 里可以用KDTreeSearcher加速。3.4 局部规划器三次多项式法这是全项目最核心的模块我单独拎出来再讲细一点。steerBVP的本质是解两点边值问题这里用的三次多项式法是“固定时间 T、构造可行轨迹、超限则增大 T”的迭代策略。它比真正的 bang-bang 最优控制简单但作为树生长的边缘连接已经够用因为 RRT* 的最优性最终由多次迭代和 rewire 来保证。真实的 kinodynamic RRT* 里局部规划器通常用多次打靶法配合非线性优化器求解。我见过有实现直接调fmincon求控制序列效果当然好但每轮扩展要跑几百次优化单次迭代时间从毫秒级涨到秒级。三次多项式法属于“牺牲一点最优性、换一个数量级的性能提升”的典型取舍在多数教学和预研场景里是更务实的选择。3.5 碰撞检测碰撞检测沿轨迹的每个采样点检查是否落在障碍物内部function free checkCollision(traj, obstacles) free true; for i 1:size(traj.x, 1) p traj.x(i, :); for j 1:length(obstacles) if inpolygon(p(1), p(2), obstacles(j).x, obstacles(j).y) free false; return; end end end end这里的障碍物是多边形用 Matlab 自带的inpolygon判断。这个函数够用但要注意采样点的密度dt取得太大轨迹上两个采样点之间可能“跨过”一个薄障碍物碰撞检测漏报。经验做法是采样间距不超过障碍物最小厚度的三分之一如果障碍物最小厚度是 0.5 米采样间距就要小于 0.17 米。3.6 rewirerewire 的代码逻辑是这套实现里第二容易出错的地方function tree rewire(tree, new_node, near_indices, obstacles, dt) for i 1:length(near_indices) idx near_indices(i); if idx new_node.parent continue; end [traj, feasible] steerBVP(new_node.x, new_node.v, ... tree(idx).x, tree(idx).v, tree(idx).cost, dt); if feasible ~checkCollision(traj, obstacles) new_cost new_node.cost traj.t(end); if new_cost tree(idx).cost tree(idx).x_traj traj; tree(idx).parent new_node.id; tree(idx).cost new_cost; end end end end这里steerBVP的 T 参数初值直接用tree(idx).cost表示当前节点的累计代价。这是一个工程上的取巧做法因为新节点到旧节点的最优时间大致不会超过从起点到旧节点的总时间用它做初值可以在多数情况下减少迭代次数。需要注意的是rewire 时轨迹是从新节点连向旧节点而不是反过来因为我们需要判断“把旧节点的父节点换成新节点”是否划算。3.7 可视化与结果输出可视化用plot绘制树、障碍物和最终轨迹。这里没有用复杂的 3D 渲染2D 平面图足够看清算法行为。我的建议是除了画轨迹也画一下路径上的速度曲线和加速度曲线这样能立刻看出轨迹是否满足动力学约束。如果加速度曲线有削顶超过amax被截断说明 steer 的迭代逻辑有 bug。4. 跑起来环境要求、参数配置与验证方法4.1 运行环境这份代码只依赖 Matlab 基础工具箱不需要额外安装 Robotics System Toolbox 或 Optimization Toolbox。我测试过的版本是 Matlab R2020a 和 R2023a都能直接跑通。代码里的inpolygon、plot这些都是内置函数不涉及工具箱依赖。运行方式很简单把整个文件夹加进 Matlab 路径在命令行执行main然后看弹出的窗口里树的生长过程。第一次运行建议先把maxIter调大到 10000因为树需要一段时间才能绕过障碍物到达目标。如果跑了 30 秒还没有任何节点插入树里大概率是参数设置有问题往下看参数表。4.2 关键参数表与作用参数默认值作用调参方向mapWidth / mapHeight100 / 100规划空间尺寸直接影响采样范围空间越大树越难覆盖amax10.0加速度限幅太小会让轨迹过于平缓、T 迭代次数暴增vmax15.0速度限幅太小会让树扩展缓慢太大则轨迹难以满足限速dt0.1离散时间步长碰撞检测精度和轨迹平滑度的平衡点goalBias0.1目标偏置概率偏置越大收敛越快但容易错过复杂通道r15.0rewire 邻居半径太大每轮重连计算量巨大太小失去最优性maxIter5000最大迭代次数空间大或障碍复杂时需要增大这些参数的敏感性差别很大。dt是最基础的一个它决定了轨迹离散化的密度改小了轨迹精度高但协同计算量增大amax和vmax是动力学模型的物理参数应该根据被控对象的真实能力设定不要为了容易收敛而人为调大否则生成的轨迹在真实系统上根本跑不出来。4.3 怎么判断跑对了跑出一个结果不算完我的习惯是看三张图轨迹图、速度曲线图、加速度曲线图。轨迹图上树应该均匀地向各个方向生长而不是聚成一团或只在某个区域反复扩展。如果树只在一个小区域打转多半是最近邻度量有问题或者采样分布不合理。速度曲线图上速度应该平滑变化不应该出现尖角或跳变。三次多项式法生成的轨迹天生是平滑的如果出现剧烈波动说明dt太大离散化误差累积。加速度曲线图上所有值都应该在[-amax, amax]区间内。如果超出说明 steer 的迭代逻辑没生效仔细检查steerBVP.m里 T 的更新是否真的执行了以及max_acc的计算方式是否正确。4.4 常见失败表现我实际跑的过程中失败表现主要有三种。第一种树长得很慢几千次迭代只扩展了几十个节点。原因几乎都是采样空间和最近邻度量不匹配或者T_init估算得太小导致 steer 频繁失败。第二种树长得很茂盛但是轨迹总是紧贴障碍物边缘滑过看起来像“擦边球”。原因是碰撞检测的采样间距太大漏报了一些穿透障碍物的轨迹段。第三种树最终找到了路径但路径上出现明显的急转弯。这说明 T 迭代太快收敛到局部可行解而没有继续增大 T 换取更平滑的轨迹。5. 避坑手册我调试这个 Demo 时踩过的 5 个坑5.1 最近邻用欧氏距离树看起来茂盛但全都连不通现象树扩展速度飞快几千个节点插进去了但最终找不到到达目标的路径。回头看树的结构发现节点确实分布在障碍物周围但每条边对应的轨迹在动力学上都是不可行的。原因欧氏距离把两个相邻但速度方向相反的状态判成“很近”steer 函数尝试连接它们时需要让物体先减速再反向加速轨迹的加速度严重超限于是大量返回feasible false。几何 RRT* 里几乎不会遇到这种情况状态空间里“近”的定义已经变了。解决换成时间启发距离度量。这个坑是我调这套代码时翻得最狠的一次整整浪费了一个晚上。从那以后我每次做 kinodynamic 规划第一件事就是检查最近邻用的什么度量。5.2 碰撞检测采样间距太大薄障碍物被直接穿过现象轨迹画出来和障碍物有交点但checkCollision返回了free true。原因dt 0.1时轨迹上相邻采样点之间的距离大约是一次速度积分的结果。如果障碍物厚度小于这个距离两个采样点分别落在障碍物两侧中间的穿透段就漏掉了。解决把dt调小到 0.02或者检查每个分段的折线相交而不是只检查采样点。前者简单后者更严谨。我最后用了“采样点 分段相交”双重检查不再漏报。5.3 rewire 时只做碰撞检测忘了检查动力学可行性现象最终路径找出来了但某一段轨迹的速度曲线有明显跳变加速度远超限幅。原因rewire 里调用了steerBVP来判断新边是否可行但有一步我写得太顺手直接把feasible的返回值忽略了只检查了checkCollision。结果就是树里插入了一批“几何上不撞、动力学上不可行”的边。解决严格检查feasible和碰撞检测两个条件同时成立才允许接边。这个坑的隐蔽之处在于RRT* 找路径时大概率不会用到这些非法边但一旦用到整个解就废了。5.4 目标偏置采样把速度设成随机值收敛速度暴跌现象目标偏置概率调到了 0.5但树反而不如偏置 0.1 时收敛得快甚至根本找不到路径。原因偏置样本的位置指向目标但速度是随机生成的。当前树节点可能正在朝远离目标的方向运动要去连接一个“位置在目标、速度随机”的状态局部规划器需要同时调整速度和方向成功率极低。解决目标偏置时把速度设为零。这个改动之后到达目标的成功率直线上升。从数学上看目标状态通常要求到达时速度为零或者很小所以速度设为零既符合任务需求也降低了 BVP 的求解难度。5.5 T 的初值估算太保守steer 函数成了性能瓶颈现象程序不报错但运行速度极慢每轮扩展都要在steerBVP里迭代十几次才返回。原因T_init用直线距离除以 vmax 估算在大多数情况下偏小。steer 函数每次都要从很小的 T 开始反复乘 1.5 直到满足加速度限幅浪费了大量计算。解决在估算初值时把加速度的影响也考虑进去直接算一个更接近可行值的 T减少迭代次数。只要让solveTInitial返回的初值误差在 30% 以内整体运行速度能提升一倍以上。6. 二次开发换动力学模型、换度量、加目标偏置拿到这份代码后最常见的需求是换掉双积分模型。双积分模型对应的是“加速度直接可控”的系统但差速机器人、四旋翼、机械臂的动力学模型都更复杂。我的建议是不要一上来就改全模型先做三步走。第一步把状态向量从 4 维扩到 6 维或更高。比如差速机器人状态是[x, y, theta, v, omega]四旋翼是[x, y, z, vx, vy, vz, phi, theta, psi]。这个改动会牵扯到sampleState、steerBVP、distanceMetric工作量和状态维数成正比。第二步把steerBVP从三次多项式替换成真正的数值最优控制求解器。一个可行的方案是用fmincon把控制序列参数化用多个离散控制量拟合轨迹目标函数设为时间与加速度的加权和。这样一个节点连接可能要 100 毫秒但换来的是更贴近真实系统物理极限的轨迹。第三步重新设计距离度量。对非完整约束系统时间代价和位置、姿态、速度都有关系简化的启发式公式不再适用。常见做法是预计算可达域表离线算出从每个离散状态出发能到达的区域边界在线查表估算代价。另外一个小技巧是加“目标区域引导”。如果终点不是一个点而是一个区域可以在目标区域内随机采样多个目标状态取steerBVP返回轨迹代价最小的那个。这个改动对手写代码的初学者很有用比硬编码一个精确目标点更容易跑通。最后说一个关键习惯改完任何模块我都会先把运行参数里amax和vmax拉满到极限值看轨迹是否会出现超出物理边界的状况然后再回到正常值验证。这么做是为了逼出 BVP 求解器和碰撞检测里隐藏的边界 bug。毕竟一个只能在参数温和时工作的运动规划算法放到真实系统上基本就是废的。这个习惯帮我避掉了不少雷希望也能帮到你。本文还有配套的精品资源点击获取