简介这份资源是Kinodynamic RRT算法的MATLAB实现对应Dustin J. Webb与Jur van den Berg的论文《Kinodynamic RRT: Optimal Motion Planning for Systems with Linear Differential Constraints》面向从事机器人运动规划、最优控制与自动驾驶路径生成方向的研究生、科研人员及工程师。它解决的是带线性微分约束系统在动力学限制下的最优运动规划问题适合已具备RRT与最优控制基础、希望复现论文算法或将其迁移到四旋翼、双积分器等平台的中高级学习者。压缩包共17个文件约14KB以13个.m脚本为主体辅以2个.mat数据文件、1个md说明与1个txt许可文件脚本覆盖rrtstar主流程、double_integrator与rrt_double_int_2d动力学建模、quad系列四旋翼状态采样与可视化、is_positive_definite等矩阵校验工具数据文件则提供障碍物与路点场景。已有1296人学习下载。读者可据此获得可直接运行的算法骨架、论文核心公式的代码映射、双积分器与四旋翼两类实验场景以及状态采样、碰撞检测与场函数绘制的完整实现思路。1. 从一份 MATLAB 源码说起为什么运动动力学 RRT* 值得你花时间跑一遍如果你做过移动机器人或者机械臂的路径规划大概率写过或跑过 RRT、RRT*。但很多人第一次把 RRT* 用到真实平台上时会发现一个尴尬的事规划出来的路径全是折线机器人根本走不了——要么转弯半径不够要么加速度超限要么在狭窄通道里来回震荡。问题出在标准 RRT* 只考虑几何可行性完全忽略了机器人的运动学与动力学约束。这份kinodynamic_rrt_star的 MATLAB 源码解决的正是这个痛点它在 RRT* 的采样与扩展框架里引入了状态空间和动力学模型让规划出的轨迹天然满足速度、加速度甚至曲率约束。整套代码用 MATLAB 写成不依赖任何工具箱适合做算法验证、课程设计也适合作为二次开发的起点。下面我按“能干什么 → 怎么跑 → 怎么改”的顺序把这份源码拆开讲清楚。2. 项目定位与技术选型为什么用 MATLAB 写运动动力学规划2.1 这个项目到底解决什么问题标准 RRT* 的节点是二维或三维空间中的点扩展时直接在两节点之间连直线然后做碰撞检测。这种做法在几何层面没问题但一旦机器人有非完整约束比如差速小车不能横移或者二阶动力学加速度有限直线连接就不可行了。kinodynamic_rrt_star的核心思路是把每个节点从“位置点”升级为“状态点”状态里包含位置和速度或更多维度扩展时不再连直线而是用一组控制输入在固定时间步长内做前向积分生成一条满足动力学方程的轨迹段。这样做的好处很直接规划出来的每一段轨迹都是机器人实际能执行的不需要后处理再做平滑。代价是状态空间维度变高采样效率下降所以代码里通常会配合启发式采样或目标偏置来加速收敛。这份 MATLAB 实现把整个流程拆成了几个独立函数方便你替换动力学模型或调整采样策略。2.2 技术栈与文件组织项目是纯 MATLAB 实现没有用到 Simulink 或任何工具箱这意味着你只要有基础 MATLAB 环境就能跑。常见的文件组织方式是这样的kinodynamic_rrt_star/ ├── main.m % 入口脚本设置参数并调用规划器 ├── kinodynamic_rrt_star.m % 核心规划函数 ├── steer.m % 控制输入采样与前向积分 ├── nearest_node.m % 最近邻搜索 ├── near_nodes.m % 近邻节点集合 ├── collision_check.m % 碰撞检测 ├── cost.m % 代价计算 └── draw_result.m % 可视化这种拆法在 MATLAB 项目里很常见每个函数职责单一方便你单独替换。比如你想换成自行车模型只需要改steer.m里的动力学方程其他部分不用动。2.3 状态空间与动力学模型的选择代码默认使用的动力学模型通常是二阶积分器double integrator状态向量为[x, y, vx, vy]控制输入为[ax, ay]。这个模型虽然简单但足以验证算法框架。如果你要做差速小车可以把状态改成[x, y, theta, v, omega]控制输入改成[a, alpha]。关键是要保证steer.m里的积分步长和main.m里的时间参数一致否则轨迹会断裂。提示MATLAB 的ode45也可以用来做前向积分但在这个场景下没必要固定步长的欧拉积分足够快而且更容易控制轨迹段长度。3. 核心模块拆解从采样到轨迹生成的完整链路3.1 主循环采样、最近邻、扩展、重连kinodynamic_rrt_star.m是整个项目的心脏它的结构和标准 RRT* 类似但每个环节都做了适配。下面这段代码是主循环的骨架% 初始化树 tree.nodes start_state; tree.edges []; tree.costs 0; for iter 1:max_iter % 1. 采样带目标偏置的随机采样 if rand() goal_bias x_rand goal_state; else x_rand sample_state(space_limits); end % 2. 最近邻在状态空间里找距离最近的节点 idx_near nearest_node(tree, x_rand, distance_metric); % 3. 扩展从最近邻出发采样控制输入并前向积分 [x_new, u_new, valid] steer(tree.nodes(idx_near), x_rand, dt, dyn_model); if ~valid continue; end % 4. 碰撞检测对整条轨迹段做检查 if collision_check(tree.nodes(idx_near), x_new, u_new, obstacles) continue; end % 5. 近邻重连在半径 r 内找更优父节点 near_idxs near_nodes(tree, x_new, radius); [parent_idx, cost_new] choose_parent(tree, near_idxs, x_new, u_new); % 6. 插入新节点 tree insert_node(tree, x_new, parent_idx, u_new, cost_new); % 7. 重连用新节点优化近邻节点的代价 tree rewire(tree, near_idxs, x_new); % 8. 检查是否到达目标 if norm(x_new(1:2) - goal_state(1:2)) goal_tolerance break; end end这段代码里有几个关键点值得展开。第一采样函数sample_state不是简单地在位置空间均匀采样而是在整个状态空间里采样包括速度维度。这意味着速度分量的范围需要根据机器人实际能力设置否则会采出大量不可行的状态。第二steer函数返回的valid标志很重要因为有些控制输入会导致积分后的状态超出物理限制比如速度超过最大值这时候应该直接丢弃这次扩展。第三碰撞检测不是只检查端点而是要对整条轨迹段做离散化检查否则会漏掉中间穿墙的情况。3.2 steer 函数控制输入采样与前向积分steer.m是运动动力学 RRT* 区别于标准 RRT* 的核心。标准 RRT* 的 steer 就是连直线而这里需要采样控制输入并做积分。下面是一个典型的实现function [x_new, u, valid] steer(x_near, x_rand, dt, dyn_model) % 采样控制输入在允许范围内随机取 u sample_control(dyn_model.u_min, dyn_model.u_max); % 前向积分用欧拉法或 RK4 x_new x_near; for t 1:dt:dt x_new x_new dyn_model.f(x_new, u) * dt; end % 检查状态是否在允许范围内 valid check_state_limits(x_new, dyn_model.state_min, dyn_model.state_max); % 可选如果离目标太远可以调整控制输入方向 % 这里不做保持随机性 end这段代码的逻辑是从最近邻节点出发随机采一个控制输入在固定时间dt内做前向积分得到新状态。dyn_model.f是动力学方程对于二阶积分器就是[vx; vy; ax; ay]。dt的选择很关键太大则轨迹粗糙太小则树增长慢。我一般会取0.1到0.5秒之间具体看机器人速度。注意如果dt固定而控制输入随机那么新状态的速度分量可能变化很大。有些实现会限制控制输入的采样范围让速度变化更平滑这样规划出的轨迹更适合实际执行。3.3 最近邻与近邻搜索距离度量怎么选在状态空间里做最近邻搜索距离度量不能只用欧氏距离。因为状态里包含位置和速度量纲不同直接算欧氏距离会导致速度分量主导。常见的做法是加权欧氏距离function d distance_metric(x1, x2, weights) diff x1 - x2; d sqrt(sum((weights .* diff).^2)); endweights是一个向量位置分量权重通常设为 1速度分量权重设为 0.1 到 0.5。这样位置差异对距离的影响更大符合规划直觉。近邻搜索near_nodes则是找半径r内的所有节点r可以固定也可以随迭代次数递减。固定半径实现简单但后期可能找不到足够的近邻递减半径更接近标准 RRT* 的理论但需要调参。3.4 碰撞检测离散化检查与障碍物表示碰撞检测在运动动力学规划里比几何规划更麻烦因为轨迹是曲线不是直线。常见做法是把轨迹按时间步离散成若干点然后逐点检查。障碍物可以用圆形、矩形或多边形表示检查函数根据障碍物类型不同而不同。下面是一个圆形障碍物的检查示例function collision collision_check(x_start, x_end, u, obstacles, dt) collision false; x x_start; for t 1:dt:dt x x dyn_model.f(x, u) * dt; for i 1:length(obstacles) if norm(x(1:2) - obstacles(i).center) obstacles(i).radius robot_radius collision true; return; end end end end这段代码把轨迹段按dt离散每个离散点都检查是否与障碍物相交。robot_radius是机器人外接圆半径用来做膨胀处理。如果障碍物是矩形可以用inpolygon或者自己写点在矩形内的判断。4. 跑起来参数配置、运行步骤与结果验证4.1 环境准备与入口脚本这份代码不需要额外工具箱但需要 MATLAB R2016b 或更高版本因为用到了string类型和某些函数句柄特性。把整个文件夹放到 MATLAB 当前路径下直接运行main.m即可。main.m里通常包含以下配置% 状态空间限制 space_limits [-5, 5; -5, 5; -1, 1; -1, 1]; % [xmin xmax; ymin ymax; vxmin vxmax; vymin vymax] % 起点和终点 start_state [0; 0; 0; 0]; goal_state [4; 4; 0; 0]; % 动力学模型参数 dyn_model.u_min [-1; -1]; dyn_model.u_max [1; 1]; dyn_model.f (x, u) [x(3); x(4); u(1); u(2)]; % 算法参数 max_iter 5000; goal_bias 0.1; dt 0.2; goal_tolerance 0.2;这些参数直接决定了规划效果。space_limits里的速度范围要和机器人实际能力匹配设太大则采样效率低设太小则可能找不到可行解。goal_bias是目标偏置概率一般取 0.05 到 0.2太大容易陷入局部最优太小则收敛慢。4.2 运行步骤与中间输出运行main.m后代码会进入主循环每轮迭代都会在图上更新树结构。如果你想看中间过程可以在循环里加draw_result调用但会拖慢速度。建议先跑完再画图。跑完后draw_result会画出树结构、最终路径和障碍物。最终路径是从目标节点回溯到根节点得到的每条边对应一段轨迹。如果跑完发现没有找到路径先检查max_iter是否够大再检查goal_tolerance是否太小。有时候目标点附近有障碍物导致永远无法精确到达这时候可以适当放宽容差。4.3 结果验证路径是否满足动力学约束规划出的路径不能只看是否到达终点还要验证每一段是否满足动力学约束。最简单的方法是检查每段轨迹的速度和加速度是否在允许范围内。可以在draw_result里加一段验证代码% 验证路径可行性 path extract_path(tree, goal_idx); for i 1:length(path)-1 x1 path(i).state; x2 path(i1).state; u path(i).control; % 检查速度限制 if any(abs(x2(3:4)) dyn_model.state_max(3:4)) warning(速度超限 at segment %d, i); end % 检查加速度限制 if any(abs(u) dyn_model.u_max) warning(加速度超限 at segment %d, i); end end这段代码遍历路径的每一段检查末端速度和加速度。如果发现超限说明steer函数里的控制采样范围设大了或者积分步长dt不合适。5. 避坑与常见问题跑不通时先看这几条5.1 现象树增长极慢迭代几千次还没到目标原因通常是状态空间维度太高或者速度范围设得太大导致采样点大量落在不可行区域。解决方法是缩小速度范围或者改用启发式采样比如在目标方向附近增加采样概率。另一个可能是dt太小每段轨迹太短树扩展慢。可以适当增大dt但不要超过 0.5 秒否则轨迹太粗糙。5.2 现象规划出的路径抖动严重机器人无法跟踪这是因为控制输入采样完全随机相邻轨迹段之间没有平滑性约束。解决方法是限制控制输入的采样范围让相邻段的控制输入变化不要太大。可以在steer里加一个参数让新控制输入与上一段控制输入的差值不超过某个阈值。另一个方法是后处理做轨迹平滑但这会破坏 RRT* 的最优性保证。5.3 现象碰撞检测漏检路径穿过障碍物原因通常是离散化步长太大轨迹段中间的点被跳过了。解决方法是减小碰撞检测的离散步长至少比dt小一个数量级。如果障碍物很薄还需要做连续碰撞检测但这在 MATLAB 里实现起来比较麻烦一般用更细的离散化就够了。5.4 现象重连后代价反而变大这是 RRT* 实现里的经典坑。重连时要用新节点作为父节点重新计算近邻节点的代价但如果代价计算函数没有考虑动力学约束或者代价传播没有正确更新就会出现代价震荡。解决方法是确保cost.m里的代价计算和rewire里的更新逻辑一致并且每次重连后都要更新整棵子树的代价。5.5 现象MATLAB 报错“索引超出矩阵维度”这种错误通常出现在near_nodes返回空集或者choose_parent里没有正确处理空集。解决方法是加判断如果近邻集合为空直接用最近邻作为父节点。另外tree.nodes如果是矩阵插入新节点时要注意维度匹配建议用结构体数组或者元胞数组存储节点。6. 二次开发换动力学模型、加约束、提效率的实操思路6.1 替换动力学模型从二阶积分器到自行车模型如果你想把这套代码用到差速小车上最直接的方法是改steer.m里的动力学方程。二阶积分器的状态是[x, y, vx, vy]自行车模型的状态是[x, y, theta, v]控制输入是[a, omega]。动力学方程变成dyn_model.f (x, u) [x(4)*cos(x(3)); x(4)*sin(x(3)); u(2); u(1)];这里x(3)是朝向角x(4)是线速度u(1)是加速度u(2)是角速度。改完方程后状态限制、距离度量、碰撞检测都要相应调整。距离度量里朝向角的差值要处理成[-pi, pi]范围否则会出现跳变。6.2 加入曲率约束与速度约束自行车模型天然有曲率约束因为角速度和线速度的比值决定了转弯半径。如果你想显式限制曲率可以在steer里加判断如果abs(u(2)/x(4)) max_curvature就丢弃这次采样。速度约束则通过状态限制实现在check_state_limits里检查x(4)是否超过最大值。6.3 提升规划效率启发式采样与并行化MATLAB 的循环效率不高如果max_iter设到几万跑起来会很慢。提升效率的几个方向一是用启发式采样比如在目标点附近增加采样概率或者用informed RRT*的思路在椭圆区域内采样二是把碰撞检测向量化一次检查多个障碍物三是用parfor并行化多个独立采样但 RRT* 的树结构是共享的并行化需要小心处理。6.4 验证改造效果一套可复用的测试流程每次改完动力学模型或参数建议按以下流程验证先用小规模场景无障碍物、起点终点距离近跑通确认能到达目标再加一个障碍物检查避障是否正常最后加多个障碍物和狭窄通道检查规划成功率和路径质量。路径质量可以用代价、平滑度、规划时间三个指标衡量。我一般会跑 20 次取平均值避免单次随机性影响判断。提示MATLAB 的rng函数可以固定随机种子方便复现结果。调试时固定种子对比不同参数的效果会更直观。6.5 一个容易忽略的细节控制输入的连续性标准 RRT* 不关心控制输入的连续性但实际机器人跟踪轨迹时控制输入突变会导致执行器饱和或震荡。一个简单的改进是在steer里让新控制输入与父节点的控制输入做加权平均权重可以设为 0.7 和 0.3。这样轨迹会更平滑但可能牺牲一点最优性。这个改动很小但效果很明显我每次做运动动力学规划都会加上。从那以后我每次改动力学模型都会先固定随机种子跑 20 次确认成功率没掉再继续调参。希望这份拆解能帮你少走点弯路把这份 MATLAB 源码真正跑起来、改起来。本文还有配套的精品资源点击获取