机器人 / 系统课时

处理末端姿态插补与路径转角

处理末端姿态插补与路径转角

六轴机械臂的 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,插补会沿四维单位球面生成连续姿态。

实现时需要处理三个细节:

  1. 先计算两个单位四元数的点积。点积为负时,将终点四元数整体取反,选择较短的旋转弧。
  2. 对夹角较小的两个姿态,可以使用线性插补后归一化,避免球面插补分母过小导致数值不稳定。
  3. 每个结果都保持单位长度,并在实际轨迹规划中通过时间参数化限制角速度与角加速度。

下面用 Python 3 标准库实现一个完整示例,只处理绕 Z 轴的旋转,以便直接观察欧拉角跨界与四元数插补的区别。

python
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}°")

预期输出:

text
原始角差 -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 沿半径为 rr 的圆弧以速度 vv 运动时,法向加速度满足 an=v2/ra_n=v^2/r。

以半径 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,可能超过工具或关节允许的运动能力。

在实际规划中可以按照以下顺序处理:

  1. 根据工件和工具坐标系确定每个关键路径点的位置及目标姿态,统一坐标系与旋转表示约定。
  2. 对相邻姿态使用四元数最短旋转插补,但同时检查期望工具旋转方向是否符合工艺和线缆约束。
  3. 区分精确停点和非接触过渡点,只对允许偏离的位置使用路径混合或圆弧过渡。
  4. 对完整位置与姿态路径进行逆运动学求解,保持连续的关节构型,避免腕部翻转和关节限位越界。
  5. 根据 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 圆角侵入了工件安全区,应缩小圆角、调整过渡点、重新规划绕障路径,或将该点设为精确停点,而不能仅通过降低速度解决几何碰撞。

通过上述验证后,再按照现场安全规程进行低速实体联调,确认实际关节运动、工具朝向和障碍物间隙与仿真一致。只有姿态插补、位置过渡、运动极限和碰撞约束同时满足要求,才能将这段轨迹用于正式工位动作。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

处理末端姿态插补与路径转角

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通