机器人 / 系统课时

用旋转矩阵描述坐标系姿态

用旋转矩阵描述坐标系姿态

面对机器人末端工具坐标系,单个转角往往不足以表达完整的三维姿态。本课把三个坐标轴在参考系中的方向写成旋转矩阵,结合绕 Z 轴旋转 90° 的例子,验证矩阵列向量、坐标变换与右手方向的一致性。

学习目标

  • 解释旋转矩阵三列分别表示哪个坐标轴、在哪个参考系下表达。
  • 根据三个正交单位向量组装旋转矩阵并转换向量坐标。
  • 检查正交性、右手方向,识别矩阵转置与平移的使用边界。

先确定参考系和变换方向

设 A 是固定参考坐标系,B 是机器人末端工具坐标系。两者均为右手系,向量统一写成列向量。ARB{}^{A}R_{B} 表示“B 相对 A 的姿态”:输入同一几何向量在 B 系中的分量,输出它在 A 系中的分量。

Av=ARB Bv{}^{A}\mathbf{v}={}^{A}R_{B}\,{}^{B}\mathbf{v}

矩阵的第一列对应 B 的 x 轴在 A 系中的方向,而不是 A 的 x 轴在 B 系中的方向。例如,B 的 x 轴若指向 A 的正 y 方向,第一列就是 (0,1,0)T(0,1,0)^{\mathsf T}。这里描述的是方向,不包含两坐标系原点间的距离。

把三个正交基向量按列排列

分别测得 B 的 x、y、z 三根单位轴在 A 系中的三个分量,将其作为矩阵的第一、第二、第三列:

ARB=[AxBAyBAzB]{}^{A}R_{B}=\begin{bmatrix}{}^{A}\mathbf{x}_{B}&{}^{A}\mathbf{y}_{B}&{}^{A}\mathbf{z}_{B}\end{bmatrix}

合格的旋转矩阵要求每列长度为 1、任意两列点积为 0,并满足右手方向:第一列叉乘第二列等于第三列。等价的矩阵检验是 RTR=IR^{\mathsf T}R=I 且 det⁡R=+1\det R=+1;只有前一项成立时,也可能得到镜像反射矩阵。

三维坐标系的三个基向量与旋转矩阵各列的对应关系

绕 Z 轴旋转 90° 的算例

从 A 的正 z 方向观察,令 B 相对 A 逆时针旋转 90°,且两坐标系原点重合。此时 B 的三根轴在 A 系中分别是 (0,1,0)T(0,1,0)^{\mathsf T}、(−1,0,0)T(-1,0,0)^{\mathsf T} 和 (0,0,1)T(0,0,1)^{\mathsf T},依次填入三列:

ARB=[0−10100001]{}^{A}R_{B}=\begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix}

某向量在 B 系中的分量为 (2,1,0)T(2,1,0)^{\mathsf T},左乘这个矩阵后,它在 A 系中的分量为 (−1,2,0)T(-1,2,0)^{\mathsf T}。反过来,要把 A 系分量转换回 B 系,应使用转置矩阵 (ARB)T({}^{A}R_{B})^{\mathsf T},而不是再次乘原矩阵。

运行验证并排除常见错误

下面的 Python 3 程序只使用标准库,先按列构造矩阵,再验证数值变换、单位正交性和右手方向。右手方向检查能识别“长度和夹角都对,但第三根轴反了”的错误。

python
from math import isclose

# 每个元组都是B的一根轴在A中的三个分量
cols = ((0.0, 1.0, 0.0), (-1.0, 0.0, 0.0), (0.0, 0.0, 1.0))
R = tuple(tuple(cols[j][i] for j in range(3)) for i in range(3))
v_b = (2.0, 1.0, 0.0)
v_a = tuple(sum(R[i][j] * v_b[j] for j in range(3)) for i in range(3))

def dot(a, b):
    return sum(x * y for x, y in zip(a, b))

orthonormal = all(
    isclose(dot(cols[i], cols[j]), float(i == j), abs_tol=1e-9)
    for i in range(3) for j in range(3)
)
# 检查第一列叉乘第二列是否等于第三列
cross = (
    cols[0][1] * cols[1][2] - cols[0][2] * cols[1][1],
    cols[0][2] * cols[1][0] - cols[0][0] * cols[1][2],
    cols[0][0] * cols[1][1] - cols[0][1] * cols[1][0],
)
right_handed = all(isclose(cross[i], cols[2][i], abs_tol=1e-9) for i in range(3))
print("A系分量:", v_a)
print("正交归一:", orthonormal)
print("右手方向:", right_handed)

预期输出:

text
A系分量: (-1.0, 2.0, 0.0)
正交归一: True
右手方向: True

对浮点计算可用 1e-9 容差,避免直接比较计算结果是否精确等于 0 或 1。还要区分向量坐标转换与点坐标转换:如果 A、B 原点不重合,点的变换除了旋转还必须加上 B 原点在 A 系中的平移向量,不能仅靠 RR 得到正确位置。

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

目录与阅读设置

阅读设置

17

记录一个想法

用旋转矩阵描述坐标系姿态

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

登录哪都通