机器人 / 系统课时
用旋转矩阵描述坐标系姿态
用旋转矩阵描述坐标系姿态
面对机器人末端工具坐标系,单个转角往往不足以表达完整的三维姿态。本课把三个坐标轴在参考系中的方向写成旋转矩阵,结合绕 Z 轴旋转 90° 的例子,验证矩阵列向量、坐标变换与右手方向的一致性。
学习目标
- 解释旋转矩阵三列分别表示哪个坐标轴、在哪个参考系下表达。
- 根据三个正交单位向量组装旋转矩阵并转换向量坐标。
- 检查正交性、右手方向,识别矩阵转置与平移的使用边界。
先确定参考系和变换方向
设 A 是固定参考坐标系,B 是机器人末端工具坐标系。两者均为右手系,向量统一写成列向量。 表示“B 相对 A 的姿态”:输入同一几何向量在 B 系中的分量,输出它在 A 系中的分量。
矩阵的第一列对应 B 的 x 轴在 A 系中的方向,而不是 A 的 x 轴在 B 系中的方向。例如,B 的 x 轴若指向 A 的正 y 方向,第一列就是 。这里描述的是方向,不包含两坐标系原点间的距离。
把三个正交基向量按列排列
分别测得 B 的 x、y、z 三根单位轴在 A 系中的三个分量,将其作为矩阵的第一、第二、第三列:
合格的旋转矩阵要求每列长度为 1、任意两列点积为 0,并满足右手方向:第一列叉乘第二列等于第三列。等价的矩阵检验是 且 ;只有前一项成立时,也可能得到镜像反射矩阵。

绕 Z 轴旋转 90° 的算例
从 A 的正 z 方向观察,令 B 相对 A 逆时针旋转 90°,且两坐标系原点重合。此时 B 的三根轴在 A 系中分别是 、 和 ,依次填入三列:
某向量在 B 系中的分量为 ,左乘这个矩阵后,它在 A 系中的分量为 。反过来,要把 A 系分量转换回 B 系,应使用转置矩阵 ,而不是再次乘原矩阵。
运行验证并排除常见错误
下面的 Python 3 程序只使用标准库,先按列构造矩阵,再验证数值变换、单位正交性和右手方向。右手方向检查能识别“长度和夹角都对,但第三根轴反了”的错误。
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)预期输出:
A系分量: (-1.0, 2.0, 0.0)
正交归一: True
右手方向: True对浮点计算可用 1e-9 容差,避免直接比较计算结果是否精确等于 0 或 1。还要区分向量坐标转换与点坐标转换:如果 A、B 原点不重合,点的变换除了旋转还必须加上 B 原点在 A 系中的平移向量,不能仅靠 得到正确位置。