简介：Kinodynamic RRT算法的MATLAB实现，面向机器人运动规划领域的研究者与进阶学习者，用于求解带线性微分约束系统的最优运动规划问题。代码对应Webb与van den Berg发表于CoRR的论文，完整包含RRT主循环、随机状态采样、最近邻搜索、局部规划器等核心模块，提供双积分器与四旋翼两类动力学仿真，并辅以球体障碍物碰撞检测、输入可行性判断等子函数，可复现论文中的对比实验，也便于替换为自定义动力学模型。压缩包共17个文件，其中13个.m源程序构成层次分明的项目框架，2个.mat文件存放障碍物和航点数据，另有README说明与许可证文本，压缩包整体体积仅14KB，轻量且无外部依赖，便于直接运行验证。已有1295人学习下载，适合课程设计、科研入门或工程参考，有助于快速理解Kinodynamic RRT*的原理与实现细节。

1. kinodynamic RRT*：从直线插值到考虑加速度限制的路径规划

做运动规划时最常见的翻车现场是：RRT* 跑出来的路径在几何上完美绕开了所有障碍，但把轨迹交给底层执行时，机器人要么在拐角处因为加速度超限直接冲出走廊，要么在终点因为速度不为零而刹不住。kinodynamic RRT* 解决的就是这类问题——它把“这条路径在几何上可行”升级为“这条路径在动力学上也跟得上”。这个 MATLAB 项目在经典 RRT* 框架里引入了速度状态、加速度控制输入和受限控制空间，让采样扩展出的每条边都是一段真正可执行的运动学轨迹。适合做移动机器人、无人机航迹规划、机械臂避障路径规划的同学，尤其是那些不想再被“规划轨迹与执行轨迹不一致”困扰的开发者。

2. 项目架构梳理：状态为什么是四维，树为什么长在（x, y, vx, vy）空间里

2.1 文件结构与数据流

整个项目按功能拆成了七个文件，核心逻辑集中在kinodynamic_rrt_star.m主循环里，其余文件分别负责状态空间定义、碰撞检测、采样、扩展、代价计算和可视化。我第一次读这个工程时最直观的感受是：它把“状态”和“控制”彻底分开了，这直接决定了后续所有代码的写法。

数据流向大概是这样的：

main 初始化地图与状态空间 → 调用 sample 生成随机目标状态 → 在已有树里搜索最近节点 → 对最近节点施加候选控制输入，前向积分生成新状态 → 碰撞检测过滤掉穿过障碍的轨迹段 → 选择最优父节点，完成插入 → rewire 重连周边节点，维护树的渐进最优性

这里有一处设计很关键：nearest函数搜索最近节点时用的距离度量不是普通的欧氏距离，而是一个带速度项权重的加权距离。常见做法是把状态差投影到控制输入空间上，或者直接对位置差和速度差分别设权重。项目里用的是后者——dist = sqrt((dx)^2 + (dy)^2 + k_v * (dvx^2 + dvy^2))，这个k_v是速度项权重。如果k_v设成 0，树会只按位置远近生长，新节点会同速度差异巨大的旧节点连接，生成的轨迹在速度衔接上会变得相当奇怪。

2.2 状态空间数学定义与参数约定

项目采用经典的 double integrator 模型作为运动学约束。状态是s = [x, y, vx, vy]，控制输入是u = [ax, ay]，动力学方程就是匀加速运动模型。前向积分用简单的欧拉法还是 RK4，取决于你对时间步长和精度的容忍度。我通常用半隐式欧拉——先更新速度再更新位置，数值稳定性比显式欧拉好不少，代码却只多了一行。

这里最容易理解错的一点是：RRT* 里的“步长”在 kinodynamic 版本里不再是几何距离，而是一个固定的扩展时长。每次扩展都从当前状态出发，在可行控制空间里采样一组加速度，前向积分固定时长后得到一组候选末状态，再从这些候选中挑代价最小的。这样做的好处是轨迹天然满足动力学约束，不需要事后做平滑；坏处是计算量显著上升，因为每扩展一个节点都要前向积分多条候选轨迹。

3. 核心模块实现：一个请求从进入到返回的完整链路

3.1 采样与最近邻居查找的动力学适配

sample函数和经典 RRT* 差异不大，只是在采样生成目标位置的同时，会给速度分量也赋一个随机值。速度分量如果直接取[-v_max, v_max]的均匀随机，会带来一个副作用——目标状态的速度分布与实际可行轨迹的速度分布不一致，导致扩展时大量采样被可行性检查拒绝。常见做法是对速度做小范围随机扰动，或者直接从[-v_max, v_max]里采样后以一定概率归零，迫使算法探索低速区域。

function x_rand = sampleState(state_space, bias_target, x_goal) if rand < bias_target % 以目标偏置概率直接返回目标状态，加速收敛 x_rand = x_goal; return; end x_rand = zeros(4, 1); x_rand(1) = rand * state_space.x_range(2) + state_space.x_range(1); x_rand(2) = rand * state_space.y_range(2) + state_space.y_range(1); % 速度采样：避免直接取极端值 x_rand(3) = randn * state_space.v_max / 3; x_rand(4) = randn * state_space.v_max / 3; x_rand(3) = max(min(x_rand(3), state_space.v_max), -state_space.v_max); x_rand(4) = max(min(x_rand(4), state_space.v_max), -state_space.v_max); end

速度采样用randn而不是rand，是很多 kinematics 版 RRT 的惯用技巧。均匀分布采样出的速度会集中在边界附近，而randn出的速度更贴近“执行器正常工作范围”，减少了无用采样。bias_target的值设到 0.05～0.1 就够了，设太高树会过度朝向目标生长，窄通道场景容易陷入局部死区。

再看最近邻居查找。经典 RRT 在这步只算位置距离，这里必须把速度差也纳进去，否则会出现“位置很接近但速度方向完全相反”的父子节点配对，后续扩展出的轨迹会绕出一个大弯来抵消速度差。

function idx = nearestNode(tree, x_rand, state_space, kv) min_dist = inf; idx = 1; for i = 1:length(tree) dx = x_rand(1) - tree(i).x(1); dy = x_rand(2) - tree(i).x(2); dvx = x_rand(3) - tree(i).x(3); dvy = x_rand(4) - tree(i).x(4); dist = sqrt(dx^2 + dy^2 + kv * (dvx^2 + dvy^2)); if dist < min_dist min_dist = dist; idx = i; end end end

kv的最佳取值和地图尺度强相关。如果地图是 10m × 10m，kv取 1 左右比较合适；如果地图改成 100m × 100m，位置差项会放大两个数量级，kv就要相应缩小到 0.01。每次改地图时不去重调kv，是我见过这个项目最常见的跑飞原因。

3.2 扩展过程：控制空间采样与前向积分

扩展函数是这个项目的灵魂。它不直接连一条直线，而是从可行控制空间里采样若干个加速度候选，前向积分后选代价最小的那个作为新节点。候选控制输入的采样数量N_control直接影响路径质量和运行速度。设 20 条左右是性价比比较高的区间，少于 10 条路径会明显变粗糙，多于 50 条运行时间翻倍但路径提升有限。

function [x_new, u_best] = extendTree(state_space, x_near, x_rand, dt, N_control) best_cost = inf; x_new = zeros(4, 1); u_best = zeros(2, 1); for i = 1:N_control % 在合作控制空间里采样加速度，这里用椭圆采样而不是均匀采样 theta = rand * 2 * pi; r = state_space.a_max * sqrt(rand); ax = r * cos(theta); ay = r * sin(theta); % 前向积分时长 T 按照目标距离自适应调整 T = min(state_space.T_max, 0.5 + norm(x_rand(1:2) - x_near(1:2)) / state_space.v_max); x_cand = integrateMotion(x_near, [ax; ay], T, dt); % 检查速度约束 if norm(x_cand(3:4)) > state_space.v_max continue; end % 代价：时间最优 cost = T; if cost < best_cost best_cost = cost; x_new = x_cand; u_best = [ax; ay]; end end end

这里有两个细节值得注意。第一，加速度控制空间用“圆内均匀采样”而不是“边界采样”，因为随机扩展阶段需要探索性，边界采样容易让轨迹一直在极限加速度上打转，反而缺乏多样性。第二，扩展时长T不是固定值，而是根据当前位置到目标状态的距离做自适应——距离远就多走一段，距离近就少走一截。这个处理模仿了经典 RRT 步长随距离变化的思想，但在 kinodynamic 场景里让末端速度更容易接近目标速度。

integrateMotion内部用半隐式欧拉：

function x_next = integrateMotion(x0, u, T, dt) x_next = zeros(4, 1); x = x0; for t = 0:dt:T-dt % 先更新速度，再更新位置 v_new = x(3:4) + u * dt; p_new = x(1:2) + v_new * dt; x = [p_new; v_new]; end x_next = x; end

参数说明：dt是积分步长，直接影响轨迹精度。设为 0.1 时，一个 1 秒的扩展段会被积分 10 次，精度一般够用；如果地图窄道特别多，建议把dt改小到 0.05，代价是运行时间翻倍。注意这里积分步长和上文扩展时长不是一回事——积分步长是数值计算精度，扩展时长是一条轨迹的总时间跨度。

3.3 树的重接：从全局重连到局部剪枝

经典 RRT* 的rewire是对新节点周围的邻居节点做重连：如果从x_new到某个邻居节点的代价小于该邻居原来的累计代价，就把那个邻居的父节点替换为x_new。kinodynamic 版本的核心改动是：重连时必须重新做碰撞检测和前向积分，因为每条边的轨迹长度不同，不能直接套用欧氏距离做近似。

function tree = rewire(tree, x_new, neighbor_idx, state_space, map, dt) for i = 1:length(neighbor_idx) idx = neighbor_idx(i); if idx == length(tree) % 跳过自身 continue; end x_neighbor = tree(idx).x; % 从 x_new 到 x_neighbor 生成候选轨迹 [x_cand, u_cand] = extendTree(state_space, x_new, x_neighbor, dt, 10); % 检查是否真的能从 x_new 到达邻居，且路径无碰撞 if isCollisionFree(map, x_new, x_cand, dt) && ... tree(idx).cost > tree(end).cost + trajectoryCost(u_cand) tree(idx).parent = length(tree); tree(idx).cost = tree(end).cost + trajectoryCost(u_cand); end end end

重连这一步是 RRT* 渐进最优性的关键，但也是计算开销最大的部分。项目里设了邻居搜索半径r_rewire，默认是地图对角线长度的 5% 再加一个常数项。如果感觉规划结果不够光滑，优先检查这个半径是不是太小；如果运行慢得无法接受，优先缩小这个半径。

3.4 碰撞检测的速度自适应离散化

碰撞检测模块处理的是轨迹段而不是直线段。一条 1 秒的轨迹，在高速段可能每秒前进 1.5 米，在低速段可能只前进 0.3 米。如果用固定步长做碰撞检测，低速段浪费算力，高速段又可能跳过障碍物边缘。

function free = isCollisionFree(map, x_start, x_end, dt) free = true; % 根据轨迹总长度动态确定检测点数 dist_total = norm(x_end(1:2) - x_start(1:2)); n_checks = max(5, ceil(dist_total / map.resolution * 3)); for i = 1:n_checks alpha = i / n_checks; x = (1 - alpha) * x_start(1) + alpha * x_end(1); y = (1 - alpha) * x_start(2) + alpha * x_end(2); if ~isPointInFreeSpace(map, x, y) free = false; return; end end end

检测点数按轨迹长度自适应，map.resolution是地图栅格的分辨率。每检查一个点需要一次碰撞查询，如果地图是 500×500 的栅格，查询就是 O(1) 的数组访问。项目里isPointInFreeSpace的实现是先做边界判断，再做障碍物查找，障碍物存在一个二维逻辑数组里，所以速度很快。

4. 避坑指南：动力学 RRT* 最容易翻车的三个实现细节

4.1 现象：树扩展出的轨迹虽然绕开了障碍，但底层执行器根本跟不上

原因：扩展阶段只约束了最大加速度和最大速度，却忽略了控制输入切换频率。现实中执行器的加速度变化率（加加速度）是有限的，算法生成的轨迹可能每 0.1 秒就从 +2 m/s² 跳到 -2 m/s²，执行器物理上做不到。

解决：在扩展函数的代价计算里加入控制平滑度惩罚项。我一般会对相邻两个控制输入的差施加二次惩罚，让轨迹优先选择控制变化平缓的候选。具体做法是在 cost 后加一个lambda * norm(u_best - u_prev)^2，lambda取 0.1 左右效果不错。这样轨迹虽然不是时间最优，但执行器跟得住，整体规划成功率反而更高。

4.2 现象：碰撞检测用固定步长检查轨迹，窄缝场景几乎每次都检测失败

原因：固定步长和地图分辨率不匹配。地图分辨率是 0.1 米，但轨迹在高速段的检测点间隔超过 0.5 米，障碍物如果正好在两个检测点之间，就会被彻底穿透。

解决：把检测步长跟地图分辨率绑定——每个检测间隔不超过map.resolution / 2。实现上先把轨迹插值成密集点序列再查碰撞，代码直观且准确。代价是每段轨迹要查的点数变多，但碰撞检测整体仍然是 O(n) 的，相比扩展阶段的多次前向积分，这部分开销可以接受。

4.3 现象：规划的路径在终点附近总是震荡，无法收敛

原因：目标状态的随机采样引入了速度噪声。sampleState中目标偏置概率bias_target只在位置维度上生效，速度维度依然随机。即使位置收敛到目标点，速度也在随机抖动，导致算法不断尝试用不同速度接近终点。

解决：对目标状态的速度做特殊处理——当采样结果落入目标偏置分支时，速度分量直接设为 0。这样做等价于告诉算法“到达目标时必须静停”，从机制上消除了终点震荡。代码只需在sampleState的目标偏置分支里加两行：x_rand(3) = 0; x_rand(4) = 0;，亲测收敛速度提升非常明显。

5. 运行与调试：MATLAB 环境下的参数调节与可视化验证

5.1 跑通主程序的参数清单

项目入口是kinodynamic_rrt_star.m。运行前需要检查几个参数：地图尺寸、障碍物位置、起始状态[x, y, vx, vy]、目标状态、最大迭代次数。默认的起始状态是[1, 1, 0, 0]，目标状态是[9, 9, 0, 0]，地图里预设了三个矩形障碍物和一个窄通道。迭代次数默认 2000，算力不够的机器建议先降到 500 验证流程通不通。

% 初始化 state_space = struct(); state_space.x_range = [0, 10]; state_space.y_range = [0, 10]; state_space.v_max = 1.5; state_space.a_max = 2.0; state_space.T_max = 2.0; state_space.dt = 0.1; start_state = [1; 1; 0; 0]; goal_state = [9; 9; 0; 0]; max_iter = 2000;

运行结束后会弹三个图：第一个是完整搜索树，绿色是最终路径；第二个是障碍物与路径的俯视图；第三个是代价下降曲线。代价曲线如果呈阶梯状下降，说明rewire在正常工作；如果曲线一直水平，说明重连机制没有生效，优先检查邻居搜索半径r_rewire。

5.2 代价曲线与树的三个常见诊断信号

从运行结果里可以读出不少信息。第一，如果搜索树密集但代价曲线几乎不降，多半是采样偏置bias_target设太高，树在目标附近反复采样但无法通过重连降低代价，把偏置降到 0.05 试试。第二，如果路径在窄通道入口处长时间停滞，说明窄道入口附近的随机采样命中率太低，可以给sampleState增加对窄道区域的权重采样。第三，如果总运行时间里有 80% 花在rewire上，说明r_rewire半径太大，这在高维状态空间尤其明显。

5.3 程序卡死或运行缓慢时的排查顺序

先看是否陷入了无限循环——检查extendTree是否在目标附近反复重试，常见做法是加一个最大尝试次数，超过阈值就返回空节点，让主循环继续采样而不是死磕。再看内存占用，树节点结构体数组在 MATLAB 里如果每帧扩容会非常慢，建议预分配一个大数组或者用容器存储。最后看batch运行模式下是否有绘图开销过大的问题，调试阶段把plotEvery设为 50 或更大，只在关键帧绘制整棵树，可视化太频繁反而成了性能瓶颈。

6. 二次开发思路：从双积分模型到任意阶系统与多目标代价

6.1 把动力学模型换成 uni-cycle

想要把项目移植到差速机器人或阿克曼底盘上，核心工作集中在integrateMotion和extendTree两个函数。uni-cycle 模型的状态是[x, y, theta, v]，控制是[v_dot, omega]，前向积分逻辑不再是对速度独立积分，而是先算角速度对朝向的影响，再按朝向分解速度。改完后nearestNode的距离度量也需要跟着变——角度差要包装到[-π, π]，速度差项保留。

function x_next = integrateUnicycle(x0, u, dt, T) x_next = zeros(4, 1); x = x0; for t = 0:dt:T-dt v_new = x(4) + u(1) * dt; theta_new = x(3) + u(2) * dt; x(1) = x(1) + v_new * cos(theta_new) * dt; x(2) = x(2) + v_new * sin(theta_new) * dt; x(3) = theta_new; x(4) = v_new; end x_next = x; end

6.2 把代价函数从时间最优改成能量最优

extendTree里那个cost = T只管时间，改成能量最优只需要把代价函数替换为控制输入平方的积分——物理意义是电机消耗的功率近似与控制量平方成正比。修改后轨迹会明显变平滑，代价是总行程时间变长。想让两者权衡时，加一个权重系数alpha：cost = alpha * T + (1 - alpha) * sum(u.^2) * dt。alpha设为 0.7 时兼顾效率与能量，设为 0.3 时更像节能模式。

从那以后我每次拿到新的运动规划项目，第一件事不是看采样策略，而是先把状态空间的度量函数和可行控制集写对——这两个地方错了，后面所有优化都是在错误的地基上打补丁。希望这篇拆解能帮你少走一段弯路，直接把这套 kinodynamic 框架跑起来、改明白。

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