莫听穿林打叶声,何妨吟啸且徐行。竹杖芒鞋轻胜马,谁怕,一蓑烟雨任平生。料峭春风吹酒醒,微冷,山头斜照却相迎。回首向来萧瑟处,归去,也无风雨也无晴。
----《定风波》 苏 轼
最近疫情风波很严重,来一首《定风波》定一下自己忐忑不安的心情!以便静心学习提升自己!
上篇很详细地讲了二维坐标系的姿态相关问题,今天讲讲三维坐标系,三维比二维复杂了不少,篇幅有限,这篇文章先讲一部分,后面会不断更新,谢谢支持!
这篇主要内容有三个部分:
1、概述三维坐标系的姿态变换;
2、概述三维坐标系的旋转方向和旋转顺序的特点;
3、三维坐标系旋转矩阵运算;
4、详解旋转方向和旋转顺序,三角度表示法;
由浅入深,图文结合,更有动画加持;希望对需要的朋友有所启示,我自己从中学到了不少自己想要的知识!有上一篇文章二维坐标系姿态的知识最好,毕竟这是一本连续的教材!
三维坐标系
概述三维坐标系:三维坐标系是二维坐标系的扩展,在X轴和Y轴基础上加一个Z轴,垂直于(正交)X轴和Y轴,Z轴的方向确定是根据右手定则;
说到右手定则,我有段时间一直和别人争论,咋回事呢?因为我的手势和别人不一样!
后来用两种手势都比划一下,发现结果是一样的,所以各位感觉哪个方便就用哪个吧,右手定则结果只有一个!如下两幅图:
两种手势,结果一样
在三维坐标系里点P该怎么表示呢?正常是这样P(x,y,z),三个坐标参数。但是在这篇文章里还是用向量的思维去表示它!如下图,向量P,是x个x单位向量,y个y单位向量,z个z单位向量相加的结果!
单位向量表示坐标系里的点
用个如下图例概述下三维坐标系下的变换;
三维坐标系变换图
上图中显示一个蓝色坐标系和一个红色坐标系,我们需要搞清楚它们之间的关系!
看着花里胡哨的,其实它们的关系并不复杂!
图中可以看出,红色坐标系{B}是通过向量t(x,y,z)平移的;
蓝色坐标系{A}通过向量t(x,y,z)平移后;
然后经过复杂的旋转得到红色坐标系{B};
和二维坐标系的平移和旋转类似,只是多了一个维度,旋转就复杂了很多,下面详解旋转部分!
下面我们探究的就是以坐标系{A}为参考坐标系的点P,和以坐标系{B}为参考坐标系的点P,两者之间的关系,和二维一样,我们会分两步表示这个关系:
第一步,旋转;
第二步,平移;
概述三维坐标系里的旋转方向和顺序特点:上图中的两个坐标系,你可以想象,把蓝色的坐标系{A}拿在手里,旋转摆弄它,让它看起来像红色的坐标系{B}。
三维坐标系有个欧拉定理,它表示任何旋转都可以看作是围绕不同坐标轴的一系列旋转。
怎么理解这个定理呢?如下图所示:显示了一个右手坐标系,以及在围绕不同的坐标轴旋转了不同角度后的显示效果。
坐标轴旋转例子
再看下图所示的两组图,可以看出为什么三维坐标系旋转比较复杂了;
两组坐标系分别旋转的角度和位置都是一样的,但是旋转顺序不一样,最终的结果也不一样了:
两种旋转顺序
正交旋转矩阵:只作旋转的分析,我们可以把坐标系X/Y/Z的轴都看成是单位向量(三个轴的长度都为1,便于写公式计算),和二维坐标系一样,三维坐标系的方向就可以用单位向量表示,如下公式中的坐标系旋转转换关系,左边是用单位向量表示的{A}坐标系,等于右边旋转矩阵R点乘用单位向量表示的{B}坐标系;
坐标系旋转变换公式
上式中的旋转矩阵R和二维坐标系的R是有所不同的,它并不是一个确定的旋转矩阵,三维坐标系的旋转,可以绕X轴旋转,可以绕Y轴旋转,也可以绕Z轴旋转,或者任意组合旋转,所以比较复杂!
分别列出绕X轴、绕Y轴、绕Z轴旋转的旋转矩阵,如下图,推导过程就不写了,有兴趣的看我以前写的文章,这次主要讲用MATLAB的算法,直接使用就好!
用MATLAB写的计算程序如下,rotx函数是计算绕X轴旋转的,里面的参数pi/2是弧度单位!
>> R = rotx(pi/2)
R =
1.0000 0 0
0 0.0000 -1.0000
0 1.0000 0.0000
用图形化显示出来,如下图a;
>> trplot(R)
原点坐标系旋转后结果
三维的旋转不太直观,可以用用matlab输出旋转的动画图,用下面这个函数:
>> tranimate(R)
旋转动图
有个小扩展,刚开始安装工具箱,运行R = rotx(pi/2)这个命令可能会得出不一样的结果,如下图,那是因为rotx这个函数,不是机器人工具箱的函数,是其它工具箱的,名称重复了,那就得先看看这个函数来自哪个工具箱:
用函数which来看看这个rotx的路径,找到那个重复的工具箱,在附加功能-管理附加功能里找到对应的工具箱删除即可。
>> which rotx
错误的结果
还可以输出3D图,加上红蓝3D眼镜看,比较牛逼,函数和眼镜和动图如下:
>> tranimate(R, '3d')
3D眼镜
旋转完X轴后,在X旋转的基础上再次旋转Y轴试试,程序和显示结果如下图b所示;
>> R = rotx(pi/2) * roty(pi/2)
R =
0.0000 0 1.0000
1.0000 0.0000 -0.0000
-0.0000 1.0000 0.0000
>> trplot(R)
原点坐标系旋转后结果
加动图,程序函数和动图如下;
>> tranimate(R)
旋转动图
如果颠倒旋转顺序,先转Y轴再转X轴,那么结果是完全不同的,程序和如下;
>> roty(pi/2)*rotx(pi/2)
ans =
0.0000 1.0000 0.0000
0 0.0000 -1.0000
-1.0000 0.0000 0.0000
输出动画
>> tranimate(R)
旋转动图
三角度表示法:插入一个欧拉旋转定理:任何旋转都可以用不超过三次绕坐标轴的旋转表示。
这意味着一般情况下,坐标系间的任意旋转可以分解为三个旋转角和相关的旋转轴序列;
意思就是说你把三维坐标系拿在手里,随便旋转(不是绕着某个轴旋转,是立体空间的随意转动),转动后的坐标系状态都可以通过只转动X轴/Y轴/或Z轴,一次,两次,或者三次得到;至于先转动哪个轴线我们就叫它旋转轴序列,下面我们会详细探究!
旋转顺序古人已经给我们定义好了,要求是绕三个轴依次旋转,但是不能连续绕同一个轴旋转两次。旋转顺序分为两种:
欧拉式;
卡尔丹式;
这是两个物理学家命名的旋转方式;
欧拉式是绕一个特定的轴重复旋转,但是不是连续的,如:
XYX、XZX、YXY、YZY、ZXZ、ZYZ;
卡尔丹式是绕着三个不同的轴旋转,如:
XYZ、XZY、YZX、YXZ、ZXY、ZYX;
这两种旋转,手动调试过机器人的可能深有体会,想把一个抓手手动移动到一个姿态,先转动哪个坐标轴再转动哪个坐标轴,有很多方法可以达到,基本上也都是下意识的操作,没有去总结其中的规律(和物理学家的差距,哈哈);
再比如会玩魔方的朋友,旋转魔方,也是按照这两种方式里的一种旋转规律操作的,只是大家不知道这个旋转还有个名字!
所有的这12种旋转顺序统称为欧拉角!
下面用ZYZ欧拉角为例证;
公式表示为:
ZYZ
程序用单轴旋转函数表示为:
>> R = rotz(0.1) * roty(0.2) * rotz(0.3);
或者用eul2r函数表示为,eul2r函数是把弧度转化为参与运算的旋转矩阵:
>> R = eul2r(0.1, 0.2, 0.3)
R =
0.9021 -0.3836 0.1977
0.3875 0.9216 0.0198
-0.1898 0.0587 0.9801
如果反过来,知道旋转矩阵,想转换为弧度,则用tr2eul函数,反函数程序如下:
>> gamma = tr2eul(R)
gamma =
0.1000 0.2000 0.3000
但是这种求反函数很大可能是不唯一的,也就是说,你可以给定三个轴的旋转角度得到唯一的旋转矩阵,
但是你知道旋转矩阵想反推旋转角度就可能不是你想要的数值了;
实际应用也会遇到同样的问题,比如用ZYZ欧拉角的机器人你示教了两个点,抓手从a点旋转到b点,轨迹是没有问题的,但是想计算出哪个坐标轴各旋转了多少度,则很大可能计算出来的不是你想要的结果!
先举例说明一下这个现象,具体解决方式后面再详解!
如果我们让上面的θ值为负值,-0.2;
我们用程序模拟一下,先转换为旋转矩阵;
>> R = eul2r(0.1 , -0.2, 0.3)
R =
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
然后再求反函数,通过旋转矩阵求弧度:
>> tr2eul(R)
ans =
-3.0416 0.2000 -2.8416
可以看出,这个弧度怎么和输入的不一样呢!
但是你再求这新的三个弧度值得旋转的矩阵:
>> eul2r(ans)
ans =
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
这个旋转矩阵又和(0.1,-0.2,0.3)这三个弧度的旋转矩阵相同;
由此我们证明了上面所说的,从旋转矩阵到欧拉角的对应关系并不是唯一的!
如果θ值为零,我们脑补下,ZYZ欧拉角旋转,其实就是Z在旋转,下面程序验证下:
>> R = eul2r(0.1, 0, 0.3)
R =
0.9211 -0.3894 0
0.3894 0.9211 0
0 0 1.0000
再求反函数:
>> tr2eul(R)
ans =
0 0 0.4000
这里求出的最后一个Z弧度是0.4,是两个Z弧度相加的和!
再举个ZYX欧拉角的例证
这种卡尔丹角,常用的是XYZ和ZYX两种,工业机器人各家用法也不同,KUKA用的是ZYX,史陶比尔用的好像是XYZ;公式如下:
ZYX
在飞机或船舶导航上Z、Y、X对应 ,偏航角(航向)、俯仰(姿态)、横滚(侧倾);
举例飞机比较直观,因为你先旋转哪个轴,立马就能实际体现出效果来;但是六轴机器人是显示最终三个轴旋转完后的结果,旋转顺序不同,只能看出结果不同!上面的旋转动图里可以看出就是直接旋转到计算结果处!
MATLAB有个工具可以模拟飞机的角度旋转,可以实际操作下,X/Y/Z对应的旋转姿态,如下调用程序和图示:
用这个函数tripleangle 调用即可;
>> tripleangle
模拟飞机航向
程序举例,ZYX的旋转,弧度为(0.1,0.2,0.3);
>> R = rpy2r(0.1, 0.2, 0.3)
R =
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.0370
-0.1987 0.0978 0.9752
反函数为:
>> gamma = tr2rpy(R)
gamma =
0.1000 0.2000 0.3000
由于图片较多,容量大小有限制,就先写到这里,后面会接着搞清楚三维坐标系的奇异点问题,绕任意点旋转,单位四元素这些非常实用的算法!
喜欢或者感觉有用的朋友关注我,我会持续给大家更新工业控制领域实用的经验!
,