机器人学——1.7-绕任意向量旋转

对于空间中两个任意姿态的坐标系，总可以在空间里找到某个轴，使其中一个坐标系绕该轴旋转一个角度就能与另一个坐标系姿态重合。以先前使用过的一个旋转

>> R = rpy2r(0.1, 0.2, 0.3)

我们可以确定如下的一个角度和一个向量：

>> [theta, v] = tr2angvec(R)
theta =
    0.3816
v =
    0.3379    0.4807    0.8092

其中，theta是旋转的角度大小，v是旋转轴的向量。
这些信息实际上包含在 $R$ 的特征值和特征向量中。使用 MATLAB 的内部函数eig可以求矩阵的特征值和特征向量：

>> [v, lambda] = eig(R)
v =
   0.6655 + 0.0000i   0.6655 + 0.0000i   0.3379 + 0.0000i
  -0.1220 - 0.6079i  -0.1220 + 0.6079i   0.4807 + 0.0000i
  -0.2054 + 0.3612i  -0.2054 - 0.3612i   0.8092 + 0.0000i
lambda =
   0.9281 + 0.3724i   0.0000 + 0.0000i   0.0000 + 0.0000i
   0.0000 + 0.0000i   0.9281 - 0.3724i   0.0000 + 0.0000i
   0.0000 + 0.0000i   0.0000 + 0.0000i   1.0000 + 0.0000i

矩阵lambda中的对角线元素即返回的特征值，其相应的特征向量位于矩阵v中对应的列向量。
一个正交旋转矩阵总有一个实特征值 $lambda=1$ ，以及一对共轭复特征值 $lambda=cos hetapm isin heta$ ，其中 $heta$ 代表旋转的角度。根据特征值和特征向量的定义有
$Rv=lambda v$ 其中， $v$ 是对应于 $lambda$ 的特征向量。当 $lambda=1$ 时：
$Rv=v$ 这意味着对应的这个特征向量 $v$ 是不随旋转发生改变的。这样的向量只有一个，而旋转就是以这个向量为轴发生的。上面的例子中第三个特征值等于 $1$ ，所以旋转轴应是矩阵 $v$ 中的第三列。

反过来，使用罗德里格斯（Rodrigues）旋转方程，可以从角度和向量计算出相应的旋转矩阵：
$R = I_{3 imes 3}+sin heta S(v)+(1-cos heta)(vv^T-I_{3 imes 3})$ 其中 $S(v)$ 表示 $v$ 的斜对称矩阵形式。
前面我们熟悉的一个绕 $x$ 轴旋转 $pi/2$ 的例子，其旋转矩阵为

>> angvec2r(pi/2, [1 0 0])
ans =
    1.0000         0         0
         0    0.0000   -1.0000
         0    1.0000    0.0000

我们注意到，绕任意轴旋转这种表示法都可以参数化为4个数字：3个代表旋转轴，1个代表旋转角度。然而，空间中的方向可以用一个只有两个独立参数的单位向量表示，因为第三个元素可以由下式计算：
$v_3=sqrt{1-v_1^2-v_2^2}$
这样真正独立的参数只有3个。或者，我们也可以把单位向量和角度相乘，得到另外一种3个参数的表示方法 $v heta$ 。虽然这些表示方法对于数据存储来说是最小和最高效的，但在分析研究时是存在问题的。其他学者还提出过多种变形，包括 $vsin( heta/2)$ 和 $v an( heta)$ ，但它们在 $heta=0$ 时都呈现病态。

矩阵指数幂

考虑一个描述绕 x 轴旋转的矩阵

>> R = rotx(0.3)
R =
    1.0000         0         0
         0    0.9553   -0.2955
         0    0.2955    0.9553

早在讨论二维情况的时候我们就使用 MATLAB 函数 logm 计算过矩阵的对数

>> S = logm(R)
S =
         0         0         0
         0         0   -0.3000
         0    0.3000         0

这也是一个反对称矩阵，求其反变换，可以得到对应轴的旋转角度

>> vex(S)
ans =
    0.3000
         0
         0

对于三位的情况，工具箱函数trlog的效果是一样的

>> [th, w] = trlog(R)
th =
    0.3000
w =
    1.0000
         0
         0