简介这份 MATLAB 代码包面向机器人运动规划学习者与科研人员围绕 Kinodynamic RRT*线性微分约束系统的最优运动规划论文实现适合复现算法、分析动力学约束下的采样扩展与轨迹优化过程。压缩包共 17 个文件、约 14KB以 13 个 .m 函数与脚本为主体另含 2 个 .mat 场景数据和 md、txt 说明文件整体结构轻量便于按模块阅读算法组织并逐文件调试。代码包含 RRT* 主流程、二重积分器、四旋翼与球体示例以及正定性判断、状态与输入可行性检测、障碍物场绘制、采样与航点数据等模块可帮助读者把论文中的代价计算、约束处理与优化步骤落到可运行示例中。已有 1302 人学习下载适合在 MATLAB 中修改参数、替换障碍与航点数据观察不同约束和采样密度对路径质量的影响并搭建自己的运动规划实验。1. 把 kinodynamic_rrt_star 当成RRT 加一点动力学约束是读源码时最先踩的坑一个反直觉的事实这套算法里最贵的操作不是采样也不是碰撞检测而是判断两个状态有多近。几何版本里这件事是一行减法kinodynamic_rrt_star 里它变成一次带终端约束的最优控制求解一次可能几十毫秒。更反直觉的是这一步同时回答了三件事——怎么连、谁最近、这条边多贵。理解了这个一个求解三处复用的设计你就理解了整套算法为什么能在带动力学约束的前提下还保留渐近最优的证明。MATLAB 在这类算法里出现得多不是因为快而是因为ode45、lsqnonlin这些求解器让先把机制跑通变得极其廉价。2. 把距离换成代价一条边为什么不再是线段2.1 状态从一个点扩成一对方程可达性就变了几何规划里状态就是位置p ∈ R²任意两点之间总存在直线所以扩展一步可以定义成朝采样点插值。动力学系统里状态变成x [p; v] ∈ R⁴演化由ẋ Ax Bu决定A 是系统矩阵B 是控制输入矩阵。对最常见的双积分器模型% 二维双积分器状态 [px; py; vx; vy]控制 [ax; ay] n 4; m 2; A [zeros(2) eye(2); zeros(2) zeros(2)]; B [zeros(2); eye(2)];这个改动看似只是升了几维实际上把问题的拓扑换掉了。两个位置点之间永远有线段但两个状态之间不一定存在可行轨迹——比如从 (0,0,1,0) 到 (0,0,-1,0)位置一样、速度反向没有哪条瞬时轨迹能不违反加速度约束地完成这个切换。所以扩展这个原语必须重新定义它不再是插值而是求解一个存在性 最优性的联合问题。常见的做法是把这一步写成固定时间、两端固定的最优控制问题给定起点x0、终点x1和时间T在所有满足x(0)x0、x(T)x1的控制输入里找让代价J ∫₀ᵀ (xᵀQx uᵀRu) dt最小的那条。Q 和 R 是对称正定权重前者惩罚偏离原点的状态后者惩罚控制能量。2.2 三个原语被一个问题同时回答RRT* 的每次迭代需要三样东西steer从最近节点朝采样点走一步得到新节点、distance在树里找最近的若干个节点、cost评估一条边的代价用来选父节点和重连。几何版本里它们分别是直线插值、欧氏距离、欧氏距离——三份实现各管一摊。kinodynamic 版本里上面那个最优控制问题的解同时给出了这三样解出来的最优轨迹就是 steer 的结果最优代价J*就是这条边的 cost而J*本身可以作为 distance 使用。这不是巧合因为三者在语义上本来就是同一件事——从 x0 到 x1 的最小代价。把这三者统一是这套算法在理论上站得住的前提RRT* 的渐近最优性证明依赖代价函数的可加性和一致有界性而最优控制代价天然满足可加性。原语几何 RRT*kinodynamic RRT*steer线段插值O(1)解一次两点边值问题返回整条状态-控制轨迹nearest / near欧氏距离最小代价内层还要对 T 做一维优化边代价欧氏长度最优代价 J*碰撞检测沿线段采样点沿状态轨迹采样点必要时同时看控制量单次扩展耗时微秒级毫秒级到几十毫秒一个容易被忽略的细节J*作为距离只是一个拟度量因为从 x0 到 x1 的最优代价和从 x1 到 x0 的未必相等——动力学系统通常不可逆倒着走可能更贵或根本不可行。RRT* 的证明要求比严格度量弱所以这不致命但在实现里如果你想用对称的 KD 树加速就得清楚自己在做近似。2.3 时间 T 是第四个未知量这让每次求距离都变成嵌套优化给定x0和x1T不是事先能定死的。T 取太小问题可能无解——物理上就是来不及T 取太大代价里∫ xᵀQx dt这一项会线性增长最优性被稀释。所以真正的代价函数定义是function [J, xt, ut, tt] cost_to_go(x0, x1, param) % 外层对 T 做一维搜索内层解一次两点边值问题 T_grid linspace(param.T_min, param.T_max, param.T_samples); J inf; xt []; ut []; tt []; for T T_grid [xtc, utc, tc, Jc, ok] solve_tpbvp(param.A, param.B, ... param.Q, param.R, x0, x1, T, param.odeopt); if ok Jc J J Jc; xt xtc; ut utc; tt tc; end end endparam.T_min一般由系统带宽估计给出param.T_max是经验值。这个网格搜索在工程实现里通常只做粗搜比如 8 到 16 个点再插值因为内层solve_tpbvp的耗时是主导项。提示如果 T 的网格太稀会出现明明可达却判定不可达的假阴性——树在某个方向上完全长不出去排查时先看这里。3. 打靶法与协态方程solve_tpbvp到底在解什么3.1 从哈密顿函数到一阶两点边值问题上面那个带约束的优化问题用变分法可以化成一个两点边值问题。构造哈密顿函数H xᵀQx uᵀRu λᵀ(Ax Bu)其中λ是协态向量。对u求极值得u -½ R⁻¹Bᵀλ代回正则方程得到一组2n维的一阶常微分方程ẋ Ax - ½ B R⁻¹ Bᵀ λ λ̇ -Qx - Aᵀ λ边界条件是x(0) x0和x(T) x1一共2n个条件正好匹配2n维系统的自由度。这是一个标准的适定两点边值问题。这里有个和 LQR 容易混淆的地方值得说清常规有限视界 LQR 是自由终端边界条件是λ(T) 0等价于 Riccati 方程终值S(T) 0正向积分一遍就完事。而这里是固定终端x(T) x1被钉死协态在终端完全自由所以必须猜一个λ(0)再往前推。前一种情况是初值问题后一种才是边值问题——用错前者去解后者代码能跑结果全错而且错得很隐蔽。3.2 打靶把函数未知降成初值未知打靶法的思路很直接λ(0)是n维未知向量猜一个值用ode45从 0 积分到 T看终端状态x(T)和x1差多少把这个偏差作为残差交给非线性最小二乘求解器去调λ(0)直到偏差足够小。function [xt, ut, tt, J, ok] solve_tpbvp(A, B, Q, R, x0, x1, T, odeopt) % 固定时间、两端固定的线性二次最优控制打靶法求解 % 未知量协态初值 lambda(0)共 n 个残差终端状态偏差共 n 个 n size(A, 1); function dz rhs(t, z) x z(1:n); lam z(n1:2*n); u -0.5 * (R \ (B * lam)); % 由协态反解控制量 dz [A*x B*u; -Q*x - A*lam]; % 状态方程 协态方程 end function err miss(lam0) [~, Z] ode45(rhs, [0 T], [x0; lam0], odeopt); err Z(end, 1:n) - x1; % 终端打在 x1 上 end lam0 lsqnonlin(miss, zeros(n,1), [], [], ... optimoptions(lsqnonlin, Display, off, MaxIterations, 50)); [tt, Z] ode45(rhs, [0 T], [x0; lam0], odeopt); xt Z(:, 1:n); ut -0.5 * (Z(:, n1:2*n) * (R \ B)); % N x m 的控制序列 ok norm(Z(end, 1:n) - x1) 1e-4; % 打靶是否收敛 J trapz(tt, sum((xt*Q).*xt, 2) sum((ut*R).*ut, 2)); end几个参数的作用值得逐个说清。odeopt建议用odeset(RelTol,1e-6,AbsTol,1e-8)因为协态方程的反向积分对容差敏感容差松了打靶会收敛到假的极小值。1e-4这个收敛阈值对应的是状态空间的绝对误差如果你规划器的位置精度要求更高得同步收紧否则ok会骗你。MaxIterations设小是有意的——解不出来的边应该尽快放弃而不是耗在一次 BVP 上。代码里ut那一行用R \ B而不是inv(R)*B这在 MATLAB 里不只是风格问题R 通常是 2×2 或 3×3\走的是分解路径数值稳定性和速度都更好更重要的是inv(R)在 R 条件数稍大时会让整条轨迹的数值噪声放大。3.3 线性系统有闭式解但没必要一开始就用对线性定常系统上面的两点边值问题可以用矩阵指数写出解析解本质是把2n维系统的状态转移矩阵拆块然后用舒尔补解一个线性方程组全程没有迭代、没有 ode45。理论上更快也更稳。但工程上我一般还是先用打靶版本。原因有两个一是闭式解需要构造4n×4n的矩阵指数n4时是 8×8还好但如果状态维数上去比如带姿态的四旋翼模型n 到 12 甚至更高矩阵指数和块分解的代码比打靶难写得多也更容易出索引错误。二是打靶版本给出的是轨迹的数值采样xt直接就能拿去碰撞检测闭式解得自己再离散化一遍中间多一层铺采样点的活。用 Riccati 方程的那条路线也值得提一句把协态写成λ 2Sx 2s的形式S 满足-Ṡ AᵀS SA - SBR⁻¹BᵀS Q。固定终端意味着S(T) → ∞数值上没法直接处理所以实践中要么把它换成一个大有限值近似固定终端要么绕回打靶。Riccati 形式的优势是控制律可以写成状态反馈实时性场景下有用劣势是长视界下 S 的元素会指数增长ode45反向积分容易炸。这也是为什么大量 MATLAB 实现在这条路上选择了打靶而不是 Riccati。4. 最近邻与重连为什么欧氏距离会把树长歪4.1 度量错位的直接后果设想两个状态(0,0,1,0) 和 (0,0,-1,0)位置完全相同速度反向。欧氏距离在四维空间里是 2看起来很近但要真的从前者走到后者得先减速到零再反向加速代价远高于走到隔壁位置的一个静止点。如果最近邻用欧氏距离挑树会频繁挑中这种看起来近、动力学上很远的节点边变贵、树形变差最终收敛到的解离最优很远。严格的解法是用代价作为度量代价最小者就是最近邻。但代价函数一次几十毫秒树长到上千节点时单次迭代要几十秒直接不可用。4.2 两级筛选先用廉价度量粗筛再用代价精算我见过的大多数实现都采用两级结构第一级用加权欧氏距离挑出k个候选第二级只对这k个解 BVP 算真实代价。加权是关键——把速度和位置的量纲拉齐否则数值上速度项会被位置项淹没。function d weighted_dist(xa, xb, w) % w 是按状态分量给的权重通常取 [1 1 1/vel_scale 1/vel_scale] d norm((xa - xb) .* w); end % 每次迭代的候选筛选 idx kdtree_knn(tree.x, x_rand, param.k_euclid); % 第一级欧氏粗筛 best_cost inf; for i idx [J, xt, ut, tt] cost_to_go(tree.x(:,i), x_rand, param); if J best_cost collision_free(xt, param) best_cost J; best_parent i; best_traj xt; end endk的取法是个权衡太小比如 5会漏掉最优父节点树收敛慢、代价下降曲线抖太大比如 50每次迭代变成 50 次 BVP慢到没法用。经验区间是 8 到 20且随树规模增长适当调大。vel_scale取系统在典型控制输入下能产生的速度量级这个值设错会直接导致上面说的速度反向陷阱——设太小速度维度权重过高树会变得过于保守、不往前走设太大退化成纯位置距离。4.3 choose parent 与 rewiring 的差别在成本结构上两种操作看起来都是改父节点但在 kinodynamic 版本里成本完全不对称。choose parent 是给新节点选父节点一次比较只需算一次 BVP失败就跳过。rewiring 是给已有节点换父节点换完之后必须重新生成那条边的轨迹还要重新做碰撞检测——因为新边的动力学轨迹和旧边完全不同旧的碰撞检测结果作废。更要命的是被重连的节点的所有后代节点其累积代价都变了得沿子树传播更新。% rewiring遍历邻居检查经新节点是否更便宜 for j near_idx [J_new, xt_new, ut_new, tt_new] cost_to_go(tree.x(:, new_id), tree.x(:, j), param); if tree.cost(j) tree.cost(new_id) J_new collision_free(xt_new, param) tree.parent(j) new_id; % 改父 tree.cost(j) tree.cost(new_id) J_new; tree.edge{j} struct(xt, xt_new, ut, ut_new, tt, tt_new); update_descendants_cost(tree, j); % 关键后代代价必须传播 end endupdate_descendants_cost这一步在几何 RRT* 里经常被省掉或者写得很随意因为直线距离可加、更新代价就是减个增量。kinodynamic 下这条捷径不存在后代节点到根的总代价里每一段都是独立求解出来的最优代价父节点一变后面每一段的数值都不能靠加个差值修正。我见过不少实现干脆不做传播只更新当前节点的 cost 字段运行起来表面上 cost 曲线还在降实际上树的代价记录已经和真实结构对不上最后输出一条次优路径——不会报错只会让你怀疑参数。4.4 邻居半径里的维度陷阱近邻集合的半径按r(n) γ · (log n / n)^(1/d)取d必须是状态空间维数不是位置维数。写二维双积分器时状态是四维d4。如果顺手写成d2因为这是二维问题嘛半径会大得离谱每次 rewiring 要遍历几百个邻居单次迭代慢一个数量级反过来如果维度写高了半径太小树退化成一条细长的链rewiring 形同虚设。γ的经验取值和 Q、R 的量级强相关——代价的量纲不是长度量纲所以这个参数没法直接照搬几何版本的常数。稳妥的做法是先跑 200 次迭代观察代价下降曲线如果曲线一开始陡然后迅速走平多半是γ太小如果树的分叉极多、看起来像灌木丛但代价不降多半是太大了。5. 避坑与排查树长不出来、长歪了、长对了但代价不降5.1 树只在起点附近炸开往外伸不出去现象很典型迭代到几百次所有节点挤在初始状态周围的一个小球里采样点明明撒满了整个空间但新节点就是不肯往远处落。原因基本落在两处。一是T_max设得太保守远处的采样点在所有 T 候选下都是无解cost_to_go返回inf这次迭代被整体丢弃二是 Q 和 R 的相对量级失衡——Q 太大时长距离边的∫ xᵀQx dt项爆炸式增长代价函数对长边有强烈的惩罚最近邻永远选中近距离节点形成自我强化的小球。排查方法是把cost_to_go单拎出来做单元测试手工给一对相距很远的x0、x1打印各个 T 候选下的ok和J。如果全部ok 0问题在T_max和odeopt如果ok 1但J五位数以上问题在权重比例把 Q 整体缩一个量级再看。5.2 打靶返回 NaN或者返回了轨迹但终端偏离很远现象是lsqnonlin的退出标志不是 1或者明明ok 1画出来的轨迹末端离x1差着一大截。前者多半是协态方程的数值发散λ̇ -Qx - Aᵀλ里的-Aᵀλ项在一部分系统上会导致 λ 随时间指数增长T 一大就溢出。解决顺序是先把odeopt的容差收紧到RelTol1e-8再把 T 的搜索范围下压最后才考虑给λ(0)一个基于距离的合理初值比如λ0 -2 * Q * (x1 - x0) / T而不是零向量。零初值在长距离边上常常把求解器带进一个平坦区域梯度消失看起来收敛了其实没动。后者通常是精度阈值骗了你ok的判据是norm(x(T)-x1) 1e-4但如果你在碰撞检测里把这 1e-4 的偏差放大到整个轨迹末端就可能扎进障碍物里。把ok的阈值和规划器的位置精度要求对齐别用默认值。5.3 加了 rewiring代价曲线还是平的最容易被当成算法就是这样而放过的一类问题。按可能性排序一是d用错见 4.4先打印一次实际近邻数量正常应该在 10 到 100 之间几百就是维度写错了二是 rewiring 后没有传播后代代价三是新边的碰撞检测被漏掉或者更糟——rewiring 时没换轨迹只换了父指针那条边的实际轨迹还是旧父节点生成的代价算出来是新的、轨迹是旧的两者对不上。第三种最阴因为路径画出来看着挺合理只有拿去做闭环仿真才会发现控制量根本走不出这条轨迹。排查时在 rewiring 分支里加一句断言新父节点的边轨迹起点必须等于新父节点的状态终点必须等于被重连节点的状态。这一条断言能拦掉上面绝大部分情况。5.4 规划出的轨迹穿墙现象路径可视化里轨迹明晃晃地穿过障碍物但算法报告成功。原因几乎总是碰撞检测的采样密度不够。BVP 解出来的轨迹在高速段状态变化极快等间隔取 10 个点相邻点之间可能横跨半个障碍物。尤其要注意碰撞检测应该在状态空间做按弧长或按速度自适应采样而不是按时间等间隔——按时间采样在高速段点距天然变大正好漏检。一个稳妥的取法控制采样间距不超过min(障碍物最小厚度 / 2, 0.1 * 状态空间对角线)。这个上界偏保守会牺牲速度但漏检的代价是整条路径作废我宁愿保守。5.5 迭代一多就慢到没法跑1000 次迭代之后每次要好几秒profile一看八成时间在cost_to_go。这是算法结构本身的代价不是 bug但能缓解把第一级粗筛的k随树规模动态下调树小的时候 k20树大了降到 k8把cost_to_go的结果缓存起来因为 rewiring 会反复查询同一对状态T 的网格搜索只做粗搜取到最优的粗网格点后再在邻域补两个细点不做全局细分。这三条做完通常能有 3 到 5 倍的提速但别指望数量级提升——量级问题得靠换语言或者换算法。6. 设计取舍渐近最优的承诺值不值得用一次边值问题去换整套算法最核心的取舍其实只有一句话为了在动力学约束下依然保有最优性保证愿意把每条边的构造从 O(1) 的插值换成一次最优控制求解。这个交换的价格在几何版本里是微秒在这里是毫秒到几十毫秒高了三个数量级。价格带来的收益是什么是 rewiring 这件事变得有意义。几何 RRT* 里重连一条边几乎免费所以可以放心地高频重连kinodynamic 版本里每次重连都要重新解 BVP、重新做碰撞检测、重新传播后代代价成本高得让人本能地想把 rewiring 关掉。但一旦关掉退化成 RRT只剩概率完备性没有渐近最优——而渐近最优正是这套算法存在的理由。所以真正的工程决策不是要不要 rewiring而是rewiring 的频率和近邻半径怎么配比才能在给定时间预算内把代价压下去。我一般的做法是保留全量 rewiring但把近邻半径取下界用更少的候选换更快的单次迭代。第二个取舍在线性假设上。协态方程和打靶法都建立在ẋ Ax Bu这个线性模型上一旦系统非线性真实的车辆、旋翼、机械臂都非线性要么在工作点附近线性化、每个边用不同的 A、B要么换成数值最优控制iLQR、DDP 之类。前者便宜但只在局部有效边长了线性化误差就吃掉精度后者每条边要迭代几十轮比打靶贵一个量级。这也是为什么你能看到的 kinodynamic 演示大多用双积分器或者简单线性系统——不是研究者偷懒是这类系统让边值问题有解且解得起这个前提成立机制才看得清楚。第三个取舍关于验证。这套算法最坑的地方是它不会报错。BVP 解发散时你丢了边树还是能长rewiring 忘了传播代价路径还是能输出碰撞检测漏检规划器还是报成功。所以验证不能只看能不能到终点得看三条独立指标代价曲线是否单调下降且后期仍有小幅下降说明 rewiring 真的在起作用、每条输出边的 BVP 终端残差是否在阈值内说明轨迹真的连得上、以及沿轨迹的高密度碰撞复查是否通过。这三条我写成脚本挂在每次跑实验之前跑一遍不到十秒。从那以后我每写一个带动力学约束的规划器都强制先把给定 x0、x1、T能不能稳定解出一条轨迹这件事单独测通再去搭树——因为树上出的问题九成能追到这一层。希望帮到你。本文还有配套的精品资源点击获取 
长河君 · 2026/10/31 12:00:00 · 阅读 1.2k Kinodynamic RRT*源码拆解：边值求解与重连避坑
简介这份 MATLAB 代码包面向机器人运动规划学习者与科研人员围绕 Kinodynamic RRT*线性微分约束系统的最优运动规划论文实现适合复现算法、分析动力学约束下的采样扩展与轨迹优化过程。压缩包共 17 个文件、约 14KB以 13 个 .m 函数与脚本为主体另含 2 个 .mat 场景数据和 md、txt 说明文件整体结构轻量便于按模块阅读算法组织并逐文件调试。代码包含 RRT* 主流程、二重积分器、四旋翼与球体示例以及正定性判断、状态与输入可行性检测、障碍物场绘制、采样与航点数据等模块可帮助读者把论文中的代价计算、约束处理与优化步骤落到可运行示例中。已有 1302 人学习下载适合在 MATLAB 中修改参数、替换障碍与航点数据观察不同约束和采样密度对路径质量的影响并搭建自己的运动规划实验。1. 把 kinodynamic_rrt_star 当成RRT 加一点动力学约束是读源码时最先踩的坑一个反直觉的事实这套算法里最贵的操作不是采样也不是碰撞检测而是判断两个状态有多近。几何版本里这件事是一行减法kinodynamic_rrt_star 里它变成一次带终端约束的最优控制求解一次可能几十毫秒。更反直觉的是这一步同时回答了三件事——怎么连、谁最近、这条边多贵。理解了这个一个求解三处复用的设计你就理解了整套算法为什么能在带动力学约束的前提下还保留渐近最优的证明。MATLAB 在这类算法里出现得多不是因为快而是因为ode45、lsqnonlin这些求解器让先把机制跑通变得极其廉价。2. 把距离换成代价一条边为什么不再是线段2.1 状态从一个点扩成一对方程可达性就变了几何规划里状态就是位置p ∈ R²任意两点之间总存在直线所以扩展一步可以定义成朝采样点插值。动力学系统里状态变成x [p; v] ∈ R⁴演化由ẋ Ax Bu决定A 是系统矩阵B 是控制输入矩阵。对最常见的双积分器模型% 二维双积分器状态 [px; py; vx; vy]控制 [ax; ay] n 4; m 2; A [zeros(2) eye(2); zeros(2) zeros(2)]; B [zeros(2); eye(2)];这个改动看似只是升了几维实际上把问题的拓扑换掉了。两个位置点之间永远有线段但两个状态之间不一定存在可行轨迹——比如从 (0,0,1,0) 到 (0,0,-1,0)位置一样、速度反向没有哪条瞬时轨迹能不违反加速度约束地完成这个切换。所以扩展这个原语必须重新定义它不再是插值而是求解一个存在性 最优性的联合问题。常见的做法是把这一步写成固定时间、两端固定的最优控制问题给定起点x0、终点x1和时间T在所有满足x(0)x0、x(T)x1的控制输入里找让代价J ∫₀ᵀ (xᵀQx uᵀRu) dt最小的那条。Q 和 R 是对称正定权重前者惩罚偏离原点的状态后者惩罚控制能量。2.2 三个原语被一个问题同时回答RRT* 的每次迭代需要三样东西steer从最近节点朝采样点走一步得到新节点、distance在树里找最近的若干个节点、cost评估一条边的代价用来选父节点和重连。几何版本里它们分别是直线插值、欧氏距离、欧氏距离——三份实现各管一摊。kinodynamic 版本里上面那个最优控制问题的解同时给出了这三样解出来的最优轨迹就是 steer 的结果最优代价J*就是这条边的 cost而J*本身可以作为 distance 使用。这不是巧合因为三者在语义上本来就是同一件事——从 x0 到 x1 的最小代价。把这三者统一是这套算法在理论上站得住的前提RRT* 的渐近最优性证明依赖代价函数的可加性和一致有界性而最优控制代价天然满足可加性。原语几何 RRT*kinodynamic RRT*steer线段插值O(1)解一次两点边值问题返回整条状态-控制轨迹nearest / near欧氏距离最小代价内层还要对 T 做一维优化边代价欧氏长度最优代价 J*碰撞检测沿线段采样点沿状态轨迹采样点必要时同时看控制量单次扩展耗时微秒级毫秒级到几十毫秒一个容易被忽略的细节J*作为距离只是一个拟度量因为从 x0 到 x1 的最优代价和从 x1 到 x0 的未必相等——动力学系统通常不可逆倒着走可能更贵或根本不可行。RRT* 的证明要求比严格度量弱所以这不致命但在实现里如果你想用对称的 KD 树加速就得清楚自己在做近似。2.3 时间 T 是第四个未知量这让每次求距离都变成嵌套优化给定x0和x1T不是事先能定死的。T 取太小问题可能无解——物理上就是来不及T 取太大代价里∫ xᵀQx dt这一项会线性增长最优性被稀释。所以真正的代价函数定义是function [J, xt, ut, tt] cost_to_go(x0, x1, param) % 外层对 T 做一维搜索内层解一次两点边值问题 T_grid linspace(param.T_min, param.T_max, param.T_samples); J inf; xt []; ut []; tt []; for T T_grid [xtc, utc, tc, Jc, ok] solve_tpbvp(param.A, param.B, ... param.Q, param.R, x0, x1, T, param.odeopt); if ok Jc J J Jc; xt xtc; ut utc; tt tc; end end endparam.T_min一般由系统带宽估计给出param.T_max是经验值。这个网格搜索在工程实现里通常只做粗搜比如 8 到 16 个点再插值因为内层solve_tpbvp的耗时是主导项。提示如果 T 的网格太稀会出现明明可达却判定不可达的假阴性——树在某个方向上完全长不出去排查时先看这里。3. 打靶法与协态方程solve_tpbvp到底在解什么3.1 从哈密顿函数到一阶两点边值问题上面那个带约束的优化问题用变分法可以化成一个两点边值问题。构造哈密顿函数H xᵀQx uᵀRu λᵀ(Ax Bu)其中λ是协态向量。对u求极值得u -½ R⁻¹Bᵀλ代回正则方程得到一组2n维的一阶常微分方程ẋ Ax - ½ B R⁻¹ Bᵀ λ λ̇ -Qx - Aᵀ λ边界条件是x(0) x0和x(T) x1一共2n个条件正好匹配2n维系统的自由度。这是一个标准的适定两点边值问题。这里有个和 LQR 容易混淆的地方值得说清常规有限视界 LQR 是自由终端边界条件是λ(T) 0等价于 Riccati 方程终值S(T) 0正向积分一遍就完事。而这里是固定终端x(T) x1被钉死协态在终端完全自由所以必须猜一个λ(0)再往前推。前一种情况是初值问题后一种才是边值问题——用错前者去解后者代码能跑结果全错而且错得很隐蔽。3.2 打靶把函数未知降成初值未知打靶法的思路很直接λ(0)是n维未知向量猜一个值用ode45从 0 积分到 T看终端状态x(T)和x1差多少把这个偏差作为残差交给非线性最小二乘求解器去调λ(0)直到偏差足够小。function [xt, ut, tt, J, ok] solve_tpbvp(A, B, Q, R, x0, x1, T, odeopt) % 固定时间、两端固定的线性二次最优控制打靶法求解 % 未知量协态初值 lambda(0)共 n 个残差终端状态偏差共 n 个 n size(A, 1); function dz rhs(t, z) x z(1:n); lam z(n1:2*n); u -0.5 * (R \ (B * lam)); % 由协态反解控制量 dz [A*x B*u; -Q*x - A*lam]; % 状态方程 协态方程 end function err miss(lam0) [~, Z] ode45(rhs, [0 T], [x0; lam0], odeopt); err Z(end, 1:n) - x1; % 终端打在 x1 上 end lam0 lsqnonlin(miss, zeros(n,1), [], [], ... optimoptions(lsqnonlin, Display, off, MaxIterations, 50)); [tt, Z] ode45(rhs, [0 T], [x0; lam0], odeopt); xt Z(:, 1:n); ut -0.5 * (Z(:, n1:2*n) * (R \ B)); % N x m 的控制序列 ok norm(Z(end, 1:n) - x1) 1e-4; % 打靶是否收敛 J trapz(tt, sum((xt*Q).*xt, 2) sum((ut*R).*ut, 2)); end几个参数的作用值得逐个说清。odeopt建议用odeset(RelTol,1e-6,AbsTol,1e-8)因为协态方程的反向积分对容差敏感容差松了打靶会收敛到假的极小值。1e-4这个收敛阈值对应的是状态空间的绝对误差如果你规划器的位置精度要求更高得同步收紧否则ok会骗你。MaxIterations设小是有意的——解不出来的边应该尽快放弃而不是耗在一次 BVP 上。代码里ut那一行用R \ B而不是inv(R)*B这在 MATLAB 里不只是风格问题R 通常是 2×2 或 3×3\走的是分解路径数值稳定性和速度都更好更重要的是inv(R)在 R 条件数稍大时会让整条轨迹的数值噪声放大。3.3 线性系统有闭式解但没必要一开始就用对线性定常系统上面的两点边值问题可以用矩阵指数写出解析解本质是把2n维系统的状态转移矩阵拆块然后用舒尔补解一个线性方程组全程没有迭代、没有 ode45。理论上更快也更稳。但工程上我一般还是先用打靶版本。原因有两个一是闭式解需要构造4n×4n的矩阵指数n4时是 8×8还好但如果状态维数上去比如带姿态的四旋翼模型n 到 12 甚至更高矩阵指数和块分解的代码比打靶难写得多也更容易出索引错误。二是打靶版本给出的是轨迹的数值采样xt直接就能拿去碰撞检测闭式解得自己再离散化一遍中间多一层铺采样点的活。用 Riccati 方程的那条路线也值得提一句把协态写成λ 2Sx 2s的形式S 满足-Ṡ AᵀS SA - SBR⁻¹BᵀS Q。固定终端意味着S(T) → ∞数值上没法直接处理所以实践中要么把它换成一个大有限值近似固定终端要么绕回打靶。Riccati 形式的优势是控制律可以写成状态反馈实时性场景下有用劣势是长视界下 S 的元素会指数增长ode45反向积分容易炸。这也是为什么大量 MATLAB 实现在这条路上选择了打靶而不是 Riccati。4. 最近邻与重连为什么欧氏距离会把树长歪4.1 度量错位的直接后果设想两个状态(0,0,1,0) 和 (0,0,-1,0)位置完全相同速度反向。欧氏距离在四维空间里是 2看起来很近但要真的从前者走到后者得先减速到零再反向加速代价远高于走到隔壁位置的一个静止点。如果最近邻用欧氏距离挑树会频繁挑中这种看起来近、动力学上很远的节点边变贵、树形变差最终收敛到的解离最优很远。严格的解法是用代价作为度量代价最小者就是最近邻。但代价函数一次几十毫秒树长到上千节点时单次迭代要几十秒直接不可用。4.2 两级筛选先用廉价度量粗筛再用代价精算我见过的大多数实现都采用两级结构第一级用加权欧氏距离挑出k个候选第二级只对这k个解 BVP 算真实代价。加权是关键——把速度和位置的量纲拉齐否则数值上速度项会被位置项淹没。function d weighted_dist(xa, xb, w) % w 是按状态分量给的权重通常取 [1 1 1/vel_scale 1/vel_scale] d norm((xa - xb) .* w); end % 每次迭代的候选筛选 idx kdtree_knn(tree.x, x_rand, param.k_euclid); % 第一级欧氏粗筛 best_cost inf; for i idx [J, xt, ut, tt] cost_to_go(tree.x(:,i), x_rand, param); if J best_cost collision_free(xt, param) best_cost J; best_parent i; best_traj xt; end endk的取法是个权衡太小比如 5会漏掉最优父节点树收敛慢、代价下降曲线抖太大比如 50每次迭代变成 50 次 BVP慢到没法用。经验区间是 8 到 20且随树规模增长适当调大。vel_scale取系统在典型控制输入下能产生的速度量级这个值设错会直接导致上面说的速度反向陷阱——设太小速度维度权重过高树会变得过于保守、不往前走设太大退化成纯位置距离。4.3 choose parent 与 rewiring 的差别在成本结构上两种操作看起来都是改父节点但在 kinodynamic 版本里成本完全不对称。choose parent 是给新节点选父节点一次比较只需算一次 BVP失败就跳过。rewiring 是给已有节点换父节点换完之后必须重新生成那条边的轨迹还要重新做碰撞检测——因为新边的动力学轨迹和旧边完全不同旧的碰撞检测结果作废。更要命的是被重连的节点的所有后代节点其累积代价都变了得沿子树传播更新。% rewiring遍历邻居检查经新节点是否更便宜 for j near_idx [J_new, xt_new, ut_new, tt_new] cost_to_go(tree.x(:, new_id), tree.x(:, j), param); if tree.cost(j) tree.cost(new_id) J_new collision_free(xt_new, param) tree.parent(j) new_id; % 改父 tree.cost(j) tree.cost(new_id) J_new; tree.edge{j} struct(xt, xt_new, ut, ut_new, tt, tt_new); update_descendants_cost(tree, j); % 关键后代代价必须传播 end endupdate_descendants_cost这一步在几何 RRT* 里经常被省掉或者写得很随意因为直线距离可加、更新代价就是减个增量。kinodynamic 下这条捷径不存在后代节点到根的总代价里每一段都是独立求解出来的最优代价父节点一变后面每一段的数值都不能靠加个差值修正。我见过不少实现干脆不做传播只更新当前节点的 cost 字段运行起来表面上 cost 曲线还在降实际上树的代价记录已经和真实结构对不上最后输出一条次优路径——不会报错只会让你怀疑参数。4.4 邻居半径里的维度陷阱近邻集合的半径按r(n) γ · (log n / n)^(1/d)取d必须是状态空间维数不是位置维数。写二维双积分器时状态是四维d4。如果顺手写成d2因为这是二维问题嘛半径会大得离谱每次 rewiring 要遍历几百个邻居单次迭代慢一个数量级反过来如果维度写高了半径太小树退化成一条细长的链rewiring 形同虚设。γ的经验取值和 Q、R 的量级强相关——代价的量纲不是长度量纲所以这个参数没法直接照搬几何版本的常数。稳妥的做法是先跑 200 次迭代观察代价下降曲线如果曲线一开始陡然后迅速走平多半是γ太小如果树的分叉极多、看起来像灌木丛但代价不降多半是太大了。5. 避坑与排查树长不出来、长歪了、长对了但代价不降5.1 树只在起点附近炸开往外伸不出去现象很典型迭代到几百次所有节点挤在初始状态周围的一个小球里采样点明明撒满了整个空间但新节点就是不肯往远处落。原因基本落在两处。一是T_max设得太保守远处的采样点在所有 T 候选下都是无解cost_to_go返回inf这次迭代被整体丢弃二是 Q 和 R 的相对量级失衡——Q 太大时长距离边的∫ xᵀQx dt项爆炸式增长代价函数对长边有强烈的惩罚最近邻永远选中近距离节点形成自我强化的小球。排查方法是把cost_to_go单拎出来做单元测试手工给一对相距很远的x0、x1打印各个 T 候选下的ok和J。如果全部ok 0问题在T_max和odeopt如果ok 1但J五位数以上问题在权重比例把 Q 整体缩一个量级再看。5.2 打靶返回 NaN或者返回了轨迹但终端偏离很远现象是lsqnonlin的退出标志不是 1或者明明ok 1画出来的轨迹末端离x1差着一大截。前者多半是协态方程的数值发散λ̇ -Qx - Aᵀλ里的-Aᵀλ项在一部分系统上会导致 λ 随时间指数增长T 一大就溢出。解决顺序是先把odeopt的容差收紧到RelTol1e-8再把 T 的搜索范围下压最后才考虑给λ(0)一个基于距离的合理初值比如λ0 -2 * Q * (x1 - x0) / T而不是零向量。零初值在长距离边上常常把求解器带进一个平坦区域梯度消失看起来收敛了其实没动。后者通常是精度阈值骗了你ok的判据是norm(x(T)-x1) 1e-4但如果你在碰撞检测里把这 1e-4 的偏差放大到整个轨迹末端就可能扎进障碍物里。把ok的阈值和规划器的位置精度要求对齐别用默认值。5.3 加了 rewiring代价曲线还是平的最容易被当成算法就是这样而放过的一类问题。按可能性排序一是d用错见 4.4先打印一次实际近邻数量正常应该在 10 到 100 之间几百就是维度写错了二是 rewiring 后没有传播后代代价三是新边的碰撞检测被漏掉或者更糟——rewiring 时没换轨迹只换了父指针那条边的实际轨迹还是旧父节点生成的代价算出来是新的、轨迹是旧的两者对不上。第三种最阴因为路径画出来看着挺合理只有拿去做闭环仿真才会发现控制量根本走不出这条轨迹。排查时在 rewiring 分支里加一句断言新父节点的边轨迹起点必须等于新父节点的状态终点必须等于被重连节点的状态。这一条断言能拦掉上面绝大部分情况。5.4 规划出的轨迹穿墙现象路径可视化里轨迹明晃晃地穿过障碍物但算法报告成功。原因几乎总是碰撞检测的采样密度不够。BVP 解出来的轨迹在高速段状态变化极快等间隔取 10 个点相邻点之间可能横跨半个障碍物。尤其要注意碰撞检测应该在状态空间做按弧长或按速度自适应采样而不是按时间等间隔——按时间采样在高速段点距天然变大正好漏检。一个稳妥的取法控制采样间距不超过min(障碍物最小厚度 / 2, 0.1 * 状态空间对角线)。这个上界偏保守会牺牲速度但漏检的代价是整条路径作废我宁愿保守。5.5 迭代一多就慢到没法跑1000 次迭代之后每次要好几秒profile一看八成时间在cost_to_go。这是算法结构本身的代价不是 bug但能缓解把第一级粗筛的k随树规模动态下调树小的时候 k20树大了降到 k8把cost_to_go的结果缓存起来因为 rewiring 会反复查询同一对状态T 的网格搜索只做粗搜取到最优的粗网格点后再在邻域补两个细点不做全局细分。这三条做完通常能有 3 到 5 倍的提速但别指望数量级提升——量级问题得靠换语言或者换算法。6. 设计取舍渐近最优的承诺值不值得用一次边值问题去换整套算法最核心的取舍其实只有一句话为了在动力学约束下依然保有最优性保证愿意把每条边的构造从 O(1) 的插值换成一次最优控制求解。这个交换的价格在几何版本里是微秒在这里是毫秒到几十毫秒高了三个数量级。价格带来的收益是什么是 rewiring 这件事变得有意义。几何 RRT* 里重连一条边几乎免费所以可以放心地高频重连kinodynamic 版本里每次重连都要重新解 BVP、重新做碰撞检测、重新传播后代代价成本高得让人本能地想把 rewiring 关掉。但一旦关掉退化成 RRT只剩概率完备性没有渐近最优——而渐近最优正是这套算法存在的理由。所以真正的工程决策不是要不要 rewiring而是rewiring 的频率和近邻半径怎么配比才能在给定时间预算内把代价压下去。我一般的做法是保留全量 rewiring但把近邻半径取下界用更少的候选换更快的单次迭代。第二个取舍在线性假设上。协态方程和打靶法都建立在ẋ Ax Bu这个线性模型上一旦系统非线性真实的车辆、旋翼、机械臂都非线性要么在工作点附近线性化、每个边用不同的 A、B要么换成数值最优控制iLQR、DDP 之类。前者便宜但只在局部有效边长了线性化误差就吃掉精度后者每条边要迭代几十轮比打靶贵一个量级。这也是为什么你能看到的 kinodynamic 演示大多用双积分器或者简单线性系统——不是研究者偷懒是这类系统让边值问题有解且解得起这个前提成立机制才看得清楚。第三个取舍关于验证。这套算法最坑的地方是它不会报错。BVP 解发散时你丢了边树还是能长rewiring 忘了传播代价路径还是能输出碰撞检测漏检规划器还是报成功。所以验证不能只看能不能到终点得看三条独立指标代价曲线是否单调下降且后期仍有小幅下降说明 rewiring 真的在起作用、每条输出边的 BVP 终端残差是否在阈值内说明轨迹真的连得上、以及沿轨迹的高密度碰撞复查是否通过。这三条我写成脚本挂在每次跑实验之前跑一遍不到十秒。从那以后我每写一个带动力学约束的规划器都强制先把给定 x0、x1、T能不能稳定解出一条轨迹这件事单独测通再去搭树——因为树上出的问题九成能追到这一层。希望帮到你。本文还有配套的精品资源点击获取