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

1 第二章 机器人的空间描述和坐标变换 2.1 位姿和坐标系描述 2.2平移和旋转坐标系映射 2.3平移和旋转齐次坐标变换 2.4物体的变换和变换方程 2.5通用旋转变换 2 z y x A p p p p 2 -1图 位 置 表 示 2.1位置方位表示与坐标系描述 1.位置描述 矢量 Ap 表示箭头指向点的位置矢量，其 中右上角标 “A”表示该点是用 A坐标系描述 的。 （ 2-2） 2.方位描述 坐标系 B与机械手末端工具固连，工具的姿态 可以由坐标系 B的方向来描述。而坐标系 B的方 向可以用沿三个坐标轴的单位矢量来表示 333231 232221 131211 rrr rrr rrr R BABABAAB ZYX X A Z A Y A A P O A 图 2-2方位表 示 （ 2-1） 旋转矩阵 描述坐标系 B的姿态，矢量 描述坐标系 B的原点位 置。 3 BoAAB RB p 3.位姿描述 固连坐标系把刚体位姿描述问题转化为坐标系的描述问题。图 2-3 中坐标系 B可以在固定坐标系 A中描述为 （ 2-3） RAB BoAP 4 1.平移坐标变换 图 2-3平移变换 A B O B O A A P B O A P B P BP为坐标系 B描述的某一空间位 置，我们也可以用 AP（坐标系 A）描 述同一空间位置。因为两个坐标系具有 相同的姿态，同一个点在不同坐标系下 的描述满足以下关系 A B A BoP P P （ 2-4） 2.2平移和旋转坐标系映 射 旋转坐标变换的任务是已知坐标系 B描述的 一个点的位置矢量 BP和 旋转矩阵 ，求在坐标 系 A下描述同一个点的位置矢量 AP。 5 2.旋转坐标变 换 X A Z A Y A A P ( B P ) X B Y B Z B RAB A B T B xA A B T B yA A B T B zA p p p XP YP ZP （ 2-5） 将（ 2-5）式写成矩阵形式得： PP Z Y X P BABB T A B T A B T A B A R （ 2-6） 图 2-4旋转变换 式（ 2-6）即为我们要求的旋转变换关系，该变换是通过两个坐 标系之间的旋转变换实现的。 6 3.复合变换 A C O B O A A P B O A P B P B 图 2-5复合变换 如果两个坐标系之间即存在平移 又存在旋转，如何计算同一个空间点 在两个坐标系下描述的变换关系？ 为了得到位置矢量 BP和 AP之 间的变换关系，我们建立一个中 间坐标系 C。 PPP BABBCBC RR A C A A B ACo B B oR P P P P P （ 2-7） （ 2-8） 为了得到位置矢量 BP和 AP之间的变换关系，只需坐标系 B 在坐标系 下 A的描述。 是 44矩阵，称为齐次坐标变换矩阵。可以理解为坐标系 B在固定坐 标系 A中的描述。 7 2.3齐次坐标变换 坐标变换（ 2-8）可以写成以下形式 1101 PPP B Bo AA B A R （ 2-9） 将位置矢量用 41矢量表示，增加 1维的数值恒为 1，我们仍然用原 来的符号表示 4维位置矢量并采用以下符号表示坐标变换矩阵 10 Bo AA BA B RT P （ 2-10 ） PP BABA T （ 2-11） TAB 齐次坐标变换的主要作用是表达简洁，同时在表示多个坐标变换 的时候比较方便。 1.齐次变换 8 2.齐次变换算子 在机器人学中还经常用到下面的变换，如图 2-8，矢量 AP1沿矢量 AQ平移至的 AQ终点，得一矢量 AP2。已知 AP1和 AQ求 AP2的过程称之为 平移变换，与前面不同，这里只涉及单一坐标系。 A O A Q A P 2 A P 1 A P 1 图 2-6平移算子 QPP AAA 12 （ 2-12） 可以采用齐次变换矩阵表示平移变换 12 )( PQP AAA T r a n s （ 2-13） )( QATrans 称为平移算子，其表达式为 1)( 0 QQ A A IT r a n s （ 2-14） 其中 I是 33单位矩阵。例如若 AQ=ai+bj+ck， 其中 i、 j和 k分别表示坐标系 A三个坐标轴的 单位矢量，则平移算子表示为 1000 100 010 001 ),( c b a cbaT r an s 9 X A Y A A P 2 Z A A P 1 q 同样，我们可以研究矢量在同一坐标系下的旋转 变换，如图 2-9， AP1绕 Z轴转 q角得到 AP2。则 图 2-7旋转算子 12 ),( PP AA zR o t q （ 2-20） Rot(z,q)称为旋转算子，其表达式为 1000 0100 00 00 ),( qq qq q cs sc zR o t （ 2-21） 同理，可以得到绕 X轴和 Y轴的旋转算子 1000 00 00 0001 ),( qq qq q cs sc xR ot 1000 00 0010 00 ),( qq qq q cs sc yR ot 10 定义了平移算子和旋转算子以后，可以将它们复合实现复杂的映射 关系。变换算子与前面介绍的坐标变换矩阵形式完全相同，因为所有描 述均在同一坐标系下，所以不需上下标描述（坐标系）。 21AAP T P （ 2-23） TAB ABR BoAP PP BABA T 21AAP T P 齐次坐标变换总结： 表示坐标系 B在坐标系 A下的描述， 的各列是坐标系 B三个坐标轴方向的单位矢量， 而表示坐标系 B原点位置 。 2. 它是不同坐标系间的坐标变换。如 3.它是同一坐标系内的变换算子。 齐次坐标变换是复杂空间变换的基础，必须认真理解和掌握。具体应 用的关键是理解它代表的是上面三种含义的哪一种，而不是简单的套用 公式！ 1. 它是坐标系的描述。 如图 2-10表示的三个坐标系，已知坐标系 A、 B和 C之间的变换矩阵 和 位置 矢量 CP，求在坐标系 A下表示同一个点的 位置矢量 AP。 11 3.复合变换 复合变换主要有两种应用形式，一种是建立了多个坐标系描述机器人 的位姿，任务是确定不同坐标系下对同一个量描述之间的关系；另一种是 一个空间点在同一个坐标系内顺序经过多次平移或旋转变换，任务是确定 多次变换后点的位置。 A C O B O A A P C P B O C 图 2-10 复合坐标变换 TAB TBC PP CBCB T PPP CBCABBABA TTT （ 2-24） （ 2-25） TTT BCABAC 根据坐标变换的定义得 （ 2-26） 12 X Y Z u v w (a) ZY顺序旋转 X Y Z u v w (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 0100 0001 0010 )90,( uv ozR o t 1 3 7 2 1 2 7 3 1000 0001 0010 0100 )90,( vw oyR o t 如果只关心最后的变换结果，可以按下式计算 ( , 9 0 ) ( , 9 0 ) ( , 9 0 )o o oR o t y R o t y R o t zw v u 0 0 1 0 7 2 1 0 0 0 3 7 0 1 0 0 2 3 0 0 0 1 1 1 计算结果与前面的相同，称 R= Rot(y,90o) Rot(z,90o) 为复合旋转算子。 13 注：固定坐标系变换，矩阵乘的顺序“自右向左” 如果改变旋转顺序，先对它进行绕 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物体的变换和变换方程 TAB TBA 已知坐标系 B相对坐标系 A的描述 求 坐标系 A相对坐标系 B的描述 一种直接的方法是矩阵求逆，另一种方法是根据变换矩阵的特点直 接得出逆变换。后一种方法更简单方便。 即齐次变换的求逆问题。 TAB TBA 等价为：已知 RAB BoAP RBA AoBP 是坐标系 B的原点在 坐标系 B中的描述，显然为零矢量。 由 （ 2-28）式得 15 根据前面的讨论，旋转矩阵关系为 TABABBA RRR 1 （ 2-27） 将坐标变换用于坐标系 B的原点得 AoBBoABABoB R PPP （ 2-28） BoBP B B A A T AA o A B o B B oRR P P P（ 2-29） 逆变换可以直接用正变换的旋转矩阵和平移矩阵表示 10 Bo ATA B TA BB A RRT P （ 2-30） 16 A沿 xA平移 3个单位，再绕新的 zA 轴转 180o得 B 1 8 0 1 8 0 0 1 0 0 1 8 0 1 8 0 0 0 1 0 0 0 1 0 0 1 A B cs R s c 因此 1 0 0 3 0 1 0 0 0 0 1 0 0 0 0 1 A BT B沿 z B平移 2个单位，然后绕 yB轴转 90o再绕新 xB轴转 150o得 C 31 22 33 11 2 2 2 2 31 22 90 0 90 1 0 0 0 0 1 1 0 0 0 0 1 0 0 15 0 15 0 0 1 0 0 0 90 0 90 0 15 0 15 0 1 0 0 0 1 0 0 B C cs R c s s c s c 图 2-12楔形块角点坐标 系 例 2-4， 如图 2-12给出的楔形块角点坐标系，求齐次坐标变换 ,A B A B C CT T T, 3311 2 2 2 2 33 11 2 2 2 2 1 0 0 3 0 0 0 3 0 1 0 0 0 0 0 0 0 0 1 0 1 0 0 2 1 0 0 2 0 0 0 1 0 0 0 1 0 0 0 1 A A B C B CT T T 因此 A沿 xA和 zA平移 3和 2，然后绕 yA轴转 90 ，再绕新 xA轴转 -30 得 C 也可以按以下方法计算 31 22 33 11 2 2 2 2 31 22 9 0 0 9 0 1 0 0 0 1 0 0 3 0 3 0 9 0 0 9 0 0 3 0 3 0 0 0 1 1 0 0 0 0 1 0 0 0 1 0 0 0 1 0 0 A C cs R c s s c s c 17 事实上，对于像本例题这种简单的情况，可以直接利用齐次坐标变换 的定义得到变换矩阵。即直接写出坐标系 C坐标轴矢量在坐标系 A下表 示得旋转矩阵，平移矢量为坐标系 C的原点在坐标系 A下的矢量表示。 18 变换方程 图 2-13表示了多个坐标系的关系图，可以用两种不 同的方式得到世界坐标系 U下坐标系 D的描述。 B U A D C TTT ADUAUD TTTT CDBCUBUD （ 2-31） （ 2-32） 由（ 2-31）和（ 2-32）可以得到变换方程 图 2-13坐标变换序列 可以利用变换方程 （ 2-33） 求解其中任意一个未知变换。例如，假设 除 以外其余变换均为已知，则该未知变换可以用下式计算 TU B 11U U A C BB A D D CT T T T T 在坐标系的图形表示方法中，从一个坐标系原点指向另一个坐标系原点 的箭头表示坐标系的描述关系。 TTT BCUBUC （ 2-35） 1U U D DC A A CT T T T （ 2-36） 19 例 2-5假设已知图机械臂末端工具坐标系 T相对基座 坐标系 B的描述，还已知工作台坐标系 S相对基座 坐标系 B的描述，并且已知螺栓坐标系 G相对工作 台坐标系 S的描述。计算螺栓相对机械臂工具坐标 系的位姿。 解：添加从工具坐标系 T原点到螺栓坐标系 G原点 的箭头，可以得到如下变换方程 TTTT TGBTSGBS （ 2-37） 螺栓相对机械臂工具坐标系的位姿描述为 TTTT SGBSBTTG 1 （ 2-38） 20 x y zf f f f i j k 1.绕任意轴旋转变换 下面讨论绕任意轴 f 旋转矩阵，轴在坐标系 A下表示为 以 f 为 Z 轴建立与 A固连的坐标系 C用 n、 o和 f表示坐标系 C三个坐标 轴的单位矢量，在坐标系 A下表示为 Z A Z C X A Y A Y C X C A p f q 图 2-18绕任意轴旋转变换 x y z x y z x y z n n n o o o f f f n i j k o i j k f i j k x x x A C y y y z z z n o f R n o f n o f 因为固连的坐标系 C与 A固连，所以绕 f旋转 等价于绕 ZC旋转。为此我们先将 Ap在坐标系 C下 表示，再绕 ZC旋转 q 角，最后再把旋转得到的矢量 用坐标系 A表示。 C A T ACAACRRp p p 1 ( , ) ( , ) AT ACC CR o t z R o t z Rqqp p p Ap1 = Rot(f,q) Ap 2.5通用旋转变换 21 再将 Cp1在坐标系 A下表示 11 ( , )A A A T AAC C C CR R R o t z Rqp p p 0 ( , ) ( , ) 0 0 0 1 x y zx x x A A T C C y y y x y z z z z x y z x x y y z zx x x y y y x x y y z z z z z x y z n n nn o f cs Ro t R Ro t z R n o f s c o o o n o f f f f n c o s n c o s n c o sn o f n n o f n s o c n s o c n s o c n o f f f f qq q q q q q q q q q q q q q q q q f 1 1 1 2 2 2 3 3 3 oa n o a n o a 因此 1 2 3 x x x x x x x x x x x y y x x y x y x y x z z x x z x z x z n n n c n o s n o s o o c f f n n n c n o s n o s o o c f f n n n c n o s n o s o o c f f q q q q q q q q q q q q 其中一个矢量 上式中的 n和 o各分量是未知的，需要用 f 的各分量表示 22 根据坐标系的右手规则知 no = f，叉积可以按下式计算 ( ) ( ) ( )x y z y z z y z x x z x y y x x y z n n n n o n o n o n o n o n o o o o i j k n o i j k ( ) , ( ) , ( )y z z y x x y x z y x y y x zn o n o f n o n o f n o n o f 再根据旋转矩阵的正交性可以得 1 , 0 x x x x x x x y x y x yn n o o f f n n o o f f ( , ) , 1 x x x y z x z y x y z y y y z x x z y y z x z z f f v c f f v f s f f v f s R o t f f v f s f f v c f f v f s v c f f v f s f f v f s f f v c q q q q q q q q q q q q q q q q q q q q q f 1 2 3 x x x x x x x x x x x y y x x y x y x y x z z x x z x z x z n n n c n o s n o s o o c f f n n n c n o s n o s o o c f f n n n c n o s n o s o o c f f q q q q q q q q q q q q x x x A C y y y z z z n o f R n o f n o f 将上式对角线相加得 r11+ r22+ r33=1+2cq cq=( r11+ r22+ r33 -1)/2 23 2.等效转轴与转角 前面讨论了给定转轴和转角可以得到旋转矩阵，那么是否任意给定的旋转 矩阵都可以确定等效的转轴 f和转角 q哪？也就是两个坐标原点重合的坐标系可以 通过绕固定轴转一定的角度来实现从一个坐标系转换到另一个坐标系。 1 1 1 2 1 3 2 1 2 2 2 3 3 1 3 2 3 3 x x x y z x z y A C x y z y y y z x x z y y z x z z f f v c f f v f s f f v f sr r r R r r r f f v f s f f v c f f v f s r r r f f v f s f f v f s f f v c q q q q q q q q q q q q q q q q q q 将关于对角线对称的两个元素分别相减得 r32-r23=2fxsq, r13-r31=2fysq, r21-r12=2fzsq 将上式平方求和得 ： 4s2q=( r32-r23)2+( r13-r31)2+(r21-r12)2 假设限定绕矢量 f 正向旋转，且 0q180o，则 2 2 23 2 2 3 1 2 3 1 2 1 1 2( ) ( ) ( ) / 2s r r r r r rq 可得 q的值 q=atan(sq/cq) 24 3 2 2 3 1 3 3 1 2 1 1 2 ( ) / 2 ( ) / 2 ( ) / 2 x y z f r r s f r r s f r r s q q q 可得矢量 f 分量的值 在应用中需要注意的是，当转角 q的值接近 0o或 180o时，方向矢量 f 各分量的值计算出现问题，属于奇异情况。