MATLAB动力学约束RRT*运动规划实现与避坑

简介该资源是论文《Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints》的 MATLAB 复现代码面向从事机器人运动规划、最优控制与采样类算法研究的高年级本科生、研究生及工程开发者用于理解并验证带线性微分约束系统的最优运动规划思路也可作为课程设计或课题入门时的对照实现。压缩包共 17 个文件、约 14KB以 13 个 .m 脚本为主体涵盖 RRT* 主循环、二重积分与四旋翼两类动力学模型、状态与输入可行性判断、状态采样及轨迹可视化等模块另附 2 个 .mat 障碍物与路点数据和说明文档便于直接运行、调试与二次修改。已有 1300 余人学习下载。读者可对照论文还原本资源中算法的整体流程理解代价函数构造、采样与扩展策略、边连接与优化迭代等细节并借助自带场景完成仿真验证为自身课题的算法复现与改进提供可运行的起点。1. 差速小车拐不过弯问题出在哪儿拿一个窄走廊场景做实验两堵墙之间只留 0.9 米小车轴距 0.5 米、最大转向角速度有限。普通 RRT 跑出来的路径是一条漂亮的折线可把这条折线交给小车执行时它在第一个直角拐弯处就贴上了墙——因为算法从头到尾假设的“边走边转向”这个动作物理上根本做不到。这就是 kinodynamic RRT* 要解决的核心矛盾规划出来的每一条边都必须是动力学模型真正能执行出来的轨迹而不是状态空间里两点之间的直线。这份 MATLAB 版本的 kinodynamic_rrt_star 实现把采样、前向仿真、碰撞检测、代价重算、rewire 这一整条链路压缩进一组短小函数里跑起来能在图上直接看到搜索树“带着惯性”往外长。它适合两类人正在做移动机器人或机械臂运动规划、需要快速验证算法思路的开发者以及在 MATLAB 里做控制/规划课程实验、不想一上来就啃大型 C 框架的同学。2. 工程骨架MATLAB 下的一套可跑通规划器长什么样2.1 目录分层与各文件职责这份代码的组织方式是很典型的“算法内核 可替换外设”结构没有把采样、碰撞、代价全塞进一个大函数里。真正跑起来只需要main.m但读代码要按层看。kinodynamic_rrt_star/ ├── main.m % 入口建图、配参、调 planner、出图 ├── config/ │ └── params.m % 所有可调参数集中在这里 ├── core/ │ ├── planner.m % 主循环sample → nearest → steer → rewire │ ├── sample_state.m % 状态空间采样 │ ├── nearest_node.m % 最近邻查询 │ ├── near_nodes.m % 邻域集合查询 │ ├── steer.m % 控制采样 前向仿真生成本地边 │ ├── propagate.m % 动力学积分单步状态转移 │ └── rewire.m % 重连与代价回传 ├── collision/ │ ├── check_state.m % 单状态碰撞 │ └── check_edge.m % 沿轨迹逐点碰撞 ├── cost/ │ └── trajectory_cost.m % 一段轨迹的代价时间/能量/长度 ├── utils/ │ ├── visualize.m % 树、障碍、最终轨迹的三层绘制 │ └── tree_struct.m % 节点结构体的初始化与扩容 └── data/ └── map_demo.mat % 示例地图障碍多边形顶点分层的好处在于想换动力学模型只动propagate.m想换碰撞检测只动collision/下两个文件想改代价定义只动trajectory_cost.m。这种耦合度对二次开发来说是刚需血泪经验是很多网上流传的 RRT 代码把steer和check写在一起改一个参数要通读两百行。层次文件改动频率说明配置层params.m高步长、采样上限、邻域半径、模型参数算法层core/*.m中主循环与各算子接口要稳定外设层collision/、cost/高与实际机器人/场景强相关展示层utils/visualize.m低调试用不影响求解结果2.2 状态向量与动力学模型的定义方式整份代码围绕一个约定展开状态是一个列向量x控制是一个列向量u模型是x_next f(x, u, dt)。所有算法函数只认这个接口不关心里面到底是几维。这个约定写进params.mfunction p params() % ---------- 动力学模型 ---------- p.model.name unicycle; % unicycle | double_integrator p.model.wheelbase 0.5; % 轴距米 p.model.v_max 1.0; % 最大线速度 p.model.w_max 1.2; % 最大角速度 p.dt 0.05; % 积分步长秒 p.integ_steps 8; % 一条边由多少个 dt 拼成 % ---------- 状态空间 ---------- p.state_dim 3; % [x, y, theta] p.state_min [-5; -5; -pi]; p.state_max [ 5; 5; pi]; % ---------- 规划器参数 ---------- p.max_iter 3000; % 最大迭代次数 p.goal_bias 0.08; % 直接朝目标采样的概率 p.goal_radius 0.25; % 判定到达的半径 p.ctrl_samples 12; % 每次 steer 在控制空间采多少个点 p.rewire_radius 1.2; % 邻域重连半径 p.dist_weights [1; 1; 0.4]; % 最近邻距离的状态加权 endstate_dim设为 3 表示单车模型如果换成双积分器状态变成[x, y, vx, vy]state_dim改成 4dist_weights也要改——因为速度分量和位置分量量纲不同不加权的话最近邻会被速度主导。这是新手最容易翻车的地方之一后面避坑章会展开。integ_steps决定了“一条边”的粒度。设成 8 意味着每条候选边由 8 个dt拼成边越长需要的仿真步数越多通常用ceil(T/dt)动态算固定值只是简化版写法。2.3 运行环境与第一次启动跑这份代码不需要任何附加工具箱纯 MATLAB 基础函数即可R2018b 以上比较稳妥用到了struct数组自动扩容和tiledlayout绘图。步骤很短# 1. 把整个文件夹拖进 MATLAB 当前路径或 addpath addpath(genpath(kinodynamic_rrt_star)); # 2. 在命令行直接跑入口脚本 mainmain.m里做的事情是固定的五步加载地图、读参数、初始化树、调用planner、调visualize出图。第一次跑建议先把p.max_iter改小到 500观察树是怎么长的确认碰撞检测和状态转移没写错再放大跑完整求解。直接上 3000 次迭代的话如果模型参数写错了你会盯着一个空图等很久还看不出错在哪。3. 核心链路一次迭代里到底发生了什么3.1 采样与最近邻距离度量决定树的“形状”采样函数本身很简单难的是最近邻的距离定义。状态里混了位置和角度角度还有周期性pi和-pi是一回事直接把三个分量求欧氏距离会让theta的差值在 ±2π 附近突然跳变。function x_rand sample_state(p, x_goal) if rand() p.goal_bias x_rand x_goal; % 有偏采样加速收敛 else x_rand p.state_min rand(p.state_dim,1) .* (p.state_max - p.state_min); end end function d state_distance(x1, x2, w) diff x1 - x2; diff(3) wrap_to_pi(diff(3)); % 角度差折算到 [-pi, pi] d sqrt(sum((w .* diff).^2)); % 加权欧氏距离 end function a wrap_to_pi(a) a mod(a pi, 2*pi) - pi; % 关键不要让角度跳变污染距离 endwrap_to_pi这个四行函数是整个最近邻正确性的基础。少了它树在theta跨越 ±π 的位置会长出莫名其妙的枝条因为算法误以为两个几乎同向的状态相隔很远。w就是p.dist_weights位置权重给 1角度权重给 0.4 左右含义是“0.4 弧度大约等价于 1 米的位置差异”具体值要按机器人尺寸调走廊窄就调小开阔场地可以调大。goal_bias设 0.08 是个经验值。设太高比如 0.3会让树过早朝目标方向聚集在障碍物后面绕不出来设 0 则完全靠随机撞运气收敛慢。0.050.1 之间是比较稳的区间。3.2 扩展一步在控制空间采样再前向仿真这是 kinodynamic 版本和普通 RRT 最本质的差别。普通 RRT 的steer是“从x_near朝x_rand走一小步”走的是直线这里的steer是“随机抽若干个控制量每个都往前仿真一段挑一个最接近x_rand且不撞的”。function [x_new, traj, ok] steer(x_from, x_rand, p) best_d inf; x_new x_from; traj []; ok false; for k 1:p.ctrl_samples % 在允许的控制范围里均匀采样 switch p.model.name case unicycle u [ (2*rand()-1)*p.model.v_max; (2*rand()-1)*p.model.w_max ]; case double_integrator u [ (2*rand()-1)*p.model.a_max; (2*rand()-1)*p.model.a_max ]; end [x_end, x_traj] simulate_edge(x_from, u, p); if isempty(x_traj), continue; end % 中途撞了直接丢 d state_distance(x_end, x_rand, p.dist_weights); if d best_d best_d d; x_new x_end; traj x_traj; ok true; end end end function [x_end, x_traj] simulate_edge(x0, u, p) x_traj zeros(p.state_dim, p.integ_steps1); x_traj(:,1) x0; for i 1:p.integ_steps x_prev x_traj(:,i); x_traj(:,i1) propagate(x_prev, u, p.dt, p.model); if ~check_state(x_traj(:,i1), p) % 逐点查碰撞 x_end []; x_traj []; return; end end x_end x_traj(:,end); end逻辑很清楚ctrl_samples个候选控制量里每个都跑完整段仿真全程不碰障碍才有资格参与“谁更接近x_rand”的评比。okfalse表示这一段没有任何控制量能走通主循环就该跳过这次迭代而不是硬插节点。simulate_edge里“逐点查碰撞”这个设计是有代价的integ_steps8意味着每条边要调 8 次碰撞检测。如果碰撞检测里有复杂多边形求交这会成为性能瓶颈。常见做法是先用粗粒度包围盒筛一遍再对可疑段做精确检测第 6 章会讲怎么接。3.3 碰撞检测沿轨迹查而不是只查端点只查端点是最隐蔽的错误。一段边从 A 到 B端点都在空闲区中间可能整个穿过一堵薄墙。check_edge.m的做法就是把traj的每一列都送进check_statefunction free check_edge(x_traj, p) free true; for i 1:size(x_traj,2) if ~check_state(x_traj(:,i), p) free false; return; end end end function free check_state(x, p) % 机器人近似成一个半径 r 的圆做膨胀检测 r p.robot_radius; for k 1:numel(p.obstacles) poly p.obstacles{k}; d point_polygon_distance(x(1:2), poly); if d r free false; return; end end free x(1) p.state_min(1) x(1) p.state_max(1) ... x(2) p.state_min(2) x(2) p.state_max(2); endpoint_polygon_distance需要自己实现点在多边形内返回负值在外返回到最近边的距离。用圆近似机器人是最省事的做法实际项目里如果车体是长方形要把x(3)也带进检测——取车身四个角点分别做点在多边形内的判断。这一步决定轨迹“贴不贴墙”后面避坑章会讲贴边抖动的成因。3.4 第一次跑通参数怎么填、图怎么看主循环骨架值得单独看一眼因为它决定了 rewire 的插入位置for iter 1:p.max_iter x_rand sample_state(p, x_goal); idx_near nearest_node(tree, x_rand, p); [x_new, traj, ok] steer(tree.state(idx_near,:), x_rand, p); if ~ok, continue; end % 没有可行扩展跳过 idx_new add_node(tree, x_new, idx_near, traj); tree rewire(tree, idx_new, p); % 见第 4 章 if state_distance(x_new, x_goal, p.dist_weights) p.goal_radius goal_idx idx_new; break; end end运行结果是一张三色图灰色细线是搜索树红色折线是最终路径黑色多边形是障碍。第一次跑通的标准不是“路径最短”而是路径上的每一段都能被propagate从上一状态复现出来。验证方法是把最终路径的每一段重新仿真一遍比较终点误差误差超过 1e-6 就说明某处状态记录和实际转移不一致。参数起步建议dt0.05、integ_steps8、ctrl_samples12、max_iter3000、rewire_radius1.2。地图尺寸 10×10 的话这套参数在普通笔记本上大约几秒到十几秒出解。4. 渐近最优rewire 是怎么把路径越修越短的4.1 Near 集合与邻域半径的取法RRT* 和 RRT 的分水岭在near_nodes每次插新节点先找出它周围一定半径内的所有已有节点尝试给新节点换一个更便宜的父节点再尝试让新节点成为这些邻居的父节点。半径怎么取直接决定收敛速度和计算量。function idxs near_nodes(tree, x_new, p) % 标准 RRT* 半径r gamma * (log(n)/n)^(1/d) n size(tree.state, 1); d p.state_dim; r min(p.rewire_radius, p.gamma * (log(n1)/(n1))^(1/d)); idxs []; for i 1:n if state_distance(tree.state(i,:), x_new, p.dist_weights) r idxs(end1) i; %#okAGROW end end endgamma是个跟状态空间体积相关的常数取 1.02.0 之间起步比较稳。公式的意义是节点少的时候半径大多探索节点多了半径收缩减少计算。如果直接固定用rewire_radius1.2不做收缩跑到几千个节点后每次 near 查询要扫全树性能会掉得很难看——这是最典型的“越跑越慢”现象。循环里idxs(end1)这种写法在 MATLAB 里会触发数组反复扩容节点上万时会明显变慢。常见做法是先预分配idxs zeros(n,1)配一个计数器或者直接把tree.state拼成矩阵一次性算距离diff tree.state - x_new; % n×d 广播 diff(:,3) wrap_to_pi(diff(:,3)); dists sqrt(sum((diff .* p.dist_weights).^2, 2)); idxs find(dists r);向量化之后同样的查询能快一个数量级代价是内存占用几千个节点完全无所谓。4.2 Rewire 的判定条件与代价重算rewire 分两个方向给新节点选更便宜的父节点以及让新节点接管邻居。两个方向的判定都必须“严格变便宜”才执行否则会写出环。function [tree, changed] rewire(tree, idx_new, p) changed false; near_idx near_nodes(tree, tree.state(idx_new,:), p); x_new tree.state(idx_new,:); % ---- 方向一给新节点挑一个更便宜的父节点 ---- best_cost tree.cost(idx_new); for i near_idx if i tree.parent(idx_new), continue; end [traj, ok] connect(tree.state(i,:), x_new, p); % 两点间求可行边 if ~ok, continue; end c tree.cost(i) trajectory_cost(traj, p); if c best_cost - 1e-9 % 严格下降才换 best_cost c; tree.parent(idx_new) i; tree.cost(idx_new) c; tree.traj{idx_new} traj; changed true; end end % ---- 方向二新节点成为邻居的父节点 ---- for i near_idx if i tree.parent(idx_new) || i idx_new, continue; end [traj, ok] connect(x_new, tree.state(i,:), p); if ~ok, continue; end c tree.cost(idx_new) trajectory_cost(traj, p); if c tree.cost(i) - 1e-9 tree.parent(i) idx_new; tree.cost(i) c; tree.traj{i} traj; changed true; end end end1e-9这个容差不是可有可无的。浮点累加会让两条代价几乎相等的路径在c cost判定上反复横跳严重时子树被反复剪接最终路径在两根枝条之间来回摆。加个极小容差就能压住这种玄学抖动。有个细节容易被忽略方向二里改了tree.cost(i)之后i的整棵子树的代价其实都变了。严格实现要做一次 DFS 向下更新所有后代简化实现只在需要时沿parent链向上累加。这份代码的常见做法是懒更新——cost只在比较时用最终输出的路径代价单独从 path 反算一遍省掉大量回传开销正确性不受影响。4.3 输出轨迹的整理与平滑rewire 之后树的父子关系可能已经大改最终路径要从goal_idx沿parent一路回溯到根把每个节点存的traj拼起来function path extract_path(tree, goal_idx) idxs goal_idx; while tree.parent(idxs(1)) ~ 0 idxs [tree.parent(idxs(1)); idxs]; %#okAGROW end path []; for k 1:numel(idxs)-1 seg tree.traj{idxs(k1)}; % 子节点存的是它到父亲的边 path [path, seg(:, 2:end)]; % 去掉重复的衔接点 end end拼接时seg(:, 2:end)把每段的起点去掉避免相邻两段共享同一个点导致后续做时间参数化时出现零长度区间。拼出来的path是稠密的状态序列间隔dt直接就能拿去给控制器做前馈。如果要做平滑别直接对x, y做样条插值——插完的曲线很可能不满足动力学约束又回到“规划出来执行不了”的老问题上。正确做法是对控制序列做平滑再重新前向仿真一遍保证结果仍然可执行。这一步在第 6 章的验证环节会再提一次。5. 避坑清单跑不出解、路径贴墙、越跑越慢怎么办5.1 现象跑满 3000 次迭代还是一条空路径原因通常有三个方向采样空间、碰撞判定、目标半径。先看state_min/state_max有没有把起点和目标包进去这是最蠢但最常见的错——sample_state采不到目标附近的点树永远长不到终点。再看goal_radius是不是设得比dt * v_max还小一步跨过去直接越界判定失败永远进不了阈值。碰撞判定里robot_radius设大了一圈也会导致窄通道被完全封死。解决办法是按顺序排查把check_state打印出来看起点是否返回true把p.goal_bias临时调到 0.5 强制朝目标采样如果能出解就说明是采样覆盖不足把robot_radius调到 0 再跑一次能出解就是膨胀半径过大。这三次二分定位基本能锁定问题在哪个文件。5.2 现象路径能出来但贴着墙角走、拐弯处来回抖原因在于碰撞检测只用圆近似车体。圆半径取的是外接圆半径时车在直道上也“变胖”算法被迫绕远取内切圆半径时车头车尾的角点实际已经蹭进墙里视觉上就是贴墙。抖动则多半来自角度分量没有做wrap_to_pi或者dist_weights里角度权重给得太小导致算法认为“反向”和“同向”差不多近。解决方式是两步把碰撞检测从圆形改成车身四角点检测x(3)必须参与计算把角度权重从 0.4 往上调同时检查所有state_distance调用点是不是都过了wrap_to_pi。抖动如果是出现在 rewire 之后先按 4.2 节加容差。5.3 现象节点过千之后每次迭代明显变卡最后像卡死这就是 4.1 节提到的问题near_nodes和nearest_node都在做全树线性扫描复杂度 O(n)n 到几千就是百万级距离计算还是在一个 for 循环里逐个算。MATLAB 在循环里做标量运算本来就慢再叠加数组扩容慢上加慢。解决分三层第一步把距离计算向量化用矩阵广播一次算完第二步把tree.state换成预分配的定长矩阵节点数上限按max_iter一次性开好用node_count计数第三步如果还要更快上 KD 树——MATLAB 自带的knnsearch对低维状态够用但角度周期性需要自己处理通常做法是把theta映射到(cos, sin)两个分量再建索引。5.4 现象改完动力学参数轨迹直接发散或者原地打转dt和integ_steps是耦合的。总时长T dt * integ_stepsdt太大而v_max又快单步位移能跨过整个障碍物碰撞检测逐点扫也扫不到中间——轨迹看起来穿墙飞过去。反过来dt太小、integ_steps不够一条边走得极短树长得像一团毛线永远够不到目标。经验规则是让单步位移不超过最小障碍物尺寸的三分之一v_max * dt min_obstacle_size / 3。改完dt记得同步改integ_steps保持T在合理范围T最好能覆盖“从一个状态到邻近状态”所需的典型时间。5.5 现象每次跑结果都不一样出了问题没法复查采样全是rand()当然每次不一样。调试阶段必须能把随机性钉死在main.m开头加rng(42)所有随机数走同一个种子。定位到具体 bug 之后再放开。更稳的做法是把每次运行的params结构体连同种子一起存成.mat出问题直接load回来复现比靠记忆靠谱。6. 二次开发换模型、接检测、加验证6.1 换动力学模型的最小改动面想从单车模型换到双积分器改动集中在三处params.m里改model.name和state_dim、state_min/max、dist_weightspropagate.m里加一个casesteer.m里控制采样的分支。算法主循环、rewire、路径提取一行都不用动这就是分层带来的好处。function x_next propagate(x, u, dt, model) switch model.name case unicycle x_next [x(1) u(1)*cos(x(3))*dt; x(2) u(1)*sin(x(3))*dt; x(3) u(2)*dt]; case double_integrator A [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; B [dt^2/2 0; 0 dt^2/2; dt 0; 0 dt]; x_next A*x B*u; end end注意双积分器的状态是[x, y, vx, vy]dist_weights要给速度分量一个合理权重否则最近邻会被“跑得快但位置偏”的节点带偏。改完先用rng固定种子跑一次确认路径能从起点连到终点再看曲率是否在允许范围内。6.2 接自己的碰撞检测与代价函数实际机器人往往有 URDF 或者一套现成的碰撞库。接入方式是把check_state和check_edge换成对外部库的封装保持函数签名不变输入状态或轨迹、返回布尔值。代价函数同理trajectory_cost.m里现在算的是时间积分想改成能量最优就换成sum(u.^2)*dt想加平滑惩罚就把控制增量的平方和加进去。接口稳定的前提下新代价函数的量纲要和旧的一致否则rewire_radius的取值要重调。换代价之后最容易出的问题是“路径变短了但抖得更厉害”因为纯能量代价不惩罚控制突变加一项lambda * sum(diff(u).^2)能压住。6.3 验证别只看图好不好看图好看不等于结果对。我会固定跑三组验证把最终路径的每一段用propagate重新仿真比对终点误差应小于 1e-6检查路径上每个点是否都通过check_state应该是全true统计 10 个不同种子下的路径代价均值和方差方差过大说明 rewire 的容差或者邻域半径没调好。err 0; for k 1:size(path,2)-1 x_pred propagate(path(:,k), u_seq(:,k), p.dt, p.model); err max(err, norm(x_pred - path(:,k1))); end assert(err 1e-6, 状态记录与仿真结果不一致检查 traj 拼接);这个断言我一般写在main.m末尾跑通一次就留着。从那以后我每次改动力学参数或者碰撞检测都强制走一遍这条重仿真比对——它拦下过不止一次“轨迹拼接少了一列”的低级错误。规划算法出问题往往不报错只是悄悄给出一条执行不了的路径能自动发现这类问题的只有这种笨办法。希望帮到你。本文还有配套的精品资源点击获取