简介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 版本要动哪些模块。对比维度几何 RRT*kinodynamic RRT*节点含义位姿点状态包含位置与速度/角速度边的含义直线段动力学可行的轨迹段代价欧氏距离时间、控制能量或混合代价近邻度量坐标球半径可达性估计或动态代价上界约束入口无采样、扩展、碰撞三处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_costchoose_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_trials100 能找到解而 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_factorylist) 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, t0.0, cost0.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_threshold30.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( states_rand, tbest_node.t len(best_traj) * dt, costbest_node.cost cost, parentbest_node, trajbest_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 参数清单与经验初始值先照抄再按任务调参数含义经验初始值dt正向传播时间步长0.05 秒max_steps单次扩展最大步数30k_trialssteer 随机试验次数20goal_bias目标偏置概率0.07max_iterations最大迭代次数10000cost_threshold近邻候选的时间上界5.0 秒inflate_radius碰撞检测膨胀半径0.2 米起步这套初始值适用于仿真里 10 米见方的地图、最高速度 2 米每秒的质点模型。每一行都对应一个可观察的现象而不是拍脑袋dt 太小你会发现单次 steer 要跑几十毫秒太大则碰撞漏检max_steps 设成 30 配合 dt0.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代价方差过大多半在重连环节。调参到这个阶段算法本身的坑基本踩完了剩下的就是坚持把这套统计习惯做进每次实验里。希望帮到你。本文还有配套的精品资源点击获取