简介围绕 Kinodynamic RRT* 的 MATLAB 实现面向运动规划学习者与机器人、控制方向研究者聚焦线性微分约束系统的最优运动规划问题可用于复现论文思路、理解采样规划与动力学可行性检验。压缩包共 17 个文件约 14KB以 13 个 .m 脚本为主体辅以 2 个 .mat 场景数据及 1 个 md 说明、1 个 txt 许可文件脚本覆盖 RRT* 主流程、二重积分与四旋翼示例、状态与输入可行性判断以及障碍场和轨迹可视化数据文件提供障碍物与路点信息。已有 1302 人学习下载。读者可据此梳理算法从采样、扩展、代价优化到动力学约束验证的实现链路借助现成脚本开展仿真实验、对照论文公式检查细节并在此基础上替换模型或场景做二次开发。1. 底层控制器一执行规划器给的折线路径就把车带出了弯道kinodynamic RRT* 要解决的就是这件事。MATLAB 里这份实现把动力学方程和执行器饱和塞进采样过程树上每条边本身就是一条能跑出来的轨迹而不是一条等着被跟踪的直线。它把状态空间定义、控制采样、前向积分、碰撞检查、选父与重连串成了一条完整链路适合正在做移动机器人或无人车轨迹规划、又不想一上来就啃 C 框架的读者拿它把算法骨架跑通、把参数手感养出来。2. 动力学约束怎么落地状态向量、饱和与实际积分几何 RRT* 里两个节点之间的边是一条直线代价是欧氏距离碰撞检查只需要扫一条线段。kinodynamic 版本把这三件事全部推翻重来边由一段常值控制积分出来的轨迹构成代价是积分出来的量碰撞检查要沿轨迹做。所以拿到这份源码第一件该看的不是主循环而是它怎么定义状态和控制、怎么积分。定义错了后面调参调得再细也是白搭。2.1 状态向量与控制量的约定最常见的入门模型是平面双积分器状态x [px; py; vx; vy]控制u [ax; ay]约束是加速度模长不超过a_max、速度模长不超过v_max。为什么不用更简单的单积分器状态只有位置控制直接就是速度因为单积分器本质还是几何规划只是把「速度」换了个名字叫控制量它做不出「加速需要时间、刹车需要距离」这个效果规划出来的路径交给真实底盘照样冲出弯道。这里有个血泪经验单位一定要在参数文件里写死别散落在各个函数里。见过太多人把a_max按 cm/s² 填进按 m/s² 算的积分器结果树要么原地不动要么一步飞出地图。另外状态维度和控制维度不要硬编码成 4 和 2从x0推出来后面想换成三阶模型带加加速度或者差速底盘时改一处就能跑。function p params() % 所有维度、单位、饱和值集中在一处禁止在函数内部写魔数 p.nx 4; % 状态维度: [px py vx vy]单位 m, m/s p.nu 2; % 控制维度: [ax ay]单位 m/s^2 p.a_max 2.0; % 加速度上限 p.v_max 3.0; % 速度上限 p.T_edge 1.0; % 单条边的积分时长, s p.n_sub 10; % 边内部子步数, dt T_edge / n_sub p.w_ctrl 0.1; % 代价里控制能量的权重 p.n_ctrl 15; % 每次扩展采样多少组候选控制 p.tol_reach 0.15; % 重连时判定扩到了的容差, m p.gamma 3.0; % 邻域半径系数 p.r_max 1.5; % 邻域半径上限, m end这份参数表里n_ctrl和tol_reach是最容易被忽略的两个。前者决定单次扩展要积分多少条候选边直接挂在运行时间上后者决定重连时的「命中判定」有多宽松设太小会导致几乎所有重连尝试都失败rewire 形同虚设。2.2 用积分器把控制量变成可碰撞检查的轨迹propagate是整个算法的地基。它接收一个起始状态和一组常值控制向前积分固定时长返回轨迹点矩阵、是否无碰、以及这条边的代价。实现上有三个点必须做对。第一饱和要作用在积分之前不是积分之后截断。先限幅再积分得到的是物理上真能执行的状态序列先积分再截断得到的是自相矛盾的轨迹后面重放仿真时约束违反度会大得离谱。第二碰撞检查必须下沉到子步内部而且要查线段而不是只查端点。高速状态下dt 0.1 s、速度 3 m/s两个积分点之间就隔了 30 cm薄墙直接从中间穿过去规划器会给你一条「看起来能通」的鬼路。第三代价要在积分过程中累加而不是事后用首尾点估算。用固定T_edge的话时间代价退化成常数RRT* 的「最优」就失去了意义必须掺进控制能量项。function [X, ok, c] propagate(x0, u, p, map) % 从 x0 出发用常值控制 u 前向积分 p.T_edge 秒 % X: nx×(k1) 轨迹点; ok: 是否全程无碰; c: 边代价 dt p.T_edge / p.n_sub; X zeros(p.nx, p.n_sub 1); X(:,1) x0(:); ok true; c 0; for k 1:p.n_sub xk X(:,k); a u(:); na norm(a); if na p.a_max % 先限幅: 保证这一段是真能执行的 a a * (p.a_max / na); end v xk(3:4) a * dt; % 半隐式欧拉: 先速度 nv norm(v); if nv p.v_max v v * (p.v_max / nv); end xn xk; xn(1:2) xk(1:2) v * dt; % 再用新速度推位置 xn(3:4) v; if ~segmentFree(xk(1:2), xn(1:2), map) % 子步内部查线段, 不是查点 ok false; X X(:, 1:k); % 截断到碰撞前一刻 return; end X(:,k1) xn; c c dt * (1 p.w_ctrl * (a * a)); end end function free segmentFree(p0, p1, map) % 沿线段等距采样, 采样间隔不超过半格, 避免高速穿墙 d norm(p1 - p0); m max(2, ceil(d / (0.5 * map.res))); free true; for t linspace(0, 1, m) q p0 t * (p1 - p0); ij world2grid(q, map); if map.occ(ij(1), ij(2)) free false; return; end end endsemi-implicit Euler在这里够用换成ode45反而会拖慢一个数量级——单次积分只有 10 个子步精度瓶颈不在这。如果后面要接真实车辆的动力学把propagate内部换成RK4或者查表模型即可接口签名一行不用改这也是把积分单独抽成函数的收益所在。2.3 距离度量与边代价为什么不能直接用欧氏距离最近邻搜索用哪个度量直接决定树长成什么形状。直接拿四维状态做欧氏距离是个典型错误位置分量单位是米速度分量单位是米每秒量纲不同、数量级也可能差一个量级结果就是速度项主导了整个距离树会朝着「速度相近」而不是「位置相近」的方向乱长。常见做法是加权位置项系数取 1速度项系数取1 / v_max^2把速度归一化到和位置同一量级。这个权重不是拍脑袋它近似了「以最大速度飞过去需要多久」的时间度量在双积分器模型上和真实代价的相关性明显更好。边代价则要满足可加性否则 RRT* 的渐近最优性无从谈起。上面propagate里用的是∫(1 w·|u|²)dt常数 1 那项对应时间二次项对应控制能量。如果想要更纯粹的「最快到达」就要让每条边的时长也参与采样——固定T_edge是没法优化总时间的这是很多人在自己改代码时踩的坑改了代价函数却忘了边时长是常数最后发现跑了半天总时间一点没降。提示如果你的场景对时间不敏感、只关心轨迹平滑把w_ctrl调到 1 以上树会明显偏向小加速度的直边反过来追求极限穿越就把w_ctrl压到 0.01 以下但成功率会掉。3. 主循环拆解采样、扩展、选父重连的 MATLAB 骨架把动力学那一层封好之后主循环本身的结构其实和几何 RRT* 高度相似差别集中在两处steer被替换成了「采样控制 前向积分 挑最优」以及 rewire 之后要级联更新整棵子树的代价。这两处也正是运行时间和出错率的大头值得单独拆开看。3.1 采样与最近邻别让搜索变成瓶颈采样阶段做两件事决定这次往哪儿长以及找到树上离它最近的节点。目标偏置goal bias一般取 5% 到 10%用rand p.goal_bias直接落到目标状态上其余情况在地图范围内均匀采样。最近邻在节点数几百的时候怎么写都行上千之后就要挑实现了。源码里的做法是矩阵化直接把T.X(:,1:T.n)整个拿出来和采样点做加权差sum掉第一维取最小值。这个写法在 N3000 时单次大概几十微秒比循环快两个数量级但它每轮都要全量重算一遍。节点数继续往上走标准做法是换成knnsearch配合createns建增量索引或者干脆自己维护一个网格哈希。function idx nearestNode(T, xrand, p) % 加权欧氏距离, 速度项按 v_max 归一化, 避免量纲主导 W diag([1, 1, 1/p.v_max^2, 1/p.v_max^2]); D T.X(:, 1:T.n) - xrand(:); d sum(D .* (W * D), 1); % 1×N 的马氏距离平方 [~, idx] min(d); end注意T.X是预分配到N_max列的只用前T.n列有效这样矩阵不会每轮重新分配内存。这一个细节能让 5000 节点规模的总耗时降三分之一以上属于零成本的改动。3.2 控制采样与扩展steer 在动力学下长什么样几何 RRT 的steer是「从最近节点朝采样点直着走一步步长固定」。动力学下这条直线没法直接走——你得给出一组控制让它积分出来的终点恰好落在那个方向。严格的解法是解一个两点边值问题代价太高实用的近似是采样 K 组候选控制每组积分一次取终点离采样点最近且无碰的那条。候选控制的分布有讲究。均匀采样在|a| a_max的圆盘里撒点好处是覆盖均匀坏处是大量候选都指向奇怪的方向。高斯采样再截断到a_max内会产生更多接近零的小加速度长出来的边更平直成功率和路径质量都更好。这也是「玄学」的一部分我一般会两种都试一遍再看统计结果。function [xnew, Xedge, cedge] extend(T, iNear, xrand, p, map) % kinodynamic 版 steer: 采 K 组控制, 取终点离 xrand 最近且无碰的那条边 best_d inf; xnew []; Xedge []; cedge inf; for j 1:p.n_ctrl u p.a_max * randn(p.nu, 1); % 高斯采样 nu norm(u); if nu p.a_max u u * (p.a_max / nu); end [X, ok, c] propagate(T.X(:,iNear), u, p, map); if ~ok || isempty(X), continue; end % 撞了就换下一组 d norm(X(1:2,end) - xrand(1:2)); % 打分只看位置 if d best_d best_d d; xnew X(:,end); Xedge X; cedge c; end end end打分只用位置分量是一个折中短边1 秒量级上速度分量对「朝哪儿长」的贡献很小全部纳入反而让最近的那条边经常被一个速度差很大的节点抢走。如果你把T_edge加到 3 秒以上这个近似就开始失真得改回全状态加权距离。3.3 选父与重连RRT* 真正花钱的地方新节点生成后要做两轮遍历。第一轮在邻域内挑一个使cost(parent) cost(edge)最小的父节点第二轮尝试把邻域内其他节点的父节点改成新节点前提是改完之后总代价确实下降。邻域半径用经典的衰减公式r min(r_max, γ·(log n / n)^(1/nx))维度nx出现在指数上四维状态下衰减得比二维慢很多这也是 kinodynamic 版本的邻域通常要开到一两米的原因。真正容易翻车的是重连之后的代价更新。改了某个节点的父节点它整棵子树的代价全部失效必须级联往下刷。用递归写最直观但 MATLAB 的递归深度限制加上函数调用开销几千节点的树上会直接卡死正确做法是维护children列表做 BFS。还有一个隐蔽的环路问题如果要把节点i的父改成新节点x_new而i恰好是x_new的祖先改完父指针就成环了后面遍历子树会无限循环。isAncestor这个检查在很多简版实现里被省掉了然后在节点数上来之后表现为「程序卡死但 CPU 不满」非常难查。function T rewire(T, iNew, p, map) % 尝试让 iNew 成为邻域内节点的父节点, 代价下降才改 r min(p.r_max, p.gamma * (log(T.n) / T.n)^(1/p.nx)); d vecnorm(T.X(:,1:T.n) - T.X(:,iNew), 2, 1); near find(d r); for i near if i iNew || T.parent(i) 0, continue; end if isAncestor(T, i, iNew), continue; end % 防止成环 [xchk, ~, c] extendFrom(T.X(:,iNew), T.X(:,i), p, map); if isempty(xchk) || norm(xchk - T.X(:,i)) p.tol_reach continue; % 没扩到, 这条边不成立 end newcost T.cost(iNew) c; if newcost T.cost(i) - 1e-9 % 留 1e-9 防止浮点抖动 T.parent(i) iNew; T.edgecost(i) c; T.cost(i) newcost; T updateSubtree(T, i); % BFS 级联刷新 end end end function T updateSubtree(T, root) queue root; while ~isempty(queue) u queue(1); queue(1) []; for v T.children{u} T.cost(v) T.cost(u) T.edgecost(v); queue(end1) v; end end end1e-9那个容差不是多余的。代价是浮点累加出来的两条理论上等长的边可能差 1e-16不加容差会触发大量无意义的「改父 刷新子树」白白拖慢速度还会让结果在多次运行之间出现微小抖动。这种细节只有真正跑过大批量试验才会注意到。4. 参数整定步长、邻域半径与代价函数的耦合调法参数表里那十几个数不是独立的改一个往往会连带影响另外两三个。很多人调参的方式是「看到不对劲就随手改一个」结果改到最后连哪次改动起了作用都说不清。靠谱的做法是先搞清楚耦合关系再用批量试验把「手感」变成曲线。4.1 步长、边时长、邻域半径的耦合关系T_edge和n_sub一起决定dt。dt太大积分精度下降、碰撞检查漏检dt太小单条边的计算量线性上升。经验值是让单条边内的位移不超过地图里最窄通道宽度的一半这样线段采样才靠得住。T_edge本身决定树能「长多快」。设得太短每条边只有十几厘米树要长很久才够到目标而且控制能量的累加次数变多代价函数被稀释设得太长单条边的碰撞概率显著上升大量候选边直接作废扩展效率反而下降。邻域半径r_max要和T_edge匹配。如果r_max远小于单边长度绝大多数邻域节点都够不着rewire 永远不触发如果r_max远大于单边长度重连时extendFrom的tol_reach又判不中同样是白算。参数典型值调大之后调小之后出问题的表现T_edge0.8 ~ 1.5 s树长得快碰撞率上升树密集扩展慢太大时扩展频繁全废n_sub8 ~ 15精度高耗时长快可能穿墙太小出现鬼路n_ctrl10 ~ 20边质量好耗时涨快方向随机太小时树长歪r_max1.0 ~ 2.0 m重连机会多耗时涨退化成 RRT与T_edge不匹配时重连全失效goal_bias0.05 ~ 0.10收敛快易陷局部探索充分慢太大时树被目标吸死w_ctrl0.01 ~ 1.0轨迹平滑激进太大时几乎不长边这张表不是标准答案是起点。真正的取值跟地图尺度、障碍密度、底盘性能强相关靠的是下面这套批量试验方法。4.2 用批量试验跑出成功率曲线单次跑通毫无意义。RRT* 是随机算法同样的参数换一个种子可能就是通和不通的差别。判断一组参数好不好至少要看 30 到 50 个种子下的成功率、平均代价、平均节点数和平均耗时四个指标。rng(0); % 外层固定, 保证整批试验可复现 seeds 1:50; stats zeros(numel(seeds), 4); for i 1:numel(seeds) rng(seeds(i)); % 每个种子单独固定, 便于复现某个失败案例 out plan_once(p, map, x0, xg); stats(i,:) [out.success, out.cost, out.n_nodes, out.t_exec]; end succ stats(:,1) 1; fprintf(成功率 %.0f%% | 平均代价 %.3f | 平均节点 %.0f | 平均耗时 %.2f s\n, ... 100*mean(succ), mean(stats(succ,2)), mean(stats(succ,3)), mean(stats(succ,4))); fprintf(代价 10%% 分位 %.3f | 90%% 分位 %.3f\n, ... prctile(stats(succ,2), 10), prctile(stats(succ,2), 90));成功率告诉你这组参数「能不能用」代价分位数告诉你「稳不稳」。只看均值会被少数极好的样本骗过去——90 分位比均值大两三倍是常态说明这个参数在地图某些区域仍然吃力。我一般要求 90 分位不超过均值的 1.5 倍才算收敛得比较均匀。4.3 向量化、预分配与绘图降频MATLAB 跑采样规划性能问题九成出在三个地方动态扩容、重复计算、每轮重绘。动态扩容指用tree(end1) node这种写法往结构体数组里塞节点。每次扩容 MATLAB 都可能重新分配整块内存并拷贝节点数上到两千之后耗时曲线明显上翘。改成预分配并行数组——X是nx × N_max的矩阵、parent和cost是N_max × 1的向量、children是N_max × 1的 cell——扩容成本直接归零。重复计算主要是最近邻。前面提过每轮全量pdist2在节点多的时候很伤改成矩阵乘法的加权距离已经快很多再往上就要做增量索引。绘图降频最容易被忽视。循环里放一个裸的plot加drawnow能把整个算法拖慢五到十倍因为渲染开销远大于规划本身。改成mod(iter, 20) 0时才刷新或者把可视化整个抽到回调里只在用户按暂停时画。% 主循环里的绘图节流: 只在每 20 次迭代或找到解时刷新 if mod(iter, 20) 0 || found drawTree(T, map); % 内部用 set(h,XData,...) 更新, 不重建图元 drawnow limitrate; % limitrate 比裸 drawnow 温和得多 enddrawnow limitrate这个用法值得记一下它会把刷新频率限制在 20 帧每秒左右既能看到树的生长过程又不会把主循环卡住。裸drawnow是同步刷新每帧都等渲染完成在 3000 节点的树上每轮能吃掉几十毫秒。5. 常见问题排查五条从跑不动到跑得慢的血泪记录这份代码跑起来之后出问题的形态其实很有限无非是树不长、目标判不到、优化越优越差、越跑越慢、换模型就崩这几类。每类的现象和根因都比较固定按现象对号入座比盲改参数快得多。5.1 现象迭代到两千次树上还是个位数节点原因碰撞检查太粗。只在积分端点上做栅格查询或者n_sub设得太小导致几乎所有候选边都被判为碰撞。另一种可能是a_max的单位填错了积分出来的位移远超地图尺度一步就飞出边界。解决把检查下沉到propagate内部每个子步并且查线段不查点见 2.2 的segmentFree。先在起点附近打一条 1 秒的直线边打印轨迹终点坐标如果位移量和0.5 * a_max * T_edge^2差一个数量级就是单位问题。5.2 现象树已经铺到目标附近了却始终判失败原因目标判定只在「节点」上做要求节点位置落在目标球内。但实际上轨迹段是可能穿过目标球的端点却在外面——尤其是高速通过的时候。另外目标球半径设成 0.05 m 这种量级采样密度根本达不到。解决把目标判定改成「轨迹段与目标球相交」在propagate返回前对每个子步线段做一次点到线段距离计算小于goal_radius就直接判定抵达并终止。半径按底盘尺寸取一般不低于 0.2 m。5.3 现象打开 rewire 之后解反而变差了偶尔还卡死原因改父时没做祖先检查父指针成环updateSubtree的 BFS 在环里无限循环。另一种是改完父没有级联刷新子树代价导致T.cost和实际路径对不上选父时拿到的是脏数据。解决改父前跑一次isAncestor(i, iNew)是祖先就跳过改父后必须调updateSubtree做 BFS 级联刷新。验证方法很简单规划结束后从终点沿parent往回走到起点统计步数如果超过T.n中途一定成环了。5.4 现象节点数过三千后每轮耗时线性上涨原因三个常见来源一是每轮全量重算距离矩阵二是用结构体数组动态扩容三是循环里裸drawnow。三者叠加能把 5000 节点的规划从 2 秒拖到 40 秒。解决按 4.3 的三条依次改——预分配并行数组、距离矩阵改矩阵乘法或增量索引、绘图节流到每 20 轮一次。改完之后用profile on跑一遍正常情况耗时应该集中在propagate积分碰撞检查上占比七成以上如果nearestNode占比超过三成说明最近邻还有优化空间。5.5 现象换成功差速底盘或三阶模型直接报维度不匹配原因源码里把nx 4、nu 2写死在各个函数里权重矩阵diag([1 1 1/v_max^2 1/v_max^2])也是硬编码的四维。改成三阶加加速度之后状态是六维一进nearestNode就报Matrix dimensions must agree。解决所有维度从p.nx、p.nu推权重矩阵按「位置项 1、速度项 1/v_max²、加速度项 1/a_max²」的规则动态拼长度不够就补 1。改完之后先用单条边自测从零状态给一个满加速度积分 1 秒末速度应该正好等于a_max在未触及v_max的前提下对不上就说明积分或维度还有问题。6. 验证与进阶怎么确认这棵树不是碰巧通的一次跑通不代表算法对可能只是种子运气好。我习惯做三层验证缺一层都不敢拿结果去跟别的方法比。第一层是单边级随机抽 100 条边把控制序列和状态序列拿出来检查|a| ≤ a_max、|v| ≤ v_max是否处处成立以及每个子步线段是否真的无碰。第二层是重放级把解路径的控制序列丢回独立的积分器不要复用propagate避免同一个 bug 自证清白重新积一遍比对终点偏差和约束违反度。第三层才是统计级跑 50 个种子看成功率和代价分布。% 独立重放: 不复用 propagate, 用最朴素的积分器验一遍 viol_a 0; viol_v 0; x x0; for k 1:size(U, 2) viol_a max(viol_a, norm(U(:,k)) - p.a_max); x x [x(3:4); U(:,k)] * p.T_edge; % 一阶欧拉, 故意和主实现不同 viol_v max(viol_v, norm(x(3:4)) - p.v_max); end fprintf(控制超限 %.3e | 速度超限 %.3e | 终点偏差 %.3f m\n, ... viol_a, viol_v, norm(x(1:2) - xg(1:2)));超限项应该是 0 或者 1e-15 量级的浮点残差终点偏差在一阶欧拉和半隐式欧拉之间会有差别但应该在厘米级如果到分米级说明T_edge相对动力学太快积分精度不够了。进阶方向有三个值得先试。一是可变边时长把T_edge也纳入采样比如在[0.5, 2.0]之间均匀取这样时间代价才真正可优化代价曲线的下降会明显得多。二是informed 采样找到第一个解之后把采样限制在「当前代价上界对应的椭圆子集」里收敛速度通常能快两三倍改动量只有采样函数那几行。三是换度量把加权欧氏距离换成「从该节点到采样点的最优控制代价」的近似比如用线性二次调节器的 cost-to-go 闭式解最近邻的质量会上一个台阶代价是每次查询都要解一个小矩阵方程。最后说个习惯。这份代码我改过很多遍最早那版为了图快把碰撞检查只放在积分端点上跑出来的路径在可视化里漂亮得不行一放到实车上第一个弯就蹭墙。从那以后我每次改完propagate都强制走一遍「单边自测 独立重放」这两步哪怕只改了一个常数。规划器最怕的不是收敛慢是它信誓旦旦地给你一条走不通的路而你还以为是自己控制器调得不好。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/31 3:21:35 · 阅读 1.2k MATLAB kinodynamic RRT*轨迹规划与调参
简介围绕 Kinodynamic RRT* 的 MATLAB 实现面向运动规划学习者与机器人、控制方向研究者聚焦线性微分约束系统的最优运动规划问题可用于复现论文思路、理解采样规划与动力学可行性检验。压缩包共 17 个文件约 14KB以 13 个 .m 脚本为主体辅以 2 个 .mat 场景数据及 1 个 md 说明、1 个 txt 许可文件脚本覆盖 RRT* 主流程、二重积分与四旋翼示例、状态与输入可行性判断以及障碍场和轨迹可视化数据文件提供障碍物与路点信息。已有 1302 人学习下载。读者可据此梳理算法从采样、扩展、代价优化到动力学约束验证的实现链路借助现成脚本开展仿真实验、对照论文公式检查细节并在此基础上替换模型或场景做二次开发。1. 底层控制器一执行规划器给的折线路径就把车带出了弯道kinodynamic RRT* 要解决的就是这件事。MATLAB 里这份实现把动力学方程和执行器饱和塞进采样过程树上每条边本身就是一条能跑出来的轨迹而不是一条等着被跟踪的直线。它把状态空间定义、控制采样、前向积分、碰撞检查、选父与重连串成了一条完整链路适合正在做移动机器人或无人车轨迹规划、又不想一上来就啃 C 框架的读者拿它把算法骨架跑通、把参数手感养出来。2. 动力学约束怎么落地状态向量、饱和与实际积分几何 RRT* 里两个节点之间的边是一条直线代价是欧氏距离碰撞检查只需要扫一条线段。kinodynamic 版本把这三件事全部推翻重来边由一段常值控制积分出来的轨迹构成代价是积分出来的量碰撞检查要沿轨迹做。所以拿到这份源码第一件该看的不是主循环而是它怎么定义状态和控制、怎么积分。定义错了后面调参调得再细也是白搭。2.1 状态向量与控制量的约定最常见的入门模型是平面双积分器状态x [px; py; vx; vy]控制u [ax; ay]约束是加速度模长不超过a_max、速度模长不超过v_max。为什么不用更简单的单积分器状态只有位置控制直接就是速度因为单积分器本质还是几何规划只是把「速度」换了个名字叫控制量它做不出「加速需要时间、刹车需要距离」这个效果规划出来的路径交给真实底盘照样冲出弯道。这里有个血泪经验单位一定要在参数文件里写死别散落在各个函数里。见过太多人把a_max按 cm/s² 填进按 m/s² 算的积分器结果树要么原地不动要么一步飞出地图。另外状态维度和控制维度不要硬编码成 4 和 2从x0推出来后面想换成三阶模型带加加速度或者差速底盘时改一处就能跑。function p params() % 所有维度、单位、饱和值集中在一处禁止在函数内部写魔数 p.nx 4; % 状态维度: [px py vx vy]单位 m, m/s p.nu 2; % 控制维度: [ax ay]单位 m/s^2 p.a_max 2.0; % 加速度上限 p.v_max 3.0; % 速度上限 p.T_edge 1.0; % 单条边的积分时长, s p.n_sub 10; % 边内部子步数, dt T_edge / n_sub p.w_ctrl 0.1; % 代价里控制能量的权重 p.n_ctrl 15; % 每次扩展采样多少组候选控制 p.tol_reach 0.15; % 重连时判定扩到了的容差, m p.gamma 3.0; % 邻域半径系数 p.r_max 1.5; % 邻域半径上限, m end这份参数表里n_ctrl和tol_reach是最容易被忽略的两个。前者决定单次扩展要积分多少条候选边直接挂在运行时间上后者决定重连时的「命中判定」有多宽松设太小会导致几乎所有重连尝试都失败rewire 形同虚设。2.2 用积分器把控制量变成可碰撞检查的轨迹propagate是整个算法的地基。它接收一个起始状态和一组常值控制向前积分固定时长返回轨迹点矩阵、是否无碰、以及这条边的代价。实现上有三个点必须做对。第一饱和要作用在积分之前不是积分之后截断。先限幅再积分得到的是物理上真能执行的状态序列先积分再截断得到的是自相矛盾的轨迹后面重放仿真时约束违反度会大得离谱。第二碰撞检查必须下沉到子步内部而且要查线段而不是只查端点。高速状态下dt 0.1 s、速度 3 m/s两个积分点之间就隔了 30 cm薄墙直接从中间穿过去规划器会给你一条「看起来能通」的鬼路。第三代价要在积分过程中累加而不是事后用首尾点估算。用固定T_edge的话时间代价退化成常数RRT* 的「最优」就失去了意义必须掺进控制能量项。function [X, ok, c] propagate(x0, u, p, map) % 从 x0 出发用常值控制 u 前向积分 p.T_edge 秒 % X: nx×(k1) 轨迹点; ok: 是否全程无碰; c: 边代价 dt p.T_edge / p.n_sub; X zeros(p.nx, p.n_sub 1); X(:,1) x0(:); ok true; c 0; for k 1:p.n_sub xk X(:,k); a u(:); na norm(a); if na p.a_max % 先限幅: 保证这一段是真能执行的 a a * (p.a_max / na); end v xk(3:4) a * dt; % 半隐式欧拉: 先速度 nv norm(v); if nv p.v_max v v * (p.v_max / nv); end xn xk; xn(1:2) xk(1:2) v * dt; % 再用新速度推位置 xn(3:4) v; if ~segmentFree(xk(1:2), xn(1:2), map) % 子步内部查线段, 不是查点 ok false; X X(:, 1:k); % 截断到碰撞前一刻 return; end X(:,k1) xn; c c dt * (1 p.w_ctrl * (a * a)); end end function free segmentFree(p0, p1, map) % 沿线段等距采样, 采样间隔不超过半格, 避免高速穿墙 d norm(p1 - p0); m max(2, ceil(d / (0.5 * map.res))); free true; for t linspace(0, 1, m) q p0 t * (p1 - p0); ij world2grid(q, map); if map.occ(ij(1), ij(2)) free false; return; end end endsemi-implicit Euler在这里够用换成ode45反而会拖慢一个数量级——单次积分只有 10 个子步精度瓶颈不在这。如果后面要接真实车辆的动力学把propagate内部换成RK4或者查表模型即可接口签名一行不用改这也是把积分单独抽成函数的收益所在。2.3 距离度量与边代价为什么不能直接用欧氏距离最近邻搜索用哪个度量直接决定树长成什么形状。直接拿四维状态做欧氏距离是个典型错误位置分量单位是米速度分量单位是米每秒量纲不同、数量级也可能差一个量级结果就是速度项主导了整个距离树会朝着「速度相近」而不是「位置相近」的方向乱长。常见做法是加权位置项系数取 1速度项系数取1 / v_max^2把速度归一化到和位置同一量级。这个权重不是拍脑袋它近似了「以最大速度飞过去需要多久」的时间度量在双积分器模型上和真实代价的相关性明显更好。边代价则要满足可加性否则 RRT* 的渐近最优性无从谈起。上面propagate里用的是∫(1 w·|u|²)dt常数 1 那项对应时间二次项对应控制能量。如果想要更纯粹的「最快到达」就要让每条边的时长也参与采样——固定T_edge是没法优化总时间的这是很多人在自己改代码时踩的坑改了代价函数却忘了边时长是常数最后发现跑了半天总时间一点没降。提示如果你的场景对时间不敏感、只关心轨迹平滑把w_ctrl调到 1 以上树会明显偏向小加速度的直边反过来追求极限穿越就把w_ctrl压到 0.01 以下但成功率会掉。3. 主循环拆解采样、扩展、选父重连的 MATLAB 骨架把动力学那一层封好之后主循环本身的结构其实和几何 RRT* 高度相似差别集中在两处steer被替换成了「采样控制 前向积分 挑最优」以及 rewire 之后要级联更新整棵子树的代价。这两处也正是运行时间和出错率的大头值得单独拆开看。3.1 采样与最近邻别让搜索变成瓶颈采样阶段做两件事决定这次往哪儿长以及找到树上离它最近的节点。目标偏置goal bias一般取 5% 到 10%用rand p.goal_bias直接落到目标状态上其余情况在地图范围内均匀采样。最近邻在节点数几百的时候怎么写都行上千之后就要挑实现了。源码里的做法是矩阵化直接把T.X(:,1:T.n)整个拿出来和采样点做加权差sum掉第一维取最小值。这个写法在 N3000 时单次大概几十微秒比循环快两个数量级但它每轮都要全量重算一遍。节点数继续往上走标准做法是换成knnsearch配合createns建增量索引或者干脆自己维护一个网格哈希。function idx nearestNode(T, xrand, p) % 加权欧氏距离, 速度项按 v_max 归一化, 避免量纲主导 W diag([1, 1, 1/p.v_max^2, 1/p.v_max^2]); D T.X(:, 1:T.n) - xrand(:); d sum(D .* (W * D), 1); % 1×N 的马氏距离平方 [~, idx] min(d); end注意T.X是预分配到N_max列的只用前T.n列有效这样矩阵不会每轮重新分配内存。这一个细节能让 5000 节点规模的总耗时降三分之一以上属于零成本的改动。3.2 控制采样与扩展steer 在动力学下长什么样几何 RRT 的steer是「从最近节点朝采样点直着走一步步长固定」。动力学下这条直线没法直接走——你得给出一组控制让它积分出来的终点恰好落在那个方向。严格的解法是解一个两点边值问题代价太高实用的近似是采样 K 组候选控制每组积分一次取终点离采样点最近且无碰的那条。候选控制的分布有讲究。均匀采样在|a| a_max的圆盘里撒点好处是覆盖均匀坏处是大量候选都指向奇怪的方向。高斯采样再截断到a_max内会产生更多接近零的小加速度长出来的边更平直成功率和路径质量都更好。这也是「玄学」的一部分我一般会两种都试一遍再看统计结果。function [xnew, Xedge, cedge] extend(T, iNear, xrand, p, map) % kinodynamic 版 steer: 采 K 组控制, 取终点离 xrand 最近且无碰的那条边 best_d inf; xnew []; Xedge []; cedge inf; for j 1:p.n_ctrl u p.a_max * randn(p.nu, 1); % 高斯采样 nu norm(u); if nu p.a_max u u * (p.a_max / nu); end [X, ok, c] propagate(T.X(:,iNear), u, p, map); if ~ok || isempty(X), continue; end % 撞了就换下一组 d norm(X(1:2,end) - xrand(1:2)); % 打分只看位置 if d best_d best_d d; xnew X(:,end); Xedge X; cedge c; end end end打分只用位置分量是一个折中短边1 秒量级上速度分量对「朝哪儿长」的贡献很小全部纳入反而让最近的那条边经常被一个速度差很大的节点抢走。如果你把T_edge加到 3 秒以上这个近似就开始失真得改回全状态加权距离。3.3 选父与重连RRT* 真正花钱的地方新节点生成后要做两轮遍历。第一轮在邻域内挑一个使cost(parent) cost(edge)最小的父节点第二轮尝试把邻域内其他节点的父节点改成新节点前提是改完之后总代价确实下降。邻域半径用经典的衰减公式r min(r_max, γ·(log n / n)^(1/nx))维度nx出现在指数上四维状态下衰减得比二维慢很多这也是 kinodynamic 版本的邻域通常要开到一两米的原因。真正容易翻车的是重连之后的代价更新。改了某个节点的父节点它整棵子树的代价全部失效必须级联往下刷。用递归写最直观但 MATLAB 的递归深度限制加上函数调用开销几千节点的树上会直接卡死正确做法是维护children列表做 BFS。还有一个隐蔽的环路问题如果要把节点i的父改成新节点x_new而i恰好是x_new的祖先改完父指针就成环了后面遍历子树会无限循环。isAncestor这个检查在很多简版实现里被省掉了然后在节点数上来之后表现为「程序卡死但 CPU 不满」非常难查。function T rewire(T, iNew, p, map) % 尝试让 iNew 成为邻域内节点的父节点, 代价下降才改 r min(p.r_max, p.gamma * (log(T.n) / T.n)^(1/p.nx)); d vecnorm(T.X(:,1:T.n) - T.X(:,iNew), 2, 1); near find(d r); for i near if i iNew || T.parent(i) 0, continue; end if isAncestor(T, i, iNew), continue; end % 防止成环 [xchk, ~, c] extendFrom(T.X(:,iNew), T.X(:,i), p, map); if isempty(xchk) || norm(xchk - T.X(:,i)) p.tol_reach continue; % 没扩到, 这条边不成立 end newcost T.cost(iNew) c; if newcost T.cost(i) - 1e-9 % 留 1e-9 防止浮点抖动 T.parent(i) iNew; T.edgecost(i) c; T.cost(i) newcost; T updateSubtree(T, i); % BFS 级联刷新 end end end function T updateSubtree(T, root) queue root; while ~isempty(queue) u queue(1); queue(1) []; for v T.children{u} T.cost(v) T.cost(u) T.edgecost(v); queue(end1) v; end end end1e-9那个容差不是多余的。代价是浮点累加出来的两条理论上等长的边可能差 1e-16不加容差会触发大量无意义的「改父 刷新子树」白白拖慢速度还会让结果在多次运行之间出现微小抖动。这种细节只有真正跑过大批量试验才会注意到。4. 参数整定步长、邻域半径与代价函数的耦合调法参数表里那十几个数不是独立的改一个往往会连带影响另外两三个。很多人调参的方式是「看到不对劲就随手改一个」结果改到最后连哪次改动起了作用都说不清。靠谱的做法是先搞清楚耦合关系再用批量试验把「手感」变成曲线。4.1 步长、边时长、邻域半径的耦合关系T_edge和n_sub一起决定dt。dt太大积分精度下降、碰撞检查漏检dt太小单条边的计算量线性上升。经验值是让单条边内的位移不超过地图里最窄通道宽度的一半这样线段采样才靠得住。T_edge本身决定树能「长多快」。设得太短每条边只有十几厘米树要长很久才够到目标而且控制能量的累加次数变多代价函数被稀释设得太长单条边的碰撞概率显著上升大量候选边直接作废扩展效率反而下降。邻域半径r_max要和T_edge匹配。如果r_max远小于单边长度绝大多数邻域节点都够不着rewire 永远不触发如果r_max远大于单边长度重连时extendFrom的tol_reach又判不中同样是白算。参数典型值调大之后调小之后出问题的表现T_edge0.8 ~ 1.5 s树长得快碰撞率上升树密集扩展慢太大时扩展频繁全废n_sub8 ~ 15精度高耗时长快可能穿墙太小出现鬼路n_ctrl10 ~ 20边质量好耗时涨快方向随机太小时树长歪r_max1.0 ~ 2.0 m重连机会多耗时涨退化成 RRT与T_edge不匹配时重连全失效goal_bias0.05 ~ 0.10收敛快易陷局部探索充分慢太大时树被目标吸死w_ctrl0.01 ~ 1.0轨迹平滑激进太大时几乎不长边这张表不是标准答案是起点。真正的取值跟地图尺度、障碍密度、底盘性能强相关靠的是下面这套批量试验方法。4.2 用批量试验跑出成功率曲线单次跑通毫无意义。RRT* 是随机算法同样的参数换一个种子可能就是通和不通的差别。判断一组参数好不好至少要看 30 到 50 个种子下的成功率、平均代价、平均节点数和平均耗时四个指标。rng(0); % 外层固定, 保证整批试验可复现 seeds 1:50; stats zeros(numel(seeds), 4); for i 1:numel(seeds) rng(seeds(i)); % 每个种子单独固定, 便于复现某个失败案例 out plan_once(p, map, x0, xg); stats(i,:) [out.success, out.cost, out.n_nodes, out.t_exec]; end succ stats(:,1) 1; fprintf(成功率 %.0f%% | 平均代价 %.3f | 平均节点 %.0f | 平均耗时 %.2f s\n, ... 100*mean(succ), mean(stats(succ,2)), mean(stats(succ,3)), mean(stats(succ,4))); fprintf(代价 10%% 分位 %.3f | 90%% 分位 %.3f\n, ... prctile(stats(succ,2), 10), prctile(stats(succ,2), 90));成功率告诉你这组参数「能不能用」代价分位数告诉你「稳不稳」。只看均值会被少数极好的样本骗过去——90 分位比均值大两三倍是常态说明这个参数在地图某些区域仍然吃力。我一般要求 90 分位不超过均值的 1.5 倍才算收敛得比较均匀。4.3 向量化、预分配与绘图降频MATLAB 跑采样规划性能问题九成出在三个地方动态扩容、重复计算、每轮重绘。动态扩容指用tree(end1) node这种写法往结构体数组里塞节点。每次扩容 MATLAB 都可能重新分配整块内存并拷贝节点数上到两千之后耗时曲线明显上翘。改成预分配并行数组——X是nx × N_max的矩阵、parent和cost是N_max × 1的向量、children是N_max × 1的 cell——扩容成本直接归零。重复计算主要是最近邻。前面提过每轮全量pdist2在节点多的时候很伤改成矩阵乘法的加权距离已经快很多再往上就要做增量索引。绘图降频最容易被忽视。循环里放一个裸的plot加drawnow能把整个算法拖慢五到十倍因为渲染开销远大于规划本身。改成mod(iter, 20) 0时才刷新或者把可视化整个抽到回调里只在用户按暂停时画。% 主循环里的绘图节流: 只在每 20 次迭代或找到解时刷新 if mod(iter, 20) 0 || found drawTree(T, map); % 内部用 set(h,XData,...) 更新, 不重建图元 drawnow limitrate; % limitrate 比裸 drawnow 温和得多 enddrawnow limitrate这个用法值得记一下它会把刷新频率限制在 20 帧每秒左右既能看到树的生长过程又不会把主循环卡住。裸drawnow是同步刷新每帧都等渲染完成在 3000 节点的树上每轮能吃掉几十毫秒。5. 常见问题排查五条从跑不动到跑得慢的血泪记录这份代码跑起来之后出问题的形态其实很有限无非是树不长、目标判不到、优化越优越差、越跑越慢、换模型就崩这几类。每类的现象和根因都比较固定按现象对号入座比盲改参数快得多。5.1 现象迭代到两千次树上还是个位数节点原因碰撞检查太粗。只在积分端点上做栅格查询或者n_sub设得太小导致几乎所有候选边都被判为碰撞。另一种可能是a_max的单位填错了积分出来的位移远超地图尺度一步就飞出边界。解决把检查下沉到propagate内部每个子步并且查线段不查点见 2.2 的segmentFree。先在起点附近打一条 1 秒的直线边打印轨迹终点坐标如果位移量和0.5 * a_max * T_edge^2差一个数量级就是单位问题。5.2 现象树已经铺到目标附近了却始终判失败原因目标判定只在「节点」上做要求节点位置落在目标球内。但实际上轨迹段是可能穿过目标球的端点却在外面——尤其是高速通过的时候。另外目标球半径设成 0.05 m 这种量级采样密度根本达不到。解决把目标判定改成「轨迹段与目标球相交」在propagate返回前对每个子步线段做一次点到线段距离计算小于goal_radius就直接判定抵达并终止。半径按底盘尺寸取一般不低于 0.2 m。5.3 现象打开 rewire 之后解反而变差了偶尔还卡死原因改父时没做祖先检查父指针成环updateSubtree的 BFS 在环里无限循环。另一种是改完父没有级联刷新子树代价导致T.cost和实际路径对不上选父时拿到的是脏数据。解决改父前跑一次isAncestor(i, iNew)是祖先就跳过改父后必须调updateSubtree做 BFS 级联刷新。验证方法很简单规划结束后从终点沿parent往回走到起点统计步数如果超过T.n中途一定成环了。5.4 现象节点数过三千后每轮耗时线性上涨原因三个常见来源一是每轮全量重算距离矩阵二是用结构体数组动态扩容三是循环里裸drawnow。三者叠加能把 5000 节点的规划从 2 秒拖到 40 秒。解决按 4.3 的三条依次改——预分配并行数组、距离矩阵改矩阵乘法或增量索引、绘图节流到每 20 轮一次。改完之后用profile on跑一遍正常情况耗时应该集中在propagate积分碰撞检查上占比七成以上如果nearestNode占比超过三成说明最近邻还有优化空间。5.5 现象换成功差速底盘或三阶模型直接报维度不匹配原因源码里把nx 4、nu 2写死在各个函数里权重矩阵diag([1 1 1/v_max^2 1/v_max^2])也是硬编码的四维。改成三阶加加速度之后状态是六维一进nearestNode就报Matrix dimensions must agree。解决所有维度从p.nx、p.nu推权重矩阵按「位置项 1、速度项 1/v_max²、加速度项 1/a_max²」的规则动态拼长度不够就补 1。改完之后先用单条边自测从零状态给一个满加速度积分 1 秒末速度应该正好等于a_max在未触及v_max的前提下对不上就说明积分或维度还有问题。6. 验证与进阶怎么确认这棵树不是碰巧通的一次跑通不代表算法对可能只是种子运气好。我习惯做三层验证缺一层都不敢拿结果去跟别的方法比。第一层是单边级随机抽 100 条边把控制序列和状态序列拿出来检查|a| ≤ a_max、|v| ≤ v_max是否处处成立以及每个子步线段是否真的无碰。第二层是重放级把解路径的控制序列丢回独立的积分器不要复用propagate避免同一个 bug 自证清白重新积一遍比对终点偏差和约束违反度。第三层才是统计级跑 50 个种子看成功率和代价分布。% 独立重放: 不复用 propagate, 用最朴素的积分器验一遍 viol_a 0; viol_v 0; x x0; for k 1:size(U, 2) viol_a max(viol_a, norm(U(:,k)) - p.a_max); x x [x(3:4); U(:,k)] * p.T_edge; % 一阶欧拉, 故意和主实现不同 viol_v max(viol_v, norm(x(3:4)) - p.v_max); end fprintf(控制超限 %.3e | 速度超限 %.3e | 终点偏差 %.3f m\n, ... viol_a, viol_v, norm(x(1:2) - xg(1:2)));超限项应该是 0 或者 1e-15 量级的浮点残差终点偏差在一阶欧拉和半隐式欧拉之间会有差别但应该在厘米级如果到分米级说明T_edge相对动力学太快积分精度不够了。进阶方向有三个值得先试。一是可变边时长把T_edge也纳入采样比如在[0.5, 2.0]之间均匀取这样时间代价才真正可优化代价曲线的下降会明显得多。二是informed 采样找到第一个解之后把采样限制在「当前代价上界对应的椭圆子集」里收敛速度通常能快两三倍改动量只有采样函数那几行。三是换度量把加权欧氏距离换成「从该节点到采样点的最优控制代价」的近似比如用线性二次调节器的 cost-to-go 闭式解最近邻的质量会上一个台阶代价是每次查询都要解一个小矩阵方程。最后说个习惯。这份代码我改过很多遍最早那版为了图快把碰撞检查只放在积分端点上跑出来的路径在可视化里漂亮得不行一放到实车上第一个弯就蹭墙。从那以后我每次改完propagate都强制走一遍「单边自测 独立重放」这两步哪怕只改了一个常数。规划器最怕的不是收敛慢是它信誓旦旦地给你一条走不通的路而你还以为是自己控制器调得不好。希望帮到你。本文还有配套的精品资源点击获取