简介这份资源是论文《Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints》的 MATLAB 实现代码面向从事机器人运动规划、最优控制与轨迹优化方向的研究生、科研人员及工程师用于在带线性微分约束的系统上复现并验证 Kinodynamic RRT* 算法。压缩包共 17 个文件约 14KB以 13 个 .m 脚本为核心辅以 2 个 .mat 数据文件、1 个 readme.md 说明与 1 个 licence.txt 授权文件脚本覆盖 rrtstar 主流程、double_integrator 与 rrt_double_int_2d 动力学建模、quad 四旋翼采样与绘图、is_positive_definite 等数值校验模块数据文件则保存障碍物与路点信息。目前已有 1294 人学习下载。读者可据此理解算法从状态采样、输入采样到最优连接与代价评估的完整链路并借助四旋翼与双积分器两个示例快速搭建仿真环境、复现论文实验为自身课题的轨迹规划与控制器设计提供可复用的代码骨架与排错参考。1. 从一份 MATLAB 版 kinodynamic_rrt_star 说起它到底能跑通什么如果你做过机械臂或者移动底盘的路径规划大概率经历过这种场面RRT 跑出来的路径折线感极强拿去给控制器执行时速度、加速度直接爆表电机啸叫、底盘打滑最后只能人工再修一遍。问题不在 RRT 本身而在于它只考虑了位置空间没把动力学约束当回事。kinodynamic_rrt_star 就是冲着这个痛点来的——它在采样的同时把状态导数速度、加速度纳入扩展过程让规划出来的轨迹天然满足运动学与动力学限制并且带上了 RRT* 的渐近最优特性。这份 MATLAB 实现把算法核心、碰撞检测、代价计算和可视化都打包好了适合做移动机器人、无人机、机械臂轨迹规划的从业者直接拿来改参数、换模型、跑自己的场景。它不追求工程级实时性但胜在结构清晰、每一行都能读懂是理解 kinodynamic 规划从公式到代码落地的一条捷径。2. kinodynamic_rrt_star 的算法骨架状态空间、扩展与重连2.1 为什么不能直接在位置空间采样普通 RRT 的采样点是一个二维或三维坐标扩展时用直线连接碰撞检测也只查这条直线。但真实系统里机器人从 A 到 B 不是瞬移它得加速、巡航、减速中间还受最大速度和最大加速度约束。kinodynamic 规划把采样空间从位置空间提升到状态空间一个状态通常写成[x, y, theta, v, omega]这样的向量前几维是位形后几维是速度或角速度。扩展时不再用直线而是对控制输入采样比如线加速度和角加速度然后通过积分前向模拟出一段轨迹。这样生成的每一段都天然满足动力学方程代价函数也可以直接定义成时间、能量或者 jerk 的积分。常见做法是固定积分步长dt对控制量在允许范围内均匀采样若干组每组积分一步得到一个候选新状态。这个过程的计算量比普通 RRT 大不少因为每扩展一次都要做一次数值积分但换来的是轨迹可执行性。MATLAB 里用ode45或者简单的欧拉积分都能做欧拉积分更快精度对规划来说通常够用。2.2 扩展、选择父节点与重连的三段式流程kinodynamic_rrt_star 的主循环和 RRT* 一样分三步采样、找最近节点、扩展并尝试重连。区别在于“最近”不再用欧氏距离而是用一个考虑状态差异的度量常见的是位置差加权加上速度差加权。扩展时从最近节点出发对控制采样积分出若干候选轨迹选一条无碰撞且代价最小的作为新节点。新节点加入后要在邻域内找一批已有节点看看把新节点接到它们后面是不是代价更低这就是选择父节点。接着反向做重连看看邻域内已有节点的父节点能不能换成新节点从而降低整棵树的代价。这两步是 RRT* 渐近最优的关键也是计算开销最大的地方。MATLAB 实现里通常用knnsearch或者手写循环找邻域邻域半径r一般按节点数动态调整比如r min(gamma * (log(n)/n)^(1/d), eta)其中d是状态空间维度gamma和eta是经验参数。下面是一段简化的扩展与重连核心逻辑用 MATLAB 写方便你对照自己的实现改% 状态: [x, y, theta, v, omega] % 控制: [a, alpha] 线加速度和角加速度 function [newNode, newCost] extend(tree, qNear, qRand, params) bestNode []; bestCost inf; % 对控制采样 for a params.aRange for alpha params.alphaRange qNew integrate(qNear.state, [a, alpha], params.dt); if ~checkCollision(qNear.state, qNew, params.map) cost qNear.cost computeCost(qNear.state, qNew, [a, alpha]); if cost bestCost bestCost cost; bestNode struct(state, qNew, cost, cost, ... parent, qNear.id, control, [a, alpha]); end end end end newNode bestNode; newCost bestCost; end % 重连: 尝试用新节点作为邻域内节点的父节点 function tree rewire(tree, newNode, params) neighbors findNeighbors(tree, newNode, params.rewireRadius); for i 1:length(neighbors) nb neighbors(i); % 从新节点到邻居的代价 costViaNew newNode.cost computeCost(newNode.state, tree.nodes(nb).state, []); if costViaNew tree.nodes(nb).cost ... ~checkCollision(newNode.state, tree.nodes(nb).state, params.map) tree.nodes(nb).parent newNode.id; tree.nodes(nb).cost costViaNew; end end end这段代码里integrate负责数值积分checkCollision做碰撞检测computeCost计算边代价。参数aRange和alphaRange是控制采样范围通常取最大加速度的若干等分点比如-amax:amax/2:amax。dt是积分步长太小会导致树增长慢太大则轨迹粗糙、碰撞检测漏检。rewireRadius是重连邻域半径太小重连效果弱太大计算量爆炸。我一般会先把dt设成 0.1 秒控制采样每维 5 个点跑通后再根据场景调。2.3 代价函数与启发式时间、能量还是 jerk代价函数直接决定规划出来的轨迹长什么样。最常见的是时间代价即每段轨迹的dt累加这样算法倾向于找最短时间路径。能量代价则对加速度平方积分轨迹会更平滑但可能绕远。还有 jerk 代价对加速度变化率积分适合对舒适性要求高的场景。MATLAB 实现里通常把代价函数单独写成一个函数句柄方便替换。启发式方面kinodynamic 规划很难像 A* 那样给出精确的启发值因为动力学约束下从当前状态到目标的最优代价不容易估计。常见做法是用欧氏距离除以最大速度作为下界或者干脆不用启发式退化成纯 RRT* 的均匀采样。如果目标点已知可以在采样时以一定概率直接采目标状态加速收敛。这个概率一般设 0.05 到 0.1太高会导致树偏向目标、探索不足太低则收敛慢。3. 在 MATLAB 里跑通第一个 kinodynamic 规划参数、积分与可视化3.1 环境准备与文件结构这份 MATLAB 实现通常包含几个核心文件主脚本main.m、树结构管理tree.m、扩展与重连extend.m和rewire.m、碰撞检测checkCollision.m、代价函数computeCost.m、积分器integrate.m以及可视化plotTree.m。不需要额外工具箱基础 MATLAB 就能跑但如果用knnsearch找邻域需要 Statistics and Machine Learning Toolbox没有的话手写循环也行。跑之前先确认 MATLAB 版本R2016b 以上都支持脚本内定义函数。把文件夹加到路径直接运行main.m。第一次跑建议把地图设简单点比如一个 10x10 的矩形区域几个圆形障碍物起点和终点手动指定。状态维度先用[x, y, theta, v]去掉角速度降低复杂度。3.2 关键参数怎么设dt、控制采样数、邻域半径参数设置是 kinodynamic 规划最玄学的地方同一套代码换个场景就得重调。下面这张表是我踩坑后总结的常用范围你可以直接抄参数含义常用范围影响dt积分步长0.05 ~ 0.2 s太小树增长慢太大轨迹粗糙aRange线加速度采样-amax:amax/2:amax采样点越多越可能找到可行轨迹计算量线性增长alphaRange角加速度采样-alphamax:alphamax/2:alphamax同上移动机器人可设小一些rewireRadius重连邻域半径1.0 ~ 3.0 m太小重连弱太大计算爆炸goalBias目标采样概率0.05 ~ 0.1太高探索不足太低收敛慢maxIter最大迭代次数5000 ~ 20000看场景复杂度跑不动就减amax和alphamax根据你的机器人实际能力设别拍脑袋。比如差速底盘线加速度一般不超过 1 m/s²角加速度不超过 2 rad/s²。设大了规划出来的轨迹控制器跟不上设小了可能找不到可行解。3.3 积分与碰撞检测的代码细节积分器是 kinodynamic 规划的心脏。简单欧拉积分长这样function qNew integrate(q, u, dt) % q [x, y, theta, v, omega] % u [a, alpha] x q(1); y q(2); theta q(3); v q(4); omega q(5); a u(1); alpha u(2); % 欧拉积分 x_new x v * cos(theta) * dt; y_new y v * sin(theta) * dt; theta_new theta omega * dt; v_new v a * dt; omega_new omega alpha * dt; % 限幅 v_new max(min(v_new, 1.5), -1.5); omega_new max(min(omega_new, 2.0), -2.0); qNew [x_new, y_new, theta_new, v_new, omega_new]; end这里v和omega做了限幅防止积分发散。dt越小积分越准但树增长越慢。如果场景对精度要求高可以换成ode45但速度会慢很多规划阶段一般用欧拉就够。碰撞检测要查整段轨迹不能只查终点。常见做法是在积分过程中每隔几步查一次或者把轨迹离散成若干点逐个查。障碍物用圆形或矩形表示圆形检测简单矩形需要做线段与矩形相交判断。MATLAB 里可以用polyxpoly或者自己写。注意别漏查起点和终点之间的中间状态我见过有人只查终点结果轨迹从障碍物中间穿过去仿真里直接翻车。3.4 可视化与结果验证可视化不只是画个图好看它是验证规划结果的第一道关。plotTree一般画三样东西树的边、障碍物、最终路径。树的边用灰色细线最终路径用红色粗线障碍物用填充多边形。跑完后重点看三处路径有没有穿过障碍物、速度曲线有没有突变、终点状态是否满足目标要求。速度曲线可以单独画一张图横轴时间纵轴线速度和角速度。如果速度曲线有尖峰说明控制采样太粗或者积分步长太大。终点状态检查包括位置误差和速度误差位置误差一般要求小于 0.1 米速度误差小于 0.1 米每秒。不满足就调goalBias或者增加迭代次数。4. 避坑与排查kinodynamic_rrt_star 常见的五个翻车现场4.1 现象树增长极慢跑十分钟还在原地打转原因通常是dt太小加上控制采样太密每扩展一次要算几十条候选轨迹每条都要积分和碰撞检测计算量直接爆炸。另一个可能是碰撞检测写得太重比如每条轨迹离散成几百个点逐个查。解决先把dt调到 0.1 秒控制采样每维降到 3 个点碰撞检测离散点降到 10 个以内。跑通后再逐步加密。如果还慢检查有没有在循环里反复分配大数组MATLAB 里预分配能省不少时间。4.2 现象规划出来的路径速度突变控制器执行时抖动原因是代价函数只考虑了位置没有惩罚加速度变化或者控制采样范围太窄导致相邻两段轨迹的加速度差异大。也可能是重连时没有检查动力学可行性把两条速度不连续的轨迹硬接在一起。解决把代价函数改成时间加加速度平方的加权和权重根据场景调。重连时除了碰撞检测还要检查速度连续性比如新节点到邻居的轨迹起始速度要等于新节点的速度。控制采样范围适当放宽但别超过机器人实际能力。4.3 现象算法偶尔能找到路径但每次跑结果差异很大原因是采样随机性太强加上重连邻域半径固定导致树的结构不稳定。goalBias设得太高也会让算法过早收敛到局部最优。解决固定随机种子方便复现。重连半径改成动态调整随节点数增加而减小。goalBias降到 0.05 左右增加探索。如果场景复杂可以跑多次取最优或者用双向 RRT* 的思路从起点和终点同时建树加速收敛。4.4 现象终点状态误差大位置到了但速度不为零原因是目标采样只采位置没有采速度或者终点判定只查位置不查速度。kinodynamic 规划要求终点状态完整匹配包括速度为零或指定值。解决目标状态设成完整向量比如[x_goal, y_goal, theta_goal, 0, 0]。终点判定用状态距离位置和速度分别加权。如果要求终点速度不为零比如移动机器人边走边转弯那就把目标速度设成期望值别默认零。4.5 现象换了个地图或机器人模型代码直接报错原因是代码里硬编码了状态维度和参数比如q(4)取速度换个模型速度变成q(5)就崩了。碰撞检测也可能假设了圆形障碍物换成矩形就失效。解决把状态维度和索引写成常量或者配置结构体别在代码里散落魔法数字。碰撞检测抽象成接口圆形、矩形、多边形各写一个函数主流程只调接口。参数全部放到配置文件或者主脚本开头换场景只改配置不改逻辑。5. 进阶技巧双向搜索、动态重连与从仿真到实机的最后一公里双向 kinodynamic RRT* 是加速收敛的常用手段。从起点和终点各建一棵树交替扩展当两棵树的节点距离小于阈值时尝试连接。连接时要检查两段轨迹拼接后的速度连续性不能硬接。MATLAB 实现里可以复用单树的扩展和重连函数只是多维护一棵树和一个连接检测。实测在复杂场景下双向搜索能把收敛速度提升三到五倍但代码复杂度也上去了建议先把单树跑稳再改。动态重连是另一个值得抠的点。固定邻域半径在节点少时重连效果弱节点多时计算量大。常见做法是用r gamma * (log(n)/n)^(1/(d1))动态调整gamma根据状态空间范围设d是状态维度。这个公式来自 RRT* 的理论分析实际用的时候gamma要调我一般从 2.0 开始试跑几次看重连次数和总代价的平衡。从仿真到实机最大的坑是控制周期和规划周期的匹配。规划出来的轨迹是连续时间的但控制器是离散执行的周期可能 10 毫秒也可能 100 毫秒。如果规划步长dt是 0.1 秒控制器 10 毫秒执行一次中间需要插值。常见做法是把轨迹按控制器周期重采样用线性插值或者样条插值速度也要插值。插值后还要再查一次碰撞防止插值出来的中间状态撞上障碍物。验证方法上我习惯先在 MATLAB 里跑 100 次随机场景统计成功率和平均规划时间。成功率低于 90% 就调参数平均时间超过 5 秒就优化代码。然后把轨迹导出成 CSV用 Python 或者 C 写个简单的轨迹跟踪器在仿真环境里跑一遍看跟踪误差。误差大就回头改代价函数加平滑项。最后才上实机实机第一次跑一定把速度限幅设低比如最大速度砍一半确认没问题再放开。从那以后我每次换场景都强制走一遍“随机场景 100 次 → 轨迹导出跟踪 → 实机低速验证”的流程少一步都可能翻车。希望帮到你。本文还有配套的精品资源点击获取
