浅析机器人学位置与姿态之坐标系绕任意轴线旋转算法

接着讲一下三维坐标系绕任意轴旋转的算法原理，这些算法可能应用的时候很少碰到，但是不可以不学，说不定哪天碰到各问题就是这个算法可以解决的，例如，想快速搞一个球面轨迹出来；建议朋友们了解下！

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

用matlab写一下程序；

先写个旋转矩阵，分别绕X/Y/Z旋转0.1、0.2、0.3弧度；

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

这个旋转矩阵是从原点坐标系通过旋转矩阵旋转到一个新位置，那么我们可以通过下面的程序得出，坐标系原点是绕着哪个向量轴旋转一定角度后得到新坐标系的；

>> [theta, v] = tr2angvec(R)
th =
0.3655
v =
0.1886 0.5834 0.7900

其中theta是旋转角度，V是绕其旋转的向量轴；

扩展知识：

其实性和谐信息都是R的特征值和特征向量中的。

什么叫特征值和特征向量呢？

设 A 是n阶方阵，如果存在数m和非零n维列向量 x，使得 Ax=mx 成立，则称 m 是A的一个特征值(characteristic value)或本征值(eigenvalue)。非零n维列向量x称为矩阵A的属于(对应于)特征值m的特征向量或本征向量，简称A的特征向量或A的本征向量。

使用函数eig可以求矩阵的特征值和特征向量；

>> [x,e] = eig(R)
x =
-0.6944 + 0.0000i -0.6944 + 0.0000i 0.1886 + 0.0000i
0.0792 + 0.5688i 0.0792 - 0.5688i 0.5834 + 0.0000i
0.1073 - 0.4200i 0.1073 + 0.4200i 0.7900 + 0.0000i
e =
0.9339 + 0.3574i 0.0000 + 0.0000i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.9339 - 0.3574i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.0000i 1.0000 + 0.0000i

矩阵e的对角线三个元素就是返回的特征值，对于的特征向量再矩阵V的对应列向量；

比如矩阵e的第三行里的特征值 1.0000 + 0.0000i， 对应的特征向量就是向量x里的第三列！

从特征值和特征向量的定义中，得出：

其中，v为特征值λ对应的特征向量。如λ=1则：

这意味着相应的特征向量v随旋转而保持不变。只有一个这样的向量，那就是以这个向量为轴发生的旋转。在这个例子中，第三个特征值等于1，所以旋转轴是矩阵x的第三列。

反过来推导；利用罗德里格斯的旋转公式，可以实现从角度和矢量到旋转矩阵的逆转问题：

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

函数里有四个输入参数，一个表示角度pi/2,其它三个[1 0 0]表示向量旋转轴;实际可以更简化，因为向量可以只用两个参数表示，另一个可以通过这两个参数求出值； 所以实际只需要输入三个参数，就可以实现转换！