四元数与欧拉角

四元数与欧拉角

RPY角与Z-Y-X欧拉角

坐标系 $B$ 相对于坐标系 $A$ 的姿态有两种方式。第一种绕自身坐标轴旋转；第二种绕固定坐标轴旋转。

绕自身坐标轴旋转

坐标系 { B } \{B\} {B} 与 坐标系 { A } \{A\} {A} 重合：坐标系 { B } \{B\} {B} 先绕自身 $x$ 轴旋转 $\alpha$；坐标系 { B } \{B\} {B} 再绕自身 $y$ 轴旋转 $\beta$；最后坐标系 { B } \{B\} {B} 绕自身 $z$ 轴旋转 $\gamma$。旋转的次序很重要，注意与矩阵的对于关系。旋转矩阵如下：$(c\alpha =cos(\alpha ),s\alpha =sin(\alpha ))$。

$${}_B^A\text{Euler}(\gamma ,\beta ,\alpha )=Rot(z,\gamma )Rot(y,\beta )Rot(x,\alpha )=\left[\begin{array}{ccc}c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma \\ s\alpha c\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma \\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{array}\right]$$

绕固定坐标轴旋转

另一种常用的旋转集合是横滚(roll，绕 $z$ 旋转)、俯仰(pitch，绕 $y$ 旋转) 和 偏转(yaw， 绕 $x$ 旋转)。旋转的顺序如下：先绕 $x$ 轴旋转 $\alpha$；再绕 $y$ 轴旋转 $\beta$；最后绕 $z$ 轴旋转 $\gamma$。矩阵表示如下：

$${}_B^A\text{RPY}(\gamma ,\beta ,\alpha )=Rot(z,\gamma )Rot(y,\beta )Rot(x,\alpha )=\left[\begin{array}{ccc}c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma \\ s\alpha c\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma \\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{array}\right]$$

由上述式子可知，两种描述最终得到的旋转矩阵是一样的，只是描述的方式不一样。

Axis-Angle和四元数

绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设该转轴的方向向量为 $\vec{K}=[k_x,k_y,k_z{]}^T$，旋转角度为 $\theta$，用四元数表示为 $q=(x,y,z,w)$，其中表达式如下：

$$\overrightarrow{\begin{array}{c}x=k_x\ast sin\frac{\theta }{2}\\ y=k_y\ast sin\frac{\theta }{2}\\ z=k_z\ast sin\frac{\theta }{2}\\ w=cos\frac{\theta }{2}\end{array}}$$

且有：$x^2+y^2+z^2+w^2=1$。

即四元数存储了旋转轴和旋转角的信息，它能方便的描述刚体绕任意轴的旋转。

用四元数表示旋转矩阵

$$R=\left[\begin{array}{ccc}1-2y^2-2x^2& 2(xy-zw)& 2(xz+yw)\\ 2(xy+zw)& 1-2x^2-2z^2& 2(yz-xw)\\ 2(xz-yw)& 2(yz+xw)& 1-2x^2-2y^2\end{array}\right]$$

用旋转矩阵表示四元数

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

则对应的四元数为：
$$\begin{array}{c}x=\frac{r_{32}-r_{23}}{4w}\\ y=\frac{r_{13}-r_{31}}{4w}\\ z=\frac{r_{21}-r_{12}}{4w}\\ w=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}}\end{array}$$

四元数的操作

定义两个四元数：
$$\begin{array}{c}q=a+\vec{u}=a+bi+cj+dk;\\ p=y+\vec{v}=t+xi+yj+zk\end{array}$$

四元数相加

跟复数、向量和矩阵一样，两个四元数之和需要将不同的元素加起来。

$$p+q=a+t+\vec{u}+\vec{v}=(a+t)+(b+x)i+(c+y)j+(d+z)k$$

四元数相乘

四元数的乘法的意义类似于矩阵的乘法，可以表示旋转的合成。当有多次旋转操作时，使用四元数可以获得更高的计算效率。
$$\begin{array}{c}pq=at-\vec{u}\cdot \vec{v}+a\vec{v}+t\vec{u}+\vec{u}\times \vec{v}\\ pq=(at-bx-cy-dz)+(ax+bt+cz-dy)i+(ay-bz+ct+dx)j+(az+dt-cx+by)k\end{array}$$

由于四元数乘法的非可换性，pq并不等于qp，qp乘积的向量部分是：

$$qp=at-\vec{u}\cdot \vec{v}+a\vec{v}+t\vec{u}-\vec{u}\times \vec{v}$$

编程

四元数、欧拉数和旋转矩阵代码如下：

#pragma GCC diagnostic error "-std=c++11"
#include <iostream>
#include <Eigen/Eigen>
#include <stdlib.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <vector>
#include <math.h>
 
using namespace std;
using namespace Eigen;
 
Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
 
    Eigen::Quaterniond q = rollAngle * pitchAngle * yawAngle;
    
    cout << "Euler2Quaternion result is:" <<endl;
    cout << "x = " << q.x() <<endl;
    cout << "y = " << q.y() <<endl;
    cout << "z = " << q.z() <<endl;
    cout << "w = " << q.w() <<endl<<endl;
    
    return q;
}

Eigen::Vector3d Quaterniond2Euler(const double x,const double y,const double z,const double w)
{
    Eigen::Quaterniond q;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    q.w() = w;
    
    Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
    
    cout << "Quaterniond2Euler result is:" <<endl;
    cout << "x = "<< euler[2] << endl ;
    cout << "y = "<< euler[1] << endl ;
    cout << "z = "<< euler[0] << endl << endl;
    
    return euler;
}

Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)
{
    Eigen::Quaterniond q;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    q.w() = w;
    
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();
    
    cout << "Quaternion2RotationMatrix result is:" <<endl;
    cout << "R = " << endl << R << endl<< endl;
    
    return R;
}

Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)
{
    Eigen::Quaterniond q = Eigen::Quaterniond(R);
    q.normalize();
    
    cout << "RotationMatrix2Quaterniond result is:" <<endl;
    cout << "x = " << q.x() <<endl;
    cout << "y = " << q.y() <<endl;
    cout << "z = " << q.z() <<endl;
    cout << "w = " << q.w() <<endl<<endl;
    
    return q;
}

Eigen::Matrix3d euler2RotationMatrix(const double roll, const double pitch, const double yaw)
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    
    Eigen::Quaterniond q = rollAngle * pitchAngle * yawAngle;
    Eigen::Matrix3d R = q.matrix();
    
    cout << "Euler2RotationMatrix result is:" <<endl;
    cout << "R = " << endl << R << endl<<endl;
    
    return R;
}

Eigen::Vector3d RotationMatrix2euler(Eigen::Matrix3d R)
{
    Eigen::Matrix3d m;
    m = R;
    Eigen::Vector3d euler = m.eulerAngles(0, 1, 2);
    
    cout << "RotationMatrix2euler result is:" << endl;
    cout << "x = "<< euler[2] << endl ;
    cout << "y = "<< euler[1] << endl ;
    cout << "z = "<< euler[0] << endl << endl;
    
    return euler;
}

int main(int argc, char **argv)
{
   euler2Quaternion(0,0,0);
  
   Quaterniond2Euler(0,0,0,1);

   Quaternion2RotationMatrix(0,0,0,1);

   Eigen::Vector3d x_axiz,y_axiz,z_axiz;
   x_axiz << 1,0,0;
   y_axiz << 0,1,0;
   z_axiz << 0,0,1;
   Eigen::Matrix3d R;
   R << x_axiz,y_axiz,z_axiz;
   rotationMatrix2Quaterniond(R);
	
   euler2RotationMatrix(0,0,0);
	
   RotationMatrix2euler(R);
   
   cout << "All transform is done!" << endl;
}

输入如下指令：`g++ -std=c++11 test.cpp -o test -I /usr/include/eigen3/`
结果如下： ``` Euler2Quaternion result is: x = 0 y = 0 z = 0 w = 1

Quaterniond2Euler result is:
x = 0
y = -0
z = 0

Quaternion2RotationMatrix result is:
R =
1 0 0
0 1 0
0 0 1

RotationMatrix2Quaterniond result is:
x = 0
y = 0
z = 0
w = 1

Euler2RotationMatrix result is:
R =
1 0 0
0 1 0
0 0 1

RotationMatrix2euler result is:
x = -0
y = 0
z = -0

All transform is done!