机器人的空间描述与坐标变换

1、1 第二章 机器人的空间描述和坐标变换 2.1 位姿和坐标系描述 2.2平移和旋转坐标系映射 2.3平移和旋转齐次坐标变换 2.4物体的变换和变换方程 2.5通用旋转变换 2 z y x a p p p p 2-1图 位置表示 2.1位置方位表示与坐标系描述位置方位表示与坐标系描述 n1.位置描述位置描述 矢量矢量 ap 表示箭头指向点的位置矢量，其表示箭头指向点的位置矢量，其 中右上角标中右上角标“a”表示该点是用表示该点是用a坐标系描述坐标系描述 的。的。 （2-2） n2.方位描述方位描述 坐标系坐标系b与机械手末端工具固连，工具的姿态与机械手末端工具固连，工具的姿态 可以由坐标系可以由

2、坐标系b的方向来描述。而坐标系的方向来描述。而坐标系b的方的方 向可以用沿三个坐标轴的单位矢量来表示向可以用沿三个坐标轴的单位矢量来表示 333231 232221 131211 rrr rrr rrr r b a b a b aa b zyx 图2-2方位表 示 （2-1） 旋转矩阵旋转矩阵 描述坐标系描述坐标系b的姿态，矢量的姿态，矢量 描述坐标系描述坐标系b的原点位的原点位 置。置。 3 bo aa b rbp 3.位姿描述位姿描述 固连坐标系把刚体位姿描述问题转化为坐标系的描述问题。图固连坐标系把刚体位姿描述问题转化为坐标系的描述问题。图2-3 中坐标系中坐标系b可以在固定坐标系可以在

3、固定坐标系a中描述为中描述为 （2-3） r a b bo a p 4 1.平移坐标变换平移坐标变换 图图2-3平移变换平移变换 bp为坐标系 为坐标系b描述的某一空间位描述的某一空间位 置，我们也可以用置，我们也可以用ap（坐标系（坐标系a）描）描 述同一空间位置。因为两个坐标系具有述同一空间位置。因为两个坐标系具有 相同的姿态，同一个点在不同坐标系下相同的姿态，同一个点在不同坐标系下 的描述满足以下关系的描述满足以下关系 aba bo ppp （2-4） 2.2平移和旋转坐标系映平移和旋转坐标系映 射射 旋转坐标变换的任务是已知坐标系旋转坐标变换的任务是已知坐标系b描述的描述的 一个点的位

4、置矢量一个点的位置矢量bp和和旋转矩阵旋转矩阵 ，求在坐标，求在坐标 系系a下描述同一个点的位置矢量下描述同一个点的位置矢量ap。 5 2.旋转坐标变旋转坐标变 换换 r a b abt b xa abt b ya abt b za p p p xp yp zp （2-5） 将（将（2-5）式写成矩阵形式得：）式写成矩阵形式得： pp z y x p ba b b t a b t a b t a b a r （2-6） 图2-4旋转变换 式（式（2-6）即为我们要求的旋转变换关系，该变换是通过两个坐）即为我们要求的旋转变换关系，该变换是通过两个坐 标系之间的旋转变换实现的。标系之间的旋转变换实

5、现的。 6 3.复合变换复合变换 图图2-5复合变换复合变换 如果两个坐标系之间即存在平移如果两个坐标系之间即存在平移 又存在旋转，如何计算同一个空间点又存在旋转，如何计算同一个空间点 在两个坐标系下描述的变换关系？在两个坐标系下描述的变换关系？ 为了得到位置矢量为了得到位置矢量bp和和ap之之 间的变换关系，我们建立一个中间的变换关系，我们建立一个中 间坐标系间坐标系c。 ppp ba b bc b c rr acaaba cobbo rppppp （2-7） （ 2-8） 为了得到位置矢量为了得到位置矢量bp和和ap之间的变换关系，只需坐标系之间的变换关系，只需坐标系b 在坐标系在坐标系

6、下下a的描述。的描述。 是是4 4矩阵，称为齐次坐标变换矩阵。可以理解为坐标系矩阵，称为齐次坐标变换矩阵。可以理解为坐标系b在固定坐在固定坐 标系标系a中的描述。中的描述。 7 2.3齐次坐标变换 坐标变换（坐标变换（2-8）可以写成以下形式）可以写成以下形式 1101 ppp b bo aa b a r （2-9） 将位置矢量用将位置矢量用4 1矢量表示，增加矢量表示，增加1维的数值恒为维的数值恒为1，我们仍然用原，我们仍然用原 来的符号表示来的符号表示4维位置矢量并采用以下符号表示坐标变换矩阵维位置矢量并采用以下符号表示坐标变换矩阵 10 bo aa ba b r t p （2-10） p

7、p ba b a t（2-11） t a b 齐次坐标变换的主要作用是表达简洁，同时在表示多个坐标变换齐次坐标变换的主要作用是表达简洁，同时在表示多个坐标变换 的时候比较方便。的时候比较方便。 1.齐次变换齐次变换 8 2.齐次变换算子齐次变换算子 在机器人学中还经常用到下面的变换，如图在机器人学中还经常用到下面的变换，如图2-8，矢量，矢量ap1沿矢量沿矢量 aq平移至的 平移至的aq终点，得一矢量终点，得一矢量ap2。已知。已知ap1和和aq求求ap2的过程称之为的过程称之为 平移变换，与前面不同，这里只涉及单一坐标系。平移变换，与前面不同，这里只涉及单一坐标系。 图图2-6平移算子平移算

8、子 qpp aaa 12 （2-12） 可以采用齐次变换矩阵表示平移变换可以采用齐次变换矩阵表示平移变换 12 )(pqp aaa trans （2-13） )( q a trans 称为平移算子，其表达式为称为平移算子，其表达式为 1 )( 0 q q a a i trans （2-14） 其中其中i是是3 3单位矩阵。例如若单位矩阵。例如若aq=ai+bj+ck， 其中其中i、j和和k分别表示坐标系分别表示坐标系a三个坐标轴的三个坐标轴的 单位矢量，则平移算子表示为单位矢量，则平移算子表示为 1000 100 010 001 ),( c b a cbatrans 9 同样，我们可以研究矢量

9、在同一坐标系下的旋转同样，我们可以研究矢量在同一坐标系下的旋转 变换，如图变换，如图2-9，ap1绕绕z轴转轴转 角得到角得到ap2。则。则 图图2-7旋转算子旋转算子 12 ),(pp aa zrot （2-20） rot(z, )称为旋转算子，其表达式为称为旋转算子，其表达式为 1000 0100 00 00 ),( cs sc zrot （2-21） 同理，可以得到绕同理，可以得到绕x轴和轴和y轴的旋转算子轴的旋转算子 1000 00 00 0001 ),( cs sc xrot 1000 00 0010 00 ),( cs sc yrot 10 定义了平移算子和旋转算子以后，可以将它们

10、复合实现复杂的映射定义了平移算子和旋转算子以后，可以将它们复合实现复杂的映射 关系。变换算子与前面介绍的坐标变换矩阵形式完全相同，因为所有描关系。变换算子与前面介绍的坐标变换矩阵形式完全相同，因为所有描 述均在同一坐标系下，所以不需上下标描述（坐标系）。述均在同一坐标系下，所以不需上下标描述（坐标系）。 21 aa pt p （2-23） t a b a br bo a p pp ba b a t 21 aa pt p 齐次坐标变换总结： 表示坐标系表示坐标系b在坐标系在坐标系a下的描述，下的描述， 的各列是坐标系的各列是坐标系 b三个坐标轴方向的单位矢量，三个坐标轴方向的单位矢量， 而表示坐

11、标系而表示坐标系b原点位置原点位置 。 2. 它是不同坐标系间的坐标变换。如它是不同坐标系间的坐标变换。如 3 3. .它是同一坐标系内的变换算子。它是同一坐标系内的变换算子。 齐次坐标变换是复杂空间变换的基础，必须认真理解和掌握。具体应齐次坐标变换是复杂空间变换的基础，必须认真理解和掌握。具体应 用的关键是理解它代表的是上面三种含义的哪一种，而不是简单的套用用的关键是理解它代表的是上面三种含义的哪一种，而不是简单的套用 公式！公式！ 1. 它是坐标系的描述。它是坐标系的描述。 如图如图2-10表示的三个坐标系，已知坐标系表示的三个坐标系，已知坐标系 a、b和和c之间的变换矩阵之间的变换矩阵

12、和和 位置位置 矢量矢量cp，求在坐标系，求在坐标系a下表示同一个点的下表示同一个点的 位置矢量位置矢量ap。 11 3.复合变换复合变换 复合变换主要有两种应用形式，一种是建立了多个坐标系描述机器人复合变换主要有两种应用形式，一种是建立了多个坐标系描述机器人 的位姿，任务是确定不同坐标系下对同一个量描述之间的关系；另一种是的位姿，任务是确定不同坐标系下对同一个量描述之间的关系；另一种是 一个空间点在同一个坐标系内顺序经过多次平移或旋转变换，任务是确定一个空间点在同一个坐标系内顺序经过多次平移或旋转变换，任务是确定 多次变换后点的位置。多次变换后点的位置。 图图2-10 复合坐标变换复合坐标变

13、换 t a b t b c pp cb c b t ppp cb c a b ba b a ttt （2-24） （2-25） ttt b c a b a c 根据坐标变换的定义得根据坐标变换的定义得 （2-26） 12 (a) zy顺序旋转顺序旋转 (b) y z顺序旋转顺序旋转 图图2-11旋转顺序对旋转顺序对 变换结果影响变换结果影响 例例2-3已知点已知点u=7i+3j+2k，先对它进行绕，先对它进行绕z轴旋转轴旋转90o 的变换得点的变换得点v，再对点，再对点v进行绕进行绕y轴旋转轴旋转90o的变换得的变换得 点点w，求，求v和和w。 1 2 7 3 1 2 3 7 1000 010

14、0 0001 0010 )90,(uv o zrot 1 3 7 2 1 2 7 3 1000 0001 0010 0100 )90,(vw o yrot 如果只关心最后的变换结果，可以按下式计算如果只关心最后的变换结果，可以按下式计算 ( ,90 )( ,90 )( ,90 ) ooo rot yrot yrot zwvu 001072 100037 010023 000111 计算结果与前面的相同，称计算结果与前面的相同，称r= rot(y,90o) rot(z,90o) 为复合旋转算子。为复合旋转算子。 13 注：固定坐标系变换，矩阵乘的顺序注：固定坐标系变换，矩阵乘的顺序“自右向左自右

15、向左” 如果改变旋转顺序，先对它进行绕如果改变旋转顺序，先对它进行绕y轴旋转轴旋转90o，再绕，再绕z轴旋转轴旋转90o，结，结 果如图果如图2-11b所示。比较图所示。比较图2-11a和图和图2-11b可以发现最后的结果并不相同，可以发现最后的结果并不相同， 即旋转顺序影响变换结果。即旋转顺序影响变换结果。 从数学角度解释就是矩阵乘法不满足交换率，从数学角度解释就是矩阵乘法不满足交换率， rot(y,90o) rot(z,90o) rot(z,90o) rot(y,90o)。 和和 ，求求 和和给定给定 计算计算 14 2.4物体的变换和变换方程物体的变换和变换方程 t a b t b a

16、已知坐标系已知坐标系b相对坐标系相对坐标系a的描述的描述 求求坐标系坐标系a相对坐标系相对坐标系b的描述的描述 一种直接的方法是矩阵求逆，另一种方法是根据变换矩阵的特点直一种直接的方法是矩阵求逆，另一种方法是根据变换矩阵的特点直 接得出逆变换。后一种方法更简单方便。接得出逆变换。后一种方法更简单方便。 即齐次变换的求逆问题。即齐次变换的求逆问题。 t a b t b a 等价为：已知等价为：已知 r a b bo a p r b a ao bp 是坐标系是坐标系b的原点在的原点在坐标系坐标系b中的描述，显然为零矢量。中的描述，显然为零矢量。 由由（2-28）式得）式得 15 根据前面的讨论，旋

17、转矩阵关系为根据前面的讨论，旋转矩阵关系为 ta b a b b a rrr 1 （2-27） 将坐标变换用于坐标系将坐标变换用于坐标系b的原点得的原点得 ao b bo ab abo b rppp（2-28） bo b p bbaat a aoabobbo rr ppp （2-29） 逆变换可以直接用正变换的旋转矩阵和平移矩阵表示逆变换可以直接用正变换的旋转矩阵和平移矩阵表示 10 bo ata b ta bb a rr t p （2-30） 16 a沿沿xa平移平移3个单位，再绕新的个单位，再绕新的za 轴转轴转180o得得 b 1801800100 1801800010 001001 a

18、 b cs rsc 因此因此 1003 0100 0010 0001 a bt b沿沿zb平移平移2个单位，然后绕个单位，然后绕yb轴转轴转90o再绕新再绕新xb轴转轴转150o得得 c 312 2 3311 2222 312 2 900901000 0 1 1000 01001501500 1 0 00 9009001501501 0 0 0100 b c cs rcs scsc 图图2-12楔形块角点坐标楔形块角点坐标 系系 例例2-4，如图如图2-12给出的楔形块角点坐标系，求齐次坐标变换给出的楔形块角点坐标系，求齐次坐标变换, aba bcc ttt, 3311 2222 3311 2

19、222 10030003 01000000 001010021002 000100010001 aab cbc tt t 因此因此 a沿沿xa和和za平移平移3和和2，然后绕，然后绕ya轴转轴转90，再绕新，再绕新xa轴转轴转 - -30得得c 也可以按以下方法计算也可以按以下方法计算 31 22 3311 2222 31 22 9 009 0100 01003 03 0 9 009 003 03 0 0011000 01000 1000100 a c cs rcs scsc 17 事实上，对于像本例题这种简单的情况，可以直接利用齐次坐标变换事实上，对于像本例题这种简单的情况，可以直接利用齐次

20、坐标变换 的定义得到变换矩阵。即直接写出坐标系的定义得到变换矩阵。即直接写出坐标系c坐标轴矢量在坐标系坐标轴矢量在坐标系a下表下表 示得旋转矩阵，平移矢量为坐标系示得旋转矩阵，平移矢量为坐标系c的原点在坐标系的原点在坐标系a下的矢量表示。下的矢量表示。 18 变换方程变换方程 图图2-13表示了多个坐标系的关系图，可以用两种不表示了多个坐标系的关系图，可以用两种不 同的方式得到世界坐标系同的方式得到世界坐标系u下坐标系下坐标系d的描述。的描述。 ttt a d u a u d tttt c d b c u b u d （2-31） （2-32） 由（由（2-31）和（）和（2-32）可以得到变

21、换方程）可以得到变换方程 图图2-13坐标变换序列坐标变换序列 可以利用变换方程可以利用变换方程（2-33）求解其中任意一个未知变换。例如，假设求解其中任意一个未知变换。例如，假设 除除 以外其余变换均为已知，则该未知变换可以用下式计算以外其余变换均为已知，则该未知变换可以用下式计算t u b 11uuacb baddc tt t tt 在坐标系的图形表示方法中，从一个坐标系原点指向另一个坐标系原点在坐标系的图形表示方法中，从一个坐标系原点指向另一个坐标系原点 的箭头表示坐标系的描述关系。的箭头表示坐标系的描述关系。 ttt b c u b u c （2-35） 1uudd caac tt t

22、t （2-36） 19 例例2-5假设已知图机械臂末端工具坐标系假设已知图机械臂末端工具坐标系t相对基座相对基座 坐标系坐标系b的描述，还已知工作台坐标系的描述，还已知工作台坐标系s相对基座相对基座 坐标系坐标系b的描述，并且已知螺栓坐标系的描述，并且已知螺栓坐标系g相对工作相对工作 台坐标系台坐标系s的描述。计算螺栓相对机械臂工具坐标的描述。计算螺栓相对机械臂工具坐标 系的位姿。系的位姿。 解：添加从工具坐标系解：添加从工具坐标系t原点到螺栓坐标系原点到螺栓坐标系g原点原点 的箭头，可以得到如下变换方程的箭头，可以得到如下变换方程 tttt t g b t s g b s （2-37） 螺栓

23、相对机械臂工具坐标系的位姿描述为螺栓相对机械臂工具坐标系的位姿描述为 tttt s g b s b t t g 1 （2-38） 20 xyz ffffijk 1.绕任意轴旋转变换绕任意轴旋转变换 下面讨论绕任意轴下面讨论绕任意轴 f 旋转矩阵，轴在坐标系旋转矩阵，轴在坐标系a下表示为下表示为 以以 f 为为 z 轴建立与轴建立与a固连的坐标系固连的坐标系c用用n、o和和f表示坐标系表示坐标系c三个坐标三个坐标 轴的单位矢量，在坐标系轴的单位矢量，在坐标系a下表示为下表示为 图图2-18绕任意轴旋转变换绕任意轴旋转变换 xyz xyz xyz nnn ooo fff nijk oijk fij

24、k xxx a cyyy zzz nof rnof nof 因为固连的坐标系因为固连的坐标系c与与a固连，所以绕固连，所以绕 f旋转旋转 等价于绕等价于绕zc旋转。为此我们先将旋转。为此我们先将ap在坐标系在坐标系c下下 表示，再绕表示，再绕zc旋转旋转 角，最后再把旋转得到的矢量角，最后再把旋转得到的矢量 用坐标系用坐标系a表示。表示。 cat aca ac rrppp 1 ( , )( , ) at acc c rot zrot zrppp ap1 = rot(f, ) ap 2.52.5通用旋转变换通用旋转变换 21 再将再将cp1在坐标系在坐标系a下表示下表示 11 ( , ) aaa

25、t aac ccc rr rot zrppp 0 ( , )( , )0 001 xyz xxx aat ccyyyxyz zzzxyz xxyyzz xxx yyyxxyyzz zzzxyz nnn nofcs rotr rot zrnofscooo noffff n co sn co sn co s nofn nofn so cn so cn so c noffff f 111 222 333 oa noa noa 因此因此 1 2 3 xxxxxxxxxx xyyxxyxyxy xzzxxzxzxz nn n cn o sn o so o cf f nn n cn o sn o so o

26、 cf f nn n cn o sn o so o cf f 其中一个矢量其中一个矢量 上式中的上式中的n和和o各分量是未知的，需要用各分量是未知的，需要用 f 的各分量表示的各分量表示 22 根据坐标系的右手规则知根据坐标系的右手规则知n o = f，叉积可以按下式计算，叉积可以按下式计算 ()()() xyzyzzyzxxzxyyx xyz nnnn on on on on on o ooo ijk n oijk (),(),() yzzyxxyxzyxyyxz n on ofn on ofn on of 再根据旋转矩阵的正交性可以得再根据旋转矩阵的正交性可以得 1,0 xxxxxxxyxyxy n no of fn no of f ( , ),1 xxxyzxzy xyzyyyzx xzyyzxzz f f vcf f vf sf f vf s rotf f vf sf f vcf f vf svc f f vf sf f vf sf f vc f 1 2 3 xxxxxxxxxx xyyxxyxyxy xzzxxzxzxz nn n cn o sn o so o cf f nn n cn o