三维坐标下的旋转

三维坐标的旋转有以下几种常见的表示形式：旋转向量，旋转矩阵，欧拉角，四元数,下面对这四种表示形式及其之间的转换进行介绍

旋转向量

　　通常为3x1的列向量，向量方向即为旋转轴，向量的模表示绕轴逆时针旋转的角度,如旋转向量v=[a,b,c],那么旋转角度theta=sqrt(a²+b²+c²),旋转轴z=[a/theta, b/theta, c/theta]

旋转矩阵

　　旋转矩阵用3x3的矩阵表示，旋转矩阵有一些很好的性质

• 旋转矩阵的逆矩阵是它的转置，即　　　　　　　　　　　　　　　　　　　

• 旋转矩阵是正交矩阵，且行列式是单位1                                                                 　　

　　旋转向量与旋转矩阵可以通过罗德里格斯(Rodrigues)变换进行转换。opencv实现Rodrigues变换的函数是：

　　　　　　　　int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 );

　生成旋转矩阵的一种简单方式是把它作为三个基本旋转的序列复合，如下面要介绍的欧拉角的复合。

欧拉角

　　欧拉角度的基本思想是将旋转分解为绕三个互相垂直轴的三个旋转组成的序列。但最有意义的是使用笛卡尔坐标系并按照一定顺序所组成的旋转序列。使用欧拉角，就要指明坐标系系统。最常用的约定，即所谓“heading-pitch-bank”约定。在这个系统中，一个方位被定义为一个heading角，一个pitch角，和一个bank角。它的基本思想就是让物体开始于“标准”方位——就是物体坐标轴和惯性坐标轴对齐。在标准方位上，让物体作heading,pitch,bank旋转，最后物体到达我们想要描述的方位。

具体解释一下heading,pitch,bank三个角度，规定使用左手坐标系，+x向右，+y向上，+z向前。heading为绕y轴的旋转量，向右旋转为正，旋转正方向是顺时针方向，经过heading旋转之后，pitch为绕x轴的旋转量，注意是物体坐标系的x轴，不是原惯性坐标系的x轴，依然遵守左手法则，向下旋转为正。最后，经过了heading,pitch后，bank为绕z轴的旋转量，依然是物体坐标系的z轴。

当我们说到旋转的顺序是heading-pitch-bank时，是指从惯性坐标系到物体坐标系，如果从物体坐标系到惯性坐标系则相反。旋转可以理解为将惯性坐标系和物体坐标系对齐的一个过程，即通过将惯性坐标系旋转heading,pitch，bank，使惯性坐标系与物体坐标系重合。heading-pitch-bank系统还有另外一个名称roll-pitch-yaw，其中的roll对应与bank，yaw对应于heading，Yaw（偏航）：绕欧拉角向量的y轴旋转，Pitch（俯仰）：绕欧拉角向量的x轴旋转，Roll（翻滚）： 绕欧拉角向量的z轴旋转

　　下面讨论欧拉角与旋转矩阵之间的转换，绕x，y，或z轴旋转θ的矩阵为：

　　　　,,

　　要计算最终的旋转，只需按一定的顺序将R_x(θ),R_y(θ),R_z(θ)做矩阵乘法，所以顺序不同，最终的旋转矩阵结果也不同。如果规定了坐标系以及旋转顺序，那么旋转矩阵是惟一的。在heading-pitch-bank系统中，旋转矩阵与欧拉角之间的转换关系：

　　其中，sa = sin(attitude)，ca = cos(attitude)，sb = sin(bank)，cb = cos(bank)，sh = sin(heading)，ch = cos(heading)

可以参考http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/

欧拉角表示的旋转会造成万向节锁（Gimbal Lock）的现象。所谓万向节锁简单的说在指某种状态下我们无论怎么旋转（当然还是要原先的顺序）都不可 　　能得到某些想要的旋转效果，除非我们打破原先的旋转顺序或者同时旋转3个坐标轴，由于万向节锁的存在，欧拉旋转无法实现球面平滑插值。

　　 旋转矩阵转成欧拉角：

• heading = atan2(-m20,m00)
• attitude = asin(m10)
• bank = atan2(-m12,m11)

　　  两种特殊情况除外：M10=1时，heading = atan2(M02,M22)，bank = 0；M10=-1时，heading = atan2(M02,M22)，bank = 0

四元数：一个四元数可以表示为q = w + xi + yj + zk，i、j、k是一种特殊的虚数单位（类比虚数表示中的i,j），并有以下运算规则：i⁰=j⁰=k⁰=1，i²=j²=k²=-1， |q|²=w²+x²+y²+z²=1,四元数的其他性质不在一一叙述。

四元数指明了旋转的角度和旋转轴，如旋转轴的方向余弦为（cosβ_x,cosβ_y,cosβ_z）,旋转角为α，则对应的四元数q=[q₀,q₁,q₂,q₃]，其中

四元数与旋转矩阵之间的转换关系（注意利用模为1的性质有一下两种表示方法）

，或，

　　　旋转矩阵转换为四元数：

　　　　m_ij表示旋转矩阵中第i行k列的元素

四元数转换为欧拉角：

　　　 首先定义

　　　  Roll – : rotation about the X-axis

Pitch – : rotation about the Y-axis

Yaw – : rotation about the Z-axis

则对应的转换为

如果已知欧拉角求四元数，可以先将欧拉角转化为旋转矩阵，再由旋转矩阵求四元数

原创文章，转载请注明出处