首页 > 其他分享 >坐标系变换——“旋转矩阵/欧拉角/四元数”

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

时间:2022-09-27 00:15:02浏览次数:86  
标签:欧拉角 矩阵 旋转 四元 坐标 坐标系 向量

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

旋转矩阵

坐标变换的作用

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

在这辆车中有激光雷达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进行坐标系旋转的自变量及因变量也就确定。和旋转矩阵一处一样,在使用四元数时一定明确在那个坐标系下表示,自变量、因变量分别是啥,否则结果不可能算对。

参考文章: 

彻底搞懂“旋转矩阵/欧拉角/四元数”,让你体会三维旋转之美

坐标系变换矩阵推导

动手学ROS(8):坐标系间的欧氏变换

动手学ROS(9):坐标系变换示例

四元数-坐标系转换-旋转-转欧拉角

坐标系的变换(Eigen库四元数表示的坐标)

Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换

目标跟踪中的坐标变换问题以及四元数

eigen 四元数进行坐标旋转

视觉SLAM十四讲 3.6.2 实际的坐标变换例子代码解析

参考视频:

四元数的可视化

可视化工具

可视化2

 

标签:欧拉角,矩阵,旋转,四元,坐标,坐标系,向量
From: https://www.cnblogs.com/zhjblogs/p/16728697.html

相关文章

  • 世界坐标系转换/相机标定/畸变矫正-Python
    PnP(Perspective-n-Point)是求解3D到2D点的对应方法。不论是相机和雷达的标定还是相机和相机的标定都可以使用PNP来解决,即通过不同坐标系下相同的点对求解变换矩阵。这里......
  • gps坐标系转高德坐标系
    详细说明:坐标转换importrequests#116.497759,39.979316#116.497759,39.979331#116.497759,39.979346defgd_map():para={'key':'8a2191b448b4de6aefe7f......
  • Euroc数据集坐标系
    https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets上面是数据原链接 这张图是在数据集放的图 我只能说这个是真的真的很不标准。。......
  • 1.3(2) 建立空间直角坐标系和确定点坐标的方法
    \({\color{Red}{欢迎到学科网下载资料学习}}\)【基础过关系列】2022-2023学年高二数学上学期同步知识点剖析精品讲义(人教A版2019)\({\color{Red}{跟贵哥学数学,so\qua......
  • 四元数的说明
    直观理解的关键在于将四元数写成轴角的形式:q=[cosθ/2,sinθ/2(ex,ey,ez)],其中e=(ex,ey,ez)为旋转轴,θ为旋转角不管刚体现在是什么姿态,总存在一个轴,使得刚体绕着这个轴转......