四元数matlab&Eigen库定义及常用运算函数

matlab的四元数运算

matlab库中有四元素的变量类型，见链接quaternion帮助文档

 初始化一个四元数有很多种方式，其中比较常用的方式就是通过四元数四个元素的值：

quat=quaternion(q1,q2,q3,q4)

其中q1是角位移余弦值，q2,q3,q4为旋转轴向量。

第二种方式是通过旋转矩阵初始化：

quat=quaternion(matrix)

姿态表示都是用单位四元数来表示，机器人工具箱提供了单位四元数的类

 可以看到有很多不同方式的构造函数，最常用的依然是通过姿态角和旋转轴矢量的方式：

quat=UnitQuaternion([1,2,3,4])

与matlab自带的quaternion初始化不同的是，他的形参不是四个数，而是四个数组成的向量，可以通过’.‘运算，索引角位移余弦值和旋转轴向量：

angle=quat.s

axis=quat.v

运行结果：

quat = 
 
0.18257 < 0.36515, 0.54772, 0.7303 >
 

angle =

    0.1826


axis =

    0.3651    0.5477    0.7303

获取两四元数姿态角函数：

angle=q1.angle(q2)

两姿态间slerp插值

q=q1.interp(q2,u)

右图是四元数幂运算插值和slerp插值效果的对比，左图是我自定义slerp插值函数与机器人工具箱slerp插补效果的对比，结果一致；

 四元数的Eigen库运算

Eigen库四元数的初始化，默认构造函数初始值全为零：

Eigen::Quaterniond Q

指定元素值：

    Eigen::Quaterniond Q(1,2,3,4);
    std::cout << "**********四元数值：x,y,z,w*************" << std::endl;
    std::cout << Q.x() << std::endl;
    std::cout << Q.y() << std::endl;
    std::cout << Q.z() << std::endl;
    std::cout << Q.w() << std::endl;

运行结果：

**********赋值前四元数值：x,y,z,w*************
2
3
4
1

可以看到，Eigen库初始化形参，第一个元素为角位移余弦值，后三个元素为旋转轴向量，可以通过"."运算访问元素值，”."运算值即可作为左值也可作为右值，对其赋值：

    Q.x() = 0.1;
    Q.y() = 0.2;
    Q.z() = 0.3;
    Q.w() = 0.4;
    std::cout << "**********赋值后四元数值：x,y,z,w*************" << std::endl;
    std::cout << Q.x() << std::endl;
    std::cout << Q.y() << std::endl;
    std::cout << Q.z() << std::endl;
    std::cout << Q.w() << std::endl;

运行结果：

**********赋值后四元数值：x,y,z,w*************
0.1
0.2
0.3
0.4

四元数转换成欧拉角：

quaternion.toRotationMatrix().eulerAngles(2, 1, 0);

2，1，0分别表示欧拉角的旋转方式，绕Z，绕Y，绕X旋转,有12种组合。

求两四元数之间角位移：

angle=Q..angularDistance(Q1);

与matlab一样，不过需要注意的是，maltab求角位移的函数返回值是，而Eigen库返回值为，且matlab没有做最小弧处理，给出的旋转角度会大于Pi，Eigen库做了最小弧处理，返回的姿态角在0-pi之间。

        还有一些其他的求模长，求逆之类的运算两者差距不大。