kinodynamic_rrt_star：动力学约束下的机器人轨迹规划实战

简介：本资源为Kinodynamic RRT算法的MATLAB实现，对应Dustin J. Webb与Jur van den Berg的论文《Kinodynamic RRT: Optimal Motion Planning for Systems with Linear Differential Constraints》，面向从事机器人运动规划、最优控制与轨迹优化的研究者及高年级学生，用于解决带线性微分约束系统的最优运动规划问题。压缩包共17个文件，以13个.m脚本为核心，辅以2个.mat数据文件、1个readme.md说明与1个licence.txt授权文件，整体约14KB，体积轻量便于快速部署。代码覆盖double_integrator、rrtstar、rrt_double_int_2d、rrt_quad_test等模块，并配套ball与quad两类场景的采样、状态检测、输入检测及可视化脚本，可直接复现论文中的仿真实验。已有1295人学习下载，适合作为算法入门复现、课程设计或科研对比的参考实现。

1. kinodynamic_rrt_star：当路径规划开始关心“能不能拐得过来”

如果你做过移动机器人或者机械臂的路径规划，大概率经历过这个场景：用 RRT 或者 RRT* 跑出一条看着很漂亮的折线路径，交给控制器执行，结果机器人要么原地打转，要么在拐角处速度掉到零再重新加速，要么干脆因为加速度超限直接报错停机。问题不在控制器，在于规划器压根没考虑过动力学约束——它以为机器人可以瞬间改变速度方向。

kinodynamic_rrt_star 要解决的就是这件事。它把机器人的动力学方程（或者简化后的运动学模型）直接塞进 RRT* 的采样和扩展过程里，让规划出来的轨迹从生成那一刻起就满足速度、加速度、曲率等约束。换句话说，它输出的不是一条“几何路径”，而是一条“可执行的轨迹”。适合谁用？做差速轮式机器人、阿克曼底盘、多旋翼、机械臂这类不能瞬间变速变向的系统，只要你被“规划出来跑不了”折磨过，这个方向就值得花时间啃。

2. 从 RRT* 到 kinodynamic：采样空间和扩展方式到底改了什么

2.1 为什么标准 RRT* 在动力学系统上会失效

标准 RRT* 的采样空间是配置空间（configuration space），比如二维平面上就是 (x, y)，扩展时直接在两棵树的节点之间做直线插值，然后检查这条直线是否碰撞。它的隐含假设是：机器人可以沿着任意方向以任意速度移动，且能在任意点瞬间停下或转向。

这个假设对差速轮式机器人就已经不成立了。差速底盘不能横向平移，转弯半径受轮距和最大轮速差限制；阿克曼底盘更严格，前轮转角有物理极限，曲率不能突变。当你把标准 RRT* 的输出喂给这类系统，控制器会发现轨迹上某些点的曲率需求超过了底盘能提供的最大值，或者需要的加速度超过了电机扭矩能产生的范围。

kinodynamic 版本的改法很直接：采样空间从配置空间扩展到状态空间（state space），状态里除了位置还包括速度甚至加速度；扩展方式从直线插值改成“施加一段控制输入，用动力学方程前向积分”。这样生成的每一段轨迹天然满足动力学约束，因为它是从动力学方程里长出来的。

2.2 状态空间设计：位置、速度、控制量怎么定

状态向量的维度直接决定计算量。以差速轮式机器人为例，常见做法是状态取 (x, y, θ, v, ω)，即位置、朝向、线速度、角速度。控制量取 (a, α)，即线加速度和角加速度。动力学方程用简单的积分模型：

# 差速轮式机器人简化动力学模型 # state: [x, y, theta, v, omega] # control: [a, alpha] 线加速度、角加速度 # dt: 积分步长 def propagate(state, control, dt): x, y, theta, v, omega = state a, alpha = control # 限制控制量在物理范围内 a = np.clip(a, -A_MAX, A_MAX) alpha = np.clip(alpha, -ALPHA_MAX, ALPHA_MAX) # 欧拉积分（步长小时精度够用） x_new = x + v * np.cos(theta) * dt y_new = y + v * np.sin(theta) * dt theta_new = theta + omega * dt v_new = v + a * dt omega_new = omega + alpha * dt # 速度和角速度也要限幅 v_new = np.clip(v_new, -V_MAX, V_MAX) omega_new = np.clip(omega_new, -OMEGA_MAX, OMEGA_MAX) return np.array([x_new, y_new, theta_new, v_new, omega_new])

这段代码里几个参数需要根据实际平台标定。A_MAX 是线加速度上限，取决于电机扭矩和车重；ALPHA_MAX 是角加速度上限，取决于转动惯量和轮距；V_MAX 和 OMEGA_MAX 是速度上限。dt 一般取 0.05 到 0.1 秒，太小会导致积分步数暴增，太大则积分精度不够，轨迹和实际执行偏差明显。

提示：如果你的平台动力学更复杂（比如带悬挂、有侧滑），欧拉积分不够用，换成 RK4 会好很多，但计算量大约翻四倍，需要权衡。

2.3 扩展策略：控制采样和前向积分

kinodynamic RRT* 的扩展不再是“朝随机点直线走”，而是“从树上最近节点出发，随机采一组控制量，前向积分若干步，得到一条轨迹段”。这条轨迹段的终点就是新节点。

def extend(tree, state_space, goal, params): # 1. 在状态空间随机采样 x_rand = sample_state(state_space) # 2. 找树上最近节点（距离度量要包含位置和速度） x_near = nearest(tree, x_rand, metric='weighted') # 3. 随机采样控制量 u_rand = np.random.uniform( low=[-A_MAX, -ALPHA_MAX], high=[A_MAX, ALPHA_MAX] ) # 4. 前向积分生成轨迹段 traj = [] x_current = x_near.state for _ in range(N_STEPS): x_current = propagate(x_current, u_rand, DT) traj.append(x_current) if collision_check(x_current): return None # 撞了就丢弃 x_new = traj[-1] # 5. 返回新节点和连接它的轨迹 return Node(state=x_new, parent=x_near, trajectory=traj)

这里有几个关键决策。距离度量不能只用欧氏距离，要把速度差异也考虑进去，否则会选出“位置近但速度差很大”的节点，导致连接轨迹需要剧烈变速。常见做法是加权：位置差权重 1.0，速度差权重 0.3 到 0.5，具体值靠实验调。N_STEPS 决定每次扩展走多远，太小则树增长慢，太大则碰撞检查负担重且轨迹不够灵活。我一般取 10 到 20 步，配合 dt=0.1，单次扩展覆盖 1 到 2 秒的行驶时间。

3. 代价函数与最优性：kinodynamic_rrt_star 的“star”体现在哪

3.1 代价怎么定义才合理

标准 RRT* 的代价通常是路径长度。kinodynamic 版本里，路径长度只是其中一个维度，还要考虑控制代价（加速度平方积分）、时间代价、甚至平滑度。常见做法是加权求和：

def compute_cost(trajectory, weights): # trajectory: 列表，每个元素是 (state, control, dt) length = 0.0 control_cost = 0.0 time_cost = 0.0 for i in range(len(trajectory) - 1): s1 = trajectory[i][0] s2 = trajectory[i+1][0] u = trajectory[i][1] dt = trajectory[i][2] # 位置变化量 length += np.linalg.norm(s2[:2] - s1[:2]) # 控制量平方和 control_cost += np.sum(u**2) * dt time_cost += dt total = (weights['length'] * length + weights['control'] * control_cost + weights['time'] * time_cost) return total

weights 的选取直接影响规划结果。length 权重大，轨迹偏短但可能加速度大；control 权重大，轨迹平滑但可能绕路；time 权重大，倾向于快速通过但可能牺牲平滑性。我一般先设 length=1.0、control=0.1、time=0.5，然后根据实际执行效果微调。如果控制器经常报加速度超限，就加大 control 权重。

3.2 重选父节点和重连：在轨迹空间里做

RRT* 的核心操作是“重选父节点”和“重连”。在 kinodynamic 版本里，这两个操作都要在轨迹空间里做，而不是简单的节点间直线连接。

重选父节点：对新节点 x_new，在它附近找若干候选父节点，对每个候选父节点尝试生成一条从该父节点到 x_new 的轨迹（用控制采样加前向积分），如果轨迹无碰撞且代价更低，就换父节点。

重连：对新节点 x_new 附近的节点，尝试生成从 x_new 到它们的轨迹，如果代价更低就更新它们的父节点为 x_new。

def rewire(tree, x_new, radius, params): neighbors = find_neighbors(tree, x_new, radius) for x_near in neighbors: # 尝试从 x_new 到 x_near 生成轨迹 traj = steer(x_new.state, x_near.state, params) if traj is None: continue # 生成失败或碰撞 new_cost = x_new.cost + compute_cost(traj, params['weights']) if new_cost < x_near.cost: x_near.parent = x_new x_near.trajectory = traj x_near.cost = new_cost # 递归更新子节点代价 update_children_cost(x_near)

steer 函数是这里的核心难点：给定起点状态和终点状态，找一组控制量把起点“开”到终点附近。这不是简单的两点插值，而是一个两点边值问题。实践中常用做法是：用启发式方法（比如比例控制）生成初始控制序列，然后前向积分看终点误差，如果误差太大就丢弃或做少量迭代优化。完全精确的求解（比如用最优控制方法）计算量太大，不适合在线规划。

注意：重连操作会让树的结构频繁变化，如果每次重连都递归更新所有子节点代价，计算量会随树规模增长。常见优化是只更新一层，或者用懒惰更新策略，在节点被选中扩展时才重新计算代价。

3.3 参数表：我常用的起步配置

这张表不是金科玉律，不同平台差异很大。关键是理解每个参数影响什么，然后有方向地调。

4. 避坑与排查：kinodynamic_rrt_star 落地时最容易翻车的五个地方

4.1 规划出来的轨迹控制器不跟踪

现象：规划器输出轨迹看着没问题，但控制器执行时偏差越来越大，最后撞墙或停下。

原因：规划用的动力学模型和控制器用的模型不一致。比如规划时假设速度可以瞬间从 0 变到 1 m/s，但实际电机有响应延迟；或者规划时忽略了地面摩擦，实际执行时减速比预期慢。

解决：先做系统辨识，把实际平台的加速度响应、延迟、摩擦特性测出来，更新规划模型参数。如果差异太大，考虑在规划器里加入不确定性裕量，比如把 A_MAX 设成实际值的 80%。

4.2 树增长太慢，规划时间不可接受

现象：跑了几十秒还没找到可行解，或者树节点数增长极慢。

原因：控制采样空间太大，大部分采样控制量生成的轨迹都撞了或者原地打转；或者 N_STEPS 太小，每次扩展走不了多远。

解决：不要均匀采样控制量，用启发式引导。比如根据当前节点到目标的方向，优先采样朝向目标的控制量。另外可以增大 N_STEPS，但要注意碰撞检查开销。还有一个技巧是“偏向采样”：以一定概率直接采样目标状态附近的点，加速向目标收敛。

4.3 轨迹抖动严重，执行时机器人一顿一顿的

现象：规划出的轨迹速度曲线锯齿状，控制器执行时机器人抖动明显。

原因：控制代价权重太低，规划器为了缩短路径选了加速度频繁切换的控制序列；或者积分步长太大，轨迹离散化误差明显。

解决：加大 WEIGHT_CONTROL，让规划器偏好平滑控制；减小 DT，提高轨迹分辨率；在轨迹后处理阶段加低通滤波或样条平滑，但要注意平滑后要重新检查动力学约束是否还满足。

4.4 重连操作导致代价震荡不收敛

现象：规划过程中节点代价反复变化，树结构不稳定，最终解的质量波动大。

原因：重连时递归更新子节点代价，但更新顺序或更新范围不对，导致代价计算出现循环依赖或遗漏。

解决：重连后只做一层更新，不递归；或者用拓扑排序确定更新顺序，确保每个节点代价只被计算一次。另外可以限制重连频率，比如每扩展 N 次才做一次重连。

4.5 碰撞检查成为性能瓶颈

现象：规划时间大部分花在碰撞检查上，树规模一大就卡死。

原因：每次扩展都要对轨迹上每个积分点做碰撞检查，如果碰撞检查本身很慢（比如用高精度网格或点云），累积开销巨大。

解决：用层次化碰撞检查，先用粗粒度包围盒快速排除大部分轨迹，只对可能碰撞的轨迹做精细检查；缓存碰撞检查结果，相同或相近的轨迹段不重复检查；如果环境是静态的，预计算距离场，碰撞检查变成查表操作。

5. 进阶技巧：让 kinodynamic_rrt_star 从能跑到好用

5.1 用启发式采样加速收敛

纯随机采样在状态空间维度高时效率很低。一个实用技巧是维护两棵树：一棵从起点生长，一棵从目标生长（双向搜索），当两棵树连接时规划成功。双向搜索能把有效采样空间缩小很多，尤其适合长距离规划。

另一个技巧是“ informed sampling”：先用快速但粗糙的方法（比如忽略动力学约束的 RRT*）找一条可行路径，然后以这条路径的代价为上界，只在可能改进代价的区域采样。这个思路来自 Informed RRT*，移植到 kinodynamic 版本需要把代价上界映射到状态空间的一个子集，实现起来稍复杂但效果显著。

5.2 轨迹后处理：从可行到可执行

规划器输出的轨迹是离散的积分点序列，直接喂给控制器可能不够平滑。常见后处理步骤：

第一步，用样条或多项式拟合离散轨迹，得到连续可导的轨迹表示。注意拟合后要重新检查动力学约束，因为拟合可能引入超出约束的局部曲率。

第二步，做时间参数化。规划时假设的控制量序列对应的时间步长是固定的，但实际执行时可以根据动力学约束重新分配时间，让轨迹在满足约束的前提下尽可能快或尽可能平滑。

第三步，如果控制器支持前馈加反馈，把规划轨迹的速度、加速度作为前馈项传给控制器，能显著提高跟踪精度。

# 简单的轨迹平滑：移动平均 + 约束检查 def smooth_trajectory(traj, window=5): smoothed = [] for i in range(len(traj)): start = max(0, i - window // 2) end = min(len(traj), i + window // 2 + 1) segment = traj[start:end] avg_state = np.mean([s for s, _, _ in segment], axis=0) smoothed.append(avg_state) # 检查平滑后是否满足速度和加速度约束 for i in range(1, len(smoothed)): v = np.linalg.norm(smoothed[i][:2] - smoothed[i-1][:2]) / DT if v > V_MAX: return None # 平滑失败，返回原始轨迹 return smoothed

这个平滑方法很粗糙，但胜在简单快速。如果对平滑度要求高，用五次多项式或 B 样条，但计算量会上去。

5.3 验证规划器是否真的在线的三个指标

第一个指标是“首次可行解时间”。从开始规划到找到第一条无碰撞且满足动力学约束的轨迹，花了多少时间。这个指标决定规划器能不能用在需要快速反应的场景。

第二个指标是“代价收敛曲线”。随着规划时间增加，最优代价是否单调下降并趋于稳定。如果代价一直震荡或下降极慢，说明重连或采样策略有问题。

第三个指标是“轨迹执行成功率”。把规划出的轨迹实际跑一遍，统计成功到达目标的次数。这个指标最诚实，前面两个指标再好，执行失败就是白搭。

我自己的习惯是：每次改完参数或代码，先跑 20 次随机场景，记录这三个指标，和上一版对比。如果首次可行解时间变长但执行成功率提高，说明参数调整方向对了；如果两个都变差，赶紧回滚。

提示：验证时一定要用和实际平台动力学特性接近的仿真环境，仿真里跑通了再上真机。仿真和真机的差距，往往就是那些“规划器没建模但实际存在”的动力学效应。

希望帮到你。

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