第2章 空间描述与坐标变换

1、1 第二章 空间描述与坐标变换 z y x A p p p p 2-1图 位置表示 2-1位置姿态表示与坐标系描述位置姿态表示与坐标系描述 n位置描述位置描述 矢量矢量 Ap 表示箭头指向点的位置矢量，其中右表示箭头指向点的位置矢量，其中右 上角标上角标“A”表示该点是用表示该点是用A坐标系描述的。坐标系描述的。 （2-2） n方位（姿态）描述方位（姿态）描述 坐标系坐标系B与机械手末端工具固连，工具的姿态可以与机械手末端工具固连，工具的姿态可以 由坐标系由坐标系B的方向来描述。而坐标系的方向来描述。而坐标系B的方向可的方向可 以用沿三个坐标轴的单位矢量来表示以用沿三个坐标轴的单位矢量来表示

2、333231 232221 131211 rrr rrr rrr R B A B A B AA B ZYX 图2-3姿态表示 （2-1） 旋转矩阵旋转矩阵 是用坐标系是用坐标系A来表示坐标系来表示坐标系B沿坐标轴方向单位矢量组沿坐标轴方向单位矢量组 成的矩阵，同样我们也可以用坐标系成的矩阵，同样我们也可以用坐标系B来表示坐标系来表示坐标系A的单位矢量得的单位矢量得 到旋转矩阵到旋转矩阵 2 旋转矩阵的元素可以用坐标系旋转矩阵的元素可以用坐标系B的单位矢量在坐标系的单位矢量在坐标系A单位矢单位矢 量上的投影来表示量上的投影来表示 T A B T A B T A B B T AB T AB T A

3、 B T AB T AB T A B T AB T AB T A B A B A B AA BR Z Y X ZZYZXZ ZYYYXY ZXYXXX ZYX （2-3） R A B R B A T B A T B A T B A A T BA T BA T B A T BA T BA T B A T BA T BA T B A B A B A BB AR Z Y X ZZYZXZ ZYYYXY ZXYXXX ZYX （2-5） 对比（对比（2-3）和（）和（2-5）可知两个旋转矩阵互为转置，再根据正交矩阵）可知两个旋转矩阵互为转置，再根据正交矩阵 的性质可得以下关系的性质可得以下关系 1 R

4、RR A B TA B B A （2-6） 旋转矩阵旋转矩阵 描述坐标系描述坐标系B的姿态，矢量的姿态，矢量 描述坐标系描述坐标系B的原点位置。的原点位置。 3 Bo AA B RBp n位姿描述位姿描述 固连坐标系把刚体位姿描述问题转化为坐标系的描述问题。图固连坐标系把刚体位姿描述问题转化为坐标系的描述问题。图2-3中中 坐标系坐标系B可以在固定坐标系可以在固定坐标系A中描述为中描述为 （2-7） R A BBo A P n平移坐标变换平移坐标变换 图图2-4平移变换平移变换 BP为坐标系 为坐标系B描述的某一空间位置，描述的某一空间位置， 我们也可以用我们也可以用AP（坐标系（坐标系A）描

5、述同）描述同 一空间位置。因为两个坐标系具有相同一空间位置。因为两个坐标系具有相同 的姿态，同一个点在不同坐标系下的描的姿态，同一个点在不同坐标系下的描 述满足以下关系述满足以下关系 ABA Bo PPP （2-8） 旋转坐标变换的任务是已知坐标系旋转坐标变换的任务是已知坐标系B描述的描述的 一个点的位置矢量一个点的位置矢量BP和和旋转矩阵旋转矩阵 ，求在坐，求在坐 标系标系A下描述同一个点的位置矢量下描述同一个点的位置矢量AP。 4 n旋转坐标变换旋转坐标变换 R A B ABT B xA ABT B yA ABT B zA p p p XP YP ZP （2-9） 将（将（2-9）式写成矩

6、阵形式得（参见（）式写成矩阵形式得（参见（2-3）式）式） PP Z Y X P BA B B T A B T A B T A B A R （2-10） 图2-5旋转变换 式（式（2-10）即为我们要求的旋转变换关系，该变换是通过两个）即为我们要求的旋转变换关系，该变换是通过两个 坐标系之间的旋转变换实现的。坐标系之间的旋转变换实现的。 例例2-1 图图2.6 给出了两个平面坐标系的位置给出了两个平面坐标系的位置 关系关系 ，计算旋转变换矩阵，计算旋转变换矩阵 和同一矢量和同一矢量P 在两个坐标系下表示之间的关系，假设矢在两个坐标系下表示之间的关系，假设矢 量长度为量长度为 r。 5 图图2-

7、6平面旋转变换平面旋转变换 R A B cossin sincos A BAA A BAA XXY YXY 根据（根据（2-3）式可知旋转变换矩阵为，）式可知旋转变换矩阵为， cossin sincos A BR cossincoscoscossinsincos() sincossincossinsincossin() AAB B r Rrr r PP 根据几何关系直接计算根据几何关系直接计算P在在A下的表示显然与上式相同，印证了坐标下的表示显然与上式相同，印证了坐标 变换方法的正确性。变换方法的正确性。 6 n复合变换复合变换 图图2-7复合变换复合变换 如果两个坐标系之间即存在平移又存如果

8、两个坐标系之间即存在平移又存 在旋转关系，如何计算同一个空间点在旋转关系，如何计算同一个空间点 在两个坐标系下描述的变换关系？在两个坐标系下描述的变换关系？ 为了得到位置矢量为了得到位置矢量BP和和AP之间的之间的 变换关系，我们建立一个中间坐变换关系，我们建立一个中间坐 标系标系C。 PPP BA B BC B C RR ACAABA CoBBo RPPPPP （2-11） （ 2-12） 为了得到位置矢量为了得到位置矢量BP和和AP之间的变换关系，只需坐标系之间的变换关系，只需坐标系B 在坐标系在坐标系 下下A的描述。的描述。 是是4 4矩阵，称为齐次坐标变换矩阵。可以理解为坐标系矩阵，称

9、为齐次坐标变换矩阵。可以理解为坐标系B 在固定坐标系在固定坐标系A中的描述。中的描述。 7 2-3齐次坐标变换齐次坐标变换 坐标变换（坐标变换（2-12）可以写成以下形式）可以写成以下形式 1101 PPP B Bo AA B A R （2-13） 将位置矢量用将位置矢量用4 1矢量表示，增加矢量表示，增加1维的数值恒为维的数值恒为1，我们仍然用原，我们仍然用原 来的符号表示来的符号表示4维位置矢量并采用以下符号表示坐标变换矩阵维位置矢量并采用以下符号表示坐标变换矩阵 10 Bo AA BA B R T P （2-14） PP BA B A T（2-15） T A B 齐次坐标变换的主要作用是

10、表达简洁，同时在表示多个坐标变齐次坐标变换的主要作用是表达简洁，同时在表示多个坐标变 换的时候比较方便。换的时候比较方便。 8 ba zzyyxx bababa n矢量的点积与叉积矢量的点积与叉积 规定两矢量的点积为一标量规定两矢量的点积为一标量 而两矢量的交积为另一个与此两相乘矢量所决定的平面垂直的矢量而两矢量的交积为另一个与此两相乘矢量所决定的平面垂直的矢量 bakji)()()( xyyxzxxzyzzy babababababa ba zyx zyx bbb aaa kji 两矢量的交积记忆方法两矢量的交积记忆方法 9 2.4齐次变换算子齐次变换算子 在机器人学中还经常用到下面的变换，

11、如图在机器人学中还经常用到下面的变换，如图2-8，矢量，矢量AP1沿矢量沿矢量 AQ平移至的 平移至的AQ终点，得一矢量终点，得一矢量AP2。已知。已知AP1和和AQ求求AP2的过程称之的过程称之 为平移变换，与前面不同，这里只涉及单一坐标系。为平移变换，与前面不同，这里只涉及单一坐标系。 图图2-8平移算子平移算子 QPP AAA 12 （2-16） 可以采用齐次变换矩阵表示平移变换可以采用齐次变换矩阵表示平移变换 12 )(PQP AAA Trans （2-17） )( Q A Trans称为平移算子，其表达式为称为平移算子，其表达式为 1 )( 0 Q Q A A I Trans （2-

12、18） 其中其中I是是3 3单位矩阵。例如若单位矩阵。例如若AQ=ai+bj+ck， 其中其中i、j和和k分别表示坐标系分别表示坐标系A三个坐标轴的三个坐标轴的 单位矢量，则平移算子表示为单位矢量，则平移算子表示为 1000 100 010 001 ),( c b a cbaTrans 10 同样，我们可以研究矢量在同一坐标系下的旋转同样，我们可以研究矢量在同一坐标系下的旋转 变换，如图变换，如图2-9，AP1绕绕Z轴转轴转 角得到角得到AP2。则。则 图图2-9旋转算子旋转算子 12 ),(PP AA zRot （2-20） Rot(z, )称为旋转算子，其表达式为称为旋转算子，其表达式为

13、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 11 定义了平移算子和旋转算子以后，可以将它们复合实现复杂的映射定义了平移算子和旋转算子以后，可以将它们复合实现复杂的映射 关系。变换算子与前面介绍的坐标变换矩阵形式完全相同，因为所有描关系。变换算子与前面介绍的坐标变换矩阵形式完全相同，因为所有描 述均在同一坐标系下，所以不需上下标描述（坐标系）。述均在同一坐标系下，所以不需上

14、下标描述（坐标系）。 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 2. 它是不同坐标系间的坐标变换。如它是不同坐标系间的坐标变换。如 3. 它是同一坐标系内的变换算子。它是同一坐标系内的变换算子。 齐次坐标变换是复杂空间变换的基础，必须认真理解和掌握。具体齐次坐标变换是复杂空间变换的基础，必须认真理解和掌握。具体 应用的关键是理解它代表的是上面三种含义的哪一种，而不是简单的套应用的关键是理解它代表的是上面三种含

15、义的哪一种，而不是简单的套 用公式！用公式！ 1. 它是坐标系的描述。它是坐标系的描述。 三个坐标轴方向的单位矢量，而三个坐标轴方向的单位矢量，而表示坐标系表示坐标系B原点位置。原点位置。 如图如图2-10表示的三个坐标系，已知坐标系表示的三个坐标系，已知坐标系 A、B和和C之间的变换矩阵之间的变换矩阵 和位和位 置矢量置矢量CP，求在坐标系，求在坐标系A下表示同一个点下表示同一个点 的位置矢量的位置矢量AP。 12 2.5复合变换复合变换 复合变换主要有两种应用形式，一种是建立了多个坐标系描述机器人复合变换主要有两种应用形式，一种是建立了多个坐标系描述机器人 的位姿，任务是确定不同坐标系下对

16、同一个量描述之间的关系；另一种是的位姿，任务是确定不同坐标系下对同一个量描述之间的关系；另一种是 一个空间点在同一个坐标系内顺序经过多次平移或旋转变换，任务是确定一个空间点在同一个坐标系内顺序经过多次平移或旋转变换，任务是确定 多次变换后点的位置。多次变换后点的位置。 图图2-10 复合坐标变换复合坐标变换 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） 13 (a) ZY顺序旋转顺序旋转 (b) Y Z顺序旋转顺序旋转 图图2-1

17、1旋转顺序对旋转顺序对 变换结果影响变换结果影响 例例2-3已知点已知点u=7i+3j+2k，先对它进行绕，先对它进行绕Z轴旋转轴旋转90o 的变换得点的变换得点v，再对点，再对点v进行绕进行绕Y轴旋转轴旋转90o的变换得的变换得 点点w，求，求v和和w。 1 2 7 3 1 2 3 7 1000 0100 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 Ro

18、t yRot yRot zwvu 001072 100037 010023 000111 计算结果与前面的相同，称计算结果与前面的相同，称R= Rot(y,90o) Rot(z,90o) 为复合旋转算子。为复合旋转算子。 和和 ，求求 和和给定给定 计算计算 14 绕固定坐标系变换，矩阵乘的顺序绕固定坐标系变换，矩阵乘的顺序“自右向自右向 左左” 如果改变旋转顺序，先对它进行绕如果改变旋转顺序，先对它进行绕y轴旋转轴旋转90o，再绕，再绕z轴旋转轴旋转90o，结，结 果如图果如图2-11b所示。比较图所示。比较图2-11a和图和图2-11b可以发现最后的结果并不相同，可以发现最后的结果并不相同

19、， 即旋转顺序影响变换结果。即旋转顺序影响变换结果。 从数学角度解释就是矩阵乘法不满足交换率，从数学角度解释就是矩阵乘法不满足交换率， Rot(y,90o) Rot(z,90o) Rot(z,90o) Rot(y,90o)。 2.6齐次变换的逆变换齐次变换的逆变换 T A B T B A 已知坐标系已知坐标系B相对坐标系相对坐标系A的描述的描述 求求坐标系坐标系A相对坐标系相对坐标系B的描述的描述 一种直接的方法是矩阵求逆，另一种方法是根据变换矩阵的特点直一种直接的方法是矩阵求逆，另一种方法是根据变换矩阵的特点直 接得出逆变换。接得出逆变换。后一种方法更简单方便。后一种方法更简单方便。 即齐次

20、变换的求逆问题。即齐次变换的求逆问题。 T A B T B A 等价为：已知等价为：已知 R A B Bo A P R B A Ao BP 是坐标系是坐标系B的原点在的原点在坐标系坐标系B中的描述，显然为零矢量。中的描述，显然为零矢量。 由由（2-28）式得）式得 15 根据前面的讨论，旋转矩阵关系为根据前面的讨论，旋转矩阵关系为 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） 逆变换可以直接用正变换的

21、旋转矩阵和平移矩阵表示逆变换可以直接用正变换的旋转矩阵和平移矩阵表示 10 Bo ATA B TA BB A RR T P （2-30） 例例2-4如图如图2-12给出的楔形块角点坐标系，求齐次坐标变换给出的楔形块角点坐标系，求齐次坐标变换, ABA BCC TTT, 图图2-12楔形块角点坐标系楔形块角点坐标系 16 A沿沿xA平移平移3个单位，再绕新的个单位，再绕新的zA 轴轴 转转180o得得 B 1801800100 1801800010 001001 A B cs Rsc 因此因此 1003 0100 0010 0001 A BT B沿沿zB平移平移2个单位，然后绕个单位，然后绕yB

22、轴转轴转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 3311 2222 3311 2222 10030003 01000000 001010021002 000100010001 AAB CBC TT T 因此因此 A沿沿xA和和zA平移平移3和和2，然后绕，然后绕yA轴转轴转90，再绕新，再绕新xA轴转轴转 - -30得得C 也可以按以下方法计算也可以按以下方法计算 31 22 3311 22

23、22 31 22 90090100 01003030 9009003030 0011000 01000 1000100 A C cs Rcs scsc 17 事实上，对于像本例题这种简单的情况，可以直接利用齐次坐标变换事实上，对于像本例题这种简单的情况，可以直接利用齐次坐标变换 的定义得到变换矩阵。即直接写出坐标系的定义得到变换矩阵。即直接写出坐标系C坐标轴矢量在坐标系坐标轴矢量在坐标系A下表下表 示得旋转矩阵，平移矢量为坐标系示得旋转矩阵，平移矢量为坐标系C的原点在坐标系的原点在坐标系A下的矢量表示。下的矢量表示。 18 2.7变换方程变换方程 图图2-13表示了多个坐标系的关系图，可以用两

24、种不表示了多个坐标系的关系图，可以用两种不 同的方式得到世界坐标系同的方式得到世界坐标系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）可以得到变换方程）可以得到变换方程 图图2-13坐标变换序列坐标变换序列 可以利用变换方程（可以利用变换方程（2-33）求解其中任意一个未知变换。例如，假设除）求解其中任意一个未知变换。例如，假设除 T U B 以外其余变换均为已知，则该未知变换可以用下式计算以外其余变换均为已知，则该未知变换可以用下式计算 11UUACB BADD

25、C TT T TT 在坐标系的图形表示方法中，从一个坐标系原点指向在坐标系的图形表示方法中，从一个坐标系原点指向 另一个坐标系原点的箭头表示坐标系的描述关系。另一个坐标系原点的箭头表示坐标系的描述关系。 TTT B C U B U C （2-35） 1UUDD CAAC TT TT （2-36） 19 例例2-5假设已知图机械臂末端工具坐标系假设已知图机械臂末端工具坐标系T相对基座相对基座 坐标系坐标系B的描述，还已知工作台坐标系的描述，还已知工作台坐标系S相对基座相对基座 坐标系坐标系B的描述，并且已知螺栓坐标系的描述，并且已知螺栓坐标系G相对工作相对工作 台坐标系台坐标系S的描述。计算螺栓

26、相对机械臂工具坐标的描述。计算螺栓相对机械臂工具坐标 系的位姿。系的位姿。 解：添加从工具坐标系解：添加从工具坐标系T原点到螺栓坐标系原点到螺栓坐标系G原点原点 的箭头，可以得到如下变换方程的箭头，可以得到如下变换方程 TTTT T G B T S G B S （2-37） 螺栓相对机械臂工具坐标系的位姿描述为螺栓相对机械臂工具坐标系的位姿描述为 TTTT S G B S B T T G 1 （2-38） 2.8姿态的欧拉角表示姿态的欧拉角表示 前面用前面用3 3的旋转矩阵描述了三维刚体的姿态，但的旋转矩阵描述了三维刚体的姿态，但9个分量中只有个分量中只有3 个独立的分量。能否使用三个独立的分

27、量描述三维刚体的姿态哪？个独立的分量。能否使用三个独立的分量描述三维刚体的姿态哪？ 答案是肯定的，比较常用的是下面的答案是肯定的，比较常用的是下面的Z-Y-Z欧拉角描述方法。欧拉角描述方法。 20 欧拉角用一个绕欧拉角用一个绕 Z轴旋转轴旋转f f 角，再角，再 绕新的绕新的 Y 轴旋转轴旋转 角，最后绕新角，最后绕新Z 的轴的轴 旋转旋转 角来描述任何可角来描述任何可能的姿态图中虚能的姿态图中虚 线表示旋转形成的新坐标轴。线表示旋转形成的新坐标轴。 图图2-16 Z-Y-Z欧拉角欧拉角 根据旋转关系可以得到变换矩阵根据旋转关系可以得到变换矩阵 , zyz RRot zRot yRot zf

28、（2-39） 具体计算结果计算如下：具体计算结果计算如下： 00 ,01000 0001 yz cscsc cc ss RRot yRot zscsc scs cs sc 0 ,00 001 zyzyz csc cc ssc c cs sc c ss cc s RRot zRscscs c cc ss c sc cs s s cs scs cs sc ff f f f f f ffff f f f f 21 另一种常用的旋转组合是横滚（另一种常用的旋转组合是横滚（roll）、俯仰）、俯仰 （pitch）和偏转（）和偏转（yaw）。这三个角度表示了）。这三个角度表示了 船航行的三个方位描述。需要

29、注意的是，横滚船航行的三个方位描述。需要注意的是，横滚 、俯仰和偏转表示都是相对固定坐标系表述的、俯仰和偏转表示都是相对固定坐标系表述的 ，而前面介绍的，而前面介绍的Z-Y-Z欧拉角描述是相对动坐欧拉角描述是相对动坐 标系描述的。标系描述的。 图图2-17 横滚、仰俯和偏转表示姿态横滚、仰俯和偏转表示姿态 规定变换顺序为，偏转规定变换顺序为，偏转 角、俯仰角、俯仰 角和横滚角和横滚f f 角。根据旋转关系角。根据旋转关系 , xyz RRot zRot yRot xf（2-41） 00100 00100 00100 xyz cscsc cc s ss cc s cs s Rsccss cs s

30、 sc cs s cc s scscsc sc c fff f f f f fff f f f f 对比旋转矩阵的横滚对比旋转矩阵的横滚、俯仰和偏转表示、俯仰和偏转表示与与Z-Y-Z Euler角表示，可以发现两者角表示，可以发现两者 顺序恰好相反！相对顺序恰好相反！相对固定固定坐标系的变换，乘的顺序是坐标系的变换，乘的顺序是自右向左自右向左；相对；相对动动坐标坐标 系的变换，乘的顺序是系的变换，乘的顺序是自左向右自左向右。一般根据变换的定义可以自然得到乘法顺。一般根据变换的定义可以自然得到乘法顺 序，不必死记硬背。序，不必死记硬背。 22 xyz ffffijk n绕任意轴旋转变换绕任意轴旋

31、转变换 下面讨论绕任意轴下面讨论绕任意轴 f 旋转矩阵，轴在坐标系旋转矩阵，轴在坐标系A下表示为下表示为 以以 f 为为 Z 轴建立与轴建立与A固连的坐标系固连的坐标系C用用n、o和和f表示坐标系表示坐标系C三个坐三个坐 标轴的单位矢量，在坐标系标轴的单位矢量，在坐标系A下表示为下表示为 图图2-18绕任意轴旋转变换绕任意轴旋转变换 xyz xyz xyz nnn ooo fff nijk oijk fijk xxx A Cyyy zzz nof Rnof nof 因为固连的坐标系因为固连的坐标系C与与A固连，所以绕固连，所以绕 f旋旋 转等价于绕转等价于绕ZC旋转。为此我们先将旋转。为此我们

32、先将Ap在坐标系在坐标系 C下表示，再绕下表示，再绕ZC旋转旋转 角，最后再把旋转得角，最后再把旋转得 到的矢量用坐标系到的矢量用坐标系A表示。表示。 CAT ACA AC RRppp 1 ( , )( , ) AT ACC C Rot zRot zRppp Ap1 = Rot(f, ) Ap 23 再将再将Cp1在坐标系在坐标系A下表示下表示 11 ( , ) AAAT AAC CCC RR Rot zRppp Ap1 = Rot(f, ) Ap 0 ( , )( , )0 001 xyz xxx AAT CCyyyxyz zzzxyz xxyyzz xxx yyyxxyyzz zzzxyz

33、 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 cf f nn n cn o sn o so o cf f 其中一个矢量其中一个矢量 上式中的上式中的n和和o各分量是未知的，需要用各分量是未知的，需要用 f 的各分量表示的各分量表示 24

34、 根据坐标系的右手规则知根据坐标系的右手规则知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