做机器人控制这些年，有一类话题几乎绕不开：轨迹生成，尤其是关节空间规划。新手接触机器人学时，最难跨过去的坎往往不是正逆运动学，而是“让机械臂从A点动到B点，中间那段路到底怎么走”。关节空间规划就是回答这个问题的一套系统方法，它负责在每个关节的角度层面生成一条随时间变化的光滑曲线，让机械臂动起来不抖、不超限、不伤电机。这篇文章我会从原理、公式、代码到坑点，完整拆一遍关节空间轨迹规划的思路和落地细节，适合刚学机器人学、想把课本知识变成实际运动指令的读者。
1. 轨迹生成的两层逻辑：先分清路径、轨迹与规划对象
1.1 路径与轨迹的区别
国内很多教材会把“路径规划”和“轨迹规划”混在一起讲，但工程上这是两回事，弄混了后面写代码会非常痛苦。
路径（Path）只描述几何关系，是一串空间的点或曲线，比如末端从点A走到点B，中间经过哪些位置，它不关心“什么时候到”“走多快”。轨迹（Trajectory）则是带时间戳的运动描述，每个时刻机械臂应该处于什么位形、速度是多少、加速度是多少。轨迹是对路径的“时间参数化”，本质上是在一条几何路径的基础上附加运动律。
关节空间规划处理的直接对象是关节角向量 $q(t) \in \mathbb{R}^n$，n是自由度个数。对六轴机械臂来说，规划的就是六个角度随时间变化的序列。为什么不在笛卡尔空间直接规划？因为在笛卡尔空间虽然直观，却要面对逆解、奇异点和冗余问题，每个插值点都要做一次逆运动学，计算量大而且可能没有解。关节空间规划避开了这些问题：只要各关节角连续变化，机器人末端自然就走出一条连续曲线。
用开车类比：笛卡尔空间规划是要求车子“必须沿着某条道路标线走”，关节空间规划则是说“只要方向盘转得平滑、不超速，具体走哪条线都行”。前者约束强但计算复杂，后者约束弱但高效可靠。
1.2 关节空间与笛卡尔空间的选择权衡
关节空间规划的优势十分明确：
- 维度低：一个6轴机器人只要规划6条角度曲线，笛卡尔空间要规划末端位姿的3+3或更多分量。
- 天然避奇异：关节空间的曲线始终定义在关节活动范围内，不存在逆运动学奇异问题。
- 计算开销小：只需要多项式插值或样条插值，单次计算成本很低，能满足很高的控制周期要求。
但它的缺点是末端路径不直观，无法保证末端走过的轨迹是直线或圆弧。如果应用场景要求末端必须走直线（比如涂胶、焊接），就得在笛卡尔空间规划，或者采用混合规划：先用笛卡尔空间采样路径点，再转换到关节空间做插值平滑。
我见过很多初学机器人学的朋友一上来就追求笛卡尔空间直线规划，结果被奇异性卡得怀疑人生。我的建议是：除非工艺明确要求直线轨迹，否则优先做关节空间规划，简单可靠，后处理难度小。
| 维度 | 关节空间规划 | 笛卡尔空间规划 |
|---|---|---|
| 规划对象 | 关节角度 q(t) | 末端位姿 p(t) |
| 计算成本 | 低，多项式插值即可 | 高，逐点逆解 |
| 奇异问题 | 不存在 | 存在，处理复杂 |
| 末端轨迹 | 不直观，非直线 | 可精确控制 |
| 适用场景 | 点到点搬运、常规关节运动 | 焊接、涂胶、雕刻等直线/圆弧工艺 |
2. 三次多项式：最常用也最好理解的关节插值
2.1 四个边界条件定一条曲线
关节空间轨迹规划里，最基础的问题是单关节从一个角度 $\theta_0$ 转到另一个角度 $\theta_f$，给定运动时间 $t_f$，要求起点和终点速度为零（或者指定速度），生成一条平滑的角度曲线。
三次多项式的通用形式是：
$$ \theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 $$
求导得到速度和加速度：
$$ \dot{\theta}(t) = a_1 + 2a_2 t + 3a_3 t^2 $$
$$ \ddot{\theta}(t) = 2a_2 + 6a_3 t $$
为什么选三次而不是二次、一次？因为要同时满足“起止位置”“起止速度”四个约束，至少需要4个系数，三次多项式刚好提供了4个待定系数。如果还有加速度约束，就得用五次多项式，后文会讲。
设定边界条件：$\theta(0)=\theta_0, \theta(t_f)=\theta_f, \dot{\theta}(0)=0, \dot{\theta}(t_f)=0$，代入推导。
由 $\theta(0)=a_0=\theta_0$ 直接得到常数项。由 $\dot{\theta}(0)=a_1=0$ 得一次项为零。联立剩下的方程：
$$ a_2 t_f^2 + a_3 t_f^3 = \theta_f - \theta_0 $$
$$ 2a_2 t_f + 3a_3 t_f^2 = 0 $$
解得：
$$ a_2 = \frac{3(\theta_f - \theta_0)}{t_f^2}, \quad a_3 = -\frac{2(\theta_f - \theta_0)}{t_f^3} $$
这个结果是关节空间规划里的“hello world”，滚瓜烂熟非常有价值。
2.2 一个手算实例
假设某关节从 $\theta_0=20^\circ$ 运动到 $\theta_f=80^\circ$，时间 $t_f=2s$，两端速度为零。先把角度转成弧度避免单位混乱：
$$ \Delta \theta = 60^\circ \approx 1.0472 , rad $$
代入计算：
$$ a_2 = \frac{3 \times 1.0472}{2^2} = 0.7854 $$
$$ a_3 = -\frac{2 \times 1.0472}{2^3} = -0.2618 $$
于是角度曲线为：
$$ \theta(t) = 0.3491 + 0.7854t^2 - 0.2618t^3 $$
其中 $\theta_0=20^\circ \approx 0.3491 , rad$。在 $t=1s$ 时，$\theta(1)=0.3491+0.7854-0.2618=0.8727 , rad \approx 50^\circ$，正好是中间点，说明曲线对称。
速度曲线是一条抛物线：$\dot{\theta}(t)=1.5708t - 0.7854t^2$，在 $t=1s$ 达到最大速度约 $0.7854 rad/s$。加速度是直线：$\ddot{\theta}(t)=1.5708 - 1.5708t$，起点为正、终点为负，实现先加速后减速。
这个运动过程很像手动挡汽车起步：踩油门匀速加速，接近目的地逐步松油门，到点刚好刹停。整段过程中没有突然的加速度跳变，对电机冲击较小。
2.3 中间点速度不为零的情况
实际应用里很多动作不需要停在每个路径点，比如连续搬运中经过一个中间点但不停顿。这时把起止速度设为零就不合适。更通用的写法是指定起点速度 $v_0$ 和终点速度 $v_f$：
$$ \theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 $$
$$ \theta(0)=\theta_0, \quad \theta(t_f)=\theta_f, \quad \dot{\theta}(0)=v_0, \quad \dot{\theta}(t_f)=v_f $$
解方程组：
$$ a_0 = \theta_0, \quad a_1 = v_0 $$
$$ a_2 = \frac{3(\theta_f-\theta_0)}{t_f^2} - \frac{2v_0 + v_f}{t_f} $$
$$ a_3 = -\frac{2(\theta_f-\theta_0)}{t_f^3} + \frac{v_0 + v_f}{t_f^2} $$
写成代码时建议用线性代数库求解，避免手推错公式。这里给一个用Python计算单段三次多项式系数的示例：
import numpy as np def cubic_coeffs(theta0, thetaf, v0, vf, tf): # 解 A * [a0,a1,a2,a3]^T = b A = np.array([ [1, 0, 0, 0], [1, tf, tf**2, tf**3], [0, 1, 0, 0], [0, 1, 2*tf, 3*tf**2] ]) b = np.array([theta0, thetaf, v0, vf]) return np.linalg.solve(A, b) # 例：20度到80度，2秒，起止速度0 coeffs = cubic_coeffs(np.deg2rad(20), np.deg2rad(80), 0, 0, 2.0) print(coeffs) # 输出约 [0.34906585, 0, 0.78539816, -0.26179939]用np.linalg.solve的好处是如果以后想加约束（比如限速），改矩阵就行，不用重新推公式。
3. 多路径点拼接：从单段运动到连续流水线作业
3.1 多段三次多项式的衔接条件
实际机械臂工作很少只做一次单关节运动，更多是一连串的点位：抓取位、提升位、过渡位、放置位，中间还要躲避障碍物。这时要把多段三次多项式首尾拼接起来。
假设有 $n+1$ 个路径点 $q_0, q_1, ..., q_n$，对应时间 $t_0, t_1, ..., t_n$，每段都是三次多项式。拼接需要满足：
- 位置连续：$\theta_i(t_i) = q_i$，每段端点严格经过路径点。
- 速度连续：$\dot{\theta}i(t_i) = \dot{\theta}{i+1}(t_i)$，避免速度突变。
- 加速度连续（可选）：$\ddot{\theta}i(t_i) = \ddot{\theta}{i+1}(t_i)$，这是三次样条的特征，三次多项式单段拼接不保证。
如果只是每段独立算三次多项式，中间点速度按零处理，机械臂会在每个路径点“停顿”一下再继续走。视觉上就是一顿一顿的，非常影响节奏和效率，也增加机械冲击。
3.2 中间点速度的三种确定方法
第一种是手动指定中间点速度。比如某工艺要求在连续路径上以恒速通过中间点，可根据任务需求直接设定。但这需要经验，而且一旦速度设得过高，关节可能超限。
第二种是自动估算中间点速度。一种简单做法是用相邻路径段的平均速度作为中间点速度：取前一段的平均速度 $\bar{v}_{i-1}$ 和后一段的平均速度 $\bar{v}_i$ 的加权平均。公式表达为：
$$ v_i = \frac{\bar{v}{i-1} \cdot \Delta t{i} + \bar{v}i \cdot \Delta t{i-1}}{\Delta t_{i-1} + \Delta t_i} $$
这个方案工程上很实用，不需要额外调参，就能让机械臂“顺滑地滑过”中间点。
第三种是采用三次样条插值，让整条轨迹作为一条全局平滑曲线，所有中间点的速度、加速度都由求解方程组自动得出，不需要人为指定。
对于多段拼接，我个人的经验是：如果控制器的刷新频率足够高（比如1kHz以上），就优先用三次样条，因为加速度连续能显著减少机械振动。如果控制器老旧或算力紧张，退而求其次用平均速度法，效果也不错但可能出现加速度跳变。
3.3 五次多项式补上加速度连续
三次多项式只能保证速度连续，加速度在细分段边界处可能突变。加速度突变意味着力突变，直观表现是机械臂在路径点附近“咯噔”一下，对精密装配或高速搬运来说会直接影响产品质量。
五次多项式引入了加速度项，完整的轨迹表达式为：
$$ \theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 $$
六个待定系数对应六个边界条件：起止位置、起止速度、起止加速度。
若设 $\theta_0, \theta_f, v_0=0, v_f=0, a_0=0, a_f=0$，解得：
$$ a_0 = \theta_0, \quad a_1 = 0, \quad a_2 = 0 $$
$$ a_3 = \frac{10(\theta_f-\theta_0)}{t_f^3}, \quad a_4 = -\frac{15(\theta_f-\theta_0)}{t_f^4}, \quad a_5 = \frac{6(\theta_f-\theta_0)}{t_f^5} $$
这个公式看起来很“重”，但实际计算量并不大，就是几次乘法和加法。五次多项式带来的平滑性提升非常明显，尤其适合高精度应用。唯一的缺点是最大速度比三次多项式略高，因为曲线需要在更短的时间内完成位移，这个代价在很多场景可接受。
下表是三次和五次多项式的直观对比：
| 特性 | 三次多项式 | 五次多项式 |
|---|---|---|
| 边界条件 | 位置+速度 | 位置+速度+加速度 |
| 加速度连续性 | 段内连续，边界可跳变 | 全程连续 |
| 最大加速度 | 相对较小 | 稍大 |
| 计算难度 | 低 | 低（公式固定） |
| 适用场景 | 常规搬运、低速运动 | 精密装配、高速高平稳要求 |
4. 梯形速度规划：工程界最常用的时间最优方案
4.1 从约束反推规划参数
前面说的多项式规划，本质上是“先定时间，再算曲线”，但如果给定时间太短，速度或加速度会超出电机和减速机的物理极限。工业界更常用的思路是“先定物理约束，再算最短时间”，这就是梯形速度规划。
梯形速度规划把运动分成三段：匀加速段、匀速段、匀减速段。速度-时间曲线呈梯形，因此得名。
给定约束：
- 最大关节速度 $v_{max}$
- 最大关节加速度 $a_{max}$
- 运动总位移 $\Delta q = q_f - q_0$
加速段所需时间为：
$$ t_{acc} = \frac{v_{max}}{a_{max}} $$
加速段位移为：
$$ \Delta q_{acc} = \frac{1}{2} a_{max} t_{acc}^2 = \frac{v_{max}^2}{2a_{max}} $$
如果 $2\Delta q_{acc} \ge |\Delta q|$，说明位移太短，还没加速到最大速度就得减速，此时梯形退化成三角形，实际峰值速度变为：
$$ v_{peak} = \sqrt{a_{max} \cdot |\Delta q|} $$
这是个关键判断，很多新手容易漏掉。
4.2 各段时间的计算方法与实例
假设某关节需要转动 $\Delta q = 200^\circ$，允许最大速度 $v_{max}=60^\circ/s$，最大加速度 $a_{max}=90^\circ/s^2$。
先算加速段时间：
$$ t_{acc} = \frac{60}{90} \approx 0.667s $$
加速段位移：
$$ \Delta q_{acc} = \frac{60^2}{2 \times 90} = 20^\circ $$
加速和减速段共占位移 $40^\circ$，剩余匀速位移 $160^\circ$。匀速段时间：
$$ t_{cruise} = \frac{160}{60} \approx 2.667s $$
总时间：
$$ T = 2 \times 0.667 + 2.667 = 4s $$
如果位移缩短到 $30^\circ$，则 $2\Delta q_{acc}=40^\circ > 30^\circ$，速度达不到最大值，按三角形规划：
$$ v_{peak} = \sqrt{90 \times 30} \approx 51.96^\circ/s $$
加速段时间 $t_{acc} = v_{peak}/a_{max} \approx 0.577s$，总时间 $T \approx 1.155s$。
这个计算过程在任何轨迹生成库中都是核心逻辑，搞清楚它能帮你在调试时快速判断“为什么运动比预设时间慢”。
4.3 梯形规划的小缺陷与改进
梯形速度规划的优点是简单、时间最优，但加速度曲线是方波，启动瞬间和停止瞬间有加速度跳变，冲击较大。如果机构刚性不足，会在启停位置出现振荡。
改进方案是S型速度规划（S-curve），让加速度本身也连续变化。S型规划将运动分为7段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。计算复杂度明显上升，但平滑性大幅改善，适合精密设备或重载搬运，价格昂贵的高速机器人普遍采用。
对标准的关节空间规划而言，梯形速度规划是最常用的基础方案，很多控制器内置的moveJ指令就是基于它。即使是梯形，配合后续提到的平滑滤波也能获得不错的效果。
5. 进阶思路：三次样条与全局平滑
5.1 三次样条的核心思想
三次样条把整条轨迹视为由多个三次多项式段组成，并强制让所有中间点处的速度、加速度都连续。因此整个轨迹是C2连续的（二阶连续可导），这在理论上是“平滑曲线”的标准定义。
假设有 $n+1$ 个路径点和 $n$ 段三次多项式。对每个中间点需要解一个方程，最终形成一个三对角矩阵。三对角矩阵可以用稀疏求解法高效计算，N个路径点的复杂度是 $O(n)$，性能非常好。
三次样条相比多段拼接的最大优势是“全局视野”：每一段的系数不仅由局部两个点决定，还受全局点的影响。这意味着某段曲线的形态会被相邻段“牵制”，整体更顺滑，不会出现某段过度弯曲的情况。
5.2 工程上如何选择规划阶次
我的习惯是分场景：
- 点到点单次运动，速度不要求精确：三次多项式就够。
- 多个路径点连续运动，有视觉观察要求：用三次样条或带速度估算的拼接。
- 对加速度敏感、或者机械臂负载较大：直接用五次多项式或S型曲线。
- 高速高精度、需要极低振动：升级为B样条或加前瞻滤波，但复杂度已经超出这篇文章的基础范围。
一个容易踩的坑是：理论上的平滑不代表实际控制稳定。关节空间轨迹必须下发到伺服驱动器，而驱动器的速度环、位置环也有滤波时间常数。如果轨迹加速度太高，实际电机会跟不上，造成跟踪误差，甚至触发跟随误差报警。所以规划的下发频率和伺服参数必须匹配。
6. 一个最小实现与验证方法
6.1 单关节三次插值的Python实现
真实项目里不会只规划单段，但最小的实现能帮你理解整套流程。先给出单段三次多项式规划的完整函数：
import numpy as np import matplotlib.pyplot as plt class JointTrajectory: def __init__(self, theta0, thetaf, tf, v0=0.0, vf=0.0): self.tf = tf self.coeffs = self._solve(theta0, thetaf, v0, vf, tf) def _solve(self, theta0, thetaf, v0, vf, tf): A = np.array([ [1, 0, 0, 0], [1, tf, tf**2, tf**3], [0, 1, 0, 0], [0, 1, 2*tf, 3*tf**2] ]) b = np.array([theta0, thetaf, v0, vf]) return np.linalg.solve(A, b) def position(self, t): a0, a1, a2, a3 = self.coeffs return a0 + a1*t + a2*t**2 + a3*t**3 def velocity(self, t): a1, a2, a3 = self.coeffs[1], self.coeffs[2], self.coeffs[3] return a1 + 2*a2*t + 3*a3*t**2 def acceleration(self, t): a2, a3 = self.coeffs[2], self.coeffs[3] return 2*a2 + 6*a3*t # 使用 traj = JointTrajectory(np.deg2rad(20), np.deg2rad(80), 2.0) t_arr = np.linspace(0, 2.0, 100) pos = [np.rad2deg(traj.position(t)) for t in t_arr] vel = [np.rad2deg(traj.velocity(t)) for t in t_arr] acc = [np.rad2deg(traj.acceleration(t)) for t in t_arr] plt.figure(figsize=(8, 6)) plt.subplot(3, 1, 1) plt.plot(t_arr, pos) plt.title("Position (deg)") plt.subplot(3, 1, 2) plt.plot(t_arr, vel) plt.title("Velocity (deg/s)") plt.subplot(3, 1, 3) plt.plot(t_arr, acc) plt.title("Acceleration (deg/s2)") plt.tight_layout() plt.show()把位置、速度、加速度画出来是轨迹规划调试最重要的步骤。可视化能让你一眼看出曲线在哪段不合理、最大速度有没有超限、加速度有没有突变。我每次调机器人之前都会先跑一遍离线仿真，宁可多花两分钟看曲线，也绝不上实体机去试错。
多段规划的代码更长了，核心是构建全局方程组或者逐段拼接后做后处理。这里不展开，思路清楚了代码自己就能写出来。
6.2 验证规划结果的几个关键指标
验证轨迹好坏，我一般看四件事：
- 终点是否精确到达：插值公式在数学上必然等于目标点，但浮点误差和控制器截断可能造成微小偏差。
- 最大速度是否超过驱动器限制：历史上有好多回因为规划速度超限导致电机过流报警，后来我养成习惯，在代码里加了一句
assert max_vel < 0.95 * driver_limit。 - 加速度是否平滑：加速度的突变幅度决定机械冲击，如果加速度曲线是折线，要对机构寿命有心理预期。
- 中间点是否与障碍物保持安全距离：关节空间规划不检查碰撞，多点路径之间的避障需要额外做路径搜索，这是另一个话题。
7. 常见问题与调试经验
7.1 常见问题速查表
把这几年在实验室和现场调试中遇到过的问题整理成了一张速查表，新项目排查时直接对照。
| 症状 | 可能原因 | 排查与解决 |
|---|---|---|
| 运动到一半电机过流报警 | 规划速度超过驱动器上限；加速度过大 | 核对关节最大速度/加速度参数，限制规划输出；降低梯形规划的 vmax/amax |
| 机械臂在路径点明显顿挫 | 中间点速度设为零；多段拼接未做速度连续 | 使用自动估算中间点速度；改用三次样条 |
| 末端轨迹振动、抖动 | 加速度突变过大；机构共振 | 改用五次多项式或S型曲线；增加低通滤波；检查机械连接刚度 |
| 运动时间比预设长很多 | 位移短而 vmax 大导致三角形规划 | 理解梯形/三角形的切换条件；计算实际峰值速度 |
| 跟踪误差持续增大 | 轨迹加速度超出伺服能力；控制周期太长 | 降低规划加速度；提高插补频率；检查伺服增益 |
| 多个关节运动不协调 | 各关节独立规划导致同步性差 | 使用相同的时间节点，所有关节共享同一个时间相位 |
7.2 实操中总结的避坑技巧
第一，规划的时间参数必须留裕量。很多项目里机械臂能承受的理论最大加速度比实际工艺能接受的大得多，真正的限制往往来自末端负载的惯量和夹具的刚性。我通常把加速度设为理论值的70%左右，既保证效率，又留出纠错空间。
第二，离线仿真和在线调试要分开。离线阶段用Matplotlib画曲线、检查超限；在线阶段用真实控制器的示波器记录实际关节角度和指令角度对比，两者差别过大说明轨迹下发频率太高或控制周期跟不上。
第三，路径点不要太密。有些人为了轨迹贴合某个期望形状，把中间点设得很密，结果导致曲线弯弯曲曲、加速度震荡。关节空间规划不是拟合工具，点越少、曲线越容易平滑。如果确实需要复杂路径，应在笛卡尔空间规划好，再转换到关节空间。
第四，别忘了单位。角度制、弧度制混用是我见过最多的低级错误。建议工程代码里统一用弧度，只在人机界面显示角度。
第五，考虑故障异常。即使规划得再好，也要在代码里加超限保护：轨迹执行过程中实时检查位置、速度是否越界，一旦异常立刻切换抱闸或急停。这不是可选项，是安全底线。
8. 最后再分享一点私货
关节空间规划这块，我个人踩过最大的坑，是刚开始做项目时迷信“高阶多项式更平滑”这句话。在某次高速搬运项目里，我一开始就写了七次多项式去拟合一条很长的连续轨迹，结果不仅计算量大、数值不稳定，还因为导数振荡导致电机发热严重。后来换成“三段式”处理，用梯形速度规划做宏观调度、每段内部用五次多项式做微观平滑，效果反而立竿见影，机械臂全程稳如老狗。
所以有一句实在话：规划阶次不是越高越好，够用就好。三次多项式能解决的，别上五次；五次能解决的，别上七次。真正决定项目成败的，是你对约束条件的理解、对参数的敬畏，以及大量时间花在仿真验证上的耐心。如果这篇文章能帮你少走哪怕一步弯路，我就很欣慰了。后续有机会，我再聊聊笛卡尔空间的姿态插值，或者S型曲线的工程实现细节，咱们到时候见。
