机器人基础之姿态矩阵

概述

机器人学中使用姿态矩阵来描述坐标系之间的角度关系。通常坐标系固连在刚体上，因此姿态矩阵也可以用来描述刚体之间的角度关系。

绕单个坐标轴旋转某一角度的旋转矩阵

绕X轴旋转$\theta$角度的旋转矩阵为： $$\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]\text{\hspace{1em}(n)}$$
绕Y轴旋转$\theta$角度的旋转矩阵为： $$\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]\text{\hspace{1em}(n)}$$
需特别矩阵左下角的$\sin \theta$ 没有负号。
绕Z轴旋转$\theta$角度的旋转矩阵为： $$\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(n)}$$

用MATLAB实现上述矩阵

function rot_X_matrix = rotX(theta)
% 计算绕X轴旋转的矩阵
rot_X_matrix = [
    1, 0, 0;
    0, cos(theta), -sin(theta);
    0, sin(theta), cos(theta)];

function rot_Y_matrix = rotY(theta)
% 计算绕Y轴旋转的矩阵
rot_Y_matrix = [
    cos(theta), 0, sin(theta);
    0, 1, 0;
    -sin(theta), 0, cos(theta);];

function rot_Z_matrix = rotZ(theta)
% 计算绕Z轴旋转的矩阵
rot_Z_matrix = [
    cos(theta), -sin(theta), 0;
    sin(theta), cos(theta), 0;
    0, 0, 1;];

坐标系绕多个坐标轴转动的姿态矩阵

这里的多个坐标系分为两类：固定坐标系和动坐标系。
对于固定坐标系，姿态矩阵依次向左乘。
对于动坐标系，姿态矩阵依次向右乘。
对此，我的记忆方法是：固左（即利用谐音“顾左右而言他”）。

等效转轴与等效转角

之前在理论力学中学过刚体定点运动的位移定理，即达朗贝尔-欧拉定理。
该定理叙述如下：定点运动刚体的任意有限位移，可以绕通过固定点O的某一轴经过一次转动来实现。
姿态矩阵是一个正交矩阵，它存在一个大小为1的特征值，对应于该特征值的特征向量的方向即为等效转轴的方向。
等效转角可用以下公式计算。$$\theta =arccos[\frac{1}{2}(trA-1)]$$
其中，trA为矩阵A的迹。
下面举例在MATLAB中编程进行计算。

% 举例计算等效转轴和等效转角
thetaX = 30 / 180 * pi;
thetaY = 60 / 180 * pi;
thetaZ = 50 / 180 * pi;

R = rotX(thetaX) * rotY(thetaY) * rotZ(thetaZ);
[V, D] = eig(R)
theta = acos(0.5 * (trace(R) - 1)) / pi * 180

特征向量矩阵V、特征值矩阵D和等效转角$\theta$的输出结果如下。

V =

  -0.2232 + 0.5348i  -0.2232 - 0.5348i   0.5730 + 0.0000i
   0.6193 + 0.0000i   0.6193 + 0.0000i   0.4826 + 0.0000i
  -0.2581 - 0.4626i  -0.2581 + 0.4626i   0.6624 + 0.0000i


D =

  -0.0103 + 0.9999i   0.0000 + 0.0000i   0.0000 + 0.0000i
   0.0000 + 0.0000i  -0.0103 - 0.9999i   0.0000 + 0.0000i
   0.0000 + 0.0000i   0.0000 + 0.0000i   1.0000 + 0.0000i


theta =

   90.5910

可见，等效转轴的方向向量为(0.573, 0.4826, 0.6624)，等效转角约为90.6度。