简介MATLAB 实现的 Kinodynamic RRT* 路径规划代码面向学习 RRT 系列算法、需要处理速度与加速度等动力学约束的开发者或研究人员。算法在经典 RRT* 基础上引入动力学可行性评估使规划路径既满足避障和最短目标也符合实际运动限制。包内共 17 个文件以 m 格式源代码为主包括主程序、RRT* 核心逻辑、动力学模型、采样与插值函数另有 mat 数据文件保存障碍物和航点以及说明文档压缩包整体仅 14KB结构紧凑便于逐行研读。目前已有 1754 人学习或下载。通过阅读源码可以掌握状态空间表示、最近邻查找、路径插值与迭代优化等关键步骤代码同时提供二维与四旋翼示例方便对比不同动力学模型下的规划效果按功能模块划分便于定位和修改适合算法入门、课程设计与二次开发。1. kinodynamic RRT*为什么普通RRT*规划出的路径真机开不出去普通RRT在地图上长出一棵越来越密的树最后捞出一条绕开障碍、直指终点的折线画面很好看。可把这条路径真正交给一台机器人第一个拐弯就会露馅差速底盘要停下原地转向汽车得打满方向盘拉出一段弧线无人机更要先把速度矢量整体换向。原因在于普通RRT的节点只有位置、边是直线段它在隐式假设机器人是个能瞬时加减速、原地掉头的质点。kinodynamic RRT*把速度、加速度这类动力学状态直接放进搜索空间让采样、扩展、重连每一步都遵守运动学与动力学约束最终产出的不是几何上好看的折线而是一串“当前状态确实执行得出来”的控制输入。理解它等于重新理解一遍随机搜索树在高维状态空间里如何被重新组装也才能读得懂那份MATLAB实现里每个算子的真实意图。2. 三重改写从几何规划到动力学规划变化的不只是状态维数2.1 普通RRT*的隐含假设画出来的线为什么开不上去把普通RRT*的算法流程摊开看主干只有五步随机采样一个位置点在树上找几何距离最近的节点沿两点连线方向延伸一个固定步长对这段线段做碰撞检测可行就挂到树上并尝试对邻近节点做重连。这五步里任何一个算子都建立在“状态只是位置、边只是直线”的前提上。直线段的动力学含义最容易被忽略。一条从A到B的线段等价于承认机器人可以瞬间把速度方向转到连线方向再以任意大小速度匀速过去。差速轮底盘的原地转向在低速时勉强能近似这种行为代价是轨迹上到处是尖角轮子磨损和能耗都不小对汽车这类非完整约束系统直线连着直线等于要求无穷小的转弯半径物理上根本不存在对无人机和机械臂速度矢量是连续量瞬时换向就意味着无穷大加速度。于是几何规划的结果落到执行层只能在每个拐点把速度先降为零、转向、再加速整条路径时间最长跟踪误差最大。所以几何RRT回答的是“位置能不能走通”kinodynamic RRT回答的才是“在速度、加速度约束下怎么走才能从状态A到状态B”。这两类问题的本质差别不是多两个变量而是“可行性”的定义变了一条边可行不再等于“能画出线段”而要等于“存在一串控制输入能把机器人从状态A驱动到状态B且全程满足执行器约束”。MATLAB实现里所有看起来很复杂的函数都是在这条新定义上重新实现随机搜索树的基本操作。2.2 维度之变树里的节点和边语义全变了最常用来入门动力学规划的模型是双积分器模拟无人机平移或者水面航行器的运动。状态从二维位置升级为四维x [p_x, p_y, v_x, v_y]^T控制输入是 u [a_x, a_y]^T并且满足执行器的范数约束 ||u||_2 ≤ u_max。对这个模型做零阶保持离散化假设一个控制量在整个步长内不变经典递推式是p_{k1} p_k v_k·dt (1/2)·u_k·dt² v_{k1} v_k u_k·dt这两行递推式的意义比它看上去重要得多。它决定了树上任意两个节点能不能连、连上要花多少时间、以及中间经过什么。kinodynamic RRT*所谓的“动力学内化”就是把这两行式子植入到树的生长逻辑里而不是像几何版本那样在规划完再做曲线平滑或者速度规划。节点含义随之变化。几何RRT*的节点只有坐标动力学版本里一个节点是一组“带速度的状态瞬间”树上每一条边也不再是一段直线而是“一段由控制输入积分出来的轨迹”。因此实现时树的字段天然比几何版本多典型的MATLAB结构是这样state4维或6维完整状态例如 [px; py; vx; vy]parent父节点索引input从父节点到本节点施加的控制序列通常是一个数组dt这一段轨迹的时长segment按时序离散的状态轨迹供碰撞检测和回放边的语义变化是最需要适应的地方规划结束时回溯出的不是一串路径点而是一串带时间戳的控制输入。很多第一次写动力学规划的人还在终点处做“路径采样插值”本质上是用几何思维去驾驶动力学树效率自然上不去这也是那份MATLAB实现里把segment直接挂在节点上的原因——它从一开始就把“结果要能执行”当成设计目标。2.3 度量与代价位置和速度混在一起怎么比“远近”状态空间升维后第一个立刻出现的尴尬是“找最近邻”。直接用欧氏距离d sqrt( Δp_x² Δp_y² Δv_x² Δv_y² )这里有双重问题。其一位置差和速度差的量纲不同强行相加数值上几乎完全被其中一项主导实际效果等同于把另一个维度忽略。其二更本质的是它不携带动力学信息两个状态在欧氏意义上非常近不代表从一个状态能在合理时间内到达另一个。典型情形是两个状态位置几乎重合一个静止、一个高速掠过距离很小但静止的那个要追上高速掠过的那个必须先花时间加速所需时间远大于“距离”所暗示的值。因此动力学树的度量应当换成“可达性代价”工程里最常用的替代是时间下界估计。对双积分器模型可以给出一组保守但好算的估计速度差本身需要 ||v_b - v_a|| / u_max 的时间来补偿位置差可以分解成“当前速度在一段时间内飞过的距离 加速度补偿的距离”反解一元二次方程得到另一个时间需求。两者取大就得到“从a到b至少要多久”的下界。这个下界既能用来排序最近邻也能直接当作代价函数也就是时间代价等价于对整段轨迹求 ∫ 1 dt。选哪种代价有讲究。时间代价驱动树长“最短时间轨迹”直观但容易生成贴着执行器极限跑的激进轨迹控制能量代价 J ∫u^T R u dt 生成省力温和的轨迹任务弹性更好但会让树生长得更慢。实际工程里常用带权组合J t λ·∫u^T R u dtλ通常调一次就不再动。下面这张对照表概括了两类规划的全体差异。维度几何RRT*kinodynamic RRT*状态表示位置 (x,y)位置速度 (x,y,vx,vy)可按需加航向角树的边直线段控制序列驱动的轨迹段最近邻度量欧氏距离时间下界或加权可达性估计路径代价路径长度时间 / 控制能量 / 加权组合碰撞检查线段与障碍求交轨迹按时序离散后逐点检测规划结果几何折线控制序列 状态序列若想保留欧氏距离的形式常见的做法是引入两个比例系数把位置差和速度差分别除以“典型速度”和“典型加速度”再求加权和。这套系数理论上有最优标定实践中多数人靠调参我一般宁可多花一次时间下界的计算换取更稳的排序也不愿意拿拍脑袋确定的系数去赌树的生长质量。3. 三个核心算子的源码级拆解采样、Steer与Rewire3.1 状态采样全状态均匀采样为什么是效率黑洞高维空间有个广为人知的直觉d维超立方体边长为1时内部半径0.5的球体积占比随维度指数下降。这意味着在4维状态空间里随机均匀撒点点出现在“位置合理且速度合理”区域的概率极低。树扩展时大量采样点要么位置落在障碍区要么速度方向让Steer无解真正能产生新节点的比例可能不到百分之几。这是动力学规划里“跑不动”的第一号元凶。对策是部分采样位置自由采样速度用启发式生成。常见实现长这样function x_rand sampleState(goal, goalBias, v_max, mapSize, rngStream) if rand(rngStream) goalBias % 目标偏向在目标点附近撒点并采用目标速度域 pos goal(1:2) 0.05 * randn(rngStream, 2, 1); vel goal(3:4); % 通常为零速度 else % 自由探索位置均匀采样速度指向目标方向 pos [mapSize(1)*rand(rngStream); mapSize(2)*rand(rngStream)]; dir goal(1:2) - pos; dir dir / (norm(dir) 1e-9); mag v_max * rand(rngStream); vel mag * dir; end x_rand [pos; vel]; end注意速度生成的两个细节。第一幅值取随机数而非固定最大值这让树生长出来的轨迹长短不一避免所有节点都以满速冲刺第二速度方向偏向目标这让树的生长天然具有方向性而不是在状态空间里做布朗运动。把这两个细节改成“全状态均匀采样”后重跑同一个场景扩展成功率会肉眼可见地下降这是最直观的对照实验。另一条路线是控制空间采样不采目标状态而是在u的控制范围内随机取一个控制输入从当前节点前向积分一段固定时间把轨迹末端作为新节点。这条路线的优势是每个生成的状态都可达树完全活在“可达流形”上不会出现“采样点接不住”的浪费代价是树的生长方向不受引导收敛到目标区域需要的时间长。工程实现里普遍把两种策略混合目标偏向采样驱动快速收敛再加一小概率的全局采样保住探索性。混合比例是经验值目标偏向设在 0.50.9 之间比例越高收敛越快但也越容易漏掉障碍另一侧的绕行路径。3.2 Steer算子从“连直线”到“解两点边值问题”普通RRT*的扩展只是沿方向向量走一步到了动力学版本“能不能从当前状态到达采样状态”成了最核心也最昂贵的判定。这个判定在数学上是一个两点边值问题给定初始状态x_a、终态x_b以及控制约束||u|| ≤ u_max找出一段控制输入使得动力学方程在时刻T把x_a送到x_b并且在所有可行解里代价最小。这比几何版本的“连线”严格得多并不总能成功。对双积分器模型这个两点边值问题存在解析解形态是经典的bang-bang控制控制要么打在最大幅值要么反方向打满中间至多切换一次。解析解比数值优化快几个数量级是MATLAB实现里最值得写的部分。下面给出一个轴间解耦的近似实现function [traj, cost, feasible] steerBang(x_from, x_to, u_max, dt) dp x_to(1:2) - x_from(1:2); dv x_to(3:4) - x_from(3:4); % 1) 计算每个轴取最短时间所需的时长取两台轴的大值 t_need zeros(2, 1); for axis 1:2 v0 x_from(2axis); % 当前轴速度 a 0.5 * u_max; % 解 0.5*u_max*t^2 v0*t - dp 0 c -dp(axis); disc v0^2 - 4*a*c; if disc 0 feasible false; traj []; cost inf; return; end t_need(axis) (-v0 sqrt(disc)) / (2*a); end T max(t_need); % 2) 按T反推平均加速度并校验执行器约束 accel dv / T; if norm(accel) u_max feasible false; traj []; cost inf; return; end % 3) 按时序分辨率离散并积分生成轨迹 ts 0:dt:T; traj zeros(length(ts), 4); p x_from(1:2); v x_from(3:4); for k 1:length(ts) traj(k, :) [p; v]; if k length(ts) p p v*dt 0.5*accel*dt^2; v v accel*dt; end end cost T; feasible true; end代码里的关键点在第二步先按共享时间T反推两端速度差对应的平均加速度再检查它是否超过u_max。这个校验让“数学上可解但物理上不可行”的状态配对在生成轨迹前就被丢弃树因此只长在真实可达的区域里。丢弃不是失败而是动力学约束在多算子层面被内化的体现——树上每一个成功创建的节点都意味着有人在背后解通过一次两点边值问题。这里有个值得记的常见误用有人把Steer写成“先修正速度、再修正位置”的两段式顺序控制每段都用满加速。看起来总是能到达目标但实际时间被严重高估树的代价体系被污染同一段距离出现两种不同代价重连时比较的是失真后的数字最终路径经常比最优路径长一大截。解析解或半解析解才是正路启发式拼接只能当兜底这也是看用这份MATLAB实现时最值得细读的函数。3.3 近邻搜索与重连高维空间里的邻域半径不是拍脑袋调的重连是RRT*渐进最优性的来源动力学版重连的关键参数是邻域半径r。理论公式长这样r min( γ·(log n / n)^(1/d), η )d在这里是状态空间维数不是物理空间维数。直观对比一下2维几何规划里半径按 (log n/n)^(1/2) 衰减4维动力学规划里衰减速度变成了 (log n/n)^(1/4)。同样节点数动力学版本的邻域半径要小得多——这是高维空间体积稀疏性的直接数学表达。很多工具里直接“先算几何半径再用”等于把d写成了2重连几乎永远找不到邻居渐进最优性名存实亡。重连的骨架如下function tree rewire(tree, idx_new, n, radius, u_max, dt) candidates neighborsWithinRadius(tree, idx_new, radius); for i 1:length(candidates) idx_old candidates(i); if idx_old tree(idx_new).parent continue; end % 方向1把旧节点的父改成新节点 [seg1, cost1, ok1] steer(tree(idx_new).state, tree(idx_old).state, u_max, dt); if ok1 tree(idx_new).cost cost1 tree(idx_old).cost tree(idx_old).parent idx_new; tree(idx_old).cost tree(idx_new).cost cost1; tree(idx_old).segment seg1; end % 方向2把新节点的父改成旧节点对称逻辑 [seg2, cost2, ok2] steer(tree(idx_old).state, tree(idx_new).state, u_max, dt); if ok2 tree(idx_old).cost cost2 tree(idx_new).cost tree(idx_new).parent idx_old; tree(idx_new).cost tree(idx_old).cost cost2; tree(idx_new).segment seg2; end end end注意两个方向各自调用了一次steer原因是动力学的可达性在时间反演下不一定对称从A到B可行从B到A可能需要的控制幅度不同、代价不同。两个方向的代价不能混用这也是下一章专门列出的一个翻车高发点。重连成功后可能需要沿子树级联更新代价严格实现里要做子树代价重算工程上可以限制重算深度或者用懒更新把差异摊到路径回溯时否则每重连一次就扫一遍整棵子树性能受不了。4. 避坑指南动力学规划里最容易翻车的五个细节动力学规划与几何规划手感差异最大的地方从来不在算法骨架而在那些“看起来差不多、跑起来差很多”的算子细节。以下五条是这份MATLAB实现里最典型、也最值得在做自己的实现之前先记下的踩坑记录。4.1 现象明明做了网格碰撞检测轨迹还是斜着穿墙而过原因动力学轨迹是一条连续曲线沿曲线移动的速度不均匀。若按固定点数例如每段边切10个点离散后检测慢速段点很密、快速段点很疏快速段一次跳跃的距离可能超过障碍物直径点没落在障碍内部碰撞就被漏掉。这是“穿了墙而检查没发现”的第一大原因。解决自适应离散。按局部速度上限反推检测间隔保证一步跨不过半个障碍这个思想在数值上就是CFL条件。实现时不必真按轨迹每一时刻的速度逐点算用该段速度上界估算即可然后把轨迹按这个间隔重新切点。遇到“预览路径笔直穿过障碍但碰撞检测说安全”的情况优先怀疑检测步长而不是碰撞函数本身十个穿墙案例有八个都是步长问题。4.2 现象最近邻找出来的节点Steer几乎总是返回不可行原因最近邻还是用纯几何距离找的。位置距离近不代表动力学可达两个节点位置靠得很近但速度方向相差悬殊从其中一个按执行器极限加速到另一个所需的时间可能远大于“位置距离”暗示的量级。树的扩展因此频繁出现“看中了节点ASteer却告诉你接不上”搜索在无效区域空转。解决最近邻分两级。第一级用位置索引粗筛出K个候选第二级用时间下界或加权可达距离精排然后再调用一次Steer确认。如果树扩展缓慢、节点数量增长正常但解迟迟不出现先看度量函数是不是还在用裸欧氏距离。做个简单对照实验把度量换成时间下界扩展成功率往往立刻翻倍。4.3 现象Rewire跑完整条路径的代价不降反升甚至有振荡原因把动力学边当成了无向边共用同一个代价。动力学Steer严格说是“从一个状态到另一个状态的最优控制解”时间反演下并不对称即使两个方向都能互相到达代价也可能不同。Rewire里若把方向1的代价硬套到方向2上比较用的数字就是错的入选的父节点并非真的更优代价自然乱涨。另一类原因是没有把代价级联更新到子树重连后父节点变了后代的cost还停留在旧值路径回溯时拼接出错误的总代价。解决两个方向分别调用Steer、分别取cost谁也别帮谁记账重连成功后将新父节点的代价差传播到子树懒更新或限深重算二选一。排查时直接在Rewire前后把目标路径的代价打印出来对比出现“非单调变化”几乎必中这类问题。4.4 现象规划成功率高但真机上机器人到终点停不下来原因目标区域被定义成“位置到达目标”就算赢完全没有对速度做约束。普通RRT*的终点是位置点沿用到动力学规划里就会得到一个“高速冲过终点”的合法路径。但在抓取、停车、对接任务里末端速度必须归零这个约束直接被目标定义吃掉了。解决目标从“位置点”放宽为“位置 速度约束的交集”——位置误差小于posTol速度范数小于velTol两者同时满足才算到达。更稳妥的一招是终点偏向采样时在终点周围撒“零速度状态”让树的最终节点天然落在静止工况。规划完顺手检查目标节点速度范数若大于执行器可吸收的余量这条路径算不上真成功。动力学版本最容易从几何概念沿革出事故的就是这一条。4.5 现象节点分布正常但距离函数算出来的值忽大忽小树生长方向很怪原因状态里带航向角θ时角是周期量直接做差在0与2π附近会跳变。同一个朝向差值为2π时被算成满距离而实际物理差值是0。度量、时间下界、最近邻排序全部被扭曲树在这种度量下变得不稳定出现“明明朝同一个方向树却往反方向拐”的错乱。解决在所有用到角差的函数入口做一次wrap把角度差限到[-π, π]。检查时在节点分布图上叠加树的边方向若出现大量接近180°的错误偏转基本就是哪个地方忘了把角度差包络回主值区间。这类问题属于“查了一下午最后发现差一两行代码”的经典事故所以最好把wrap做成公共距离函数的一部分强制全局复用。5. MATLAB工程实现从“算法演示”到“能跑几千次规划”的细节5.1 树的数据结构结构体数组、字段设计与预分配的血泪取舍动力学版本一个节点挂着一堆字段选错数据结构会让几万次循环慢到无法忍受。用结构体数组最顺手关键是要做预分配N_max 20000; tree struct(state, [], parent, 0, cost, 0, ... input, [], dt, 0, segment, []); tree(N_max).state zeros(4,1); % 预分配让后续新增避免整块拷贝 count 1;预分配写一行收益好几倍不预分配时每次tree(end1)都会触发整块数组复制节点一多就是平方级灾难。segment字段是cell类型预分配无法完全避免cell扩容但把tree本体固定住至少让主要压力集中在segment的追加上整体开销可控。另一个容易踩的坑是“对象封装”。把节点写成句柄类、把树写成类调试时好看但MATLAB句柄类的属性访问开销在几十万次调用后非常可观。更稳的顺序是先写结构体数组版本跑通逻辑再视性能瓶颈把最内层循环通常是Steer和碰撞检测单独抽成函数或mex而不是把整棵树包装成class。结构体数组的额外好处是按索引取邻居、按索引回溯、整体传给parfor都不需要额外转换改并行时最省事。5.2 最近邻与邻居查询用KDTreeSearcher做两阶段粗筛精排树节点上万后每次扩展都做全树扫描最近邻是O(n)级别整个规划会被拖到分钟级。工程标准做法是把查找拆成两阶段model KDTreeSearcher(tree.state(1:2, :)., Distance, euclidean); [k_ids, k_dists] knnsearch(model, x_rand(1:2)., K, 8);先用位置粗筛8个候选再做完整4维状态距离精排最后把候选索引交给Steer。这一步把“全树扫描”变成“几次局部查询”节点数到达20000时能加速一到两个数量级。注意KDTreeSearcher每次树更新后都要重建索引这是固有开销工程里可以设置重建周期比如每50次扩展重建一次。索引滞后几十个节点对邻居质量的影响很小性能收益却很明显只要不把滞后设到超过节点总数的五分之一这种近似是安全的。5.3 碰撞检测向量化用矩阵运算替代内层循环碰撞检测是另一个热点每生成一段轨迹都要把几十个离散点跟几十个障碍物做位置关系判断。逐点逐障碍的双重循环会直接把性能拖垮改成矩阵批量运算才是正路% pts: 3xN矩阵每列是一个展平后的检测点 % obs: 3xM矩阵每列是一个障碍圆 [x; y; r] d2 sum((reshape(pts, 2, 1, N) - reshape(obs(1:2,:), 2, M, 1)).^2, 1); hit squeeze(any(any(d2 obs(3,:).^2, 1), 2));这段代码把“点与障碍”的距离计算全部向量化用any一锤定音。道理很简单把程序里最内层的if换成矩阵比较MATLAB才真正发挥价值。障碍是线段或多边形时逻辑相同——先把多边形展开成一条条线段对轨迹点做线段距离的向量化计算再统一取最小值比较。向量化的收益在障碍数超过几十、轨迹点数上百的时候才显著小规模问题不必过度优化。5.4 路径回溯把树里的控制序列拼成可直接执行的结果搜索成功后的回溯反而最简单从目标节点沿parent链一路走到根把每段segment首尾相接。正因为事先在边里存了控制序列最终输出天然是“控制序列 状态序列 时间戳”仿真器拿到它可以直接逐帧执行不需要再做逆运动学或插值。segments {}; idx goal_idx; while idx ~ 0 segments{end1} tree(idx).segment; % 每条segment都带时间戳 idx tree(idx).parent; end % 倒序拼接得到从起点到终点的完整轨迹 full_traj [segments{end:-1:1}];回溯得到的结果在时间上等于全部路径段时间相加中间不会有缝隙。若有缝隙说明某节点segment字段记录的是“控制输入 起点状态”而不是已经积分好的轨迹拼接时必须按动力学从起点重新积分一次。保存控制序列而不是只存端点这个习惯在仿真转实机时会直接决定调试速度——实机控制器手里就该是一串输入而不是一串要靠逆动力学反猜输入的位置点。6. 设计取舍的思考解析Steer、度量选择与参数敏感性的性价比动力学RRT*从理论到工程几乎每一步都在做取舍。最核心的一组是“Steer做多精确”和“用什么度量”它们共同决定树是长得快还是长得准。解析Steer比如双积分器的时间最优bang-bang解能一步算出两个状态间的最优转移代价是这一判断绑定了模型换个动力学模型就得重写一整套解析推导前向积分式Steer换模型成本低但新节点很少恰好落在目标状态上收敛慢每条轨迹都要回溯验证是否当真最优本质是用数值精度换通用性。工程里常见的折中是双模式扩展大部分时间用无偏的前向积分保持树的覆盖小部分时间用解析Steer对准目标点。这样的树既探索充分又能快速收敛到解。度量函数的选择同样在性能和最优性之间走钢丝。真正精确的最小时间下界往往和Steer本身一样贵用它选最近邻等于把最优控制做了两遍便宜的上界估计则可能贪婪地引导树进入局部死胡同。我一般的做法是最近邻用便宜上界排候选真正建立父子关系时才用解析Steer验证两次计算的精度不同各司其职。邻域半径也有类似的尴尬——理论半径随维度衰减得厉害分得太密则根本没有邻居可重连渐进最优性名存实亡放大半径又让每次重连做几十上百次Steer单步代价爆炸。更现实的做法是保留理论半径作为基准同时限定每轮重连最多尝试的邻居个数兼顾最优性与开销。实机上还有一层隐藏取舍规划器的动力学参数该不该留余量。用理想值规划轨迹时间最短但一点模型误差就会让跟踪偏差累积把规划的u_max、v_max人为缩小到真实能力的80%轨迹变保守换来的是鲁棒性和安全性。这个比例没有公论需要结合跟踪控制器带宽去试。写过几轮这样的规划后我的习惯已经固定下来任何新的状态空间或模型先跑一个“从静止到静止、无障碍直飞”的最小算例检查Steer是否精确到点、代价是否符合解析值再上真实地图。从那以后我每次改动度量函数或邻域半径公式都会强制走一遍这个最简算例再继续它帮我筛掉了一大半“看起来合理实则玄学”的参数组合。希望帮到你——动力学规划的门槛不在把RRT*背下来而在每个小算例里把动力学想清楚之后算法才真正被驯服。本文还有配套的精品资源点击获取