已知世界坐标系姿态矩阵，求解6DOF机械臂末端局部坐标系姿态

• 核心问题：旋转矩阵乘法顺序搞反
  你遇到的问题本质是坐标系变换的矩阵乘法顺序错误。先明确几个关键定义：

  • P6_0是静止状态下末端坐标系{6}到世界坐标系{0}的旋转矩阵（即{6}的基向量在{0}中的坐标表示）。由于旋转矩阵是正交矩阵，其逆矩阵等于自身的转置：P6_0⁻¹ = P6_0ᵀ。
  • 假设你给定的世界坐标系下的目标姿态矩阵为R₀T（表示末端目标姿态在{0}中的坐标），你需要得到的是该目标姿态相对于末端初始坐标系{6}的旋转矩阵R₆T。

  根据坐标系变换的链式法则，姿态矩阵的关系为：
  $$R_{0T} = R_{06} \times R_{6T}$$
  求解R₆T需要将R₀6的逆矩阵左乘R₀T：
  $$R_{6T} = P6_0^{-1} \times R_{0T}$$

  而你使用的是R₀T × P6_0⁻¹，完全颠倒了变换顺序——只有当R₀T与P6_0的旋转轴完全重合（乘法可交换）时结果才会正确，其他场景下都会得到错误姿态。

• 为什么顺序会影响结果？
  旋转矩阵的乘法顺序对应变换的先后逻辑：

  • 左乘逆矩阵：表示先将世界坐标系下的姿态映射到末端初始坐标系的父坐标系（世界坐标系），再转换到末端坐标系，符合从全局到局部的变换逻辑。
  • 右乘逆矩阵：相当于将末端初始坐标系的反向变换叠加到目标姿态上，完全偏离了你需要的“目标姿态相对于末端局部坐标系”的定义。

• 验证示例
  比如目标姿态是绕世界X轴旋转90°的矩阵：
  $$R_{0T} = \begin{bmatrix}1 & 0 & 0 \0 & 0 & -1 \0 & 1 & 0\end{bmatrix}$$
  用正确方法计算：
  $$P6_0^T \times R_{0T} = \begin{bmatrix}0 & 0 & 1 \0 & 1 & 0 \-1 & 0 & 0\end{bmatrix} \times \begin{bmatrix}1 & 0 & 0 \0 & 0 & -1 \0 & 1 & 0\end{bmatrix} = \begin{bmatrix}0 & 1 & 0 \0 & 0 & -1 \-1 & 0 & 0\end{bmatrix}$$
  而用你的方法得到：
  $$R_{0T} \times P6_0^T = \begin{bmatrix}1 & 0 & 0 \0 & 0 & -1 \0 & 1 & 0\end{bmatrix} \times \begin{bmatrix}0 & 0 & 1 \0 & 1 & 0 \-1 & 0 & 0\end{bmatrix} = \begin{bmatrix}0 & 0 & 1 \1 & 0 & 0 \0 & 1 & 0\end{bmatrix}$$
  两者结果完全不同，后者是错误的局部姿态。

内容的提问来源于stack exchange，提问作者Alex Niadore