简介：这套MATLAB实现对应论文《Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints》，面向机器人运动规划与最优控制方向的研究者、高年级本科生及工程师。代码覆盖线性微分约束下的运动规划核心流程，包含双积分器与四旋翼模型示例、状态与输入可行性检测、启发式采样及RRT主循环等模块，并附障碍物与航点数据，便于复现论文对比实验。资源共17个文件，以13个m脚本为主体，辅以2个mat数据文件、1个说明文档和1个许可文件，整个压缩包仅14KB，结构精简、适合快速研读。已有1294人学习下载，对于希望深入理解Kinodynamic RRT算法原理与MATLAB工程实现的读者，是一份可直接运行的参考代码。 去年在给一台双轮差速底盘做全局规划时，我用经典 RRT* 在仿真里跑出了无数条“完美”路径，结果真正把轨迹下发给控制器那一刻才发现，自己把最重要的东西丢了——那条几何路径里没有任何速度信息。控制器的第一次反应就是原地转圈，整整排查了三天，最后确认问题不是控制，而是规划。那次之后我把规划从配置空间搬进状态空间，开始系统性地使用 kinodynamic_rrt_star（下文简称 KRRT*）这类算法，才真正理解“可执行路径”和“可行轨迹”之间差着多少个数量级。这篇文章会把算法原理、关键模块和我在落地时的调参经验完整拆开讲，适合正在做无人机、无人车、机械臂运动规划，准备从几何路径规划过渡到动力学可行轨迹的工程师和研究者。
1. 从几何路径到状态轨迹：RRT* 的天真假设是怎么撑不住真实机器人的
1.1 经典 RRT* 在做什么，盲区在哪
经典 RRT* 的流程大家都很熟：随机采样构型、找最近邻、直线扩展、碰撞检测，然后通过 ChooseParent 和 Rewire 两步重连来优化树。它输出的本质上是一条配置空间里的连续几何路径，通常表现为一条多段线，代价函数是路径长度，渐近最优性建立在“机器人的可执行集合等于所有几何曲线”这个假设之上。
这个假设对自由空间中的理想点当然成立，但对真实机械系统几乎总是错的。一个最简单也最致命的例子是二重积分模型：状态是位置加速度，控制是加速度，机器人从静止出发，以最大加速度冲到某个点再减速停下。RRT* 给了一条直线路径，但完全没有告诉你速度怎么变化、加速度会不会超限。你拿着这条几何路径发给控制器，控制器只能靠自己的启发式去“猜”速度曲线，一旦猜错，就是我在开头说的原地打转。
打个比方：经典 RRT* 像地图导航只告诉你路线，但不告诉你每个路口应该踩多少刹车；真开车的人必须同时知道加减速曲线、转向角度和路面摩擦限制。kinodynamic 规划要解决的就是把“路线”升级成“驾驶操作时序”，这已经不是几何问题，而是状态空间中的最优控制问题。
1.2 kinodynamic 状态空间到底长什么样
在 kinodynamic 规划里，节点不再是配置空间里的单个构型，而是包含广义坐标和其导数的状态。最常见的定义是：
x = (q, q_dot) u ∈ U x_dot = f(x, u)这里 q 可以是平面位置、无人机三维坐标、机械臂关节角；q_dot 是速度；u 是控制输入，例如加速度、力矩或线速度和角速度。所有可行运动都必须是动力学方程的一个解轨迹。
拿两个高频模型举例。一个是双积分模型：
x = (p, v) p_dot = v v_dot = a |a| ≤ a_max另一个是差速底盘运动学：
x = (x, y, theta, v) x_dot = v * cos(theta) y_dot = v * sin(theta) theta_dot = omega可以看到，边不再是直线，而是时间参数化的轨迹 x(t)。这也意味着同一个几何路径，因为施加在时间轴上的速度曲线不同，可能产生完全不同的可执行性。比如一个 90 度拐弯，高速通过与会前减速到零再走的轨迹，动力学可行性天差地别。
1.3 哪些任务非要 kinodynamic 不可
纯几何规划不是没用，只要机器人运动缓慢、服从指令、有充分时间修正，几何路径转成轨迹的误差可以被底层控制器吸收。但下面几类任务，硬用几何路径几乎都会出问题：
- 无人机穿林、巡检：速度突变会让控制器产生大幅振荡，姿态瞬间跟着乱，严重时直接炸机。
- 自动泊车：车辆受非完整约束，不能横移，几何路径上哪怕一点点横向偏差都不可执行。
- 机械臂高速分拣：末端轨迹必须平滑，jerk 过大会加剧机械磨损和振动，几何折线根本不能直接下发给轨迹插补器。
这些问题共同指向一个需求：在规划阶段就同时考虑几何避障和动力学约束。这正是 kinodynamic_rrt_star 存在的意义。
2. 状态树的核心设计：节点、扩展、代价这三样全都得换血
2.1 节点与采样：状态空间随机点长什么样
KRRT* 树上的节点是状态向量，例如二维双积分模型里每个节点是四维：
x = (x, y, vx, vy)采样时不能再只采位置，必须连速度一起采。常见做法是在位置范围内均匀采样坐标，速度在某个区间内均匀采样，同时以一定概率把速度设为零，模拟启停场景。
这里有个新手很容易踩的坑：只采样位置，然后默认速度为零或沿某个固定方向，这样得到的采样状态分布严重偏离真实可达状态，树会浪费大量扩展在不可达区域。我的经验是速度分量的采样范围要和动力学模型匹配，比如最大速度是 v_max，那么速度方向的采样可以按球面分布、幅值在 [0, v_max] 上均匀随机，保证采出的状态理论上可达。
另外一个常用的收敛加速技巧是终点偏置：以一定概率（比如 0.1 到 0.3）直接在目标状态附近采样，或者直接采样目标状态本身上。在状态空间里，终点偏置强烈影响首解速度，因为随机控制序列要凑出一个恰好落到目标状态的点的概率实在太低。
2.2 边的生成：两条技术路线与我的选择
树中的边在几何 RRT* 里是直线段，在 KRRT* 里必须变成满足动力学方程的轨迹。工程上有两条主流路线：
前向积分扩展：从当前节点出发，随机采样一个控制输入 u，以固定步长 dt 对 x_dot = f(x,u) 做数值积分，持续一段时间 T，得到一条轨迹。优点是实现简单，只要动力学能正推就行，不要求系统反向可控。缺点是扩展方向完全随机，跟目标状态没有关联，树长得又密又乱。
BVP（两点边值问题）扩展：随机采样一个目标状态 y，然后求解从当前节点 x_near 到 y 的可行轨迹，这条轨迹满足动力学方程和边界条件。优点是扩展方向明确，可以直接连到采样点。缺点是系统必须可控，且复杂非线性模型不一定有闭式解，求解成本可能很高。
我实际项目里用的是混合策略：树的主体用前向积分加运动基元库生成候选边，末端接近目标时再用 BVP 精确连接。如果你只想快速跑通一个最简单的版本，我强烈建议先用前向积分，因为 BVP 的调试成本高得惊人，稍后我会专门讲 BVP 的坑。
两种方式的对比如下：
| 特点 | 前向积分扩展 | BVP 扩展 |
|---|---|---|
| 实现难度 | 低，一个积分器即可 | 高，需要求解最优控制问题 |
| 目标定向性 | 无，随机扩展 | 强，直接连向采样目标 |
| 计算耗时 | 中等，需要采样大量控制序列 | 通常更高，尤其无法闭式求解时 |
| 适用模型 | 任意动力学模型 | 要求系统可控，至少能数值求解 BVP |
| 调试难度 | 低 | 高，容易遇到无解、数值不稳定 |
前向积分的伪代码长得这样：
for i in range(N): u = sample_control() xn = forward_integrate(x_near, u, dt, T) if is_state_valid(xn) and is_traj_collision_free(x_near, xn): add_edge(x_near, xn, cost=traj_cost(x_near, xn))如果你要定向扩展，就变成：
y = sample_state() traj = solve_bvp(x_near, y) if traj is not None and is_traj_collision_free(traj): add_edge(x_near, y, cost=traj.cost())2.3 代价函数改版：时间、能量还是急动度
几何 RRT* 的边代价是欧氏距离，但状态树里的边代价一般是积分泛函加时间项。选择不同代价函数，最终轨迹形态完全不同：
- 时间最优：代价 = T，轨迹会尽量用满加速度，接近 bang-bang 控制，速度快但很“冲”。
- 控制能量最优：代价 = ∫ ||u||^2 dt，轨迹平滑，但可能偏慢。
- 最小 jerk / minimum snap：代价 = ∫ ||q^(3)||^2 dt，轨迹非常平滑，常用于无人机和机械臂，但计算稍重。
从 RRT* 框架的角度看，这些代价都是正定可加的，都能被树的优化过程吸收，但会影响收敛速度。我的建议是先在简单模型上用时间或控制能量做调通，等树结构稳定了再换高阶导数代价。因为高阶导数代价下，BVP 求解和碰撞检查的数值敏感性明显更高，一步到位容易把自己劝退。
3. BVP 的闭式解实现：一条多项式轨迹是怎么算出来的
3.1 最小 jerk 轨迹的系数求解
BVP 的核心是把两个状态之间的连接转化成一个最优控制问题。最经典、应用最广的模型是以位置、速度、加速度为状态，以 jerk 为输入的模型，也就是大家常说的 minimum jerk 轨迹。一大批无人机、机械臂轨迹生成任务都能用这套框架近似。
问题的数学形式是：给定一维边界条件
p(0) = p0, p_dot(0) = v0, p_ddot(0) = a0 p(T) = p1, p_dot(T) = v1, p_ddot(T) = a1把轨迹表示成五阶多项式：
p(t) = c0 + c1*t + c2*t^2 + c3*t^3 + c4*t^4 + c5*t^5系数有六个，边界条件有六个，方程闭合。用矩阵求解很容易：
import numpy as np def min_jerk_coeffs(p0, v0, a0, p1, v1, a1, T): A = np.array([ [1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 2, 0, 0, 0], [1, T, T**2, T**3, T**4, T**5], [0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4], [0, 0, 2, 6*T, 12*T**2, 20*T**3] ]) b = np.array([p0, v0, a0, p1, v1, a1]) c = np.linalg.solve(A, b) return c高维情况就每个维度各算一组系数，例如二维轨迹就是 p_x(t) 和 p_y(t) 分别求解，但两个维度的 T 必须一致。这里的 T 是整个轨迹的总时长，而不是某个采样间隔，很多刚上手的人容易把这里的 T 跟积分步长 dt 混淆。
之所以选五阶而不是更低阶，是因为五个多项式系数根本不够同时满足位置、速度、加速度六个边界条件；如果你还想指定首末 jerk，那就得用七阶多项式。阶数越高轨迹越平滑，但数值敏感性也越高，系数矩阵的条件数会变大，高 T 时尤其明显。
3.2 时间 T：一个容易被忽略的隐藏优化变量
BVP 里最容易被忽略的就是时间 T。很多人固定 T = 1 算多项式，然后直接拿去用，结果出现加速度超限或者轨迹看起来“怪怪的”。
原因在于 T 对轨迹形态的影响是全局性的。同样的边界条件，T 越小，速度曲线必须越陡，加速度和 jerk 就越大；T 越大，轨迹越“松”、越慢，但可能占用太多时间不满足任务要求。
工程上我常用二分搜索策略：
- 用几何路径长度除以最大速度得到一个粗糙下限 T_lower。
- 给一个上限 T_upper = T_lower * 4 或 5。
- 对 T 做二分，检查当前 T 对应的多项式轨迹是否满足速度、加速度约束和碰撞约束。
- 找到一个可行 T 后，再乘一个 1.2 到 1.5 的安全系数输出。
这样比直接用数值优化器求解最优 T 要稳得多。底层控制器对 T 的小幅变化通常不敏感，但对碰撞检查结果和约束满足度却非常敏感，所以留裕量是划算的。
3.3 碰撞检查的时间分辨率问题
轨迹是连续时间函数，但碰撞检查必然落在离散采样点上。一个常见 bug 是均匀按时间采样，结果在高速段“一步跨过”了一个细小的障碍物，轨迹看起来通过但实际已经穿模。
正确做法是按空间步长自适应采样：每隔大约机器人半径的五分之一到十分之一取一个点，轨迹经过的区域都用保守包围球或包围盒连接做扫掠体检查。另一个技巧是提前把障碍物烘焙成欧氏距离场，这样每个采样点只需查一次距离场查表，比逐点跟障碍物做空间求交快一个数量级。
如果检查到碰撞，不一定要扔掉这条轨迹。可以先试试增大 T，拉长轨迹、降低速度和曲率，往往就能绕开碰撞；实在不行才重新采样。
4. 最近邻与重连机制：几何距离在这个算法里是个大坑
4.1 为什么欧氏距离会毁掉整棵树
经典 RRT* 用欧氏距离做最近邻选择是有前提的：在无障碍环境下，欧氏距离等于真实代价（路径长度）。但一进入状态空间，这个前提立刻崩塌。
举个直观反例：两个状态的位置完全相同，但速度方向完全相反，例如 x1 = (0, 0, 5, 0)，x2 = (0, 0, -5, 0)，它们的欧氏距离是零，但要从 x1 开到 x2，必须先减速、转向再反向加速，代价非常巨大。如果用欧氏距离找最近邻，树会频繁选中这种位置相近但速度状态天差地别的节点，扩展出来的轨迹根本没法衔接，整个树就像在原地“打摆子”。
更麻烦的是几何 RRT* 的 Rewire 逻辑也依赖于“近邻节点的代价近似等于到达代价”这个假设。在动力学场景下这个假设失效，随便重连反而会把代价变得更差，这也是很多人把 RRT* 直接改成 kinodynamic 后效果还不如普通 RRT 的原因。
4.2 工程上怎么近似“动力学距离”
既然精确动力学距离太贵，我们能做的是用近似或调用真实代价计算。我实测下来最稳定的方案有两种：
方案一是加权伪度量。如果模型近似双积分、代价是控制能量，两个状态之间的可达代价可以用可控性 Gramian 之类的二次型做近似，表达式大致是 (Δx)^T W (Δx) 的形式，W 里包含位置差和速度差的相对权重。这个公式不需要精确解 BVP，算得很快，可以用来做粗筛。
方案二是 K 候选加真实代价验证。先用普通欧氏距离取 K 个最近节点，K 通常在 16 到 64 之间，然后对每个候选调用 BVP 或前向扩展试算真实轨迹和代价，选择真实代价最小的作为父节点。这个思路对任意动力学模型都成立，代码改动也小，我对它评价很高。
重连时的逻辑大致是这样：
candidates = k_nearest(tree, x_new, K, metric=euc_dist) best_parent = None best_cost = inf for cand in candidates: traj = connect(cand, x_new) if traj is not None and is_traj_collision_free(traj): cand_cost = cand.cost + traj.cost() if cand_cost < best_cost: best_cost = cand_cost best_parent = cand这个“先粗筛再精算”的模式，本质上是用少量 BVP 调用换取了真正的动力学代价信息，成本可控，精度大幅提升。
4.3 重连半径的工程取舍
理论上的 RRT* 重连半径和状态维度 d 有关：
r_n = gamma * (log n / n)^(1/d)问题在于状态维度一旦到 6、8 甚至 10，这个半径衰减得非常快，需要极其庞大的节点数量才能触发有效的重连。这意味着严格按理论半径在工程上是不可行的。
我的做法是固定 K 邻域重连，重连前加一个代价下界剪枝：如果候选节点的当前代价加上一个保守的 cost-to-go 下界已经大于新节点的代价，就直接跳过，不调用 BVP。这样既保留了 RRT* 的优化潜力，又避免在无望的候选上浪费计算。
如果你想做偏研究性质的严格最优性实验，可以周期性对全局做一次大规模重连作为“精修阶段”，代价是耗时激增，但能明显看到轨迹总代价稳步下降。
5. 落地笔记：从仿真到真机，我踩过的几个坑和调参基线
5.1 数据结构：KD 树距离度量与维度
KD 树只有在欧氏几何距离下才是高效的数据结构，但 KRRT* 里我们常用的是自定义非欧距离。硬把自定义距离塞进 KD 树会得到完全错误的结果，因为 KD 树的划分依赖坐标轴的单调性，非欧距离不满足这个性质。
如果你的状态维度不高，比如 4 到 8 维，可以用覆盖树或者度量树来存自定义距离；维度再高的话，直接线性扫描加向量化比任何空间树都快。我通常在工程里用两层方案：第一层用 KD 树按位置分量粗筛出两倍左右候选，第二层用自定义距离精排，再把前 K 个候选交给 BVP 或前向积分做真实代价验证。
5.2 初始解太慢的急救方法
KRRT* 最常见的新手抱怨是“跑了半天一个可行解都没有”。这通常是前向积分扩展的随机性太强造成的：随机控制序列要恰好让轨迹伸向目标区域，概率低得可怜。几个急救手段：
一是提高终点偏置概率，从常见的 0.05 提到 0.2 左右，让树定向向目标附近采样。二是先别急着在状态空间裸跑，先用普通几何 RRT* 找一条粗路径，把这条粗路径附近的区域作为高概率采样区，状态树就会优先在管道内扩展，首解速度能提升数倍。三是加“精确到达”机制：当某个节点离目标状态足够近时，调用 BVP 直接尝试连接目标，如果可行就直接返回。
我现在的习惯是先做几何粗搜，再注入状态空间采样，首解时间和纯随机版本相比通常能快一个数量级。
5.3 调参基线：一张可以直接抄的参数表
下面这张表是我在二维双积分和简单无人机模型上反复调出来的基线，可以当作起点，再根据你的动力学模型量纲做缩放。
| 参数 | 推荐范围 | 说明 |
|---|---|---|
| 积分步长 dt | 0.01 ~ 0.05 s | 接近底层控制周期，太大轨迹失真，太小计算量大 |
| 单步控制样本数 N | 20 ~ 100 | 决定每次扩展的宽度，过少树稀疏，过多计算爆炸 |
| K 候选数 | 16 ~ 64 | 做最近邻候选和重连候选，看你单次 BVP 求解开销 |
| 终点偏置概率 | 0.1 ~ 0.3 | 越大首解越快，但过度偏置会降低随机探索能力 |
| 碰撞采样空间步长 | 机器人半径的 1/5 ~ 1/10 | 保证不穿过细障碍物 |
| T 的搜索上限 | 几何路径长度 / 最大速度 * 4 | 给 T 留足够裕量再二分收缩 |
注意这个表不是万能药。不同动力学模型的量纲相差巨大，比如真实无人机和双积分仿真里“最大加速度”可能差两个数量级，参数必须重新标定。我的做法是先在一个二维双积分模型上调到性能可接受，再移植到真实模型，这样能快速暴露参数适配问题。
5.4 最小验证 demo 及可视化建议
如果你想验证自己的 KRRT* 实现，我建议别直接上真机，先在二维双积分模型上跑通基础功能。搜索空间是四维状态 x = (x, y, vx, vy)，障碍物画在地图上，设定好最大速度和最大加速度，重点检查三件事：
- 树能否在合理时间内找到初始解；
- Rewire 之后整棵树的总代价是否持续下降；
- 最终轨迹的碰撞点是否真的在采样密度上被覆盖，而不是“看起来没撞”。
可视化时别只画位置曲线，把速度曲线、加速度曲线也画出来，用颜色或箭头表示速度方向。很多问题只有看到速度曲线才能发现，比如轨迹末端速度没有归零、加速度曲线出现尖峰等。把这些检查清楚，再切换成无人机或车辆的真实动力学模型，通常只需要改动力学函数和 BVP 求解器，树的框架不用动。
回过头来看，krtt* 这套算法真正难调的不是随机采样，而是距离度量、重连机制和 BVP 可行性三者之间的耦合。我先在普通 RRT* 上做对比基线，再逐步增加动力学模块，每加一层都重新看轨迹和耗时曲线，这样定位问题会轻松很多。现在我做新机器人项目时，一般会先用双积分模型搭一个 KRRT* 验证需求，再切换到真实动力学，这个流程帮我省掉了很多真机调试时间，也希望这篇文章能让你少走几个坑。
本文还有配套的精品资源，点击获取
