四元数”

向量的旋转一共有三种表示方法：旋转矩阵、欧拉角和四元数，接下来我们介绍一下每种旋转方法的原理以及相互转换方式。

旋转矩阵

坐标变换的作用

在一个机器人系统中，每个测量元件测量同一物体得出的信息是不一样的，原因就在于“每个测量元件所测量的数据是基于不同坐标系所测量的”，例如：

在这辆车中有激光雷达M和激光雷达W，这两个雷达测量的数据截然不同，但是这辆汽车相对于测量物体的位置是唯一的，这就说明“由不同位置雷达测量的数据代表的物理含义（即都表示汽车与被测物体的相对位置）是相同的”。那既然被测物体在不同坐标系中的坐标不同但物理含义相同，这就涉及到不同坐标系中坐标的相互转化。下面这个视频将使你对坐标变换有一个初步的认识：

https://www.bilibili.com/video/av808747163/?zw

我们会疑惑：world坐标系是什么？

在空间中会有n+1个坐标系，其中只有一个坐标系起到标定作用，也就是说“其他n个坐标系全都是基于该坐标系找到自己在空间中的位置的”。只有大家都知道了自己在该坐标系空间中的具体位置，坐标转换才可以顺利进行下去。

实现坐标变换所需的数据

我们常用出发于坐标系原点终止于坐标点的向量，来表示坐标点相对于坐标原点的位置（距离+方位）。坐标系的相互转化必须以地球坐标系为媒介才可以实现，即坐标系的相互转化必须已知“任意坐标系中各个坐标轴在world坐标系中的坐标”：

在描述机器人运动时，我们常常提及“位姿”，其实位姿是一个合成词，我们可以将其拆解为“位置+姿态”。位置就是指“机器人某个运动关节/测量传感器在世界坐标系中的具体位置，姿态就是”基于该点的坐标系相较于世界坐标系所进行的旋转“，如下所示：

坐标变换中旋转的实质

坐标变换的实质就是“投影”。首先，我们解读一下向量是如何转化为坐标的：

理解向量坐标的由来对于理解坐标变换的实质至关重要！接下来我们考虑一下单位向量在坐标系中的投影：

我们将坐标系A作为参考坐标系（world坐标系），基于坐标系A表示坐标系B的各个坐标轴并且将各个向量单位化，由此我们得到一个旋转矩阵，旋转矩阵各个元素的含义如下：

我们先前提到过，向量坐标的计算无非就是投影，那么向量坐标从坐标系B转换至坐标系A无非就是两次投影而已：

坐标转换的实际意义无非就是将向量P在坐标系A中各个轴的投影分别累加起来形成一个新的坐标。那问题来了，累加如何操作呢？这就涉及旋转矩阵以及矩阵乘法运算了。

其实，这个矩阵的乘法与卷积有着异曲同工之妙。旋转矩阵的性质：

坐标变换中平移的实质

向量可以在坐标系中任意移动，只要不改变向量的方向和大小，向量的属性不会发生变化。但是我们研究的是坐标系B中一个坐标点在坐标系A中的映射，因此需要考虑坐标系B的原点O2相较于坐标系A原点O1平移的距离。

每个基于坐标系B的向量先进性旋转变换，再与向量O1O2求和，即可得到向量P再坐标系A中的实际映射。

如何计算坐标系B各坐标轴在坐标系A上的投影？（多坐标变换）

首先，我们要知道世界坐标系下坐标系A/坐标系B的各个坐标轴在世界坐标系（参考坐标系）的坐标：

我们使用旋转矩阵的性质可以得到坐标系B变换至坐标系A的旋转矩阵：

坐标变换流程如下：

如何实现坐标变换？

我们将P点在坐标系B中的坐标转化为P点在坐标系A中对应的坐标：

这样不利于矩阵运算，我们可以改写为如下形式：

其中O₁O₂是从O₁指向O₂的向量。

欧拉角

欧拉角的作用

欧拉角遵循的是右手系规则，即大拇指指向坐标轴正方向，四指旋转的方向即为转动的正方向，欧拉角包含三个自由量：yaw(偏航角)、pitch(俯仰角)、roll(翻滚角)。

我们将三次旋转分开讨论，我们以绕Z轴旋转为例来进行说明：

绕Z轴旋转的三维立体图如上所示，为了方便，我们查看一下二维旋转图：

四元数

数学的美妙不在于形象的表达变换的逻辑，而在于抽象简单的给出表达式。四元数就是如此，四元数的三维表达晦涩难懂，但是四元数的表达式可以优雅的表达三维中的旋转操作，不但避免了欧拉角的死锁问题而且也避免了旋转矩阵的复杂计算。

坐标系转换的常用方法：

旋转矩阵

传感器坐标系与车辆坐标系之间，相差一个欧式变换，其中欧式变换由旋转加平移构成，即传感器坐标系到车辆坐标系的变换可以看做，车辆坐标系经过旋转再平移得到的传感器坐标系。
坐标系的旋转不会影响物体点的位置，如下图所示中xoy旋转theta，不会影响A点到O点的距离（OA向量的长度），影响的只是投影下旋转前和旋转后坐标轴上的投影。在下图中，A点在旋转后坐标系中的坐标，可由中间的旋转矩阵与原坐标乘积进行计算，其中旋转矩阵可以看做一种映射函数，将在原坐标系下的物体坐标映射到旋转后坐标下的坐标。

注意：在构建函数中，一定会先确定自变量和因变量，然后才能得到从自变量到因变量的映射关系（函数）；因此在使用旋转矩阵时，一定要清楚，旋转矩阵是表示由哪个坐标系到哪个坐标系的旋转变换。
例如下图中，旋转矩阵A是表示由蓝色坐标下到红色坐标下的旋转变换，所以旋转矩阵A是将蓝色坐标系中的点坐标转换到红色坐标系下坐标的一种映射，即物体在蓝色坐标系下的坐标是自变量，而物体在红色坐标下的坐标是因变量，旋转矩阵A是蓝到红的映射函数。
这点一定要明确好，否则如果使用了红到蓝的旋转矩阵，而自变量还是物体蓝色坐标系下的坐标，因变量为物体在红色坐标下的坐标，结果不可能对；相当于我的函数（映射矩阵）的自变量是红坐标系下的坐标，因变量是蓝坐标系下的坐标，但我带入的自变量确实蓝坐标系下的坐标，驴唇不对马嘴，结果怎么可能对。

　二维坐标系旋转

三维坐标系旋转

如何将传感器下坐标系中的坐标转换到车身坐标系下呢？目前，只是完成了坐标系的旋转，一般传感器的坐标下原点与车辆坐标系的原点不会重合，都会有一定的偏移，如下图所示。
A点在x1o1y1坐标系下的坐标转换到在xoy坐标系下坐标，可以分步进行。

第一步旋转，转换到旋转后x1'o1y1'坐标系下的坐标，采用旋转矩阵进行变换。
第二步平移，x1'o1y1'坐标系相对于xoy坐标系，在x轴及y轴上进行了平移，因此旋转后的坐标需要加上平移量，其中OO1表示平移向量，即最终转换的坐标可以表示为蓝色O1A向量加上OO1向量所得到的OA向量，向量的坐标表示即为最终转换的坐标。

注意：在加平移向量时，一定要明确平移向量OO1坐标表达形式下，其所采用的坐标系；采用x1'o1y1'坐标系与xoy坐标系表示平移向量OO1，虽然向量大小不变，但是向量的坐标表示不同。
因为最后的平移是相对于旋转后的坐标系下（这里指的是XOY坐标系下），坐标系轴上的平移；因此一定确保采用旋转后的坐标系表示的平移向量，才能保证最后能够加上平移向量（因为平移向量各个坐标，表示旋转坐标系后与车身坐标系在轴向上的平移量）。

变换矩阵

变换矩阵在旋转矩阵的基础上融入了平移信息，单独的变换矩阵就能表示欧式变换。至于变换矩阵的使用好处，不言而喻。下面时slam14讲中对于变换矩阵的解释，非常清晰，就不在这里进行过多的解释了。

通过上面的描述，可以看出变换矩阵在多个坐标系进行转换时，是非常便捷的。

动手学ROS（8）：坐标系间的欧氏变换，这里有很形象的多坐标系转换介绍（采用变换矩阵）

四元数

在上文中对四元数的介绍不多，主要是因为目前对四元数了解的不够深，另外时不想把文章写的太枯燥。本文还是希望介绍四元数的使用。要想深入了解可以参考下文中的参考文章与视频。

终于到了重头戏了，写这篇博客的目的就是学习四元数，为了吃这口醋特地包的饺子，四元数就是这口醋，前面的都是饺子 ^_^ 。

四元数一般采用（w, i , j, k）表示，w为实部，i、j、k为虚部。
四元数的通俗理解，就是表示物体姿态的，与上面的欧拉角相似（这里只是表达在理解位姿一词上的相似）；当然也可以理解为一种旋转算法，与旋转矩阵及变换矩阵相似（这里的相似只的是在使用时）。通俗的解释完了，看下四元数如何表示旋转以及如何进行坐标系转换的吧。

四元数表示旋转：

四元数进行坐标系转换：

在坐标系转换中，采用Eigen库以及slam14讲中的一个例子进行解释。
（《视觉SLAM十四讲》第三讲习题7）设有小萝卜一号和二号在世界坐标系中。一号位姿q1 = [0.35, 0.2, 0.3, 0.1]，t1=[0.3, 0.1, 0.1]。二号位姿q2=[-0.5, 0.4, -0.1, 0.2], t2=[-0.1, 0.5, 0.3].某点在一号坐标系下坐标为p=[0.5, 0, 0.2].求p在二号坐标系下的坐标

假设在世界坐标系中p点的坐标为P。
用四元数做旋转则有（在Eigen中四元数旋转为q×v，数学中则为q×v×q^-1）：

q1 × P + t1 = p1
q2 × P + t2 = p2

由上两式分别解算出：

P = q1^-1 × (p1 - t1)
P = q2^-1 × (p2 - t2)

两式联立求解则得到：

p2 = q2 × q1^-1 × (p1 - t1) + t2

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
using namespace std;

int main()
{
    //四元数
    Eigen::Quaterniond q1 = Eigen::Quaterniond(0.35, 0.2, 0.3, 0.1).normalized();
    Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.5, 0.4, -0.1, 0.2).normalized();
    //平移向量
    Eigen::Vector3d t1 = Eigen::Vector3d(0.3, 0.1, 0.1);
    Eigen::Vector3d t2 = Eigen::Vector3d(-0.1, 0.5, 0.3);
    //目标向量
    Eigen::Vector3d p1 = Eigen::Vector3d(0.5, 0, 0.2);
    Eigen::Vector3d p2;

    //打印输出
    // cout << q1.coeffs() << "\n"
    //      << q2.coeffs() << "\n"
    //      << t1.transpose() << "\n"
    //      << t2.transpose() << endl;

    //四元数求解
    p2 = q2 * q1.inverse() * (p1 - t1) + t2;
    cout << p2.transpose() << endl;
}

值得一提的是，在Eigen中四元数表示旋转是 q ∗ P，然而数学上应该是 q ∗ P ∗ q^-1，很显然在Eigen中Quaterniond类中对 * 号进行了重载。

因此这里就有一种更加容易理解的方法（针对于Eigen中对四元数表示旋转的方式），可以看出在q ∗ P这种表示中，与旋转矩阵表示坐标系旋转很相似。
q类似于旋转矩阵的功能，p表示旋转前坐标系下的物体坐标，q ∗ P的结果就是旋转后坐标系下的物体坐标。之前在介绍旋转矩阵时说道，可以将旋转矩阵看做时一种映射函数，那么这里四元数进行坐标系转换与旋转矩阵相似，因此也可以采用映射函数的形式，对四元数坐标系转换进行理解。
四元数q表示一种映射函数，那么自变量与因变量分别是什么呢？需要看四元数在进行坐标转换时做了什么，四元数q是在绝对坐标系下的物体姿态，可以理解为绝对坐标系通过四元数q的变换下，能够转换到物体坐标系下（这里的转换仅只旋转不包含平移）。既然四元数q是将绝对坐标系旋转到物体坐标系，那么与旋转矩阵相同，也可以将绝对坐标系下的坐标点作为自变量，而物体坐标系下的坐标作为因变量。其余理解可参考旋转矩阵的介绍。
注意：这里与旋转矩阵中需要注意的点相同，旋转矩阵要注意是从谁变换到谁的旋转矩阵，而四元数要注意四元数q是在哪个坐标系下表示的，比如四元数q是在绝对坐标系下表示的，那么四元数q就表示的是绝对坐标系到物体坐标系下的旋转算法。相对的以四元数q进行坐标系旋转的自变量及因变量也就确定。和旋转矩阵一处一样，在使用四元数时一定明确在那个坐标系下表示，自变量、因变量分别是啥，否则结果不可能算对。

总结

在进行坐标系转换时，常会用到四元数、旋转矩阵以及变换矩阵，上面对其原理及使用进行了解释，文章的最后就用上面四元数中小萝卜的例子，采用这三种方式进行坐标系转换的演示。

#include <iostream>
#include <vector>
#include <algorithm>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);//定义两个小萝卜自身姿态的两个四元数
  q1.normalize();// 对两个四元数进行归一化
  q2.normalize();
 
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);      //  定义两个小萝卜的位置的两个三维坐标
  Vector3d p1(0.5, 0, 0.2);    //  用来表示小萝卜一号坐标系下该点坐标（题干给出）
  Vector3d p2;     //  用来表示小萝卜二号坐标系下该点坐标

  //  !下面分别用四元数以、变换矩阵及旋转矩阵得到小萝卜二号的该点观测信息
  //以下用四元数求解p2坐标
  p2 = q2 * q1.inverse() * (p1 - t1) + t2;    //  求解p2坐标
  //  这里的inverse是相反的意思，也就是求q1的逆矩阵
  //  公式为：p2 = q2  * q1^-1 * (p1 - t1) + t2
  cout << "四元数求得的p2坐标" << endl;
  cout << p2 << endl;    //  列向量显示
  cout << p2.transpose() << endl;    //  行向量显示，transpose是转置矩阵的意思

  //  以下用欧拉矩阵求解p2坐标
  Isometry3d T1w(q1), T2w(q2);    //  欧式变换矩阵Isometry（虽然称为3d，实质上是4＊4的矩阵）
  T1w.pretranslate(t1);    //  设置平移向量，我的理解是加入这个平移向量
  T2w.pretranslate(t2);

  Vector3d p3;      // 用来表示小萝卜二号坐标系下该点坐标（变换矩阵方法）
  p3 = T2w * T1w.inverse() * p1;    //  求解p3坐标
  cout << "变换矩阵/欧拉矩阵求得的p2坐标" << endl;
  cout << p3.transpose() << endl;

  // 以下采用旋转矩阵求解p2坐标
  Eigen::Matrix3d q1t, q2t;
  q1t = q1.toRotationMatrix();
  q2t = q2.toRotationMatrix();
  Vector3d p4;
  p4 = q2t * q1t.inverse()*( p1 - t1) + t2;
  cout <<  "旋转矩阵求得的p2坐标" <<endl;
  return 0;
}

额外介绍（Eigen库）

Eigen库的结构体对应的名称如下：
旋转矩阵（3×3） Eigen::Matrix3d
旋转向量（3×1）Eigen::AngleAxisd 旋转向量也是一种进行坐标转换的形式，目前用过，在这里还未总结
欧式变换矩阵（4×4） Eigen::Isometry3d
四元数（4×1）Eigen::Quaterniond

坐标系转换如图
在这里插入图片描述