第一部分:思维方式概述概述捋顺一下理解位姿的思维方式 空间中的点是数学中常见的概念,可以用坐标向量来描述,向量是既有大小又有方向的一种量;
坐标系里的一个点好表示,但是大多数情况下,是坐标系里的一个物体,而这个物体又包含很多点; 分两步分析: 第一,确定这个物体的位置和方向,如图b,向量B表示小车的位置和方向; 第二步,以这个B点为原点,设定小车自己的坐标系,这样就可以知道小车上1、2、3、4这四个点的位置和方向,如图b用四个向量表示! 接下来要约定一下用符号表示坐标系相互关系的方法; 以前我都是忽略这些约定符号,感觉看着累,不直观,也不容易理解;但是越深入学习越感觉这些符号公式的重要性,因为简单的可以直观理解,或者用语言就可以描述,但是简单的是基础,最终都会参与到复杂的运算,复杂的运算只能靠符号和公式解决,不然会毫无章法! 利用上面这些约定,画个如下图所示的坐标系,有两个坐标系{A},和{B}; 坐标系和向量的关系可以写成下面的公式,表示A坐标系下的P向量; 可以分两步表示,先表示以A坐标系作为参考坐标系表示B坐标系; 然后点乘B作为参考坐标系下的P向量; 上面例子表示出一个重要思路,那就是一个坐标系可以用另一个坐标系表示,这在机器人学里面是应用非常多的一个思路,要记住这个思路,这就说明机器人实际应用中,无论工件上设定的点在哪里,只要它有自己的工件坐标系,工件坐标系可以和机器人基坐标系关联起来,那么这个工件上的点也就可以用机器人基坐标系表示! 所以机器人坐标系可以千变万化,但是机器人最终计算的只有一组相对于世界坐标系的数值。 更复杂点的三个坐标系关联; 坐标系{A},{B},{C},如下图所示: 那么向量P的表示可以用下面的公式,A坐标系下的向量P可以分三步表示; 首先表示A坐标系下的B坐标系; 然后再表示B坐标系下的C坐标系; 最后表示C坐标系下的P向量;公式中间那个加号加圆圈代表坐标系转换关系。 接下来再举个三维坐标系的例子 帮助大家理解坐标系的转换关系;如下图所示,在世界坐标系{O}里有一个机器人坐标系为{R},机器人上安装一台相机,相机坐标系为{C},屋顶上有个固定的相机,固定相机坐标系为{F},还有一个工件,工件坐标系为{B}; 如下面两个变换等式; 第一个等式表示,世界坐标系下{O}的固定相机坐标系{F},变换到固定相机{F}坐标系下的工件坐标系{B},等于 世界坐标系{O}下的机器人坐标系{R},变换到机器人{R}坐标系下的机器人相机坐标系{C},再变换到机器人相机{C}坐标系下的工件坐标系{B}; 第二个等式表示,世界坐标系{O}下的固定相机坐标系{F},变换到固定相机{F}坐标系下的机器人坐标系{R},等于 世界坐标系{O}下的机器人坐标系{R};上图坐标系和下面等式中大家看到世界坐标系下的坐标系{F},{R}没有左上标,那是因为定义了世界坐标系可以不写左上标! 把上面的三维坐标系关系简化成下图所示: 上面的变换关系在实际应用中还是很多地方用到的,比如一个移动机器人带着个相机的项目,就可以精确地抓取到工件了! 第二部分,详解二维空间的位姿描述二维坐标系变换概述,需要的三个参数 用向量的思维,二维坐标系下的一个点P,应该用向量P表示,向量P又可以用X轴和Y轴的单位向量表示;如下图的公式,因为单位向量长度(模)为1,所以向量P是X个X轴单位向量与Y个Y轴单位向量的和; 下图表示了一个红色坐标系和一个蓝色坐标系的关系,图中明显可以看出,红色坐标系{B}是蓝色坐标系{A},通过向量t=(x,y)移动得到;然后再逆时针旋转θ角度。 由上可以看出,二维坐标系下,以坐标系{A}为参考坐标系,坐标系{B}的位置与姿态的确定,需要三个向量,(x,y,θ),这三个向量,涉及到三角函数的计算,无法直接用这三个值转换坐标系,所以下面我们用一种方法来表示旋转; 旋转变换的推导 看上图,任意点P相对于坐标系{A}和{B}的向量分别是AP和BP,我们要确定AP和BP之间的关系,要分两步走,旋转然后平移; 具体如上图所示,先只考虑旋转的情况,点P在参考坐标系{V}中,用单位向量组合可表示为如下等式: 上式最终写成行向量和列向量的点积形式,矩阵的概念马上要得到应用了;假设上图中坐标系{V}和{B}的轴长都是1,也就是单位向量,那么坐标系{V}经过旋转θ角度后,得到坐标系{B},坐标系{B}上X轴和Y轴的单位向量可表示为: 写成矩阵的形式如下: 这个单位向量旋转的关系就出来了, 接着点P在参考坐标系{B}中,用单位向量组合可表示为如下等式: 代入上式得: 和等式结合运算,最后可以写成 这个等式就是表示坐标系旋转后,参考坐标系{B}里的P点,怎么变换成参考坐标系{V}里的P点;等式中间的二维矩阵就叫旋转矩阵,用符号表示为: 上式可以写成符号表示: 同样的道理,坐标系旋转后,坐标系{V}里的点变换为坐标系{B}里的点,公式可以表示成:
MATLAB模拟旋转变换程序 下面用号称可以节省生命的超级神器MATLAB来做一下旋转矩阵的模拟: >> R = rot2(0.2)R =0.9801 -0.19870.1987 0.9801 这段程序里rot2(θ)是2*2的旋转矩阵,θ的单位是弧度! 旋转矩阵有一些性质可以用下面这个函数实验下:
这里的det(A)是矩阵行列式,结果是矩阵A的行列式;
工具箱也支持用符号代替数字计算,如下声明一个符号theta: >> syms theta>> R = rot2(theta)R =[ cos(theta), -sin(theta)][ sin(theta), cos(theta)] 矩阵的指数和对数计算 下面还是把0.3的弧度转换为旋转矩阵;
我们可以用logm函数计算矩阵的对数 >> S = logm(R)S =0.0000 -0.30000.3000 0.0000 然后用工具箱函数vex来解析S矩阵;
得出的结果是又算出了旋转角度的弧度值0.3; 矩阵对数logm的倒数(逆)是矩阵的指数函数expm; >> expm(S)ans =0.9553 -0.29550.2955 0.9553 结果得到了原来的旋转矩阵,还原了logm计算之前的数值; 矩阵的对数也就是矩阵指数的倒数(逆)!矩阵的指数或对数的计算还是比较复杂的,我觉得我会用这个函数就行了,想深入了解的要去看看《李群论》! 进而推导出二维的齐次变换矩阵 上面讲了旋转矩阵,这里旋转后再加上平移讲一下坐标系原点之间的平移,二维坐标系平移,就是简单的向量加法结合下面两图得出: 进而可以简写成: 上公式中t=(x,y),方向是{A}到{B}的变换; 再写成P点的变换: 其中的T就是齐次变换矩阵: MATLAB函数计算齐次变换 矩阵 下面用MATLAB工具箱里面的函数具体计算一下; 程序是表示平移(1,2),再旋转30度,得到其次变换矩阵T1;
transl2函数表示二维坐标系下的平移,trot2函数表示二维坐标系下的旋转; MATLAB图形化输出程序: Plotvol(0 5 0 5)表示创建一个X轴长度为5,Y轴长度也为5的二维坐标系; trplot2(T1, 'frame', '1', 'color', 'b') 表示建立一个坐标系T1,显示坐标系名称为1,颜色是b,蓝色; >> plotvol([0 5 0 5]);>> trplot2(T1, 'frame', '1', 'color', 'b') 如下图所示的蓝色坐标系: 接着再创建一个T2坐标系,只从原点平移(2,1);输出图形为上图的红色坐标系:
再创建一个绿色的坐标系T3,它的其次变换是T1*T2;也就是先平移(1,2),再旋转30度,再在坐标系{1}的基础上平移(2,1),上图中可以看出最终绿色坐标系{3}的位置并不是(3,3),那是因为第二次平移是在坐标系{1}的基础上平移的!程序如下: >> T3 = T1*T2T3 =0.8660 -0.5000 2.23210.5000 0.8660 3.86600 0 1.0000>> trplot2(T3, 'frame', '3', 'color', 'g'); 在创建一个T4,和T3对比下,T4的变换是T2*T1,变换顺序就相反了,它是先平移(2,1),再平移(1,2),再旋转30度;如上图中的坐标系{4}所示,程序如下:
图中可见,其次变换矩阵左乘和右乘的结果是完全不同的; 我们再创建一个点P(3,2); 如上图所示;程序如下: >> P = [3 ; 2 ];>> plot_point(P, 'label', 'P', 'solid', 'ko'); 下面再来研究这个点P相对与各个坐标系的坐标 看看P点在坐标系{1}里的表达,前面学到的术语在这里用到了,在坐标系{0}里的P点,等于坐标系{0}变换到坐标系{1},再表示坐标系{1}下的P点,等式如下: 然后转换一下,变成坐标系{1}下面的P点坐标的表达,也就是坐标系{1}变换为坐标系{0},再点乘坐标系{0}下的P点坐标向量! 用程序表示为下,得出P点的其次变换形式,P点的矩阵里面加个1就是为了和其次变换格式相同,函数inv是矩阵求逆;
也可以试试下面的形式,函数e2h是将欧几里得坐标系转换成其次变换坐标形式,h2e再逆转换,把其次变换形式转换成普通坐标形式; >> h2e( inv(T1) * e2h(P) )ans =1.7321-1.0000 详细探究纯旋转的旋转中心例子 首先创建一个X坐标轴是-5到4,Y坐标轴是-1到5的坐标系{0};再创建一个目标坐标系{X};函数eye是创建单位矩阵,下面程序中说明T0是3*3的单位矩阵;
再创建一个2弧度的旋转; >> R = trot2(2); 输出如下两种组合的图形,如下图所示,坐标系{RX}是围绕坐标系{O}原点旋转2弧度(115度),坐标系{XR}是围绕坐标系{X}旋转2弧度,可以看出两种不同的组合的输出结果不同了吧!
深入点应用,如果我们想绕任何一点旋转该怎么做呢? 我们先创建任意一点C,如上图所示,程序如下: >> C = [1 2]';>> plot_point(C, 'label', ' C', 'solid', 'ko') 然后写一下绕C点旋转的程序,理解为先平移到C点,再按照旋转矩阵R旋转,再把矩阵里面的每个元素变成负数,再平移,得到了{X}绕C点旋转的坐标系{XC},如上图所示;这个在实际应用中很有用,比如想让机器人绕抓手上任意一点旋转,TCP的理论基础!
输出图形结果: >> trplot2(RC*X, 'framelabel', 'XC', 'color', 'r'); 扭转函数Twist 绕任意点旋转在机器人工具箱里有个更方便的函数Twist,例如我们想创建上面的绕C点的旋转2弧度,可以写成,
Twist函数里面包含很多参数,我们看看它生成的旋转矩阵是啥样的: >> tw.T(2)ans =-0.4161 -0.9093 3.23470.9093 -0.4161 1.92300 0 1.0000 是不是和上面一堆公式推导出的RC旋转矩阵是一样的,这就是MATLAB工具箱的牛逼地方,节省生命的工具!我觉得会用这些函数就好,如果应用到实际机器上,MATLAB可以转换成很多种语言,包括PLC的ST语言!
关于三维的齐次变换详解,后面会接着更新,感觉有用和喜欢的朋友关注我,我会持续给大家更新一些实用的经验! |
|