机器人 / 系统课时
处理末端姿态插补与路径转角
处理末端姿态插补与路径转角
六轴机械臂的 TCP 即使沿连续的位置路径移动,工具方向仍可能因为欧拉角跨越 ±180° 而出现跳变,直线路径在拐角处也可能突然减速。通过一组姿态转换和一个 90° 路径转角,掌握四元数最短旋转插补、圆滑过渡的几何约束,以及联调时的验证方法。
学习目标
- 识别欧拉角数值跳变与真实工具旋转的区别,避免错误的长路径旋转。
- 使用四元数球面线性插补生成相邻工具姿态,并验证旋转方向与连续性。
- 根据转角半径、速度和安全间隙选择路径过渡方式,检查机器人关节与 TCP 运动约束。
识别欧拉角跳变与真实姿态旋转
机器人末端位姿由 TCP 位置和工具方向共同组成。位置可以用三维坐标表示,方向则常使用欧拉角、旋转矩阵或四元数表示。欧拉角便于人工查看,但直接对三个角度分别做线性插值,可能产生不符合预期的旋转路径。
假设工具相对于基座坐标系只绕 Z 轴旋转,Roll 和 Pitch 均为 0°,两个相邻路径点的 Yaw 为:
- 起点 A:Yaw = +170°。
- 终点 B:Yaw = -170°。
- 直接相减:-170° - 170° = -340°。
- 最短等价旋转:170° → 180° → 190°,总旋转 +20°。
如果把 Yaw 从 +170°直接线性插值到 -170°,工具会沿负方向旋转 340°。实际上,+190°与 -170°表示相同的最终朝向,只需要沿正方向旋转 20°就能到达。

欧拉角还有另一个限制:对于常见的 ZYX 旋转顺序,当 Pitch 接近 ±90°时,Yaw 与 Roll 的表示会发生耦合,可能出现万向节锁问题。因此,规划时通常使用四元数或旋转矩阵描述姿态,欧拉角主要用于显示和人工输入。
使用四元数生成最短姿态插补
四元数使用四个数 (w, x, y, z) 表示三维旋转,避免直接插值欧拉角时的跨界问题。对于单位四元数,q 和 -q 表示完全相同的空间姿态,但插值时必须处理符号,否则可能选择较长的旋转路径。
常用方法是球面线性插补 SLERP。给定起点四元数 q0、终点四元数 q1 和参数 t,其中 t 从 0 变化到 1,插补会沿四维单位球面生成连续姿态。
实现时需要处理三个细节:
- 先计算两个单位四元数的点积。点积为负时,将终点四元数整体取反,选择较短的旋转弧。
- 对夹角较小的两个姿态,可以使用线性插补后归一化,避免球面插补分母过小导致数值不稳定。
- 每个结果都保持单位长度,并在实际轨迹规划中通过时间参数化限制角速度与角加速度。
下面用 Python 3 标准库实现一个完整示例,只处理绕 Z 轴的旋转,以便直接观察欧拉角跨界与四元数插补的区别。
import math
def yaw_quat(degrees):
h = math.radians(degrees) / 2
return (math.cos(h), 0.0, 0.0, math.sin(h))
def slerp(a, b, t):
dot = sum(x * y for x, y in zip(a, b))
if dot < 0:
# q 和 -q 表示同一姿态,翻号后选择较短旋转弧
b = tuple(-x for x in b)
dot = -dot
dot = max(-1.0, min(1.0, dot))
if dot > 0.9995:
# 两姿态非常接近时,使用归一化线性插补
q = tuple((1 - t) * x + t * y for x, y in zip(a, b))
else:
theta = math.acos(dot)
s = math.sin(theta)
q = tuple(
(math.sin((1 - t) * theta) * x
+ math.sin(t * theta) * y) / s
for x, y in zip(a, b)
)
length = math.sqrt(sum(x * x for x in q))
return tuple(x / length for x in q)
def yaw(q):
w, x, y, z = q
return math.degrees(
math.atan2(2 * (w * z + x * y),
1 - 2 * (y * y + z * z))
)
a, b = yaw_quat(170), yaw_quat(-170)
raw = -170 - 170
shortest = (raw + 180) % 360 - 180
print(f"原始角差 {raw:+.0f}°;最短角差 {shortest:+.0f}°")
for t in (0, 0.25, 0.5, 0.75, 1):
print(f"t={t:.2f},yaw={yaw(slerp(a, b, t)):6.1f}°")预期输出:
原始角差 -340°;最短角差 +20°
t=0.00,yaw= 170.0°
t=0.25,yaw= 175.0°
t=0.50,yaw= 180.0°
t=0.75,yaw=-175.0°
t=1.00,yaw=-170.0°输出中的 +180°到 -175°看起来出现数值跳变,但对应的连续旋转实际上是 +180°到 +185°,两次采样之间只转动了 5°。这说明四元数生成的姿态是连续的,跳变来自 Yaw 的显示范围。
工程上还要区分姿态插补与关节运动。SLERP 只能保证末端姿态按所选旋转弧变化,不能保证六个关节一定连续、不会越过限位或穿过奇异区域。特别是接近腕部奇异位置、存在多个逆运动学解,或者两姿态恰好相差 180°时,应结合当前关节构型选择可执行的解和旋转方向。
对直线路径转角建立圆滑过渡
处理好工具方向后,还要处理 TCP 的位置轨迹。假设机器人在固定 Z 高度的 XY 平面中依次通过三个点:
- A = (0.00, 0.00) m。
- B = (0.30, 0.00) m。
- C = (0.30, 0.30) m。
路径在 B 点形成 90°转角。如果要求 TCP 严格沿两段直线运动并精确经过 B 点,运动方向会在拐角发生突变。实际机器人必须在 B 附近减速,通常需要停下或接近停止,再向新的方向加速。
如果 B 只是非接触过渡点,可以使用与两条直线相切的圆弧替代尖角。例如选择圆角半径 0.05 m,得到以下几何关系:
| 几何量 | 坐标或数值 |
|---|---|
| 进入圆弧的切点 T1 | (0.25, 0.00) m |
| 离开圆弧的切点 T2 | (0.30, 0.05) m |
| 圆弧圆心 O | (0.25, 0.05) m |
| 圆角半径 | 50 mm |
| 圆弧距原转角 B 的最近距离 | 约 20.7 mm |
圆弧从 T1 连续转向 T2,切线方向与前后两段直线一致,因此 TCP 不必在 B 点瞬间改变方向。但圆弧不会精确经过 B,原路径点附近的实际运动位置发生了变化。

圆滑过渡不仅受空间间隙限制,也受法向加速度限制。TCP 沿半径为 的圆弧以速度 运动时,法向加速度满足 。
以半径 0.05 m 为例,如果转角速度为 0.20 m/s,法向加速度达到 0.80 m/s²。若本次规划为 TCP 分配的法向加速度上限为 0.50 m/s²,则转角速度不能超过约 0.158 m/s。这个上限只考虑笛卡尔圆弧的法向加速度,实际还要满足机器人各关节的速度、加速度和力矩约束。
将位置与姿态插补协调执行
机器人实际执行的是完整的 TCP 位姿轨迹,位置圆滑不代表姿态自动圆滑。对于一段同时发生平移和旋转的运动,位置插补与四元数姿态插补应使用协调的路径进度或时间参数,并由运动规划器统一进行速度和加速度约束处理。
例如,某段路径要求 TCP 在 0.30 m 的平移过程中绕 Z 轴旋转 20°。如果平移速度为 0.15 m/s,理想匀速平移时间为 2.0 s,工具平均角速度只需要 10°/s。若把平移时间压缩到 0.5 s,则平均角速度上升至 40°/s,可能超过工具或关节允许的运动能力。
在实际规划中可以按照以下顺序处理:
- 根据工件和工具坐标系确定每个关键路径点的位置及目标姿态,统一坐标系与旋转表示约定。
- 对相邻姿态使用四元数最短旋转插补,但同时检查期望工具旋转方向是否符合工艺和线缆约束。
- 区分精确停点和非接触过渡点,只对允许偏离的位置使用路径混合或圆弧过渡。
- 对完整位置与姿态路径进行逆运动学求解,保持连续的关节构型,避免腕部翻转和关节限位越界。
- 根据 TCP 速度、角速度、加速度及关节运动限制重新分配运行时间;必要时降低转角速度,而不是只修改路径几何。
尤其要注意,工具的实体外形会随姿态改变。即使 TCP 位置没有靠近障碍物,较长的夹爪仍可能因为末端旋转而扫过工件边缘。因此需要检查整个工具和夹具的扫掠空间,而不能只用 TCP 点位判断碰撞。
在仿真中检查姿态连续性与安全间隙
联调前先使用离线仿真验证,不直接以生产速度在真实设备上试探。可以针对前面的 170°到 -170°旋转和 50 mm 圆角建立一个最小测试工况,记录 TCP 位姿、关节角度与运动速度。
建议以 20 ms 为采样间隔导出运动数据,并按照以下示例验收要求检查:
| 检查项目 | 本例验收条件 |
|---|---|
| 目标姿态 | 起点 +170°,终点 -170° |
| 旋转路径 | 期望短弧总转角 20° |
| TCP 终点姿态误差 | 不超过 1° |
| 圆弧过渡半径 | 50 mm |
| 规划法向加速度 | 不超过 0.50 m/s² |
| 转角速度 | 不超过约 0.158 m/s |
| 工具及夹具与障碍物间隙 | 不小于 30 mm |
| 关节运动 | 不超限、无异常构型跳变 |
以上阈值用于说明验收方法,并非所有工业机器人的通用安全或精度标准。真实工程必须依据设备制造商的运动限制、工具尺寸、工艺公差及现场风险评估确定允许值。
检查数据时,先计算相邻四元数的真实旋转差,不把 Yaw 显示跨界误判成姿态翻转。随后观察每个关节的角度、速度和加速度曲线。如果末端姿态连续,但 J4、J5 或 J6 出现异常的大幅旋转,应重点检查逆运动学分支选择、腕部奇异位置及工具姿态约束。
对于圆角路径,应同时检查实际 TCP 轨迹到障碍物的距离,以及夹具在姿态变化过程中的扫掠空间。如果 50 mm 圆角侵入了工件安全区,应缩小圆角、调整过渡点、重新规划绕障路径,或将该点设为精确停点,而不能仅通过降低速度解决几何碰撞。
通过上述验证后,再按照现场安全规程进行低速实体联调,确认实际关节运动、工具朝向和障碍物间隙与仿真一致。只有姿态插补、位置过渡、运动极限和碰撞约束同时满足要求,才能将这段轨迹用于正式工位动作。