简介：这套MATLAB代码实现Kinodynamic RRT算法，对应2012年关于线性微分约束系统最优运动规划的论文，面向运动规划、机器人控制与自动驾驶方向的学习者和研究者，可用于理解带动力学约束的采样规划、代价优化与轨迹生成流程。压缩包共17个文件，以13个m脚本为主体，另含2个mat数据文件、1个md说明和1个txt许可文件，整体约14KB，体量轻便，便于直接阅读和调试。内容覆盖双积分器与四旋翼等示例场景，包含状态采样、输入可行性判断、障碍物与路点数据、RRT主流程及可视化辅助函数，能帮助读者梳理算法结构、复现实验并理解扩展步骤。读者可据此核对主循环、采样与代价计算之间的对应关系，并尝试替换系统模型或约束条件。已有1302人学习，适合作为课程项目、论文复现或算法二次开发的参考代码。

1. 从一次「路径跑出来却开不了」说起

第一次把规划出来的折线路径丢给底盘执行，车在原地抖了三秒才动，转弯处直接冲出了走廊。问题不在控制器，而在规划器——那条路径只考虑了位置，没考虑车的速度和加速度能不能跟得上。kinodynamic_rrt_star 这个 MATLAB 项目解决的就是这件事：它把动力学约束直接塞进采样树的生长过程，树上每一条边都是一段真实控制输入积分出来的轨迹，不是两点之间连一根直线。项目面向做移动机器人、无人机、机械臂轨迹规划的开发者，手上有 MATLAB、想快速验证算法而不是先搭一套 C++ 工程的人最合适。它不追求实时性能，追求的是把「动力学可行」这件事在几十行核心代码里讲明白。

2. 算法骨架：一条边是怎么被「积分」出来的

2.1 状态空间、控制空间与离散动力学

理解这套代码的第一道坎，是接受「树上的节点不是位置点，而是状态点」。在几何 RRT* 里，节点就是 (x, y)；在这里，节点是 [px, py, vx, vy] 这样的完整状态向量，边也不再是直线段，而是一串控制输入作用在动力学方程上滚出来的曲线。项目里最常用的模型是二阶积分器，也就是把机器人当成一个可以被加速度推动的质点。

离散化是整个实现的地基。连续时间下的 ẋ = Ax + Bu 要变成 x_{k+1} = A x_k + B u_k，A 和 B 随采样周期 dt 变化。这段代码通常单独抽成一个函数，因为后面所有模块都要用同一套矩阵，一旦两处 dt 不一致，路径就会在视觉上「接不上」，这种问题排查起来相当玄学。

function model = double_integrator_model(dt, u_max) % 状态 x = [px; py; vx; vy]，控制 u = [ax; ay] % 采用零阶保持离散化，假设控制在一个采样周期内保持不变 model.name = 'double_integrator'; model.nx = 4; model.nu = 2; model.dt = dt; model.u_max = u_max; % 加速度幅值上限，决定轨迹的「激进程度」 model.A = eye(model.nx); model.A(1,3) = dt; % px <- px + vx*dt model.A(2,4) = dt; % py <- py + vy*dt model.B = zeros(model.nx, model.nu); model.B(1,1) = 0.5*dt^2; % px <- px + 0.5*ax*dt^2 model.B(2,2) = 0.5*dt^2; model.B(3,1) = dt; % vx <- vx + ax*dt model.B(4,2) = dt; end

参数说明上，dt 决定了轨迹的分辨率：取太小，树长得慢、内存吃紧；取太大，积分误差积累，碰撞检测会漏掉障碍物边角。u_max 是硬约束，控制采样必须在这个圆盘内取值，否则算出来的边物理上执行不了。我一般从 dt = 0.1 s、u_max = 1.0 m/s² 起步，先看树能不能长到目标，再考虑细化。

真正让 kinodynamic RRT* 区别于「RRT* 加轨迹平滑」的，是采样发生在状态空间和控制空间两侧。状态采样给出「我想往这个方向探索」，控制采样给出「我用什么动作过去」，两者之间靠前向积分连接。如果只采样状态、然后用直线连，动力学约束就形同虚设。

2.2 主循环：从采样到插入节点的六个动作

主循环的逻辑和经典 RRT* 骨架一致，但每个动作都多了一层动力学含义。把它拆成六步看会更清楚：采样、找最近邻、生成边、碰撞检测、选父节点、重接。

for iter = 1:cfg.max_iter % 1) 状态采样：带目标偏置的均匀采样 if rand() < cfg.goal_bias x_rand = cfg.x_goal; else x_rand = [rand()*cfg.map_w, rand()*cfg.map_h, 0, 0]'; end % 2) 最近邻：这里用的是加权状态距离，不是纯欧氏 idx_near = nearest_node(tree, x_rand, cfg); % 3) 生成边：采样若干组控制，选一组让新状态最接近 x_rand 的 [u_best, x_new, edge_cost] = steer(tree(idx_near).x, x_rand, cfg); if isempty(u_best) continue; end % 4) 边碰撞检测：按控制序列逐步积分，逐状态检查 if ~is_edge_collision_free(tree(idx_near).x, u_best, cfg) continue; end % 5) 选父节点：在邻域里找一个总代价更小的已有节点 [parent_idx, u_best, edge_cost] = choose_parent(tree, x_new, cfg); % 6) 插入并重接：把邻域内能以更低代价到达的节点改挂到新节点下 tree = insert_node(tree, parent_idx, x_new, u_best, edge_cost); tree = rewire(tree, cfg); end

第 3 步的 steer 是整个实现的灵魂。常见做法是控制采样 + 前向积分 + 择优：随机生成 K 组满足 |u| ≤ u_max 的控制序列，每组积分若干步，最后挑一个末端状态离 x_rand 最近的结果。K 越大越容易连上，但每增加一组就多一次积分和一次碰撞检测，实际调参时 K 取 10～30 已经够用。

第 5 步的选父节点不是简单的「找最近的」，而是在一个半径 r 的邻域里，逐个尝试「从该邻居出发能不能连到 x_new」，能连上的取 cost(parent) + edge_cost 最小者。这里的 r 不是固定值，常见做法是按节点数量做对数衰减，节点越多邻域越小，否则后期每次重接都要扫几百个邻居。

2.3 代价函数与 rewiring 的一致性约束

这里有个很多人第一版就翻车的点：边代价的定义必须和整个算法的最优性目标一致。如果你声明自己优化的是「控制能量」，边代价就得是 ∫|u|²dt；如果声明优化「到达时间」，边代价就得是步数乘以 dt。两者混用，rewiring 会做出自相矛盾的决策——把一条能量更省但时间更长的边挂上去，最后路径看起来更长。

以控制能量为例，边代价可以写成离散累加：

function c = edge_cost(u_seq, dt) % 二次型控制代价，离散步长 dt 与积分时保持一致 c = 0; for k = 1:size(u_seq,1) c = c + (u_seq(k,:) * u_seq(k,:)') * dt; end end

rewiring 的触发条件同样要对齐：只有当 cost(parent_new) + edge_cost(new_edge) < cost(parent_old) + edge_cost(old_edge) 时，才值得改挂。改挂之后还有一件常被漏掉的事——子树的代价要跟着更新。树里每个节点存的 cost_to_come 是相对根节点的累计值，父节点一变，它下面所有后代的这个值都过期了。小规模场景下可以偷懒，等到取路径时再顺着 parent 指针重算；规模一大就必须在改挂时做一次子树 DFS 更新，否则你看到的路径代价和实际代价值会对不上。

提示：如果暂时不确定该优化什么，先把边代价设成「时间 × 权重 + 控制能量 × 权重」的加权和，两个权重各取 1，跑通之后再逐步调到符合你场景的比例。这比一上来就纠结理论最优要实际得多。

3. 目录结构与核心模块拆解

3.1 工程目录与文件职责

这类 MATLAB 项目的目录，通常会按「入口 - 核心 - 模型 - 工具」四层切分，好处是换动力学模型时不用动主循环，改可视化时不用碰算法。下面这套结构是从项目里整理出来的典型形态：

kinodynamic_rrt_star/ ├── main.m % 入口脚本：加载参数、建图、跑算法、画结果 ├── config/ │ └── params.m % 所有可调参数集中在这里，返回一个 cfg 结构体 ├── core/ │ ├── rrt_star_kinodynamic.m % 主循环，返回树结构和统计信息 │ ├── sample_state.m % 状态采样（含 goal bias） │ ├── nearest_node.m % 最近邻搜索 │ ├── steer.m % 控制采样 + 前向积分 + 择优 │ ├── choose_parent.m % 邻域内选父节点 │ ├── rewire.m % 重接 │ ├── is_edge_collision_free.m % 边级碰撞检测 │ └── tree2path.m % 从目标节点回溯出状态序列和控制序列 ├── models/ │ ├── double_integrator_model.m │ └── unicycle_model.m % 独轮车/自行车模型，状态多一维朝向 ├── utils/ │ ├── draw_tree.m % 画树 │ ├── draw_trajectory.m % 画最终轨迹 │ └── load_map.m % 读取障碍物多边形 └── data/ └── map_corridor.mat % 障碍物顶点坐标

这个划分的实际价值体现在改动成本上。想把二阶积分器换成独轮车，只需要新增一个模型文件并让 cfg 指向它，主循环、碰撞检测、回溯全都不用动；想换成三维空间，改动量集中在模型文件和采样函数两处。反过来说，如果你拿到的版本里 A/B 矩阵是硬编码在主循环里的，第一步就该把它抽出来。

3.2 参数集中管理：params.m 里的每个数都影响收敛

参数散落在各个函数里是这类项目的通病，改一个 dt 要翻五个文件。集中到一个 cfg 结构体之后，调参才变成一件可以记录、可以复现的事。

function cfg = params() cfg.map_w = 20; cfg.map_h = 20; % 地图尺寸，单位与状态位置一致 cfg.start = [1; 1; 0; 0]; % 起点状态，速度必须为 0 cfg.goal = [18; 18; 0; 0]; % 终点状态，速度同样置 0 cfg.dt = 0.1; % 积分步长 cfg.u_max = 1.0; % 控制幅值上限 cfg.n_control = 15; % 每次 steer 采样的控制序列条数 cfg.n_steps = 10; % 每条边积分多少步 cfg.goal_bias = 0.08; % 直接采样目标的概率 cfg.goal_tol = 1.0; % 距目标多近算到达 cfg.max_iter = 5000; % 迭代上限 cfg.neighbor_r = 2.5; % 重接邻域半径 cfg.robot_radius= 0.3; % 机器人外接圆半径，用于障碍膨胀 cfg.seed = 42; % 固定随机种子，保证结果可复现 end

几个数值得单独说。n_control 和 n_steps 相乘决定了单次 steer 的计算量，15 × 10 = 150 次矩阵乘加，看着不多，但乘上 max_iter 就是几十万次，这也是后面性能问题的来源之一。neighbor_r 取太大会让每次插入都要尝试几十个邻居连接，取太小则 rewiring 几乎不起作用，路径质量退化成普通 RRT。seed 固定与否直接影响你复现问题的能力，调试阶段务必固定，出图之前再换随机种子跑多次看分布。

3.3 前向积分与边碰撞检测：最容易写错的两个函数

碰撞检测写错是这套代码里最隐蔽的 bug，因为它不会报错，只会在可视化里表现为「看起来擦着边过去了」。节点级检测只判断离散状态点是否落在障碍内，而动力学轨迹在两步之间是弯的，弯的那一段完全没被检查。

function ok = is_edge_collision_free(x0, u_seq, cfg) % 输入：起点状态、控制序列、配置 % 沿控制序列逐步积分，每个中间状态都做一次圆-多边形检测 x = x0; for k = 1:size(u_seq,1) x = cfg.model.A * x + cfg.model.B * u_seq(k,:)'; if is_state_in_collision(x, cfg) ok = false; return; end end ok = true; end function hit = is_state_in_collision(x, cfg) % 把机器人当成半径 robot_radius 的圆，与膨胀后的障碍多边形求交 hit = false; p = x(1:2)'; for i = 1:numel(cfg.obstacles) poly = cfg.obstacles{i}; if inpolygon(p(1), p(2), poly(:,1), poly(:,2)) hit = true; return; end % 边界情形：圆心在多边形外但距离小于半径，同样算碰撞 for j = 1:size(poly,1)-1 if point_segment_distance(p, poly(j,:), poly(j+1,:)) < cfg.robot_radius hit = true; return; end end end end

这段代码有两个关键点。第一，积分和碰撞检测用的是同一组 A/B 矩阵和同一个 dt，任何一处不一致都会导致「规划时的轨迹」和「检测时的轨迹」不是同一条。第二，障碍膨胀做了两层：inpolygon 处理圆心落在障碍内部的情况，点到线段距离处理圆心在外但贴得太近的情况。少写第二层，路径就会擦着墙角过去，仿真里没事，实车上就是血泪经验。

还有一种更省事的做法：把地图预先栅格化成占用网格，碰撞检测退化成查表加双线性插值。代价是精度受栅格分辨率限制，好处是速度快一个数量级，地图大的时候值得换。

3.4 树的存储与回溯：tree2path 怎么把节点串成轨迹

树用什么结构存，直接决定了后面能不能改成 C++。MATLAB 里最直觉的写法是结构体数组，每个元素带 x、parent、u、cost 四个字段：

tree(1).x = cfg.start; tree(1).parent = 0; % 0 表示根节点 tree(1).u = []; tree(1).cost = 0;

parent 存的是父节点在数组里的下标，这条指针链就是整棵树。回溯的时候从目标节点一路走到 0，把状态和控制反序拼起来：

function [path, ctrl] = tree2path(tree, goal_idx) % 从目标节点回溯到根，得到状态序列和控制序列 path = []; ctrl = []; i = goal_idx; while i ~= 0 path = [tree(i).x, path]; % 前插，保证时间顺序 if ~isempty(tree(i).u) ctrl = [tree(i).u, ctrl]; % 控制序列长度比状态少一段 end i = tree(i).parent; end end

注意 path 和 ctrl 的长度差一段：每个节点存的是「从父节点到自己」的控制序列，根节点没有控制，所以 ctrl 比 path 少一个元素。这个细节在把轨迹交给控制器执行时很关键，长度对不上会直接索引越界。

回溯出来的 path 只是状态序列，真正要下发给底层的是带时间戳的轨迹。我一般会在 tree2path 外面再包一层，把每个状态配上 k*dt 的时间标签，输出成三列或四列矩阵，直接喂给轨迹跟踪器。

4. 跑起来：从 clone 到出图的一整套操作

4.1 环境与依赖检查

这个项目对 MATLAB 版本的要求不高，核心算法只用了矩阵运算和 inpolygon，属于基础函数，没有工具箱依赖。唯一要注意的是脚本内定义局部函数的语法，老版本 MATLAB 不支持，遇到报错就把局部函数拆成独立 .m 文件，功能完全一样。

跑之前的检查清单：

• 当前工作目录切到项目根目录，否则 config 和 core 子目录不在路径里，会报 undefined function；
• 确认 data 目录下的地图文件能被 load_map.m 正确读取，地图格式变了是最常见的起不来原因；
• 确认 cfg.start 和 cfg.goal 落在障碍物外面，起点在障碍里的话第一次碰撞检测就返回，树一个节点都长不出来。

如果用的是开源 Octave，inpolygon 是有的，但某些绘图函数行为不一致，可视化可能出问题，算法本身能跑。

4.2 第一次运行：默认场景跑通

main.m 通常就几十行，流程是「读参数 → 建图 → 调主循环 → 画结果」：

clc; clear; close all; addpath(genpath(pwd)); % 把所有子目录加进搜索路径 cfg = params(); % 一次性读入所有参数 cfg.model = double_integrator_model(cfg.dt, cfg.u_max); cfg.obstacles = load_map('data/map_corridor.mat'); rng(cfg.seed); % 固定随机种子，方便复现 [tree, goal_idx, stats] = rrt_star_kinodynamic(cfg); if goal_idx > 0 [path, ctrl] = tree2path(tree, goal_idx); fprintf('找到路径，节点数 %d，代价 %.3f，用时 %.2f s\n', ... numel(tree), tree(goal_idx).cost, stats.elapsed); draw_tree(tree, cfg); draw_trajectory(path, cfg); else fprintf('未找到路径，迭代 %d 次，树节点数 %d\n', cfg.max_iter, numel(tree)); end

第一次跑不要急着调参，先看两件事：树有没有长起来、有没有到达目标。树完全没长说明起点状态非法或者 u_max 太小导致所有边都碰撞；树长满了但到不了目标，问题多半在采样策略上，这就进入下一节的调参环节。

运行时间上有个心理预期：默认参数下 5000 次迭代，普通笔记本上大概几十秒到两分钟，具体取决于 steer 里的控制采样条数和碰撞检测的精细程度。如果你的机器上跑了好几分钟还没出结果，先去看 nearest_node 是不是每次都全树线性扫描。

4.3 可视化与结果解读

出图通常有三层信息：障碍物多边形、树的枝干、最终轨迹。树的画法是从每个节点向它的父节点连一段折线，注意这里不能画直线，要把控制序列积分出来的中间状态串起来画，否则你看到的树和实际树不是一回事。

轨迹图要重点看两处：转弯处的曲率是否连续、末端是否真的停在目标状态。有些实现只检查位置到达，速度没收敛到零，画出来轨迹在终点「急刹」，实车上就是一次顿挫。

stats 里通常记录迭代次数、节点数、耗时、最终代价。把这几项打出来对比不同参数组合，比单纯看图靠谱得多。我习惯固定 seed 跑三组不同 u_max，把代价和节点数列成表，一眼就能看出哪组参数在「探索能力」和「收敛速度」之间取得了平衡。

4.4 调参顺序：先让它到，再让它好

参数一大把，乱调只会让自己陷入玄学。可行的顺序是：

1. 先保证能找到路径。把 goal_bias 调到 0.1～0.15，neighbor_r 调小到 1.0 左右，先让树能连到目标。这一步只看成功与否，不看代价。
2. 再保证路径质量。关掉或减小 goal_bias 到 0.05，把 neighbor_r 调回 2.0～3.0，打开 rewiring，看代价能不能降下来。
3. 最后压时间。降低 n_control 和 n_steps，或者把碰撞检测换成栅格查表，观察成功率和耗时之间的折中。

每一步只改一到两个参数，改完记录结果。同时改三个参数然后发现效果变好了，你也不知道是哪个起的作用，下次换个场景又得从头试。

注意：调参过程中如果突然出现「一条路径都找不到」，先回退到上一次能跑通的参数，再逐个加回去。别在原参数上继续微调，那样很容易把问题归因到错误的地方。

5. 避坑与排查：那些跑不出结果和跑出怪结果的情况

5.1 树上长满节点，就是到不了目标

现象：跑满 5000 次迭代，树把整个地图铺得很均匀，但目标附近一个节点都没有，goal_idx 始终为 0。

原因：状态采样是完全均匀的，而目标状态在四维状态空间里是个测度为零的点，靠随机采样命中的概率极低；再加上 cfg.goal 的速度分量如果设成非零值，前向积分几乎不可能正好落到那个状态上。

解决：加目标偏置，按 5%～10% 的概率直接把 x_rand 设成 cfg.goal；同时把目标状态的速度分量置零，并在判定到达时用一个容差球而不是精确相等。goal_tol 取 1.0 意味着位置误差在一米内就算到达，这个值要按地图尺度和机器人尺寸调，不要照抄。

5.2 打开 rewiring 之后代价不降反升

现象：关掉 rewiring 时路径长 25 米，打开之后变成 28 米，还出现了几段明显绕回去的轨迹。

原因：两个地方容易出问题。一是边代价和节点代价的定义不统一，比如边代价用了时间、节点代价用了控制能量，两者相加没有物理意义；二是改挂父节点时只改了 parent 指针，没有更新子树的 cost_to_come，导致后续的代价比较全部基于过期数据。

解决：先统一代价定义，全篇只用一个度量；再确保 rewire 之后对受影响子树做一次 DFS 更新，或者至少在 tree2path 取路径时重算一遍累计代价并和存储值比对。新边在改挂前必须重新做碰撞检测，这一点漏掉的话，你会得到一条「数学上更优但物理上撞墙」的路径。

5.3 迭代几百次后速度断崖式下跌

现象：前 200 次迭代每秒钟能加好几个节点，跑到几千节点时每次插入要等好几秒，最后卡死。

原因：最近邻搜索是 O(n) 的全树线性扫描，节点一多就是平方级复杂度；再加上 tree 用动态追加的方式增长，每次扩容都要复制整个结构体数组，内存开销也是平方级。

解决：预分配一个 max_iter 大小的结构体数组，用一个计数器记录当前有效长度；最近邻搜索如果不能用 knnsearch（因为距离度量是加权的、不是纯欧氏），至少做分块处理，或者维护一个粗粒度的空间网格索引，只在邻近网格里做精确比较。这两步做完，万级节点的场景基本能跑得动。

5.4 规划出的轨迹贴着障碍物边沿走

现象：可视化里轨迹和障碍物之间只隔一条线，仿真跑起来没事，换成带真实尺寸的模型就擦碰。

原因：碰撞检测只判断了离散节点、没判断边上的连续状态；或者障碍物没有按机器人尺寸做膨胀，把机器人当成了一个点。

解决：碰撞检测必须沿控制序列逐步积分检查；障碍膨胀半径取机器人外接圆半径加安全余量。如果地图是手工标注的多边形，还要检查顶点顺序是否闭合，不闭合的多边形会让 inpolygon 判断出错，这种错误在图上不一定看得出来。

5.5 换个动力学模型就报维度不匹配

现象：照着 models 目录新增了一个独轮车模型，主循环里 A * x 直接报错，提示矩阵维度不一致。

原因：nx 和 nu 被硬编码在多个函数里，比如碰撞检测里写死了 x(1:2) 是位置、积分函数里写死了 4×4 的 A。换模型之后状态维度从 4 变成 3，这些地方全部失效。

解决：把 nx、nu 放进 model 结构体，所有函数从 cfg.model.nx 取维度；位置索引单独抽一个 model.pos_idx 字段，二阶积分器是 [1,2]，独轮车也是 [1,2]，但换成三维飞行器就要改成 [1,2,3]。前向积分用 switch 分发到不同模型的实现，而不是在主循环里写 if-else。

6. 二次开发：换模型、加约束、接到自己的工程里

改造的第一步是新增动力学模型。在 models 目录下建一个文件，实现 f(x,u) 的离散形式和对应的 A/B 矩阵，然后让 cfg.model 指向它。独轮车是个好练手对象：状态 [x; y; θ; v]，控制 [a; ω]，是非线性的，没法直接用矩阵乘法积分，得手工写欧拉或者 RK4 步进。这里有个容易忽略的点——非线性模型的边代价要用积分出来的实际弧长或能量，不能沿用线性模型那套解析式。

第二步是加时变障碍。原版的障碍是静态多边形，改造成动态只需要在碰撞检测里多传一个时间戳参数，障碍位置按时间偏移。这会带来一个新问题：同一段边在不同时刻的碰撞结果不一样，rewiring 时判定的合法性可能在执行时已经失效。工程上的折中做法是给动态障碍加一个预测时间窗，只检查窗口内的碰撞，窗口外的部分交给底层避障兜底。

第三步是把结果导出给自己的控制器。tree2path 出来的 path 是状态序列，多数控制器要的是「时间 - 位置 - 速度」三列数据：

t = (0:size(path,2)-1) * cfg.dt; traj = [t', path(1:2,:)']; writematrix(traj, 'traj.csv'); % 直接给底层跟踪器读

验证改造是否成功，别只看单次运行的图。固定其他参数，换 20 个不同的随机种子各跑一次，统计成功率和最终代价的均值、方差。如果换了模型之后成功率从 95% 掉到 60%，那不是模型写错了就是代价函数没跟着改。我吃过这个亏：改完模型看着能跑，路径也画得挺漂亮，结果方差大到没法用，回头查才发现边的积分步数和碰撞检测的步数用了两个不同的 dt。

从那以后我每次改动力学模型或者代价函数，都强制走一遍「20 个种子 + 成功率 + 代价分布」的流程，哪怕只是改了一行。这套流程麻烦，但比事后对着一条玄学路径猜原因要省时间得多。希望帮到你。

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