简介：这份 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* 的渐近最优性证明依赖代价函数的可加性和一致有界性，而最优控制代价天然满足可加性。

一个容易被忽略的细节：J*作为"距离"只是一个拟度量，因为从 x0 到 x1 的最优代价和从 x1 到 x0 的未必相等——动力学系统通常不可逆，倒着走可能更贵或根本不可行。RRT* 的证明要求比严格度量弱，所以这不致命，但在实现里如果你想用对称的 KD 树加速，就得清楚自己在做近似。

2.3 时间 T 是第四个未知量，这让每次求距离都变成嵌套优化

给定x0和x1，T不是事先能定死的。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 end

param.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(n+1: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(:, n+1: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的矩阵指数，n=4时是 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 end

k的取法是个权衡：太小（比如 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 end

update_descendants_cost这一步在几何 RRT* 里经常被省掉或者写得很随意，因为直线距离可加、更新代价就是减个增量。kinodynamic 下这条捷径不存在：后代节点到根的总代价里，每一段都是独立求解出来的最优代价，父节点一变，后面每一段的数值都不能靠"加个差值"修正。我见过不少实现干脆不做传播，只更新当前节点的 cost 字段，运行起来表面上 cost 曲线还在降，实际上树的代价记录已经和真实结构对不上，最后输出一条次优路径——不会报错，只会让你怀疑参数。

4.4 邻居半径里的维度陷阱

近邻集合的半径按r(n) = γ · (log n / n)^(1/d)取，d必须是状态空间维数，不是位置维数。写二维双积分器时状态是四维，d=4。如果顺手写成d=2（因为"这是二维问题嘛"），半径会大得离谱，每次 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的容差收紧到RelTol=1e-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随树规模动态下调（树小的时候 k=20，树大了降到 k=8）；把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，能不能稳定解出一条轨迹"这件事单独测通，再去搭树——因为树上出的问题，九成能追到这一层。希望帮到你。

本文还有配套的精品资源，点击获取