3. 轨迹规划基础
在进入多约束轨迹规划与实时优化之前，必须先厘清一组最基础却最容易混淆的概念：路径与轨迹、配置空间与工作空间、自由度与运动学模型。它们决定了后续约束如何表达、优化变量如何选取、求解器在哪个空间里搜索可行解。如果这些概念边界不清，后续的碰撞约束、速度约束、力矩约束就会“张冠李戴”，导致规划结果看似合理却无法执行。
一、路径与轨迹：几何与时间的分界
路径（Path）只描述几何形状，是空间中的一条连续曲线，不包含时间信息。例如“从A点沿直线到B点，再绕过障碍物到C点”，它回答的是“走哪条线”。轨迹（Trajectory）则在路径基础上加入时间参数，形成 q(t)，回答“什么时候到达哪里、以多快速度通过”。因此轨迹天然携带速度、加速度甚至加加速度信息，是控制器真正跟踪的对象。
这一区别直接决定约束的归属：碰撞、几何边界、关节限位属于路径层约束；速度上限、加速度上限、力矩饱和、实时性属于轨迹层约束。多约束轨迹规划的本质，是在路径可行集与时间参数化可行集之间做联合搜索。
二、配置空间与工作空间：两种坐标视角
配置空间（C-space）以机器人所有独立自由度为坐标轴。一个n自由度机械臂的配置是 q=[q1,q2,…,qn]，C-space 是n维空间。它的最大优势是把机器人本体“压缩”为一个点，障碍物则膨胀为C-space障碍区域，碰撞检测转化为点是否落入禁区的问题，非常适合采样规划与优化建模。
工作空间（Workspace）描述末端执行器在笛卡尔空间中可达的位置与姿态集合，是任务层最直观的表达。工作空间分为可达工作空间与灵巧工作空间：前者只要求至少存在一组逆解，后者要求在该位置能以任意姿态到达。工作空间受关节限位、连杆长度、奇异位形共同约束。
两者关系可概括为：C-space 是“关节怎么动”，工作空间是“末端到哪去”。轨迹规划通常在C-space中保证动力学与碰撞可行，在任务空间校验工艺点与姿态要求。
三、自由度与运动学建模：从关节到末端
自由度（DOF）是描述系统位形所需的最少独立坐标数。串联机械臂通常每个关节贡献一个自由度，6自由度机械臂可在工作空间内实现任意位置与姿态；冗余机械臂（如7自由度）在到达同一末端位姿时存在无穷多组关节解，这为避障、避奇异、优化关节力矩提供了额外空间，但也使规划问题维度升高。
运动学建模分两步：正运动学由关节角 q 计算末端位姿 x=f(q)，通常用DH参数或旋量法建立；逆运动学由目标位姿求关节角 q=f⁻¹(x)，可能无解、唯一解或多解。轨迹规划中，正运动学用于碰撞检测与可视化，逆运动学用于把任务空间路径点转换为关节空间轨迹点。
对于实时优化，还需引入微分运动学：ẋ = J(q)q̇，其中 J 为雅可比矩阵。它把关节速度映射为末端速度，是速度级约束、奇异分析、力/力矩传递的核心工具。当 J 降秩时进入奇异位形，某些方向末端速度无法产生，逆解不稳定，规划器必须提前规避或降速通过。
四、方法流程与场景案例
以一个6自由度机械臂在狭长通道内抓取工件为例，典型流程如下：
- 在任务空间给定抓取位姿与放置位姿，确认均位于灵巧工作空间内。
- 用逆运动学求取起止关节配置，检查是否满足关节限位。
- 在C-space中构造路径，将通道壁面与工件膨胀为C-space障碍。
- 对路径做时间参数化，施加关节速度、加速度、力矩约束。
- 用雅可比检查全程是否接近奇异，必要时调整冗余自由度或路径。
- 输出 q(t)、q̇(t)、q̈(t) 交给控制器执行。
若跳过第3步直接在任务空间插值，末端虽走直线，但关节可能大幅摆动甚至撞墙；若跳过第5步，轨迹在奇异点附近速度会突增，实际执行时出现抖动或保护停机。
| 概念 | 描述对象 | 典型维度 | 在规划中的作用 |
|---|---|---|---|
| 路径 | 几何曲线 | 与DOF同维 | 确定走哪条线，承载几何约束 |
| 轨迹 | 带时间的运动 | q(t) 及导数 | 承载速度、加速度、力矩约束 |
| 配置空间 | 关节坐标集合 | n维 | 碰撞检测与采样优化 |
| 工作空间 | 末端位姿集合 | 3D位置+姿态 | 任务可达性与工艺校验 |
| 自由度 | 独立坐标数 | 整数n | 决定优化维度与冗余性 |
| 雅可比 | 速度映射矩阵 | 6×n | 奇异分析、速度约束、力传递 |
五、常见误区与复盘
误区一：把路径当轨迹直接下发。控制器需要的是带时间律的轨迹，仅有几何路径无法执行。
误区二：只在工作空间做规划。末端可行不代表关节可行，必须回到C-space校验碰撞与限位。
误区三：忽视奇异位形。雅可比降秩处逆解病态，速度指令可能发散，需提前降速或调整路径。
误区四：冗余自由度随意取值。冗余解虽多，但不同解对力矩、避障、平滑性影响巨大，应作为优化变量而非任意选择。
复盘建议：每次规划失败时，先判断问题出在“路径层”还是“时间层”，再判断是C-space约束还是工作空间约束，最后检查雅可比条件数。按这个顺序排查，能快速定位是几何不可行、动力学不可行，还是奇异导致数值问题。
# 典型检查顺序（伪代码）
q_start = IK(x_start) # 逆解起点
q_goal = IK(x_goal) # 逆解终点
path = plan_in_cspace(q_start, q_goal, obstacles)
traj = time_parameterize(path, v_max, a_max, tau_max)
for q in traj:
if not within_joint_limits(q): reject()
if near_singularity(J(q)): slow_down_or_replan()
execute(traj)
掌握路径与轨迹的分界、C-space与工作空间的映射、自由度与雅可比的作用，就为后续多约束建模打下了坐标系与变量基础。后续所有约束——碰撞、速度、力矩、实时性——都将落到这些空间与变量之上。