简介面向机器人运动规划研究者和 MATLAB 开发者该代码包实现了论文《Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints》中的核心算法可用于带线性微分约束系统如双积分器、四旋翼模型的最优轨迹规划与仿真验证。包内共 17 个文件以 13 个 M 脚本为主体涵盖 RRT* 主程序、状态采样、障碍物/输入合法性检测、二维/四旋翼场景绘图与双积分器动力学建模等模块并辅以 2 个 MAT 数据文件提供障碍物与航点配置另含 readme 说明文档与 licence 许可证文本整体仅 14KB。资源已有 1295 人学习适合需要快速复现算法、理解运动规划中微分约束处理细节的学生或工程师参考可直接在 MATLAB 中运行主要例程观察随机扩展与最优收敛过程。1. kinodynamic_rrt_star 解决的核心痛点RRT 只能给点不能给轨迹做机器人或自动驾驶运动规划的人大概率都经历过这个尴尬标准 RRT* 跑得挺快路径也避开了障碍物可把路径点扔给控制器时车辆根本走不出来——要么速度跳变无法跟踪要么转弯半径不够直接撞墙。原因在于 RRT* 的规划结果只包含几何位置没有速度、加速度和时间参数。Kinodynamic RRT* 就是在这个背景下被反复提起的它把动力学模型直接融入采样和扩展过程最终输出的是时间参数化的可执行轨迹。这份基于 MATLAB 的 kinodynamic_rrt_star 资源正好覆盖从理论到实现的完整链路适合正在研究采样规划算法的开发者也适合在 MATLAB 仿真环境中验证轨迹跟踪算法的工程师。2. 核心原理推导标准 RRT* 到 kinodynamic 必须重写的三个环节很多初次接触 kinodynamic RRT* 的人会误以为它只是在 RRT* 的扩展步骤上加了一个速度约束。实际动手后发现完全不是这样因为从数据结构到距离度量再到扩展方式几乎每个环节都要重新设计。为了不让后面的 MATLAB 代码看起来像一堆堆在一起的函数先把三个最根本的环节讲透。2.1 第一道坎状态空间重构采样维度从 2D 变成 4D 甚至更高标准 RRT* 的采样点在配置空间中进行对平面移动机器人来说就是 (x, y) 两个维度。树节点也只保存位置信息扩展时直接连一条直线。但 kinodynamic RRT* 必须面对完整的状态空间以最简单的双积分器为例状态是四个维度x 方向位置y 方向位置x 方向速度 vxy 方向速度 vy控制输入则是加速度 (ux, uy)。为什么非得这样因为轨迹的可执行性恰恰由速度连续性决定。如果规划器不知道当前节点的速度就无法判断下一步能不能在加速度限制内停下来更谈不上生成平滑轨迹。在 MATLAB 中定义这个状态空间非常直接我的习惯是先写一个离散传播函数把连续动力学转成确定性的矩阵运算function x_next propagate_state(x, u, dt) % 双积分器离散时间传播 % 输入: x [x; y; vx; vy], u [ux; uy] % 输出: x_next A*x B*u A [1, 0, dt, 0; 0, 1, 0, dt; 0, 0, 1, 0; 0, 0, 0, 1]; B [0.5*dt^2, 0; 0, 0.5*dt^2; dt, 0; 0, dt]; x_next A * x B * u; end这个传播函数是整个算法的基础后续的最近邻、steer、碰撞检测全部依赖它。参数 dt 是控制周期决定了一次扩展在时间上的跨度。dt 取 0.1 秒时每个节点到其子节点的轨迹就代表 0.1 秒内的运动。如果 dt 太大比如取 1 秒树的分支会变得很粗障碍物之间的细缝就穿不过去dt 太小则计算量暴增1500 次迭代可能只覆盖地图的一小块区域。有个隐蔽的细节值得提醒位置增量是 0.5*dt^2 而不是 dt^2这是因为加速度在 dt 时间内对位置的影响是二次积分。很多初版实现会在这里写错导致轨迹在长距离传播时位置漂移。用零输入状态验证传播函数是排查这类问题的标准做法输入 u0 时位置应该只按当前速度匀速移动。2.2 第二道坎扩展机制从直线连点变为受控传播标准 RRT* 扩展就是“朝随机点方向走一步”连接两点的线段就是规划路径。这在运动学层面隐含了一个强假设机器人是全向的任意方向都能直线运动。但真实机器人受动力学约束速度向量不能瞬间改变方向加速度也有上限。所以 kinodynamic RRT* 的扩展必须改为“受控传播”从当前状态出发选定一个控制输入 u在固定时间步长内做一次动力学积分得到新的状态。控制输入怎么选直接决定树的探索质量和收敛速度。常见策略有完全随机、目标偏向、解析求解最优控制三类。完全随机覆盖面广但收敛很慢解析求解最精确但每次扩展都要处理两点边值问题在 MATLAB 里循环调用成本偏高工程上最常用的折中是目标偏向加随机扰动兼顾探索性和收敛性。下面这个 steer 函数就是典型的目标偏向实现用比例控制的思想把状态拉向采样点function [x_new, u] steerToState(x_near, x_rand, cfg) % 启发式 steer: 用PD控制思路生成满足动力学约束的控制输入 p_err x_rand(1:2) - x_near(1:2); v_err x_rand(3:4) - x_near(3:4); dist_now norm(p_err); % 时间尺度: 根据距离和最大加速度估计一个合理的过渡时间 T_scale max(sqrt(dist_now / cfg.u_max), cfg.dt); % PD形式的加速度指令 u_des 2 * p_err / (T_scale^2) - 2 * v_err / T_scale; % 加入随机扰动维持探索性 u u_des 0.15 * (rand(2, 1) - 0.5); % 裁剪到输入约束范围 if norm(u) cfg.u_max u u / norm(u) * cfg.u_max; end x_new propagate_state(x_near, u, cfg.dt); end这个函数的逻辑核心在 T_scale 的计算上。如果把 T_scale 固定为 dt当采样点距离当前节点很远时位置误差项会生成一个远大于 u_max 的加速度经过饱和裁剪后轨迹几乎变成最大加速度冲刺树的分支会直直射向远处甚至飞出地图。用 sqrt(dist / u_max) 估算过渡时间后位置项的量级自然被控制在最大加速度附近扩展步长会随距离自适应变化。噪声系数 0.15 是经验范围值太小则树扩展轨迹几乎全部朝向采样点随机性不足太大则受控传播退化成随机游走重连接的收敛优势发挥不出来。我一般从 0.1 起步看到树覆盖面不足时再调大到 0.2 附近。2.3 第三道坎距离度量与成本函数欧氏距离在这里失效标准 RRT* 选择最近节点和计算路径成本时都用欧氏距离这在几何空间没问题。但在状态空间加进速度维度后欧氏距离会产生严重的误导。举一个最直观的例子两个节点位置相同一个速度朝向目标方向另一个速度背离目标方向。欧氏距离完全一样可前者到达目标的时间明显更短因为后者需要先减速、掉头、再加速。用欧氏距离选最近节点会随机选中背离目标的节点树的扩展效率大幅下降。工程上一个简单的替代方案是用带权重的扩展距离位置差和速度差分别计算再加权求和function d stateDistance(x1, x2, cfg) % 状态空间距离: 位置差 速度差加权 w cfg.vel_weight; % 速度项权重, 一般取0.2~0.5 d norm(x1(1:2) - x2(1:2)) w * norm(x1(3:4) - x2(3:4)); end速度权重 w 的选择对算法行为影响很大。取 0 时退化成纯位置距离无法区分同位置不同速度的节点取 0.5 以上时速度差异主导距离位置接近但速度略有不同的节点会被排除树扩展就变得拘谨。我在大多数场景下用 0.3 作为起点。成本函数则更讲究。最简单的设计是直接用扩展时间 dt 作为单步成本累计时间就是路径的总时长。如果想兼顾能量消耗可以加输入惩罚项function c edgeCost(u, dt, lambda) % 边成本 时间 能量惩罚项 c dt lambda * norm(u)^2 * dt; endlambda 取 0.1 左右时时间占主导算法会尽量找快速路径lambda 拉到 1 以上算法开始在意输入幅度路径会变得更平滑但总时间可能增加。值得注意的是成本函数和距离度量必须匹配如果距离度量选了纯位置欧氏距离而成本函数是时间累积最近邻选出的节点很可能在动力学上不是最优的这种不一致会让重连接逻辑出现震荡。先跑通再调优的顺序应该是固定成本为纯时间用带权状态距离做最近邻确认整棵树能收敛后再引入能量项。3. MATLAB 端到端实现五个关键模块的搭建细节有了理论基础接下来就是把算法落到 MATLAB 代码里。这一章按模块拆解每个模块都给出可以直接复用的函数骨架和关键参数说明。整体结构上我按五个模块组织参数与节点存储、采样与 steer、最近邻搜索、重连接、碰撞检测。3.1 参数初始化与节点存储先把运行环境摆好我把所有可调参数放进一个 cfg 结构体避免在函数间传递零散变量。这样批量实验时只需改一处cfg struct; cfg.map [-10, 10; -10, 10]; % 地图边界 [xmin xmax; ymin ymax] cfg.obstacles [2, 5, -3, 2; % 矩形障碍物: 每行 [x0 x1 y0 y1] -6, -4, 5, 7]; cfg.u_max 2.0; % 最大加速度幅值 cfg.dt 0.1; % 控制周期 cfg.max_iter 2000; % 最大迭代次数 cfg.start [-8; -8; 0; 0]; % 初始状态 [x; y; vx; vy] cfg.goal [8; 8; 0; 0]; % 目标状态 cfg.goal_bias 0.1; % 采样目标偏置概率 cfg.goal_tol 0.6; % 到达判定阈值(位置) cfg.vel_weight 0.3; % 最近邻速度权重节点容器我用结构体数组预分配。MATLAB 里结构体数组动态增长非常慢每 append 一次都要重新分配内存迭代 2000 次时性能差异明显。预分配的写法nodes repmat(struct(state, [], cost, [], parent, [], ... input, [], time, []), cfg.max_iter, 1); node_count 1; nodes(1).state cfg.start; nodes(1).cost 0; nodes(1).parent 0; nodes(1).time 0;这里每个节点保存五类信息状态、累计成本、父节点索引、产生该节点的控制输入、到达时间。保存 input 很重要因为最终提取轨迹时需要回放每一段的控制量才能把路径点序列还原成完整轨迹。3.2 采样与 steer随机探索和目标引导共存主循环中采样部分最核心的逻辑是目标偏置。每次迭代以 goal_bias 概率直接采样目标状态其余概率在地图范围内随机采样for iter 1:cfg.max_iter % 目标偏置采样 if rand() cfg.goal_bias x_rand cfg.goal; else x_rand sampleState(cfg); end % 最近邻 idx_near findNearest(nodes(1:node_count), x_rand, cfg); % 受控传播 [x_new, u_new] steerToState(nodes(idx_near).state, x_rand, cfg); % 碰撞检测 if ~isTrajValid(nodes(idx_near).state, x_new, cfg) continue; end % 将新节点加入树 node_count node_count 1; nodes(node_count).state x_new; nodes(node_count).cost nodes(idx_near).cost edgeCost(u_new, cfg.dt, 0.1); nodes(node_count).parent idx_near; nodes(node_count).input u_new; nodes(node_count).time nodes(idx_near).time cfg.dt; % 重连接 nodes rewire(nodes, node_count, node_count, cfg); endsampleState 函数里有个容易忽略的细节随机采样状态时速度怎么采。很多人直接在 [-vmax, vmax] 均匀采样这在动力学上是合法的但会让大量采样点落在速度很大的区域树需要花费很多次扩展才能达到目标静止状态。我的习惯是速度分量从均值为零、标准差较小的正态分布采样让速度自然偏向低速区这样更符合真实机器人从静止出发的工况。3.3 最近邻搜索与重连接RRT* 精髓的动力学适配最近邻搜索在第 2 章已经给出了带权距离公式这里给出完整实现function idx findNearest(nodes, x_rand, cfg) n numel(nodes); if n 0 idx -1; return; end states reshape([nodes.state], 4, n); d_pos sqrt(sum((states(:, 1:2) - x_rand(1:2)).^2, 2)); d_vel sqrt(sum((states(:, 3:4) - x_rand(3:4)).^2, 2)); dist d_pos cfg.vel_weight * d_vel; [~, idx] min(dist); end这里用向量化替代 for 循环把结构体数组一次性转成状态矩阵速度提升明显。节点数超过 300 时建议检查一下这段代码是否成了性能瓶颈。重连接是 RRT* 与 RRT 拉开差距的关键。动力学场景下不能直接套用原始的重连逻辑原因在于状态转移不可逆——从节点 A 到节点 B 可行不代表从 B 到 A 可行。所以我在实现中用单向重连尝试从已有节点向新节点传播如果能到达新节点附近且成本更低就替换新节点的父节点。function nodes rewire(nodes, node_count, idx_new, cfg) % 单向重连: 遍历已有节点, 看能否成为新节点的更优父节点 for i 1:node_count-1 if i idx_new continue; end % 邻域裁剪: 位置距离超过阈值的跳过 dx nodes(i).state(1:2) - nodes(idx_new).state(1:2); if norm(dx) cfg.dt * cfg.u_max * 2 continue; end % 从节点i向新节点传播 [x_trial, u_trial] steerToState(nodes(i).state, nodes(idx_new).state, cfg); % 判断是否真的到达新节点附近(位置速度) if norm(x_trial(1:2) - nodes(idx_new).state(1:2)) 0.15 ... norm(x_trial(3:4) - nodes(idx_new).state(3:4)) 0.3 new_cost nodes(i).cost edgeCost(u_trial, cfg.dt, 0.1); if new_cost nodes(idx_new).cost nodes(idx_new).parent i; nodes(idx_new).cost new_cost; nodes(idx_new).input u_trial; end end end end两个关键点。第一邻域裁剪阈值我用cfg.dt * cfg.u_max * 2这是理论上一段受控传播能覆盖的最大距离。如果不裁剪而遍历全部节点节点数越多性能越差裁剪过小则可能错过真正的最优重连。第二到达判定同时检查位置和速度这是动力学重连和几何重连的重要区别。几何重连只要线段不碰障碍就算连接成功动力学重连必须保证到达时速度也匹配否则新节点保存的 input 无法作为后续轨迹回放的依据。3.4 碰撞检测沿轨迹逐点检查而不是只检查终点几何 RRT* 通常只需要判断新节点是否在自由空间因为两点之间的线段是有效路径。但动力学轨迹的中间状态完全可能穿过障碍物哪怕起点和终点都在自由空间。所以碰撞检测必须覆盖整段受控传播的轨迹function valid isTrajValid(x0, x1, cfg) % 沿轨迹离散采样检查碰撞 % 根据速度大小自适应采样点数 v_max max(norm(x0(3:4)), norm(x1(3:4))); N max(6, ceil(v_max * cfg.dt / 0.3)); valid true; for k 0:N s k / N; x_interp (1 - s) * x0 s * x1; if ~isStateFree(x_interp, cfg) valid false; return; end end end采样点数 N 的自适应逻辑很关键。固定 N8 时如果控制周期 dt0.1 秒且速度很高轨迹相邻采样点之间可能间隔 1 米以上而障碍物宽度只有 0.5 米就会漏检。我在实现里让采样间距控制在 0.3 米以内用最大速度乘以控制周期除以期望间距来估算 N。地图中障碍物尺寸越小这个间距阈值应该设得越小。isStateFree 函数本身没什么特别就是边界判断加矩形障碍物判断function free isStateFree(x, cfg) free true; if x(1) cfg.map(1,1) || x(1) cfg.map(1,2) || ... x(2) cfg.map(2,1) || x(2) cfg.map(2,2) free false; return; end for i 1:size(cfg.obstacles, 1) if x(1) cfg.obstacles(i,1) x(1) cfg.obstacles(i,2) ... x(2) cfg.obstacles(i,3) x(2) cfg.obstacles(i,4) free false; return; end end end这段代码还有一个可以优化的点地图障碍物判断放在一个循环里逐一遍历障碍物数量增多时开销变大。如果地图是静态的可以预先做障碍物栅格化把 isStateFree 改造成查表操作。但栅格分辨率的选择又会引入新的边界效应所以在障碍物少于 20 个的场景中直接遍历矩形列表反而更可靠。4. 参数速查与避坑指南五个高频问题与修复算法跑通之后接下来的工作基本就是和参数较劲。这一章先给一张参数速查表再列出我实际调试中遇到频率最高的五个问题每个都按现象、原因、解决三步说明。4.1 参数速查dt / u_max / goal_bias / 速度权重怎么选参数含义推荐范围失效表现dt控制周期0.05~0.2s过大: 树分支粗糙, 细缝无法穿越; 过小: 收敛极慢u_max最大加速度地图尺寸的 0.05~0.2倍过大: 轨迹近似直线, 动力学约束失效goal_bias采样目标偏置0.05~0.25过小: 收敛慢; 过大: 探索不足, 易陷入局部vel_weight最近邻速度权重0.2~0.50: 无法区分速度方向; 过大: 树扩展拘谨lambda能量惩罚系数0~1.0过大: 路径虽平滑但总时间明显增加一个值得单独强调的经验u_max 不要直接取机器人物理极限而要结合地图尺寸考虑。地图跨度 20 米、u_max 取 20 意味着任何轨迹都接近直线算法退化成几何 RRT*u_max 取 0.5 则大多数扩展都因加速度饱和而失败树始终覆盖不了地图。我的起点是地图短边的十分之一然后逐步调整。4.2 五个高频踩坑现象、原因、解决办法问题一迭代几千次树节点数几乎不增长现象是算法循环一直在跑但每次扩展生成的新节点都落在障碍物里或地图外node_count 长期徘徊在个位数。排查后发现是 steerToState 的时间尺度估算出了问题。一开始实现的版本把 T_scale 设成固定值 dt位置误差项会生成远超 u_max 的加速度饱和裁剪后轨迹几乎笔直冲向采样点大量扩展超出地图边界。原因是位置误差项和速度误差项的比例失衡。解决方法是把 T_scale 改成自适应值用 sqrt(dist / u_max) 估算合理的过渡时间让位置项的量级自然回落到最大加速度附近。这个修改之后树的扩展成功率显著提升。问题二重连接后节点的 cost 出现反弹现象是主循环里加了 rewire 之后新节点的累计成本有时反而增加或者路径在两条轨迹之间反复横跳。检查发现 rewire 函数里没有对到达状态做严格判定传播试算得到的 x_trial 离目标节点还差很远就直接用试算输入更新了父节点。原因是动力学传播的不可逆性导致“试算”和“实际”不一致。解决方法是增加位置和速度的双重到达判定距离阈值分别取 0.15 和 0.3不满足就不更新父节点。这一步会过滤掉大量无效连接虽然每次迭代计算量略增但树的整体质量提升明显。问题三路径可视化时发现轨迹穿过了障碍物现象是路径在图上看起来没问题但局部放大后发现轨迹从矩形障碍物的角上穿过。原因是碰撞检测的固定采样点间隔太大采样点恰好落在障碍物两侧中间区域被跳过。如果 dt0.1 秒且运动速度较高固定 8 个采样点会导致相邻采样间距超过障碍物尺寸。解决方法是让采样点数 N 随速度自适应调整。代码中我用max(6, ceil(v_max * cfg.dt / 0.3))保证采样间距不超过 0.3 米。障碍物更小或速度更高时把 0.3 换成 0.15 即可。问题四最近邻搜索成为性能瓶颈现象是节点数超过两百后单次迭代耗时明显增加整个算法跑了十分钟还没收敛。定位后发现时间消耗集中在 findNearest 的 for 循环里每次迭代要逐个节点计算带权距离。原因是 MATLAB 的标量 for 循环在节点数多时效率很低而且 rewire 函数里也嵌套了类似循环。解决方法是把最近邻搜索改成向量化运算一次 reshape 把所有节点状态转成矩阵再用矩阵运算同时计算所有距离。这段优化之后单次迭代耗时几乎降了一个数量级。rewire 的邻域裁剪用同样的向量化思想生成候选节点索引列表再对列表做循环。问题五设置了 goal_bias 但目标点附近的树始终不收敛现象是迭代后期大量迭代都在目标附近增加节点但就是达不到到达判定阈值路径提取出来是断的。原因是目标状态包含了速度为零的约束而采样到的随机状态速度几乎不可能精确为零导致树反复向目标位置扩展却总在最后一步偏差过大。解决方法是把到达判定从全状态改成位置距离为主速度距离作为次要条件。比如如果位置距离小于 0.6 且速度差小于 1.0就认为到达。同时确认 goal_bias 触发时采样的是完整的目标状态而不是只有位置。4.3 向量化性能优化400 个节点后的必做改动前面提到 findNearest 的向量化这里给出完整代码顺带把 rewire 的邻域筛选也向量化function [dist, idx] findNearestVec(nodes, n, x_rand, cfg) % 向量化最近邻: 一次计算所有节点距离 states reshape([nodes(1:n).state], 4, n); d_pos sqrt(sum((states(:, 1:2) - x_rand(1:2)).^2, 2)); d_vel sqrt(sum((states(:, 3:4) - x_rand(3:4)).^2, 2)); dist d_pos cfg.vel_weight * d_vel; [min_dist, idx] min(dist); end性能提升的关键在于避免在循环中反复读取结构体字段。[nodes(1:n).state]这个表达式把结构体数组的 state 字段一次性拼接成 4×n 矩阵reshape 成 n×4 后可以用矩阵广播完成所有距离运算。rewire 里的邻域裁剪也可以先算一次位置距离矩阵一次性筛出候选节点再逐个试算而不是每个节点都做一次完整判断。5. 进阶收敛技巧自适应 dt 与轨迹回放验证算法能跑通后下一步是提升收敛速度和轨迹质量。这里分享两个我常用的手段自适应控制周期和轨迹回放验证。自适应控制周期的思路是树在远离目标区域时用较大的 dt 快速扩展靠近目标时切换为较小的 dt 精细搜索。好处是兼顾全局探索速度和局部收敛精度。% 根据新节点与目标的距离动态调整扩展步长 dist_to_goal norm(nodes(idx_near).state(1:2) - cfg.goal(1:2)); if dist_to_goal 3 dt_eff cfg.dt * 0.5; elseif dist_to_goal 6 dt_eff cfg.dt * 0.8; else dt_eff cfg.dt; end注意这个逻辑放在主循环的 steer 调用前传给 steerToState 的不是 cfg.dt 而是 dt_eff。代价函数也要跟着调整用 dt_eff 计算边成本否则成本口径不一致会导致重连接决策混乱。我一般会在 cfg 里额外存一个 dt_original方便恢复到统一步长模式。第二个技巧是轨迹回放验证它同时也是排查路径质量问题最直接的手段。提取路径后把每个节点的 input 字段按时间顺序回放一遍用同一套 propagate_state 函数重新生成轨迹点序列再和障碍物做一次完整碰撞检测% 提取路径节点序列 path_idx extractPath(nodes, node_count, cfg); % 回放验证: 从起点开始逐段重放控制输入 x_trace cfg.start; for k 1:length(path_idx)-1 u_k nodes(path_idx(k)).input; x_next propagate_state(x_trace, u_k, cfg.dt); % 检查回放轨迹是否与节点存储轨迹一致 if norm(x_next - nodes(path_idx(k1)).state) 1e-6 warning(第 %d 段轨迹回放不一致, k); end x_trace x_next; end这个验证能同时发现两类问题节点存储的 state 与 input 不匹配以及路径回溯时父节点链接断裂。我一般会在每次调参后强制跑一遍这个验证脚本它就像一颗后悔药能在算法表现不错但实际轨迹有问题时快速定位到具体节点。还有一个容易被忽略的细节extractPath 回溯时要注意 parent 索引是否正确因为 rewire 会频繁修改 parent 字段。提取完成后从头到尾逐段检查每个节点的 position 是否在自由空间内并确认相邻节点的位置差不超过该段控制输入对应的理论传播距离这些都是在 MATLAB 里快速检验路径可用性的手段。从那以后我每次跑 kinodynamic RRT* 都会强制走一遍这个流程先跑通单步传播再做十次迭代的冒烟测试确认节点数增长最后调参前先跑一次回放验证脚本。这样折腾一轮下来后面所有实验都稳当得多希望帮到你。本文还有配套的精品资源点击获取