引言
运动规划要回答：从当前位形到目标位形，走哪条路。看似简单，难在高维与约束。六轴臂的构型空间是六维（若算上冗余与移动基座则是十几维），障碍物在构型空间里是复杂的非凸区域，且路径必须满足关节限位、速度、加速度、力矩以及避障等多重约束。这就是为什么规划器分成了采样、图搜索、优化三大流派，每派适合的场景不同。
工程上的难点在于「规划器好用但不可靠」。采样规划器（RRT 类）概率完备——理论上时间足够总能找到解，但实际中可能几毫秒找到、也可能几秒找不到，这个不确定性在产线上是不可接受的。优化规划器（TrajOpt 类）能给出平滑且满足约束的轨迹，但依赖初值，初值不好会收敛到局部最优甚至失败。因此实际系统往往需要「多规划器组合 + 超时兜底 + 缓存」。
第二个难点是碰撞检测的性能。规划器 90% 以上的时间花在碰撞检测上，因此碰撞检测的实现质量直接决定规划速度。FCL、HPP-FCL、Bullet 的性能差异在高维场景下可达数倍。
本文按「问题形式化 → 构型空间与碰撞 → 采样规划 → 图搜索 → 优化规划 → 轨迹参数化 → 时间最优 → 移动机器人 → 选型调参」的顺序展开，给出算法伪码、参数取值与工程经验。示例以 MoveIt2 与 OMPL 为参照实现。
读完本文你应当能够：为具体场景选择合适的规划器组合、理解各规划器的参数含义、实现碰撞检测的加速策略、以及把规划出的几何路径变成可执行的时间参数化轨迹。
目录
- 规划问题的形式化
- 构型空间与碰撞检测
- 采样规划：RRT 与 RRT*
- 图搜索：PRM、BIT* 与 Informed RRT*
- 优化规划：CHOMP、STOMP 与 TrajOpt
- 轨迹参数化：多项式与梯形速度
- 时间最优轨迹生成与动力学约束
- 移动机器人规划：Nav2 与代价地图
- 规划器选型、超时策略与调参
1. 规划问题的形式化
规划问题的标准表述是在构型空间中找一条连续路径，满足无碰撞与约束。
给定：
C ：构型空间（六轴臂为 R^6，受关节限位约束成 6 维超矩形）
C_free ：无碰撞的构型集合，C_obs 为其补集
q_start ：起始构型
q_goal ：目标构型（或目标区域）
求：
连续映射 σ: [0,1] → C_free
满足 σ(0) = q_start, σ(1) = q_goal
附加要求（按场景）：
平滑性：曲率/加加速度有界
动力学可行：速度、加速度、力矩不超限
最优性：路径长度、时间、能耗最小
鲁棒性：与障碍保持安全距离
三条路线的本质区别在于「如何探索 C_free」：
采样法：随机采样构型，用碰撞检测判断是否在 C_free，连成图或树
完备性：概率完备（时间→∞ 时找到解的概率→1）
优点：高维通用、无需障碍物显式表示
缺点：解的质量不稳定、路径不平滑
图搜索法：把 C 离散成图（栅格或路标），用 A*/Dijkstra 搜索
完备性：分辨率完备
优点：给定图后最优性可保证
缺点：维度灾难（6 维栅格不可行）
优化法：把路径参数化成优化变量，直接最小化代价函数
完备性：无保证，依赖初值
优点：解平滑、可直接编码约束
缺点：局部最优、可能失败
2. 构型空间与碰撞检测
碰撞检测是规划的算力黑洞。三个层次的加速手段，收益递减但都值得做。
层次 1：包围体层次（BVH）
用 AABB/OBB/k-DOP 包围几何，先做粗筛
FCL 的默认 BVH 是 OBB，对凸多面体效果好
典型加速：无 BVH 时 100 µs/次 → 有 BVH 时 2 µs/次
层次 2：连续碰撞检测（CCD）与离散采样
离散：在路径上取 n 个采样点检测（快但可能漏检）
连续：扫掠体（swept volume）检测（准但慢）
工程折中：离散采样间隔小于最薄障碍厚度的一半
层次 3：碰撞缓存与提前退出
缓存「已知无碰撞的构型」，重复查询直接命中
检测到第一个碰撞立即返回，不做全量
MoveIt 的 CollisionEnv 支持缓存，命中率高时收益显著
// 用 FCL 做单次碰撞查询的骨架
#include <fcl/fcl.h>
fcl::CollisionRequest request;
fcl::CollisionResult result;
request.num_max_contacts = 1; // 只要一个接触点，提前退出
request.enable_contact = false; // 不需要接触信息时关掉，快 30%
fcl::CollisionObjectd robot_obj(robot_mesh);
fcl::CollisionObjectd env_obj(env_mesh);
fcl::collide(&robot_obj, &env_obj, request, result);
bool in_collision = result.isCollision();
一个实用的自碰撞检查优化：不是所有连杆对都可能碰撞，可以预先用「相邻连杆不检查」「已知不可达的连杆对不检查」构建检查矩阵，通常能减少 50%~70% 的检查量。MoveIt 的 ACM（Allowed Collision Matrix）正是这个机制。
3. 采样规划：RRT 与 RRT*
RRT（快速扩展随机树）是最常用的采样规划器，核心是「朝随机点扩展最近的树节点」。
RRT 伪码：
T.init(q_start)
while not timeout:
q_rand ← sample() // 均匀采样，或偏向目标采样
q_near ← nearest(T, q_rand)
q_new ← steer(q_near, q_rand, step) // 朝 q_rand 走一步，步长 step
if collisionFree(q_near, q_new):
T.add(q_new, parent=q_near)
if reached(q_new, q_goal): return path(T, q_new)
关键参数：
step（步长）：太大易撞、太小收敛慢，典型取关节空间最大范围的 5%~10%
采样偏置：以 5%~10% 概率直接采样目标点，加速收敛
目标判定：距离阈值（如 0.1 rad）或精确匹配
RRT* 在 RRT 基础上加入「重选父节点」与「重连」，保证渐近最优：
RRT* 的两处增量：
1. 重选父节点：q_new 加入前，在半径 r 内的邻居中选代价最小的作父节点
r = min(γ·(log n / n)^(1/d), η)，d 为维度
2. 重连：检查邻居是否能通过 q_new 获得更小代价，是则改父节点
代价：路径长度（或时间、能耗）
RRT* 的代价是收敛慢——找到第一个解后需要继续采样数千次才能接近最优。工程上的实用变体是 Informed RRT*：找到初始解后，把采样限制在以 q_start 与 q_goal 为焦点的椭球内，大幅加速收敛。
| 规划器 | 首次解速度 | 渐近最优 | 适用 |
|---|---|---|---|
| RRT | 快 | 否 | 只要可行解、时间紧 |
| RRT-Connect | 很快 | 否 | 高维、双向扩展 |
| RRT* | 慢 | 是 | 需要较短路径、可等待 |
| Informed RRT* | 中 | 是 | 有初始解后精化 |
| BIT* | 中 | 是（近似） | 综合最优性与速度 |
4. 图搜索：PRM、BIT* 与 Informed RRT*
PRM（概率路标图）适合「同一环境下多次查询」的场景，如固定工位的机械臂。
PRM 两阶段：
构建阶段（离线）：在 C_free 中随机撒 N 个路标点，
把距离小于 r 且连线无碰撞的点对连边
查询阶段（在线）：把 q_start 与 q_goal 连入图，用 A*/Dijkstra 搜索
参数：
N：路标点数，2D 用 100~500，6D 用 1000~5000
r：连接半径，需保证图的连通性，典型 0.5~2.0（归一化后）
优势：一次构建、多次查询，查询是毫秒级
劣势：环境变化需重建（或增量更新，如 Lazy PRM）
BIT*（Batch Informed Trees）是近年综合表现最好的采样规划器之一，它把采样与搜索统一在一个框架里：用一个隐式随机几何图（RGG）表示问题，用 A* 式的启发式搜索加批处理采样，既保证渐近最优又比 RRT* 快数倍。
# 在 OMPL 里切换规划器只需改名字
ros2 param set /move_group ompl.planning_plugin ompl_interface/OMPLPlanner
ros2 param set /move_group ompl.planner_configs.RRTstar.type geometric::RRTstar
ros2 param set /move_group ompl.planner_configs.RRTstar.range 0.15
ros2 param set /move_group ompl.planner_configs.RRTstar.goal_bias 0.05
ros2 param set /move_group ompl.planner_configs.BITstar.type geometric::BITstar
5. 优化规划：CHOMP、STOMP 与 TrajOpt
优化规划把路径参数化成变量，直接最小化「平滑代价 + 障碍代价」。
统一形式：
min_ξ c_smooth(ξ) + c_obstacle(ξ) + c_constraint(ξ)
ξ：路径参数（离散点序列或样条控制点）
c_smooth：平滑项，通常用加速度或加加速度的平方积分
c_obstacle：障碍项，用符号距离场（SDF）构造可微代价
c_constraint：关节限位、速度上限等，用罚函数或增广拉格朗日
三种算法的区别在梯度来源与收敛特性：
CHOMP：用预计算的 SDF 梯度，梯度是解析的，收敛快
缺点：SDF 需离线预计算，动态障碍不适用
STOMP：不用梯度，用随机采样估计下降方向
优点：可处理不可微代价（如离散碰撞检测）
缺点：收敛慢，需要更多迭代
TrajOpt：用序列凸优化（SCP），把非凸问题逐次线性化
优点：能显式处理约束、速度快
缺点：依赖初值，需要好的初始化
# TrajOpt 风格的序列凸优化骨架（伪码）
def trajopt(x0, goal, env, iters=50):
x = init_trajectory(x0, goal) # 直线插值作为初值
for k in range(iters):
# 在当前轨迹附近线性化碰撞代价与约束
A, b = linearize_collision(x, env) # 基于 SDF 的线性化
# 求解凸子问题（QP 或 SOCP）
x_new = solve_qp(cost_smooth(x), A, b, joint_limits)
if converged(x, x_new):
return x_new
x = x_new
return x
优化规划的关键是初值质量。用 RRT 先找到一条粗糙可行路径作为初值，再用 TrajOpt 精化，是工业界的标准组合，兼顾了完备性与解质量。
6. 轨迹参数化：多项式与梯形速度
规划器给出的是几何路径（一串构型），必须转成带时间的轨迹才能交给控制器。
三次多项式（给定起止位置与速度）：
q(t) = a0 + a1·t + a2·t² + a3·t³
约束：q(0)=q0, q(T)=q1, q̇(0)=v0, q̇(T)=v1
解：a0 = q0
a1 = v0
a2 = (3(q1-q0) - (2v0+v1)T) / T²
a3 = (2(q0-q1) + (v0+v1)T) / T³
缺点：加速度在端点不连续（jerk 无穷大）
五次多项式（再加加速度约束）：
q(t) = Σ a_i·t^i, i=0..5
约束：位置、速度、加速度共 6 个 → 唯一确定 6 个系数
优点：加速度连续，jerk 有界
代价：计算量略大，末端加速度被强制为零（可能不自然）
Eigen::VectorXd quinticCoeffs(double q0, double q1,
double v0, double v1,
double a0, double a1, double T) {
Eigen::VectorXd c(6);
c[0] = q0;
c[1] = v0;
c[2] = 0.5 * a0;
const double T2 = T*T, T3 = T2*T, T4 = T3*T, T5 = T4*T;
c[3] = (20*(q1-q0) - (8*v1 + 12*v0)*T - (3*a0 - a1)*T2) / (2*T3);
c[4] = (30*(q0-q1) + (14*v1 + 16*v0)*T + (3*a0 - 2*a1)*T2) / (2*T4);
c[5] = (12*(q1-q0) - 6*(v1 + v0)*T + (a0 - a1)*T2) / (2*T5);
return c;
}
多段轨迹要保证拼接点的连续性（C¹ 或 C²）。工程上常用「分段五次 + 拼接点速度/加速度约束」或直接用 B 样条（天然高阶连续）。B 样条是更现代的选择：控制点不直接等于路径点，因此可以自由调整平滑度，且局部修改不影响全局。
7. 时间最优轨迹生成与动力学约束
给定几何路径，求「最快走完」的时间参数化，是时间最优轨迹生成（TOTG）问题。
路径参数化：q(s)，s ∈ [0,1]
速度约束：|q'(s)·ṡ| ≤ q̇_max → ṡ ≤ min_i(q̇_max,i / |q'_i(s)|)
加速度约束：|q''(s)·ṡ² + q'(s)·s̈| ≤ q̈_max
求解：对每个 s 求最大可行的 ṡ(s)，同时满足加速度耦合约束
经典的 Bobrow 算法：先在 s-ṡ 相平面上做「最大速度曲线」与「切换点」
现代做法：用 TOPP-RA（Time-Optimal Path Parameterization by Reachability Analysis）
把问题离散成线性约束的凸问题，鲁棒且快
# TOPP-RA 的接口（Python 绑定）
import toppra as ta
import numpy as np
# 定义路径：关节空间的样条
path = ta.SplineInterpolator(np.linspace(0, 1, 50), waypoints)
# 定义约束：速度与加速度上限
pc_vel = ta.constraint.JointVelocityConstraint(vlim)
pc_acc = ta.constraint.JointAccelerationConstraint(alim)
# 求解
instance = ta.algorithm.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel")
jnt_traj = instance.compute_trajectory(0, 0) # 起止速度为零
print(jnt_traj.duration)
时间最优轨迹的特点是「贴着约束走」——某些关节始终处于速度或加速度上限。这在理论上最快，但实际执行时因为模型误差会频繁触碰限位，导致控制器报警。工程上通常把约束乘一个安全系数（0.8~0.9）留出余量，或改用「时间近似最优」的平滑版本。
8. 移动机器人规划：Nav2 与代价地图
移动机器人的规划是「全局 + 局部」两层结构，Nav2 是 ROS2 的标准实现。
全局规划器（Global Planner）：
在全局代价地图上规划，频率 0.1~1 Hz
Nav2 内置：NavFn（Dijkstra/A*）、Smac（状态格，考虑运动学）、Theta*
输出：路径点序列
局部规划器（Local Planner / Controller）：
在局部代价地图上高频重规划，频率 10~20 Hz
内置：DWB（DWA 的 ROS2 版）、TEB、MPPI、RPP（纯追踪）
输出：速度指令
代价地图分层：
静态层：来自 SLAM/地图，只更新一次
障碍层：来自传感器，实时更新（感知侧的做法见[机器人视觉与抓取](/robotics-perception-grasping/)）
膨胀层：给障碍加安全半径（膨胀半径应 ≥ 机器人内切半径）
体素层：3D 障碍投影（用于三维感知）
# Nav2 关键参数
controller_server:
ros__parameters:
controller_frequency: 20.0
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
max_vel_x: 0.8 # 最大前进速度 m/s
max_vel_theta: 1.5 # 最大角速度 rad/s
acc_lim_x: 1.0
acc_lim_theta: 2.0
xy_goal_tolerance: 0.15
yaw_goal_tolerance: 0.1
vx_samples: 20
vtheta_samples: 40
sim_time: 1.7 # 前向仿真时长，太短会撞、太长会犹豫
local_costmap:
local_costmap:
ros__parameters:
width: 6
height: 6
resolution: 0.05
inflation_layer:
inflation_radius: 0.55 # 通常为机器人半径的 2~3 倍
cost_scaling_factor: 3.0
膨胀半径与代价衰减是最难调的参数。半径太小，机器人贴墙走容易刮蹭；太大，狭窄通道通不过。判据是「膨胀半径 < 通道宽度的一半 - 机器人半径」，如果实际环境不满足，就得用更小的半径并依赖局部规划器的避障。
9. 规划器选型、超时策略与调参
实际系统的可靠性来自「组合与兜底」，而不是单一天才规划器。
推荐组合（机械臂）：
1. 尝试缓存路径（命中率在固定工位下可达 60%~80%）
2. 尝试 RRT-Connect，超时 200 ms
3. 尝试 Informed RRT*，超时 500 ms
4. 尝试 TrajOpt（用步骤 2/3 的结果作初值），超时 300 ms
5. 全部失败 → 上报失败并保持当前位置，绝不用「最近可行点」硬凑
推荐组合（移动机器人）：
全局：Smac（考虑运动学）或 NavFn（快）
局部：MPPI（平滑、适合差速）或 TEB（适合阿克曼）
失败兜底：局部规划失败 → 原地旋转重定位 → 仍失败 → 停车求助
调参优先级从高到低：碰撞检测的 ACM 配置（漏配会导致规划极慢）→ 规划超时与重试策略 → 采样偏置与步长 → 平滑与安全距离。不要一上来就调算法参数，先确认 ACM 与代价地图没有配置错误，这两处问题占了「规划很慢」投诉的大半。
规划耗时必须打点监控。规划时间分布通常是重尾的（p50 可能 20 ms，p99 可能 2 s），只看均值会严重误判。目标是把 p99 控制在预算内，而不是把均值压低。
权衡取舍
| 决策 | 选 A | 选 B |
|---|---|---|
| 采样 vs 优化 | 采样：高维、动态环境、要完备性 | 优化：要平滑轨迹、约束明确 |
| RRT vs RRT* | RRT：只要可行解、时间紧 | RRT*：要短路径、可等待 |
| PRM vs RRT | PRM：固定环境多次查询 | RRT：单次查询、环境常变 |
| 轨迹表示 | 多项式：简单、可解析 | B 样条：高阶连续、局部可调 |
| 时间参数化 | 梯形速度：简单 | TOPP-RA：最优、约束完整 |
| 全局 vs 局部 | 全局：长距离、静态地图 | 局部：动态障碍、高频重规划 |
核心原则：规划器的选择由「环境是否动态」与「是否需要最优」两个维度决定。静态且要最优，用 PRM + 优化；动态且只要可行，用 RRT-Connect + 高频局部重规划。
常见坑清单
- ACM（允许碰撞矩阵）漏配，机器人检查与自身所有连杆的碰撞，规划慢十倍——按相邻与不可达关系配置 ACM。
- 碰撞检测用离散采样但间隔大于最薄障碍厚度，穿透漏检——采样间隔小于最薄障碍的一半。
- 膨胀半径大于通道宽度一半，狭窄区域永远规划失败——半径与通道宽度联合标定。
- RRT 步长过大，路径频繁撞障被拒，收敛极慢——步长取关节范围的 5%~10%。
- 只用均值评估规划耗时，掩盖了 p99 的 2 秒长尾——监控 p99 与超时率。
- 优化规划初值用直线插值，穿越障碍时收敛失败——用采样规划的结果作初值。
- 时间最优轨迹贴着约束执行，模型误差导致频繁触发限位报警——约束乘 0.8~0.9 安全系数。
- 规划失败后用「最近可行点」硬凑，机器人做出意外动作——失败必须显式上报而非降级凑合。
- 局部规划器的
sim_time太短，高速时来不及避障——sim_time × 最大速度 ≥ 最小制动距离。 - 代价地图只更新障碍层不做膨胀，机器人贴障擦碰——膨胀层是必需而非可选。
小结
运动规划的三条路线各有明确的适用边界：采样法胜在高维通用与概率完备，图搜索胜在固定环境下的最优性，优化法胜在解的平滑与约束表达。真实系统靠组合使用并设置超时兜底来获得可靠性，而不是指望某个算法万能。
下一步建议阅读机械臂控制与抓取规划 ，看 MoveIt2 如何把规划、运动学、控制串成完整管线；以及机器人仿真 ，理解如何在 Isaac Sim 与 Gazebo 里做大规模规划回归测试。移动平台的规划与 SLAM 紧密耦合，定位建图那一篇会讲清代价地图的位姿来源。
最后一句：把规划耗时与失败率当作一等指标监控。规划器不是「配好就不管」的组件，它的长尾行为需要在生产中持续观察。
继续阅读
探索更多技术文章
浏览归档，发现更多关于系统设计、工具链和工程实践的内容。