机器人 / 系统课时

用四元数完成旋转插值

用四元数完成旋转插值

给定机器人末端的起止旋转矩阵,我们需要计算两者之间平滑的中间姿态,而不是直接对矩阵元素插值。本课使用单位四元数表示旋转,通过球面线性插值(SLERP)生成中间姿态,并以绕 Z 轴旋转 90° 的例子验证转换结果和插值角度。

学习目标

  • 理解单位四元数的分量、归一化约束与正负等价关系。
  • 使用 Python 完成旋转矩阵、四元数之间的双向转换。
  • 使用 SLERP 生成中间姿态,理解最短路径与常见数值边界。

单位四元数怎样表示三维旋转

三维旋转可以使用四元数 q=(x,y,z,w)q=(x,y,z,w) 表示,其中前三个分量构成向量部分,ww 是标量部分。本课统一采用右手坐标系、主动旋转、列向量约定,以及 SciPy 默认的 (x, y, z, w) 分量顺序。

一个合法的单位旋转四元数满足:

x2+y2+z2+w2=1x^2+y^2+z^2+w^2=1

假设绕单位旋转轴 u=(ux,uy,uz)u=(u_x,u_y,u_z) 旋转角度 θ\theta,对应的四元数为:

q=(uxsin⁡(θ/2),uysin⁡(θ/2),uzsin⁡(θ/2),cos⁡(θ/2))q=(u_x\sin(\theta/2),u_y\sin(\theta/2),u_z\sin(\theta/2),\cos(\theta/2))

例如,绕正 Z 轴逆时针旋转 90°,旋转轴为 (0,0,1)(0,0,1),得到:

q=(0,0,sin⁡45∘,cos⁡45∘)≈(0,0,0.7071,0.7071)q=(0,0,\sin45^\circ,\cos45^\circ)\approx(0,0,0.7071,0.7071)

这里有两个容易混淆的性质。

  • 四元数和它的相反数表示同一个旋转:qq 与 −q-q 都对应相同的旋转矩阵,因此不能直接用四元数四个分量是否完全相等判断姿态是否相同。
  • 单位长度是旋转表示的约束:自行执行四元数运算时应监测模长并在必要时归一化。不能把任意四维向量直接当作合法旋转使用。

与欧拉角相比,四元数表示旋转时没有欧拉角参数化的万向节死锁问题,也适合旋转组合与姿态插值。不过,将结果重新转换为欧拉角时,仍然可能遇到欧拉角的奇异位置。

旋转矩阵与四元数如何互相转换

机器人控制程序经常使用 3×33\times3 旋转矩阵存储坐标系朝向,而动画和轨迹插值程序更适合使用四元数。转换前必须确定两个表示采用相同的坐标轴方向和旋转约定。

合法的三维旋转矩阵应满足:

RTR=I,det⁡(R)=1R^{\mathsf T}R=I,\qquad \det(R)=1

第一项要求矩阵正交,第二项排除镜像反射。实际读取传感器或标定数据时,建议检查这两个条件,而不是直接将任意矩阵交给转换接口。

以绕正 Z 轴旋转 90° 为例:

Rz(90∘)=[0−10100001]R_z(90^\circ)=\begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix}

这个矩阵对应的四元数是 (0,0,0.7071,0.7071)(0,0,0.7071,0.7071)。转换回来应恢复相同的旋转矩阵,允许浮点计算产生约 10−1510^{-15} 量级的误差。

在 SciPy 的 scipy.spatial.transform.Rotation 中,常用接口如下:

操作 接口 结果
矩阵转旋转对象 Rotation.from_matrix(R) 创建旋转对象
旋转对象转四元数 rotation.as_quat() 默认返回 xyzw 顺序
四元数转旋转对象 Rotation.from_quat(q) 创建旋转对象
旋转对象转矩阵 rotation.as_matrix() 返回 3×3 旋转矩阵

球面线性插值为什么沿最短旋转路径运动

给定起点四元数 q0q_0、终点四元数 q1q_1 和插值比例 tt,SLERP 在单位四元数所在的四维单位球面上沿大圆弧插值。对两点之间的一段非退化最短弧,它产生按参数均匀变化的旋转角速度。

设两个单位四元数之间的球面夹角为:

Ω=arccos⁡(q0⋅q1)\Omega=\arccos(q_0\cdot q_1)

当两端点不重合且 sin⁡Ω\sin\Omega 非零时,插值公式为:

SLERP⁡(q0,q1,t)=sin⁡((1−t)Ω)sin⁡Ωq0+sin⁡(tΩ)sin⁡Ωq1\operatorname{SLERP}(q_0,q_1,t)=\frac{\sin((1-t)\Omega)}{\sin\Omega}q_0+\frac{\sin(t\Omega)}{\sin\Omega}q_1

这里的 Ω\Omega 是四元数球面上的夹角,不是直接测得的机器人实体旋转角。对于本例的 90° 实体旋转,两端四元数之间的球面夹角为 45°。

由于 qq 与 −q-q 表示同一个姿态,进行最短路径插值前还需要处理符号。如果两个四元数的点积小于零,可以先将终点取反,再执行 SLERP,避免选择不必要的长弧。SciPy 的 Slerp 已经处理了相邻关键旋转之间的最短路径选择。

四元数最短弧插值与机器人中间姿态示意

对于本例,起点姿态是单位旋转,终点绕正 Z 轴旋转 90°。如果希望在 2 秒内完成这段旋转,可以分别取 t=0t=0、0.250.25、0.50.5、0.750.75、11,对应姿态角度应为 0°、22.5°、45°、67.5°、90°。这里的均匀角度变化只适用于这一固定旋转轴的示例。

使用 Python 生成并验证中间姿态

下面使用 Python 3.10+、NumPy 和 SciPy 的 Rotation、Slerp 接口。先通过 python -m pip install numpy scipy 安装依赖,然后将完整代码保存为 quaternion_slerp.py,执行 python quaternion_slerp.py。

代码先构造起止旋转矩阵,检查矩阵是否合法,再转成四元数。之后将时间 0 和 1 对应的姿态作为关键帧,求出时间 0.5 的中间姿态。

python
import numpy as np
from scipy.spatial.transform import Rotation, Slerp

# 右手坐标系中绕正 Z 轴逆时针旋转 90 度
angle = np.deg2rad(90.0)
start_matrix = np.eye(3)
end_matrix = np.array([
    [np.cos(angle), -np.sin(angle), 0.0],
    [np.sin(angle),  np.cos(angle), 0.0],
    [0.0, 0.0, 1.0],
])

# 转换前排除非正交矩阵与反射矩阵
for matrix in (start_matrix, end_matrix):
    orthogonal = np.allclose(matrix.T @ matrix, np.eye(3), atol=1e-8)
    proper = np.isclose(np.linalg.det(matrix), 1.0, atol=1e-8)
    if not (orthogonal and proper):
        raise ValueError('输入不是有效旋转矩阵')

key_rotations = Rotation.from_matrix(np.stack([start_matrix, end_matrix]))
q0, q1 = key_rotations.as_quat()

# 两个关键帧分别对应时间 0 和 1,查询中点姿态
interpolator = Slerp([0.0, 1.0], key_rotations)
mid = interpolator([0.5])

print('q0 (xyzw):', np.round(q0, 3).tolist())
print('q1 (xyzw):', np.round(q1, 3).tolist())
print('中点四元数:', np.round(mid.as_quat()[0], 3).tolist())
print('中点旋转矩阵:', np.round(mid.as_matrix()[0], 3).tolist())
print('中点绕 Z 轴角度:', round(float(mid.as_euler('zyx', degrees=True)[0, 0]), 1))

预期输出:

text
q0 (xyzw): [0.0, 0.0, 0.0, 1.0]
q1 (xyzw): [0.0, 0.0, 0.707, 0.707]
中点四元数: [0.0, 0.0, 0.383, 0.924]
中点旋转矩阵: [[0.707, -0.707, 0.0], [0.707, 0.707, 0.0], [0.0, 0.0, 1.0]]
中点绕 Z 轴角度: 45.0

中点四元数中的 z≈0.3827z\approx0.3827、w≈0.9239w\approx0.9239,对应绕 Z 轴旋转 45°。通过 as_matrix() 还原后,矩阵左上角的二维旋转部分也符合 45° 的正弦和余弦关系。

如果需要进一步检查往返转换精度,可以将中点四元数使用 Rotation.from_quat() 转回旋转对象,再比较还原矩阵与 mid.as_matrix()[0]。正常情况下,二者应该在浮点容差内一致。

处理实际机器人轨迹中的插值边界

SLERP 解决的是两个朝向之间如何插值,并不自动满足机器人关节限位、角速度限制或避障要求。实际接入机器人轨迹时,需要同时处理以下情况。

情况 错误做法 正确处理
两个姿态几乎相同 直接套用含很小分母的公式 采用数值稳定的库实现,手写时可用归一化线性插值近似
四元数点积小于零 忽略正负等价,走较长路径 先反转一个端点的符号再插值
四元数模长偏离 1 不检查就连续累积运算 归一化并检查非法零长度输入
多段姿态轨迹 认为每段 SLERP 自动保证速度连续 检查关键帧处的角速度变化,必要时使用平滑轨迹方法
仅插值末端朝向 直接发送给机器人执行 联合逆运动学、关节约束与碰撞检测验收

Slerp 要求关键帧时间严格递增,查询时间处于给定时间范围内。若轨迹周期为 20 ms,而一段姿态变化持续 2 秒,可在每个周期计算 t=k/100t=k/100,其中 kk 从 0 增加到 100,再将插值后的姿态和位置轨迹组合成目标末端位姿。

需要特别注意:末端姿态沿最短旋转路径变化,不代表各个机器人关节也沿最短路径运动。若某个中间姿态无法通过逆运动学获得满足限位的关节解,就不能把这段插值结果直接当作可执行运动轨迹。

验收本例时,至少检查三个数值条件:起点旋转角为 0°、中点为 45°、终点为 90°;同时检查每个插值四元数的模长接近 1。满足这些条件,才能确认当前姿态表示、插值方向与数学计算一致。

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

目录与阅读设置

阅读设置

17

记录一个想法

用四元数完成旋转插值

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

登录哪都通