四元数、欧拉角、旋转矩阵之间互相转换

一、理论

四元数

 

欧拉角

用R表示旋转矩阵，yaw(偏航) 、pitch（俯仰角） rol（横滚角）l分别表示Z　Ｙ　Ｘ轴的转角，q=[q0,q1,q2,q3]'表示单位四元数;

欧拉角有两种：

静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止，所以称为静态

动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态

使用动态欧拉角会出现万向锁现象；静态欧拉角不存在万向锁的问题。一个典型的万向锁问题可以表述如下：

先仰45°再俯90°，这与先俯90°再仰45°是等价的。事实上，一旦选择±90°作为俯角，就会导致第一次旋转和第三次旋转等价，

整个旋转表示系统被限制在只能绕竖直轴旋转，丢失了一个表示维度；这种角度为±90°的第二次旋转使得第一次和第三次旋转的旋转轴相同的现象，称作万向锁。

相关连接： http://v.youku.com/v_show/id_XNzkyOTIyMTI=.html

 

二、代码

#include 
#include 
#include 
#include 
#include 
#include 
#include 
 
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 yawAngle(yaw, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
 
    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
    cout << "Euler2Quaternion result is:" <