第二章 数学基础

A
A ቤተ መጻሕፍቲ ባይዱ p B R p A p B 0
2.3 齐次坐标变换
2.3.1 齐次变换
(2-13)式的一般映射矩阵可以写为:
A px r11 A B A A P A py B R P PBO r21 A pz r31 r12 r22 r32 r13 B p x A pBOx B A r23 p p y BOy B A r33 p z pBOz r12 r22 r32 0 r13 r23 r33 0 pBOx B p x A p x A pBOy B p y A p y A A B pz pBOz pz 1 1 1
这些旋转变换可以通过右图推导
A A A
p x p x cos p y sin
B B
p y B p x sin B p y cos pz B pz
A p x cos sin 0 B px A B p y sin cos 0 p y B A pz 0 0 1 p z
或 V=[-12 -16 -20 -4]T
几个特定意义的齐次坐标: [0, 0, 0, ω]T—坐标原点矢量的齐次坐标,ω为任 意非零比例系数; [1 0 0 0]T—指向无穷远处的OX轴; [0 1 0 0]T—指向无穷远处的OY轴; [0 0 1 0]T—指向无穷远处的OZ轴.
为什么要引进齐次坐标,它有什么优点?
A B
R

A
xB
A
yB
A
zB

r11 r12 r13 r r r 21 22 23 r31 r32 r33
A B
R 表示刚体B相对于坐标系{A}的方位
A A
xB A xB A y B A y B A z B A z B 1 xB A y B A y B A z B A z B A xB 0
A

A
y
A
z 1 , P x
B
A A BR BT 0 A

T B

B
y
z 1

T
而齐次变换公式和变换矩阵变为:
A
P T P,
A B B
PB 0 (2-15,16) 1
齐次坐标
所谓齐次坐标就是将一个原本是n维的向量用一个n+1维 向量来表示.有一个特定的投影附加于n维空间,也可以把 它看作一个附加于每个矢量的比例系数.
例:动坐标系{B}中有一点 P i 2 j 3k ,此坐标系绕固定坐标系
绕x轴旋转90°.求旋转后该点相对于固定坐标系的坐标 A P .
1 0 A B B P A T P Rot x , 90 P B 0 0
0 0 0 1 1 0 0 0
0 1 0 2 0 3 1 1
这是绕Z轴的旋转. 其它两轴只要把坐标次序调换可得 上页结果.
旋转矩阵的几何意义: A (1) B R 可以表示固定于刚体上的坐标系{B}对参考坐标 系的姿态矩阵. A (2) B R 可作为坐标变换矩阵.它使得坐标系{B}中的点的 A R 可作为算子, 坐标 B p 变换成{A}中点的坐标 A p . B 将{B}中的矢量或物体变换到{A}中.
A
B A
P R P
A B B
R R R
A B A B
1
T
B A
R 和
A B
R 都是正交矩阵,两者互逆.
图2.4 旋转变换
复合坐标变换
一般情况原点既不重和,方位也不同.这时有:
A A PB RB P A PB0
图2.5 复合变换
例2.1 已知坐标系{B}的初始位姿与{A}重合,首 先{B}相对于{A}的ZA轴转30°,再沿{A}的XA轴 移动12单位,并沿{A}的YA轴移动6单位.求位置 矢量APB0和旋转矩阵BAR.设点p在{B}坐标系中 的位置为BP=[3,7,0],求它在坐标系{A}中的位置.
n o
i
a
2.1 位置和姿态的表示
2.1.1 位置描述
在直角坐标系A中,空间任意一点p的位置 (Position)可用3x1列向量(位置矢量)表示:
A
P [ px
py
p z ]T
图2.1 位置表示
2.1.2 方位描述 空间物体B的方位(Orientation)可由某个固接于此 物体的坐标系 {B} 的三个单位主矢量 [xB,yB,zB] 相 对于参考坐标系A的方向余弦组成的3x3矩阵描述.
2.1.3 位姿描述
要完全描述刚体B在空间的位姿,通常将物体B与某一坐 标系{B}相固接.{B}的坐标原点一般选在物体B的特征点 上,如质心等.相对参考系{A},坐标系{B}的原点位置和坐 标轴的方位,分别由位置矢量和旋转矩阵描述.这样,刚体 B的位姿可由坐标系{B}来描述,即有:
B
A B
第2章 数学基础
Mathematic Preparation for Robotics 2.1 位置和姿态的表示 2.2 坐标变换 2.3 齐次坐标变换 2.4 物体的变换和逆变换 2.5 通用旋转变换
(1) 被抓目标物体的位置和 姿态的描述;(目标坐标系) (2) 末端执行器的位置和姿 态的描述;(工具坐标系) (3) 机器人运动刚体的位置 和姿态的描述;(动坐标系) (4) 这些坐标系与固定坐标 之间的转换关系.
A
r11 B B B A A r11 p x r12 p y r13 p z pBOx p x r21 B B B A A r21 p x r22 p y r23 p z pBOy p y r31 r B p r B p r B p Ap A p BOz z 31 x 32 y 33 z 0
1 0 R ( x, ) 0 c 0 s 0 c 0 s c 0 1 0 R ( z , ) s s R ( y , ) c s 0 c 0 s 0 c 0 0 1
图2.2 方位表示
A B
R 是正交矩阵.
r 11 r A R B 21 r31
r 12 r22 r32
r 13 r23 r33
上述矩阵称为旋转矩阵,它是正交的.即 A 1 A T A R R B B B R 1 若坐标系B可由坐标系A,通过绕A的某一坐标轴获得, 则绕x,y,z三轴的旋转矩阵分别为
0.866 0.5 0 12 0.5 ; A p 6 A 0 R R ( z , 30 ) 0 . 866 0 B B0 0 1 0 0
0.902 12 11.908 6 13.562 7 . 562 0 0 0
用非零常数乘以变换矩阵的每个元素,不改变特性. 例2-3:求矢量2i+3j+2k被矢量4i-3j+7k平移得到的新矢量.
1 0 0 0 0 1 0 0 0 0 1 0 4 2 6 3 0 3 9 7 2 1 1 1
2.3.3 旋转齐次坐标变换
0 1 0 c 0 s c s 0 R ( y, ) 0 1 0 R ( z, ) s c 0 R ( x, ) 0 c s 0 1 0 s c s 0 c 0
机器人的坐标变换主要包括平移和旋转变换 , 平移是矩 阵相加运算 , 旋转则是矩阵相乘 , 综合起来可以表示为 p’ = m1*p + m2(m1旋转矩阵,m2为平移矩阵,p为原向量,p’ 为变换后的向量).引入齐次坐标的目的主要是合并矩阵 运算中的乘法和加法,合并后可以表示为p' = M*p的形式. 即它提供了用矩阵运算把二维、三维甚至高维空间中的 一个点集从一个坐标系变换到另一个坐标系的有效方法.
例2.2 试用齐次变换方法求解例2.1中的 A P
3
2.3.2 平移齐次坐标变换
{B} 分别沿{A} 的 X、 Y、 Z 坐标轴平移 a、 b 、 c距离的平 移齐次变换矩阵写为:
1 0 Trans( a, b, c) 0 0 0 1 0 0 0 0 1 0 a b c 1
齐次变换矩阵,综合表示了平移变换和旋转变换的复合
2.3 齐次坐标变换
2.3.1 齐次变换
(2-13)式可以写为:
A A P B R 1 0 A
PB 0 B P 1 1
(2-14)
B
P点在{A}和{B}中的位置矢量分别增广为:
A
P x
①Rot(x, 90)
（2-1）
②Rot(z, 90)
③Rot(y, 90)
0 - 1 1 0 P '' 0 0 0 0 0 0 0 1 P ''' - 1 0 0 0
例子:在动坐标中有一固定点 P 1 2 3 1 ,相对固定参考坐 标系 Oxyz 做如下运动:①Rot(x, 90);②Rot(z, 90);③Rot(y,90).求 点在固定参考坐标系 Oxyz 下的位置.
T
解1:用画图的简单方法
解2:用分步计算的方法
1 0 P' 0 0 0 0 0 -1 1 0 0 0 0 1 1 2 3 0 0 3 2 1 1 1
R
A
pB0

图2.2 方位表示
2.2 坐标变换
2.2.1 平移坐标变换
坐标系 {A} 和{B} 具有相同的方位 , 但原点不重合 .则点P 在两个坐标系中的位置矢量满足下式:
A
P P PB0
B A
图2.3 平移变换
2.2.2 旋转坐标变换
坐标系{A}和{B}有相同的原点但方位不同,则点P的在两 个坐标系中的位置矢量有如下关系:
例:动坐标系{B}中有一点 P i 2 j 3k ,此坐标系绕固定坐标系
绕x轴旋转90°,并沿z轴平移10个单位,沿y轴平移5个单位.求平移
旋转后该点相对于固定坐标系的坐标 .
1 0 A B P A T P B 0 0
0 0 0 1 2 0 1 5 1 0 10 3 0 0 1 1
三维直 角坐标
v x y z
T
齐次 坐标
v wx wy wz w
T
显然 ,齐次坐标表达并不是唯一的 ,随 w值的不同而不同 . 在计算机图学中,w 作为通用比例因子,它可取任意正值, 但在机器人的运动分析中,总是取w=1.
[例]:
V 3i 4 j 5k
可以表示为： V=[3 4 5 1]T 或 V=[6 8 10 2]T
将上式增广为齐次式:
1 0 R ( x, ) 0 0 0 0 0 c c s 0 0 R ( y, ) s s c 0 0 0 1 0 0 1 0 0 s 0 c 0 0 c s 0 0 0 s c 0 0 R ( z , ) 0 0 1 0 0 1 0 0 0 1