坐标系变换——“旋转矩阵/欧拉角/四元数”

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

旋转矩阵

坐标变换的作用

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

在这辆车中有激光雷达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中对应的坐标:

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

其中O1O2是从O1指向O2的向量。

欧拉角

欧拉角的作用

欧拉角遵循的是右手系规则,即大拇指指向坐标轴正方向,四指旋转的方向即为转动的正方向,欧拉角包含三个自由量: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

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

参考文章: 

posted @ 2022-09-26 23:55  北极星!  阅读(6177)  评论(0编辑  收藏  举报