向量的旋转一共有三种表示方法:旋转矩阵、欧拉角和四元数,接下来我们介绍一下每种旋转方法的原理以及相互转换方式。
旋转矩阵
坐标变换的作用
在一个机器人系统中,每个测量元件测量同一物体得出的信息是不一样的,原因就在于“每个测量元件所测量的数据是基于不同坐标系所测量的”,例如:
在这辆车中有激光雷达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中对应的坐标:
![](/i/l/?n=22&i=blog/1959382/202209/1959382-20220925211143375-1740055296.png)
坐标变换=旋转+平移,因此坐标变换表达式如下所示:
这样不利于矩阵运算,我们可以改写为如下形式:
其中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进行坐标系旋转的自变量及因变量也就确定。和旋转矩阵一处一样,在使用四元数时一定明确在那个坐标系下表示,自变量、因变量分别是啥,否则结果不可能算对。
参考视频:
标签:欧拉角,矩阵,旋转,四元,坐标,坐标系,向量 From: https://www.cnblogs.com/zhjblogs/p/16728697.html