简介这套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, costtraj_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, costtraj.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, metriceuc_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 调参基线一张可以直接抄的参数表下面这张表是我在二维双积分和简单无人机模型上反复调出来的基线可以当作起点再根据你的动力学模型量纲做缩放。参数推荐范围说明积分步长 dt0.01 ~ 0.05 s接近底层控制周期太大轨迹失真太小计算量大单步控制样本数 N20 ~ 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* 验证需求再切换到真实动力学这个流程帮我省掉了很多真机调试时间也希望这篇文章能让你少走几个坑。本文还有配套的精品资源点击获取