简介：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_{k+1} = p_k + v_k·dt + (1/2)·u_k·dt² v_{k+1} = v_k + u_k·dt

这两行递推式的意义比它看上去重要得多。它决定了树上任意两个节点能不能连、连上要花多少时间、以及中间经过什么。kinodynamic RRT*所谓的“动力学内化”，就是把这两行式子植入到树的生长逻辑里，而不是像几何版本那样在规划完再做曲线平滑或者速度规划。

节点含义随之变化。几何RRT*的节点只有坐标，动力学版本里一个节点是一组“带速度的状态瞬间”，树上每一条边也不再是一段直线，而是“一段由控制输入积分出来的轨迹”。因此实现时，树的字段天然比几何版本多，典型的MATLAB结构是这样：

• state：4维或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，λ通常调一次就不再动。下面这张对照表概括了两类规划的全体差异。

若想保留欧氏距离的形式，常见的做法是引入两个比例系数，把位置差和速度差分别除以“典型速度”和“典型加速度”再求加权和。这套系数理论上有最优标定，实践中多数人靠调参，我一般宁可多花一次时间下界的计算换取更稳的排序，也不愿意拿拍脑袋确定的系数去赌树的生长质量。

3. 三个核心算子的源码级拆解：采样、Steer与Rewire

3.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.5～0.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(2+axis); % 当前轴速度 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几乎总是返回不可行

原因：最近邻还是用纯几何距离找的。位置距离近不代表动力学可达：两个节点位置靠得很近，但速度方向相差悬殊，从其中一个按执行器极限加速到另一个所需的时间可能远大于“位置距离”暗示的量级。树的扩展因此频繁出现“看中了节点A，Steer却告诉你接不上”，搜索在无效区域空转。

解决：最近邻分两级。第一级用位置索引粗筛出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(end+1)都会触发整块数组复制，节点一多就是平方级灾难。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{end+1} = 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*背下来，而在每个小算例里把动力学想清楚之后，算法才真正被驯服。

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