Kinodynamic RRT*：融合动力学约束的机器人运动规划算法解析

简介：这份 Kinodynamic RRT* 算法的 MATLAB 实现，面向机器人路径规划研究者和爱好者，解决在几何与动力学约束下搜索可行最优路径的问题。资源将 RRT* 的渐进最优性与动力学模型相结合，覆盖状态空间表示、距离函数设计、随机采样、近邻查找、路径插值及动力学评估等核心环节。压缩包含 17 个文件，其中 13 个 .m 源文件提供核心算法与可视化脚本，2 个 .mat 文件存放障碍物与参考路径点数据，另有说明文档和许可证文件，整个包仅 14KB，轻量易读。代码结构层次清晰，便于学习者逐模块阅读修改，也适合作为课程设计或课题项目的基础框架。已有 1748 人学习下载，用于理解算法原理、调试参数或扩展双积分器、四旋翼等运动模型，具有较高的参考价值。 做机器人运动规划，如果只会写几何 RRT*，那你迟早会碰一个问题：画出来的路径是折线，车或者无人机根本走不出来。说白了，几何路径只约束了“位置”，没管“速度和加速度能不能跟上”，于是就有了 Kinodynamic RRT*——把动力学约束直接塞进随机扩展树里的一类规划器。

这篇文章我结合自己做运动规划课题时写的一份 Matlab 实现，讲清楚 Kinodynamic RRT* 的核心思路，并且给出双积分系统下可以直接参考的解释版代码骨架。适合读过一些路径规划文献、想动手实现但被“动力学扩展”卡住的人。你不需要一开始就搞懂全部数学推导，先把四个核心模块——状态空间、代价度量、动力学 Steer、Rewire 打通，后面再回头补数学就顺了。

1. Kinodynamic RRT* 到底是什么，和普通 RRT* 差在哪

1.1 从“构型空间搜索”到“状态空间搜索”

普通 RRT* 里，树的每个节点是一个位形，比如二维平面上的一个坐标 (x, y)，边是一条直线段。算法在几何空间里采样、找最近节点、连接、碰撞检测、重连，整个过程不关心机器人是差速底盘还是四旋翼。

Kinodynamic RRT* 就不一样了。它把“状态”从位形扩展到同时包含广义位置和广义速度，比如四维状态 [x, y, vx, vy]。树的边也不是直线段，而是一段满足运动学/动力学约束的状态轨迹。换句话说，普通 RRT* 回答的是“怎么走能到达”，Kinodynamic RRT* 回答的是“按照这个系统的物理规律，怎么控制才能到达”。

这个区别带来的连锁反应很大。首先，搜索空间维度翻倍，采样和近邻搜索都变贵了。其次，两个状态节点之间的“连线”不再能一笔画出，必须靠系统的状态转移方程去推，这就引出了 Steer 函数。最后，代价也不能只看路径长度，得看时间、控制能量或者其他和动力学相关的指标。

1.2 生成可行轨迹的两条技术路线

实现 Kinodynamic RRT* 时，最核心的拦路虎是“给定两个状态，怎么生成一段可行轨迹”。业内做法基本分两派。

第一派是前向传播（forward propagation）。从当前状态出发，随机采样控制输入 u 和持续时间 T，向前积分微分方程，得到一批候选末端状态，再从里面挑满足条件的。这种做法实现简单、对模型几乎没要求，任意复杂的非线性系统都能用，缺点是漫无目的，扩展的效率完全靠候选数量硬撑。

第二派是精确两点边值求解（TBVP / OBVP）。给定起点状态和终点状态，解一个最优控制问题，直接得到唯一的最优轨迹。精度高、效率好，但只对特定模型成立，比如双积分系统、线性系统，一旦换复杂模型就得重新推导。

实际工程里绝大多数人走的是第一派，或者两派混着用。我这份 Matlab 实现也以前向传播为主，在目标偏置采样时加了些技巧，后面详细说。

2. 核心模块拆解：状态、代价、碰撞检测

2.1 状态空间定义与距离度量

双积分系统是最经典的验证平台，动力学方程很简单：p 是二维位置，v 是二维速度，控制输入 u 是加速度，满足 |u| ≤ a_max，同时要求 |v| ≤ v_max。

matlab % 状态： [px, py, vx, vy] x_start = [0, 0, 0, 0]; x_goal = [8, 6, 0, 0]; a_max = 1.0; % 最大加速度 m/s^2 v_max = 1.5; % 最大速度 m/s

这里有一个特别容易踩的坑：距离度量。几何 RRT* 里直接用欧氏距离当度量没问题，但在状态空间里，几何距离近的两个状态，动力学可达性可能差得很远。比如一个节点在左边高速向左飞，目标点在它右边很近的位置，几何上很近，但按动力学约束它必须先减速再反向加速，实际代价很大。 如果拿几何距离去做近邻搜索，树会偏向连接那些“看着近但实际难到达”的节点，扩展效率急剧下降。我的做法是：近邻搜索用几何距离（带权重的位置距离加速度距离）做启发式，代价函数则用真正的时间或控制能量。这样既保证了搜索速度，又不会把代价值算歪。 ### 2.2 代价函数与可行轨迹的度量 Kinodynamic RRT* 的渐近最优性建立在代价函数定义合理的前提下。常用两种： 第一种是最小时间代价： J = T 对于双积分系统，给定起点和终点，在速度、加速度约束下，最短时间轨迹就是“加速到最大速度—匀速—减速到目标速度”的梯形速度剖面，距离太短时退化成三角剖面。 第二种是最小控制能量代价： J = ∫₀ᵀ ‖u(t)‖² dt 这个更贴近实际能耗，但计算量稍大。对前向传播实现来说，每个候选轨迹只需要记录它的 T 和 ∑‖u‖²·dt 就行，非常方便。 我个人的建议是初版实现先用最小时间代价，因为好调试、收敛快。等整条管线跑通了，再切换成能量代价做对比。代价函数一变，Steer 的选优逻辑、Rewire 的成本比较都会随之变化，先固定一个变量很重要。 ### 2.3 动力学碰撞检测 几何 RRT* 的碰撞检测只需要判断线段是否穿过障碍物，Kinodynamic RRT* 则要判断一整段状态轨迹是否安全。 轨迹上的每个离散点都要做几何碰撞检测，但采样间距不能拍脑袋定。如果固定 dt=0.05s，而 v_max 很大，那么相邻采样点之间的距离可能超过障碍物最小尺寸，轨迹会从障碍物边缘“穿过去”却不被发现。反过来，dt 太小，碰撞检测的时间开销成倍上涨。 实用的做法是根据速度自适应采样步长。我习惯这样写：先按固定 dt 生成轨迹，然后算相邻点的最大位移，如果最大位移超过了障碍物最小尺寸的一半，就局部加密重采样。或者更简单一点，用保守策略：把障碍物做膨胀，膨胀半径等于最大轨迹间隔。这样既不会漏检，代码也简单。 ## 3. Matlab 实现的关键步骤与代码骨架 ### 3.1 节点数据结构和初始化 Kinodynamic RRT* 的节点比几何 RRT* 多存两个东西：到达时刻或者时间累积量，以及这条轨迹对应的控制输入。控制输入不一定必须存，但存了之后做轨迹回放、调试、后处理都非常方便。 matlab % 节点数据结构 node.state = [x, y, vx, vy]; % 四维状态 node.parent = []; % 父节点索引 node.cost = 0; % 从起点到该节点的累计代价 node.time = 0; % 到达该节点的时刻 node.input = []; % 进入该节点所用的控制输入 node.traj = []; % 从父节点到该节点的状态轨迹（调试用）

初始化时只需要一个起始节点，goal 不预先插入树里。目标通常是“靠近目标状态集合”就算成功，比如位置误差小于 0.2m 且速度小于 0.1m/s。

3.2 主循环：采样、扩展、插点、重连

主循环骨架和几何 RRT* 长得差不多，但每一步都更重。先看代码：

matlab for iter = 1:N_iter % 1. 采样：目标偏置 + 随机采样 if rand < goal_bias p_target = x_goal(1:2); else p_target = [randmap_width, randmap_height]; end

% 2. 按几何距离找最近节点 idx_near = find_nearest(tree, p_target); % 3. 动力学扩展：生成可行轨迹 [x_new, feasible, traj] = steer(tree(idx_near), p_target, dt, v_max, a_max); if ~feasible, continue; end % 4. 碰撞检测：整段轨迹都要安全 if ~collision_free_traj(traj, obstacles), continue; end % 5. 插入新节点 idx_new = length(tree) + 1; tree(idx_new).state = x_new; tree(idx_new).parent = idx_near; tree(idx_new).cost = tree(idx_near).cost + T; tree(idx_new).time = tree(idx_near).time + T; tree(idx_new).input = u; % 6. 重连 tree = rewire(tree, idx_new, v_max, a_max);

end

注意这里采样目标是二维位置 p_target，不是四维状态。速度不采样，因为前向传播会自动产生末端速度。如果你强行采样速度和位置，前向传播很难精确命中，大概率都是废点，反而拖慢搜索。 ### 3.3 动力学Steer：写一个能用的双积分版本 这是全文最核心的函数。给定当前节点状态和采样目标位置，我生成 N=20 个随机控制候选，每个都前向积分一段随机时长，然后选离目标最近的可行轨迹。代码如下： matlab function [x_new, feasible, traj] = steer(x_near, p_target, dt, v_max, a_max) feasible = false; N = 20; % 候选轨迹数量 best_dist = inf; for k = 1:N T = 0.5 + 2.0 * rand(); % 随机机动时长，单位秒 u = (rand(1, 2) * 2 - 1) * a_max; % 随机加速度控制 t = 0:dt:T; v = x_near(3:4) + u .* t'; % 速度轨迹，每一行一个时刻 p = x_near(1:2) + x_near(3:4) .* t' + 0.5 * u .* t'.^2; if any(abs(v(:)) > v_max) continue; % 速度超限，直接丢弃 end % 离目标位置越近越好 dist = norm(p(end, :) - p_target); if dist < best_dist best_dist = dist; traj = [p, v]; % 状态轨迹 Nx4 x_new = traj(end, :); u_best = u; T_best = T; feasible = true; end end if feasible x_new = traj(end, :); end end

这段代码有几个细节值得说。T 的范围我取 0.5~2.5s，如果地图很大或者 v_max 很小，这个范围要放大，否则每个候选都飞不远，树长得极慢。u 是二维随机加速度向量，范围在 [-a_max, a_max] 之间随机均匀采样。速度上限检查用的是整个轨迹的最大瞬时速度，这样能保证整段轨迹都物理可行。

但也要坦白说，这种随机 Steer 的效率不高，大量候选会被速度约束或者碰撞检测筛掉。想提高效率，可以做目标偏置：以一定概率把 u 的方向设置为指向采样点方向，这样候选轨迹更容易延伸到目标附近。实现也不难，就是把随机 u 归一化后乘 a_max，再乘个小幅度噪声。

3.4 Rewire 为什么难写，以及工程妥协

Rewire 是 RRT* 渐近最优性的关键。几何 RRT* 里 Rewire 很简单：把新节点周围半径内的候选父节点都试一遍，看当前父节点是否最优。但 Kinodynamic RRT* 里有一个隐藏问题——前向随机传播产生的轨迹，末端状态是随机分布的，没法精确到达某个指定状态。

所以严格意义上，用随机 Steer 做 Rewire 是做不到的。Rewire 必须能解“从候选父节点状态到新节点状态的精确轨迹”，这需要两点边值求解器。双积分系统可以推导 bang-bang 控制解析解，但代码量不小；更复杂的系统基本无解。

我当时的工程妥协是这样的：Rewire 阶段只针对那些状态本身离新节点较近的节点，用固定时间 T 计算控制 u=(v_new−v_i)/T，然后检查位置是否能对上。如果位置误差小于阈值，就认为连接成功，再做碰撞检测和代价比较。位置对不上就跳过，说白了就是一个“尽力而为”的 Rewire。实验下来，路径质量比完全不 Rewire 好很多，虽然理论上不保证渐近最优，但工程上够用。

4. 常见问题与调参实战

4.1 树长不到目标怎么办

这是最常见的问题。出现这个情况，先别急着改代码，按顺序排查。

第一，检查采样范围限制。状态空间里的速度分量也要有边界，如果随机初始速度过大，轨迹很容易飞出地图边界，或者速度超限被大量丢弃。第二，看 T 的范围。地图 10m×10m，如果 T 最大只有 0.5s，而 v_max 只有 1m/s，那轨迹最多延伸 0.5 米，树要扩展非常多的代才能到目标。T_max 大致设为地图对角线长度除以 v_max 的两倍，比较合理。第三，调大候选数量 N，从 20 加到 50，成功率会明显提升，代价是单次扩展变慢。

如果这些都没问题，再看看目标偏置概率。goal_bias=0.1 是常用的起点，但如果树已经能长到目标附近却永远无法进到目标邻域，可以考虑把目标偏置提高到 0.2，同时把目标成功判定半径稍微放大。

4.2 路径抖、代价不收敛

路径抖动通常意味着 Rewire 没生效，或者代价定义不一致。我调试时遇到过一个很隐蔽的问题：近邻搜索用的距离度量和代价函数不一致。近邻搜索按几何距离找最近节点，但代价函数按时间算，结果就是树总是往“几何近但时间远”的方向长，路径自然乱。

解决办法是让近邻搜索度量和代价函数尽量对齐。哪怕是近似对齐也有帮助。比如近邻搜索用 (v_max·Δt) 估算距离，或者简单地把几何距离除以 v_max 当作代价值，树的扩展方向就会稳定得多。

另一个原因是重连半径太小。固定半径 Rewire 在节点稀疏的时候根本找不到可替换的父节点，代价完全靠运气。我建议先把重连半径设成扩展步长的 3~5 倍，后面再调小。

4.3 Matlab 性能优化：别让循环毁掉你

Matlab 跑 RRT* 这种迭代型算法，最大的敌人就是动态数组扩展和循环内散操作。我第一版代码直接在循环里 tree(end+1) = node，地图稍微大一点就慢得让人抓狂。后来改成预分配一个大数组，满了再翻倍扩容，速度提升非常明显。

还有几个实战细节：

• 碰撞检测里，尽量用向量化判断，避免对轨迹的每个点都写一个 if。一次算完所有轨迹点是否在障碍物矩形内，再用 any 汇总。
• 近邻搜索不要用全遍历，节点上千之后全遍历就很痛了。我用的是 matlab 自带的 KDTreeSearcher，每次构造一棵树查最近点，或者手写一个二维网格哈希表，速度差一个数量级。
• 随机数生成和矩阵运算都比较耗时的，如果 N=20 个候选轨迹用的都是同一个 dt，可以一次生成 N 个随机 u 和 T，用矩阵一次性积分，避免 for 循环逐个积分。

当然，如果只是做毕业设计的小场景，上面这些优化不是必须的，但如果你想跑大图或者做参数实验，性能优化绝对值得花半天时间提前做。

4.4 关键参数速查表

5. 扩展建议与个人体会

5.1 从双积分到更高阶系统

双积分模型对应的是加速度控制，很多地面机器人、缩比无人机都能近似用。如果你想做更真实的模型，可以把状态扩展到 [x, y, z, vx, vy, vz]，控制变成三维加速度向量，算法框架完全不用改，只要把 Steer 里的随机控制向量从二维变成三维。

再往高阶走，比如四旋翼的微分平坦特性，输出是位置及其各阶导数，控制量变成 jerk 或者 snap。前向传播的方法依然适用，只是每次积分时状态转移方程要换成三积分或四积分模型。代价函数最好也从“时间”改成“时间 + 控制能量加权”，因为纯时间最优的高阶系统很容易出现抖振控制。

5.2 值得做的后续改进

写完基础版之后，有几个改进方向性价比很高。

第一个是 Informed RRT* 思想。树扩展早期全图采样没问题，一旦找到第一条可行路径，就可以把采样范围约束到以起点和终点为焦点的椭圆区域内，样本集中，收敛速度肉眼可见地提升。这个思想在 Kinodynamic 版本里同样适用，只是椭圆的定义要放到状态空间里处理。

第二个是轨迹后处理。Kinodynamic RRT* 给出的轨迹虽然可行，但通常比较粗。后面接一个时间最优或者控制能量最优的轨迹优化器（类似多项式轨迹优化）是业界标准做法。RRT* 只负责提供一个靠谱的初始解，精细化交给优化器。

第三个是动态障碍物场景。Kinodynamic RRT* 天然适合处理时空联合规划，因为节点带时间信息。如果你想做移动机器人避障，可以把障碍物设计成随时间变化的“时空障碍”，碰撞检测时按轨迹点的时间戳查对应时刻的障碍物位置，就能直接扩展出动态版本。

5.3 一点个人实战体会

最后分享一个我这几年调 Kinodynamic 类算法最深的一点体会：这个算法最耗时间的不是核心逻辑，而是调试 Steer 的数值边界问题。随机采样控制输入看起来很简单，但一旦速度上限、加速度上限、T 的范围设置不合理，代码就会产生海量的丢弃样本，现象是树长得慢、路径烂，你甚至会怀疑是碰撞检测写错了。

我的建议是，先在一维空间里把双积分 Steer 调通，只控制一维位置，画速度曲线和位置曲线，确认轨迹符合物理约束，再扩展到二维。不要在二维里直接硬调，否则问题混在一起很难定位。另外调试时把控制输入随时间变化的曲线也画出来，比只看路径直观得多。Kinodynamic RRT* 不是那种一次写对就能跑的算法，耐心迭代参数是常态，但一旦跑通了，那种“路径真的能被执行器走出来”的成就感，是很值得的。

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