做机器人控制这些年有一类话题几乎绕不开轨迹生成尤其是关节空间规划。新手接触机器人学时最难跨过去的坎往往不是正逆运动学而是“让机械臂从A点动到B点中间那段路到底怎么走”。关节空间规划就是回答这个问题的一套系统方法它负责在每个关节的角度层面生成一条随时间变化的光滑曲线让机械臂动起来不抖、不超限、不伤电机。这篇文章我会从原理、公式、代码到坑点完整拆一遍关节空间轨迹规划的思路和落地细节适合刚学机器人学、想把课本知识变成实际运动指令的读者。1. 轨迹生成的两层逻辑先分清路径、轨迹与规划对象1.1 路径与轨迹的区别国内很多教材会把“路径规划”和“轨迹规划”混在一起讲但工程上这是两回事弄混了后面写代码会非常痛苦。路径Path只描述几何关系是一串空间的点或曲线比如末端从点A走到点B中间经过哪些位置它不关心“什么时候到”“走多快”。轨迹Trajectory则是带时间戳的运动描述每个时刻机械臂应该处于什么位形、速度是多少、加速度是多少。轨迹是对路径的“时间参数化”本质上是在一条几何路径的基础上附加运动律。关节空间规划处理的直接对象是关节角向量 $q(t) \in \mathbb{R}^n$n是自由度个数。对六轴机械臂来说规划的就是六个角度随时间变化的序列。为什么不在笛卡尔空间直接规划因为在笛卡尔空间虽然直观却要面对逆解、奇异点和冗余问题每个插值点都要做一次逆运动学计算量大而且可能没有解。关节空间规划避开了这些问题只要各关节角连续变化机器人末端自然就走出一条连续曲线。用开车类比笛卡尔空间规划是要求车子“必须沿着某条道路标线走”关节空间规划则是说“只要方向盘转得平滑、不超速具体走哪条线都行”。前者约束强但计算复杂后者约束弱但高效可靠。1.2 关节空间与笛卡尔空间的选择权衡关节空间规划的优势十分明确维度低一个6轴机器人只要规划6条角度曲线笛卡尔空间要规划末端位姿的33或更多分量。天然避奇异关节空间的曲线始终定义在关节活动范围内不存在逆运动学奇异问题。计算开销小只需要多项式插值或样条插值单次计算成本很低能满足很高的控制周期要求。但它的缺点是末端路径不直观无法保证末端走过的轨迹是直线或圆弧。如果应用场景要求末端必须走直线比如涂胶、焊接就得在笛卡尔空间规划或者采用混合规划先用笛卡尔空间采样路径点再转换到关节空间做插值平滑。我见过很多初学机器人学的朋友一上来就追求笛卡尔空间直线规划结果被奇异性卡得怀疑人生。我的建议是除非工艺明确要求直线轨迹否则优先做关节空间规划简单可靠后处理难度小。维度关节空间规划笛卡尔空间规划规划对象关节角度 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_10$ 得一次项为零。联立剩下的方程$$ 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_020^\circ$ 运动到 $\theta_f80^\circ$时间 $t_f2s$两端速度为零。先把角度转成弧度避免单位混乱$$ \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_020^\circ \approx 0.3491 , rad$。在 $t1s$ 时$\theta(1)0.34910.7854-0.26180.8727 , rad \approx 50^\circ$正好是中间点说明曲线对称。速度曲线是一条抛物线$\dot{\theta}(t)1.5708t - 0.7854t^2$在 $t1s$ 达到最大速度约 $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 多段三次多项式的衔接条件实际机械臂工作很少只做一次单关节运动更多是一连串的点位抓取位、提升位、过渡位、放置位中间还要躲避障碍物。这时要把多段三次多项式首尾拼接起来。假设有 $n1$ 个路径点 $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}{i1}(t_i)$避免速度突变。加速度连续可选$\ddot{\theta}i(t_i) \ddot{\theta}{i1}(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_00, v_f0, a_00, a_f0$解得$$ 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连续的二阶连续可导这在理论上是“平滑曲线”的标准定义。假设有 $n1$ 个路径点和 $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, v00.0, vf0.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型曲线的工程实现细节咱们到时候见。
做机器人控制这些年有一类话题几乎绕不开轨迹生成尤其是关节空间规划。新手接触机器人学时最难跨过去的坎往往不是正逆运动学而是“让机械臂从A点动到B点中间那段路到底怎么走”。关节空间规划就是回答这个问题的一套系统方法它负责在每个关节的角度层面生成一条随时间变化的光滑曲线让机械臂动起来不抖、不超限、不伤电机。这篇文章我会从原理、公式、代码到坑点完整拆一遍关节空间轨迹规划的思路和落地细节适合刚学机器人学、想把课本知识变成实际运动指令的读者。1. 轨迹生成的两层逻辑先分清路径、轨迹与规划对象1.1 路径与轨迹的区别国内很多教材会把“路径规划”和“轨迹规划”混在一起讲但工程上这是两回事弄混了后面写代码会非常痛苦。路径Path只描述几何关系是一串空间的点或曲线比如末端从点A走到点B中间经过哪些位置它不关心“什么时候到”“走多快”。轨迹Trajectory则是带时间戳的运动描述每个时刻机械臂应该处于什么位形、速度是多少、加速度是多少。轨迹是对路径的“时间参数化”本质上是在一条几何路径的基础上附加运动律。关节空间规划处理的直接对象是关节角向量 $q(t) \in \mathbb{R}^n$n是自由度个数。对六轴机械臂来说规划的就是六个角度随时间变化的序列。为什么不在笛卡尔空间直接规划因为在笛卡尔空间虽然直观却要面对逆解、奇异点和冗余问题每个插值点都要做一次逆运动学计算量大而且可能没有解。关节空间规划避开了这些问题只要各关节角连续变化机器人末端自然就走出一条连续曲线。用开车类比笛卡尔空间规划是要求车子“必须沿着某条道路标线走”关节空间规划则是说“只要方向盘转得平滑、不超速具体走哪条线都行”。前者约束强但计算复杂后者约束弱但高效可靠。1.2 关节空间与笛卡尔空间的选择权衡关节空间规划的优势十分明确维度低一个6轴机器人只要规划6条角度曲线笛卡尔空间要规划末端位姿的33或更多分量。天然避奇异关节空间的曲线始终定义在关节活动范围内不存在逆运动学奇异问题。计算开销小只需要多项式插值或样条插值单次计算成本很低能满足很高的控制周期要求。但它的缺点是末端路径不直观无法保证末端走过的轨迹是直线或圆弧。如果应用场景要求末端必须走直线比如涂胶、焊接就得在笛卡尔空间规划或者采用混合规划先用笛卡尔空间采样路径点再转换到关节空间做插值平滑。我见过很多初学机器人学的朋友一上来就追求笛卡尔空间直线规划结果被奇异性卡得怀疑人生。我的建议是除非工艺明确要求直线轨迹否则优先做关节空间规划简单可靠后处理难度小。维度关节空间规划笛卡尔空间规划规划对象关节角度 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_10$ 得一次项为零。联立剩下的方程$$ 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_020^\circ$ 运动到 $\theta_f80^\circ$时间 $t_f2s$两端速度为零。先把角度转成弧度避免单位混乱$$ \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_020^\circ \approx 0.3491 , rad$。在 $t1s$ 时$\theta(1)0.34910.7854-0.26180.8727 , rad \approx 50^\circ$正好是中间点说明曲线对称。速度曲线是一条抛物线$\dot{\theta}(t)1.5708t - 0.7854t^2$在 $t1s$ 达到最大速度约 $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 多段三次多项式的衔接条件实际机械臂工作很少只做一次单关节运动更多是一连串的点位抓取位、提升位、过渡位、放置位中间还要躲避障碍物。这时要把多段三次多项式首尾拼接起来。假设有 $n1$ 个路径点 $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}{i1}(t_i)$避免速度突变。加速度连续可选$\ddot{\theta}i(t_i) \ddot{\theta}{i1}(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_00, v_f0, a_00, a_f0$解得$$ 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连续的二阶连续可导这在理论上是“平滑曲线”的标准定义。假设有 $n1$ 个路径点和 $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, v00.0, vf0.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型曲线的工程实现细节咱们到时候见。
平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测)
