ROS旋转矩阵

Microstation MDL(memo)
作为备用知识，memo

学过矩阵理论或者线性代数的肯定知道正交矩阵（orthogonal matrix）是一个非常好的矩阵，为什么这么说？原因有一下几点：

正交矩阵每一列都是单位矩阵，并且两两正交。最简单的正交矩阵就是单位阵。

正交矩阵的逆（inverse）等于正交矩阵的转置（transpose）。同时可以推论出正交矩阵的行列式的值肯定为正负1的。

正交矩阵满足很多矩阵性质，比如可以相似于对角矩阵等等。

以上可以看出正交矩阵是非常特殊的矩阵，而本文题目中的旋转矩阵就是一种正交矩阵！它完美的诠释了正交矩阵的所有特点。

先说一下什么是旋转矩阵？如图1所示，我们假设最开始空间的坐标系XA，YA，ZA就是笛卡尔坐标系，这样我们得到空间A的矩阵VA={XA，YA，ZA}T，其实也可以看做是单位阵E。进过旋转后，空间A的三个坐标系变成了图1中红色的三个坐标系XB，YB，ZB，得到空间B的矩阵VB={XB，YB，ZB}T。我们将两个空间联系起来可以得到VB=R•VA，这里R就是我们所说的旋转矩阵。

图1

由于XA={1,0,0}T，YA={0,1,0}T，ZA={0,0,1}T，结合图2可以看出，旋转矩阵R就是由XB，YB，ZB三个向量组成的。讲到这里，大家应该会发现旋转矩阵R满足第一个条件，因为单位向量无论怎么旋转长度肯定不会变而且向量之间的正交性质也不会变。那么旋转矩阵就是正交阵！不过这还不能说明问题，下面我更进一步利用数学公式进行证明。

图2

进一步讨论之前，我们先说两点数学知识。（1）点乘（dot product）的几何意义：如图3，我们从点乘的公式可以得到α•β相当与β的模乘上α在β上投影的模，所以当|β|=1时，α•β就是指α在β上投影的模。这一点在下面的内容中非常重要。（2）旋转矩阵逆的几何意思：这个比较抽象，不过也好理解。旋转矩阵相当于把一个向量（空间）旋转成新的向量（空间），那么逆可以理解为由新的向量（空间）转回原来的向量（空间）。

图3

接下来就是重点了，我们结合图4进行分析。上面已经说明了，旋转矩阵R就是由XB，YB，ZB三个向量组成的。我们来看看XB，YB，ZB究竟是什么？由于图中所有的向量均是单位向量，所以XB与XA点乘的结果可以看成XB在XA上的投影的模，也就是XB在空间A中x轴的分量！！图中中间的位置列出了XB向量中的三个分量分别为XB在XA上的投影的模、XB在YA上的投影的模和XB在ZA上的投影的模。这从几何角度很好理解。以此类推，可以得出的旋转矩阵R的表达形式。我们根据图4可以惊喜的发现，矩阵R的第一行就是XA在XB，YB，ZB上的投影的模，也就是XAT。

图4

这个发现有什么用呢？图5做出解释。根据上面公式可以推出A到B的旋转矩阵等于B到A的旋转矩阵的转置。根据我们上一段所说的A到B的旋转矩阵的逆就是等于B到A的旋转矩阵，因此很容易推出R-1等于RT！这满足正交矩阵的第二个条件，又一次证明了旋转矩阵就是正交阵。在平时的工作中，我也测试过所有的旋转矩阵的行列式的值都是为1的，所以旋转矩阵满足正交阵的一切性质，可以说是很完美的矩阵。

图5

现在以三个欧拉角中的RotX为例（其余两个欧拉角以此类推），验证一下以上说的结论。

首先结合图5的公式，计算出RotX的旋转矩阵Rrotx。

由于X轴是垂直于YoZ平面的，所以XA和YB，ZB的点乘结果为0，同时XB和YA，ZA的点乘结果也为0。

由于XA，XB都是单位向量，所以XA和XB的点乘结果为1。

由于绕x轴旋转，所以我们观察YB和ZB分别在YA和ZA上的投影情况，如图6，我已经将坐标标注了。

图6

这样就完成旋转矩阵Rrotx，我们接下来验证一下。

我们计算每一行每一列的模，都为1；并且任意两个列向量或者任意两个行向量都是正交的。所以满足上文列出的第一个性质。

我们计算Rrotx的行列式，很简单可以算出为1。这时我们计算一下该矩阵的逆和转置，这里我不写出来了是相等的。所以满足上文列出的第三个性质。

第三个性质要牵扯到更多的数学知识，在这里就不验证了。

总结一下：旋转矩阵是一个完美的矩阵——正交矩阵。它的行列式为1，且每个列向量都是单位向量且相互正交，它的逆等于它的转置。、

—————————————————————————————————

三维空间的旋转(3D Rotation)是一个很神奇的东东：如果对某个刚体在三维空间进行任意次的旋转，只要旋转中心保持不变，无论多少次的旋转都可以用绕三维空间中某一个轴的一次旋转来表示。表示三维空间的旋转有多种互相等价的方式，常见的有旋转矩阵、DCM、旋转向量、四元数、欧拉角等。本篇文章主要梳理一下这些表示方式及相互转换的方法。

1. 欧拉角(Euler Angle)

最直观的表示方式是绕刚体自身的X、Y、Z三个轴分别进行旋转某个角度，这就是所谓的欧拉角(Euler Angle)表示方式。

需要注意的是，欧拉角的表示方式里，yaw、pitch、roll的顺序对旋转的结果是有影响的。给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！换言之，若刚体需要按照两种不同的旋转顺序旋转到相同的朝向，所需要的欧拉角角度值则是不同的！

另外需要注意的是，在欧拉角的表示方式里，三个旋转轴一般是随着刚体在运动，即wikipedia中所谓的intrinsic rotation，见下图动画所示(图来自wikipedia)。相对应的另一种表示方式是，三个旋转轴是固定的，不随刚体旋转而旋转，即extrinsic rotation，这种表示方式在计算机视觉中不是很常用。

欧拉角的表示方式比较直观，但是有几个缺点：

(1) 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。比如，同样的yaw-pitch-roll顺序，(0,90,0)和(90,90,90)会将刚体转到相同的位置。这其实主要是由于万向锁(Gimbal Lock)引起的，关于万向锁的解释，有条件的同学看看Youtube的视频或许会比较直观。

(2) 欧拉角的插值比较难。

(3) 计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin, cos，计算量较大。

2. 旋转矩阵(Rotation Matrix)和方向余弦矩阵(Direction Cosine Matrix)

在计算坐标变换时，旋转更方便的表示形式是旋转矩阵(Rotation Matrix)。三维空间的旋转矩阵可以表示成3x3的矩阵，将欧拉角转换为旋转矩阵的计算方式如下，假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma，则旋转矩阵可以计算如下：

其中，

这里也可以看出，如果yaw、pitch、roll的顺序有改变，矩阵相乘的顺序需要作出相应改变，所得的旋转矩阵结果也会有所改变。

需要注意的是，旋转矩阵的虽然有9个元素，但是只有3个自由度，所以不是任何矩阵都可以作为旋转矩阵，旋转矩阵需要是正交矩阵(即逆矩阵等于转置矩阵)。

此外，旋转矩阵的另一个名字叫方向余弦矩阵(Direction Cosine Matrix)，简称DCM，在陀螺力学领域较为常用。DCM的名字来历其实是用欧拉角之外的另一种用3个角度值表示三维旋转的方式，假设刚体在起始朝向时三个坐标轴的向量为I,J,K，而刚体在目标朝向时的三个坐标轴的向量为i,j,k，则该旋转可以通过三个坐标轴分别与原始坐标轴的夹角表示，如下图所示：

DCM可以通过三个夹角的余弦计算如下：

这就是DCM名称的由来。其实可以验证，DCM其实就是旋转矩阵，所以，下文不再区分DCM和旋转矩阵的称呼。

在Matlab中(R2006a以后的版本中，需安装Aerospace Toolbox)，可以方便地用angle2dcm和dcm2angle来转换欧拉角和旋转矩阵。下面的Matlab代码可以验证，两个不同的欧拉角方式可以转换到相同的旋转矩阵：

% Matlab code by MulinB, Aerospace Toolbox is needed

% Gimbal Lock experiments

yaw1 = 0;

pitch1 = 90;

roll1 = 0;

yaw2 = 90;

pitch2 = 90;

roll2 = 90;

R1 = angle2dcm(yaw1/180pi,pitch1/180pi,roll1/180*pi);

R2 = angle2dcm(yaw2/180pi,pitch2/180pi,roll2/180*pi);

disp(R1);disp(R2);

3. 四元数(Quaternion)、旋转向量(Rotation Vector)、轴-角表示(Axis-Angle)

旋转的一个神奇之处就在于，三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)来表示，theta可以用一个角度值来表示，直观来讲，一个四维向量(theta,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值theta。这样以来，可以用一个三维向量(thetax, thetay, theta*z)就可以表示出三维空间任意的旋转，前提是其中(x,y,z)是单位向量。这就是旋转向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是这种表示方法来表示旋转(见OpenCV相机标定部分的rvec)。

Axis-Angle的表示方法还可以推导出另一种很常用的三维旋转表示方法，叫四元数(Quaternion)，这里有一篇非常通俗易懂介绍四元数的文章。同上，假设(x,y,z)是axis方向的单位向量，theta是绕axis转过的角度，那么四元数可以表示为[cos(theta/2), xsin(theta/2), ysin(theta/2), z*sin(theta/2)]。注意，这里可以推导出，用于表示旋转的四元数向量也必须是单位向量。四元数的神奇之处在于，对于三维坐标的旋转，可以通过四元数乘法直接操作，与上述旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。首先，四元数的乘法是如下规定的：

由此定义，四元数的逆也可以求出。作为旋转四元数，由于其单位向量的特性，四元数的逆其实等于四元数的共轭，也就是如果四元数q=[a,b,c,d]，由于a^2+b2+c^2+d2=1，那么q的逆和共轭都是q’=[a,-b,-c,-d]。需要注意的是，四元数的乘法是不可交换的。通过四元数计算旋转的方式为(将三维空间一个点v_I旋转到v_B，四元数是q)：

在Matlab里，可以用quatmultiply计算四元数乘法，用quatinv来计算四元数的逆，用quatconj来计算四元数的共轭。四元数的旋转和旋转矩阵的旋转可以由以下matlab代码验证：

% Matlab code by MulinB, Aerospace Toolbox is needed

pt = [10,20,30]; % point coordinate

yaw = 45;

pitch = 30;

roll = 60;

q = angle2quat(yaw/180pi,pitch/180pi,roll/180*pi);

R = angle2dcm(yaw/180pi,pitch/180pi,roll/180*pi);

pt1 = R*pt’;

pt2 = quatmultiply(quatconj(q), quatmultiply([0,pt],q)); % NOTE the order

disp(pt1’);disp(pt2(2:4));

从上述代码里也可以看到四元数和欧拉角和dcm的转换，在matlab里可以很方便的用quat, dcm, angle之间的转换来任意互转。另外，从四元数计算axis和angle，可以用以下代码计算：% Matlab code by MulinB, Compute the axis and angle from a quaternion

function [axis, theta] = quat2axisangle(q)

theta = acos(q(1)) * 2;

axis = q(2:4)/sin(theta/2);

从OpenCV的rotation vector和quaternion的互转可以用以下代码：

% Matlab code by MulinB, Convert a quaternion to a rotation vector

function rvec = quat2rvec(q)

theta = acos(q(1)) * 2;

axis = q(2:4)/sin(theta/2);

axis = axis / norm(axis);

rvec = axis*theta;

% Matlab code by MulinB, Convert a rotation vector to a quaternion

function q = rvec2quat(rvec)

theta = norm(rvec);

axis = rvec/theta;

sht = sin(theta/2);

q = [cos(theta/2), axis*sht];

参考

http://blog.csdn.net/mulinb/article/details/51227597

http://www.cnblogs.com/caster99/p/4703033.html