机械臂工具坐标系，旋转矩阵含义解释

从末端坐标系到基座坐标系的旋转矩阵，就是把一个在末端坐标系中表示的向量，变换到基座坐标系中。

1. 矩阵数值含义

假设：

• 基座坐标系三个轴：

• 末端坐标系三个轴：

旋转矩阵：

其中每个元素：

也就是基座第 i 个轴和末端第 j个轴之间的方向余弦。

2. 列含义

列向量是末端坐标系的轴在基座坐标系中的表示。

即：

注意：这里的

都是用基座坐标表示的，每一列是末端坐标系的坐标轴在基座坐标系中的方向，比如第三列就是末端 z 轴在基座坐标系中的方向向量。

所以：

• 第 1 列：末端 X 轴在基座坐标系中的方向
• 第 2 列：末端 Y 轴在基座坐标系中的方向
• 第 3 列：末端 Z 轴在基座坐标系中的方向

例如，如果第 3 列是：

就说明末端 Z 轴正好沿着基座 Z 轴方向。如果想得到工具坐标系下的 z 轴方向在基座中的表示，直接取第三列即可。

3. 行含义

行向量是基座坐标系的轴在末端坐标系中的表示。

即：

这里每一行表示基座某个轴在末端坐标系中的分量。

所以：

• 第 1 行：基座 X 轴在末端坐标系中的表示
• 第 2 行：基座 Y 轴在末端坐标系中的表示
• 第 3 行：基座 Z 轴在末端坐标系中的表示

4. 坐标变换关系

如果末端坐标系中有一个点或向量 p_e，那么它在基座坐标系中为：

其中 tt 是 SE3 中的平移部分，表示末端坐标系原点在基座坐标系中的位置。

反向变换为：

因为旋转矩阵是正交矩阵：

5. 矩阵示例

如果末端坐标系只是绕基座 Z 轴旋转了角度 θ ，那么旋转矩阵为：

此时：

• 第 1 列是：

它是末端 X 轴在基座中的方向。

• 第 1 行是：

它是基座 X 轴在末端坐标系中的表示。

6. Pinocchio 中常见读取方式

import pinocchio as pin

# 读取某个关节或末端 frame 相对于基座的位姿
M = data.oMi[joint_id]      # 关节坐标系
# 或
M = data.oMf[frame_id]      # 末端 frame 坐标系

R = M.rotation     # Python 绑定中通常是属性；部分版本可能用 M.rotation()
t = M.translation

# 将末端坐标系中的点变换到基座坐标系
p_base = R @ p_end + t

• 列：末端轴在基座下的方向
• 行：基座轴在末端下的方向
• R 的物理意义：把末端坐标旋转变到基座坐标，表示“末端坐标系 → 基座坐标系”的旋转矩阵。

6. 举个例子

求解：如果要读取重力在末端坐标系下，对末端的正压力投影。

设：末端坐标系下，垂直于末端平面的正压力方向是 末端 z 轴

重力在基座下是 [0,0,−9.81]，末端 z 轴在基座下的变换就是 R 的第三列，那么重力在末端 z 轴上的投影就是 gbase 和 R 第三列的点积

方法一：把末端法线变换到基座，再与重力点积

如果 n_end=[0,0,1]，则 n_base 就是 R 的第三列，于是：p=gbase⋅(R 的第三列)

方法二：把重力变换到末端坐标系，再取 z 分量

这两种方法在数学上是完全等价的，因为：

• 如果要把末端法线变到基座：用 R左乘 n_end。

如果要把基座重力变到末端：用 R^T左乘 g_base