简介：Kinodynamic RRT*的MATLAB实现，面向机器人运动规划、自动驾驶与空中机器人领域的算法研究者及高年级学生，用于解决具有线性微分约束系统的最优运动规划问题，是对Webb与van den Berg论文核心方法的可运行复现。压缩包共17个文件，包含13个.m脚本、2个.mat数据文件、1个README说明文档和1个licence许可文本，整体仅14KB，结构紧凑、便于下载后直接部署和修改。已有1295人学习下载。代码覆盖RRTstar主程序、双积分器与四旋翼动力学模型、状态与输入采样、碰撞检测、障碍物/航点数据加载以及可视化模块，并配套quad与ball两种测试场景，可帮助读者对照论文梳理算法流程，理解最优运动规划中的随机扩展与修剪机制，还可作为课程设计或科研实验的起点。整体源码遵循论文框架，函数粒度清晰，便于二次开发。

1. kinodynamic_rrt_star 是什么：一句话说清楚它的本事和门槛

很多人拿到几何版 RRT* 的代码，以为给状态加两个速度变量、把直线边换成曲线边就能跑动力学约束，结果树长了上万个节点，一条能落地的轨迹都搜不出来。kinodynamic_rrt_star 之所以值得单独学，是因为它把运动学与动力学约束写进了采样、连接和重优化的每一步，而不是在规划完之后再做一次轨迹平滑。它解决的是无人机、差速车、机械臂这类“有质量、有加速度上限”的系统怎么在障碍环境里找到一条物理上可执行的路径。适合已经写过基础 RRT 系列、现在被动力学约束卡住的人。

2. 把问题先建模：状态空间、控制输入和约束的三种入口

2.1 两种常用运动模型的取舍：二阶积分模型还是差速模型

kinodynamic RRT*\u7684第一步不是写搜索，而是定模型。常见做法是先把机器人简化成二选一的模型：二阶积分模型或者差速模型。二阶积分模型的状态一般是s = (x, y, vx, vy)，控制量是加速度u = (ax, ay)，适合多旋翼、AGV 这类可以近似成质点的对象。差速模型的状态是s = (x, y, theta, v, omega)，控制量是线加速度和角加速度，适合轮式底盘，因为它必须维护朝向角 theta，转向半径和角速度上限直接决定轨迹能不能走。

选模型时先问自己三个问题：机器人的最小转弯半径能不能忽略？控制周期内速度变化大不大？规划输出是要给底盘直接执行还是只给参考路径？如果答案是“半径不能忽略”或者“要直接执行”，就别贪便宜用二阶积分模型，否则后面扩展出来的轨迹再怎么圆滑也无法落地。如果只做路径参考，二阶积分模型计算量小、调试快，先跑通再往上加约束也来得及。

模型定了，状态空间的维度也就定了。维度直接决定采样和近邻搜索的复杂度，所以要在建模阶段就把用不到的状态砍掉。例如只做平面规划，就别把 z 轴姿态塞进来；只关心速度上限，就别为加加速度单独加一维状态。状态每多一维，可行采样率往往会掉一个数量级，这是后面所有调试痛苦的根源。

2.2 直接套用几何 RRT* 会踩的三条硬伤

几何 RRT* 在自由空间里连接两个节点用的是直线，代价用欧氏距离，近邻节点按坐标球半径找。这三招在只有运动学约束的问题里成立，一旦介入动力学约束，会逐一失效。第一条硬伤是直线边不满足速度连续性：起点朝北、终点朝东，直线轨迹在拐点处速度方向突变，真实机器人做不到。第二条硬伤是欧氏距离与真实代价偏差太大：绕障碍一段路可能欧氏距离很短，但受加速度限制需要减速、转向，实际时间代价高得多，用欧氏距离排序会误导搜索方向。

第三条硬伤最隐蔽，近邻节点的度量不对。几何 RRT* 里“离得近”就是坐标距离小，但在动力学约束下，两个点坐标接近完全不代表可达代价小：一个高速点和一个静止点虽然位置挨着，但从静止加速到那个速度需要的时间和能量，可能比飞一段远路还贵。近邻集合一旦选错，chooseParent 和 rewiring 都会白做，整棵树的结构退化，代价收敛性直接失效。

我把三条硬伤整理成一张对比表，方便你判断自己手里的几何 RRT* 改造成 kinodynamic 版本要动哪些模块。

2.3 约束进代码：统一验证器里的三个入口

建模完成后，约束不能散落在各个函数里，我一般会收敛到一个独立的验证器里，统一起三个入口：状态可行性、控制可行性和轨迹碰撞。这样后面调参只需要改这一个文件，不用满树找判断条件。状态可行性检查状态是否在边界内，比如速度不能超过 v_max、位置不能越过地图边界；控制可行性检查控制量是否落在加速度上下界内；轨迹碰撞检查离散轨迹点是否与障碍相交。

class KinodynamicValidator: def __init__(self, state_bounds, control_bounds, obstacle_map, dt): self.state_bounds = state_bounds # 每个状态分量的 [min, max] self.control_bounds = control_bounds # 每个控制分量的 [min, max] self.obstacle_map = obstacle_map # 障碍物栅格或几何表达 self.dt = dt # 正向传播的时间步长 def is_state_feasible(self, state): # 只要有一个状态分量越界，就认为该状态不可行 return all(lo <= s <= hi for s, (lo, hi) in zip(state, self.state_bounds)) def is_control_valid(self, control): # 控制量越界直接拒绝，避免扩展出不可行的轨迹段 return all(lo <= u <= hi for u, (lo, hi) in zip(control, self.control_bounds)) def is_traj_collide(self, traj, inflate_radius): # 对轨迹逐点做碰撞检测，膨胀半径留给外部传入 for state in traj: if self.obstacle_map.collides(state[:2], inflate_radius): return True return False

这段代码的逻辑很直白：三个方法对应三条判断链路，任何一个返回 False，这条边就不能进入树。参数说明里最有讲究的是 inflate_radius，它不是机器人半径本身，而是“机器人半径 + 当前时刻速度 × 控制周期”，目的是补偿离散化带来的漏检。state_bounds 里速度下界通常设为负值，因为倒车场景下速度本来就是双向的，不要天真地以为速度分量的下界是 0。

3. 核心实现：采样、正向传播与重连的三段关键代码

3.1 采样函数：在可行状态空间里撒点而不是全图撒点

kinodynamic RRT*\u7684采样函数和几何版本长得像，但多了两个关键差异：第一，采样点必须满足状态可行性，比如速度幅值不能超过 v_max；第二，要保留 goal_bias，但 bias 的语义从“直接返回目标点”变成“返回目标状态附近的一个可达状态”。直接返回精确目标状态往往导致 steer 失败率高，因为从当前节点到精确目标点之间未必存在满足动力学约束的连接，尤其是目标点速度不为零的时候。

import random def sample_state(state_bounds, goal_state, goal_bias, v_max): if random.random() < goal_bias: # 目标偏置：在目标附近采样，避免硬连接 return ( goal_state[0] + random.uniform(-0.5, 0.5), goal_state[1] + random.uniform(-0.5, 0.5), random.uniform(-v_max, v_max), random.uniform(-v_max, v_max) ) while True: state = tuple(random.uniform(lo, hi) for lo, hi in state_bounds) # 单独过滤速度幅值，防止采样出“位置合法但速度过大”的状态 if state[2] ** 2 + state[3] ** 2 <= v_max ** 2: return state

这个采样函数里，goal_bias 的经验值在 0.05 到 0.1 之间。太小会导致树漫无目的地长，太大则会让搜索过度集中在目标附近，窄通道场景容易钻牛角尖。while True 循环看起来浪费，实际开销很小，因为绝大多数状态都在速度圆内，只要 state_bounds 不是设得离谱，一两轮就能采出合法点。真正要小心的是把 state_bounds 的速度范围设成和 v_max 不一致，例如 bounds 里写的速度上限是 3.0，而 v_max 传的是 2.0，那 while 循环就会频繁重采，白白烧 CPU。

3.2 steer 函数：用控制序列正向传播，把不可行边提前滤掉

steer 是整个 kinodynamic RRT*\u7684心脏。论文原版会求解一个两点边界值的最优控制问题，但工程实现里更常见、也更容易调试的是正向传播：从当前状态出发，随机生成一组控制序列，按固定时间步 dt 逐步积分，得到一个候选轨迹，然后从多个候选里挑代价最小的。这个做法不需要解析解，换模型时只要改积分函数，其他逻辑不用动。

def integrate(state, control, dt): # 二阶积分模型：位置按速度更新，速度按控制量更新 x, y, vx, vy = state ax, ay = control new_x = x + vx * dt new_y = y + vy * dt new_vx = vx + ax * dt new_vy = vy + ay * dt return (new_x, new_y, new_vx, new_vy) def steer(start_state, sample_state, dt, control_bounds, max_steps, k_trials): best_traj, best_cost = None, float("inf") for _ in range(k_trials): # 随机生成控制序列，每个控制分量都在上下界内 control_seq = [ ( random.uniform(*control_bounds[0]), random.uniform(*control_bounds[1]) ) for _ in range(max_steps) ] traj = [start_state] cur = start_state for u in control_seq: nxt = integrate(cur, u, dt) traj.append(nxt) cur = nxt cost = len(traj) * dt # 用时间代价评估，可以换成控制能量 if cost < best_cost: best_cost = cost best_traj = traj return best_traj, best_cost

代码里有三个参数决定了这个函数的行为。dt 是正向传播的时间步长，设得大会让轨迹糙、碰撞检测容易漏，设得小会让单次扩展的计算量线性上涨，经验值是控制周期的四分之一到二分之一。max_steps 是单次扩展的最大步数，决定了边的最长时长，地图大就调大，但不要超过 50，否则一次 steer 的循环体太大。k_trials 是随机试验次数，10 到 30 之间比较平衡，再往上加对轨迹质量的提升会进入平台期。

这里有一个容易翻车的点：control_bounds 的形状要和模型匹配。二阶积分模型是一组二维控制量，所以 control_bounds 传((min_ax, max_ax), (min_ay, max_ay))；差速模型是线加速度和角加速度，就得传四元组。很多人在这一步不统一，导致 steer 里 zip 报错或者控制量取值完全错误，排查半天发现是数据结构对不上。

3.3 邻域重连：动态代价下 RRT* 的 chooseParent 与 rewiring

近邻搜索的动态化是 kinodynamic 和几何 RRT* 在结构上最本质的区别。几何版拿一个固定半径搜出候选节点，然后算欧氏距离；kinodynamic 版必须用“从候选节点到新节点的动态代价估计”来筛，否则重连后代价不降反升。我一般用一个保守的可达性估计：候选节点的累计代价加上当前节点到它的直线时间下界，如果这个值已经大于新节点的累计代价，就直接跳过。

def near_nodes(tree, state, cost_threshold): candidates = [] for node in tree: # 用位置距离除以最大速度作为时间下界，不够精确但足够做初步筛 dx = state[0] - node.state[0] dy = state[1] - node.state[1] lower_bound = (dx * dx + dy * dy) ** 0.5 / v_max if node.cost + lower_bound <= cost_threshold: candidates.append(node) return candidates def choose_parent(candidates, new_state, dt, control_bounds, max_steps, k_trials): best_node, best_traj, best_cost = None, None, float("inf") for node in candidates: traj, cost = steer(node.state, new_state, dt, control_bounds, max_steps, k_trials) total_cost = node.cost + cost if total_cost < best_cost: best_cost = total_cost best_node = node best_traj = traj return best_node, best_traj, best_cost

choose_parent 的逻辑和几何 RRT* 一样，是“试连所有候选，选累计代价最小者”，但代价计算换成 steer 返回的动态代价。算法到这里，已经有完整搜索能力了。注意 rewiring 的时机：要先对新节点做完 chooseParent 并插入树之后，再拿它去重连周围节点，顺序反了会破坏树的代价结构。我见过不少实现把这两步写在同一层循环里，导致插入后的新节点还没参与重连，周围的旧节点就白重连了。

4. kinodynamic RRT* 避坑记录：5 个高频翻车点的现象、原因与改法

4.1 扩展老是返回空轨迹

现象：树一直在长，但 steer 返回 None，新节点始终插不进去，迭代几千次树上还是只有起点附近那几个节点。原因多数出在约束的“不可达组合”上，最常见的有两种：一是 max_steps 设得太小，从当前状态到采样点之间的动力学距离大于单次扩展的能力上限；二是采样状态里速度项方向与当前位置到目标位置的方向冲突太大，比如目标状态要求向左飞，而当前位置的速度是向右的，短时间根本掰不回来。

解决：先给 steer 函数加日志，打印每一次试验失败的原因，是状态越界还是终点超出最大步数。如果是步数问题，把 max_steps 从 20 提到 50；如果是速度方向冲突，放松采样函数，不要在速度维度上撒太开，让采样速度尽量沿着当前位置到目标方向。还有一个技巧是把 steer 的 k_trials 临时调到 100，看是否只是随机性太强导致搜不到。如果你发现 k_trials=100 能找到解而 20 找不到，说明控制序列采样太稀疏，应该增加的是 k_trials 而不是盲目调模型参数。

4.2 rewiring 之后代价反而变大

现象：同一棵树，跑完 rewiring，部分节点的累计代价比重连前还高，整棵树的 cost 曲线出现抖动。原因几乎总是近邻度量和代价度量不一致：near_nodes 用位置距离筛人，choose_parent 用动态代价选边，结果选出来的“邻居”在动力学上根本不在可达范围内，steer 只能返回一条绕远路的高代价轨迹。这种实现等于在往树上强加不可行近邻，越重连越差。

解决：把 near_nodes 里的 cost_threshold 从几何距离改成时间上界，也就是“位置距离 / 最大速度 × 安全系数”。安全系数取 1.2 到 1.5，留出加减速的余量。改完之后每一步重连都要验证一条不变量：新节点插入后的 cost 必须小于等于连接前的候选值，不满足就放弃这次插入。这个断言加上去，cost 抖动问题基本消失。

4.3 高速段碰撞检测漏检

现象：规划出来的轨迹在仿真里看离障碍物还有一段距离，但真机或者高精度仿真总是撞上。原因是离散碰撞检测只检查了轨迹点本身，两个采样点之间轨迹扫过的区域被忽略了。速度越快，相邻两个状态点之间的位移越大，漏检区域就越宽。几何 RRT* 里这个问题不明显，因为直线段短、膨胀小；kinodynamic 的边是长轨迹，一秒钟可以跑出好几米，逐点检查根本不够。

解决：把碰撞检测从逐点改成逐段，每段的膨胀半径等于机器人半径加上该段两端最大位移的一半。这个补偿量不是拍脑袋，它的物理含义是：在这段位移里，机器人可能出现在线段附近任意位置，只能靠膨胀覆盖。还有一个更省算力的办法是先在采样阶段把轨迹按速度做自适应细分，速度超过阈值就把 dt 减半再检查一次。

4.4 高维状态空间里近邻搜索变成性能黑洞

现象：状态维度一上 5，树长到一两万个节点之后，单次迭代时间从毫秒级涨到几十毫秒，越跑越慢。很多人在几何版里用 KD-tree 习惯了，直接照搬到 kinodynamic 版本，结果 KD-tree 在非欧氏度量下分割失效，查询结果和暴力搜索一样，却多了建树开销。根因是动态代价度量不满足坐标轴对齐分割的条件，KD-tree 的高效性建立在欧氏距离上。

解决：先别急着上高级数据结构，把 near_nodes 的线性扫描保持住，但缩小扫描范围。常见做法是先用节点列表按 x 坐标排序做一次低维过滤，只处理 x 距离在阈值内的节点，再对这些节点做完整的动态代价估计。这个两级过滤在实际问题里能把候选节点数量砍掉八成，代码量不到十行。如果仍然不够，就把状态降维，把速度项从 near 度量里拿掉，只用位置维度建 KD-tree，查询后再补动力学过滤。

4.5 地图确实有通路，规划器却总是超时

现象：肉眼可见地图中间有条通道，RRT*\u5374在通道口反复绕圈，迭代上限到了还是返回失败。原因通常是 goal_bias 太大或太小。太大时树被目标点吸引，不断尝试从当前位置直连目标，但直连轨迹被通道口障碍挡住，所有 steer 都失败，树根本不在通道内部扩展；太小时树漫无目的，收敛极慢。另一个原因是 terminate 条件太苛刻，要求新节点必须是目标状态的精确邻域，而窄通道出口处很难凑巧采到那个状态。

解决：把 goal_bias 调到 0.05 到 0.08，同时把采样函数改成“目标环形采样”，只在目标状态周围一个环形区域内采样，避免集中在一个精确点。再把终止条件从“新节点接近目标”改成“树中存在一个节点与目标的动态代价小于某阈值”，这个阈值可以按最大控制能力估算，给一个宽松的上界。这两个改动合在一起，超时问题基本能解决。

5. 搭一个能跑通的最小工程：数据结构、参数清单与验收顺序

5.1 节点结构与主循环：一段干净的最小 Python 骨架

前面几章的代码都是零散函数，现在把它们组装成一个最小工程。节点结构是第一个要定清楚的：一个节点除了状态和父指针，还应该存累计代价、到达它的控制序列或轨迹、以及时间戳 t。累计代价用于 chooseParent 的比较，轨迹用于碰撞检测和可视化，时间戳用于判断两条轨迹在时间上是否冲突，后续做多机规划时也用得到。

from dataclasses import dataclass, field from typing import Optional, Tuple, List @dataclass class Node: state: Tuple[float, ...] t: float cost: float = float("inf") parent: Optional["Node"] = None traj: List[Tuple[float, ...]] = field(default_factory=list) control_seq: Optional[List[Tuple[float, ...]]] = None def kinodynamic_rrt_star( start_state, goal_state, validator, dt, max_iterations, goal_bias, max_steps, k_trials ): tree = [Node(start_state, t=0.0, cost=0.0, traj=[start_state])] for _ in range(max_iterations): s_rand = sample_state(validator.state_bounds, goal_state, goal_bias, v_max) candidates = near_nodes(tree, s_rand, cost_threshold=30.0) if not candidates: continue best_node, best_traj, cost = choose_parent( candidates, s_rand, dt, validator.control_bounds, max_steps, k_trials ) if best_node is None or validator.is_traj_collide(best_traj, inflate_radius): continue new_node = Node( state=s_rand, t=best_node.t + len(best_traj) * dt, cost=best_node.cost + cost, parent=best_node, traj=best_traj ) tree.append(new_node) # 判断是否到达目标区域 if cost_estimate(new_node.state, goal_state) < goal_threshold: return extract_path(new_node) return None

主循环把前面几章的模块串在一起，逻辑上已经没有新东西了，重点是参数之间的关系。cost_threshold 是 near_nodes 的可达性门槛，它决定每次扩展考察多少个候选，设小了每次只有一两个候选，近似退化成长 RRT；设大了每步都要跑几十次 steer，性能负担重。inflate_radius 在碰撞检测里被硬化，前面说过要按速度自适应。这两个参数是除了控制边界之外最值得花时间调的东西，而且它们互相耦合，调的时候最好固定一个、扫另一个，不要同时动。

5.2 参数清单与经验初始值：先照抄再按任务调

这套初始值适用于仿真里 10 米见方的地图、最高速度 2 米每秒的质点模型。每一行都对应一个可观察的现象，而不是拍脑袋：dt 太小你会发现单次 steer 要跑几十毫秒，太大则碰撞漏检；max_steps 设成 30 配合 dt=0.05，单次扩展最多覆盖 1.5 秒轨迹，足够穿过大多数地图块。如果你跑的是差速车模型，还需要把角速度上限折算成 cost_threshold 的缩放系数，因为同样时间下，角速度受限的车走不了那么远。

5.3 验收顺序：先看通，再看代价，最后看方差

我刚写完一版 kinodynamic RRT*\u65f6犯过一个错：直接上复杂地图，调了一晚上参数也没跑通，因为是“能通”和“通得好”混在一起排查。后来改成四步验收，问题定位快了很多。第一步，无障碍物直线场景，验证采样、steer、重连三段代码是否完整跑通，此时只看一条路径能不能出来。第二步，加一个圆柱障碍，验证碰撞检测和重连是否正常，此时看路径是否能绕障。第三步，加窄通道，验证 goal_bias 和 near 度量是否合理，此时看路径是否穿过通道而不是绕远路。第四步，固定随机种子，连续跑 10 次，统计成功率、平均代价和方差。

第四步往往能暴露真正的问题：如果三次成功、七次失败，问题大概率出在随机采样阶段，而不是 steer；如果十次都成功但代价方差很大，问题出在重连的候选集合不够稳定。我习惯用固定种子列表跑对比实验，每次只改一个参数，把每个参数的效果单独记录下来。这个方法听起来土，但在高维状态空间里，比靠肉眼观察树形图靠谱得多。

6. 从“能用”到“好用”：三个升级技巧与一套统计验证习惯

6.1 双向搜索、延迟碰撞检测与原语库：三个便宜且有效的升级

第一个升级是双向搜索。几何 RRT*\u7684双向版本实现起来要小心，kinodynamic 版本收益更明显，因为动力学约束会让搜索从单侧出发很慢，两侧同时长树能互相引导。常见做法是维护两棵树，每隔固定迭代次数交换采样目标：A 树向 B 树最近的节点扩展，B 树向 A 树最近的节点扩展。第二个升级是延迟碰撞检测，把 is_traj_collide 从插入前挪到路径提取时统一做。它的思想是先假设边可行，快速长出一棵无视障碍的树，找到一条低代价路径后，再对这条路径做完整碰撞检测，如果碰撞就剔除相关节点重新搜索。第三个升级最实用：把 steer 里的随机控制序列换成预生成的原语库。离线随机生成 100 条控制序列存起来，在线每次从库里挑，减少在线计算量，还让轨迹质量稳定，不再依赖每次运行时的随机数种子。

6.2 把“跑 50 次”当成习惯：成功率比单条路径更能说明问题

import random from statistics import mean, variance def evaluate_planner(run_planner, scenario, seeds, max_time): success_times, costs = [], [] for seed in seeds: random.seed(seed) path, cost, elapsed = run_planner(scenario, max_time) if path is not None: success_times.append(elapsed) costs.append(cost) success_rate = len(costs) / len(seeds) print("成功率: %.2f%%" % (100 * success_rate)) if costs: print("平均代价:", mean(costs), "代价方差:", variance(costs)) print("平均规划时长:", mean(success_times))

这段统计代码把“能通”和“能用”区别开来。我吃过亏：当时只跑三次实验，三次都成功就宣布调参完成，结果在真实任务里成功率只有六成。后来改成默认跑 50 次，看成功率和代价方差的分布，很多隐藏问题才暴露出来。成功率过低先查采样和 goal_bias，代价方差过大多半在重连环节。调参到这个阶段，算法本身的坑基本踩完了，剩下的就是坚持把这套统计习惯做进每次实验里。希望帮到你。

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