简介这套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_{k1} A x_k B u_kA 和 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 取 1030 已经够用。第 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 endrewiring 的触发条件同样要对齐只有当 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 固定与否直接影响你复现问题的能力调试阶段务必固定出图之前再换随机种子跑多次看分布。参数作用调大的后果调小的后果dt积分步长单步跨度大碰撞检测易漏检树增长慢节点数暴涨u_max控制上限轨迹激进容易撞障碍探索能力不足绕远路n_control控制采样条数连接成功率高单步耗时上升大量 steer 失败树稀疏goal_bias目标偏置概率收敛快但易陷入局部极小探索充分但收敛慢neighbor_r重接邻域半径路径更优计算量陡增退化为普通 RRT3.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(j1,:)) 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 落在障碍物外面起点在障碍里的话第一次碰撞检测就返回树一个节点都长不出来。如果用的是开源 Octaveinpolygon 是有的但某些绘图函数行为不一致可视化可能出问题算法本身能跑。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 调参顺序先让它到再让它好参数一大把乱调只会让自己陷入玄学。可行的顺序是先保证能找到路径。把 goal_bias 调到 0.10.15neighbor_r 调小到 1.0 左右先让树能连到目标。这一步只看成功与否不看代价。再保证路径质量。关掉或减小 goal_bias 到 0.05把 neighbor_r 调回 2.03.0打开 rewiring看代价能不能降下来。最后压时间。降低 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 个种子 成功率 代价分布」的流程哪怕只是改了一行。这套流程麻烦但比事后对着一条玄学路径猜原因要省时间得多。希望帮到你。本文还有配套的精品资源点击获取