第三章_机器人运动学

1、 第三章机器人运动学第三章机器人运动学 2021-7-91 第三章第三章 机器人运动学机器人运动学 第三章机器人运动学第三章机器人运动学 目 录 3.1齐次坐标齐次坐标 3.2刚体位姿描述刚体位姿描述 3.3 齐次坐标变换与变换矩阵齐次坐标变换与变换矩阵 3.4齐次变换矩阵运算齐次变换矩阵运算 3.5变换方程变换方程 3.6欧拉角与欧拉角与RPY角角 2021-7-9 第三章机器人运动学第三章机器人运动学 2021-7-9 引 言 机器人的位置和姿态描述：机器人的位置和姿态描述： u 机器人一端固定，另一端是用于安装末端执行器（如手爪）的自由端机器人一端固定，另一端是用于安装末端执行器（如手爪

2、）的自由端 u 机器人由机器人由N N个转动或移动关节串联而成一个开环空间尺寸链个转动或移动关节串联而成一个开环空间尺寸链 u 机器人各关节坐标系之间的关系可用齐次变换来描述机器人各关节坐标系之间的关系可用齐次变换来描述 i n o a 机器人（机器人（机械手机械手）末端执行器相对于固）末端执行器相对于固 定参考坐标系的空间几何描述（即机器定参考坐标系的空间几何描述（即机器 人的运动学问题）是机器人动力学分析人的运动学问题）是机器人动力学分析 和轨迹控制等相关研究的基础和轨迹控制等相关研究的基础 机器人的运动学即是研究机器人手臂末机器人的运动学即是研究机器人手臂末 端执行器位置和姿态与关节变量

3、空间之端执行器位置和姿态与关节变量空间之 间的关系间的关系 第三章机器人运动学第三章机器人运动学 2021-7-9 丹纳维特（Denavit）和哈顿贝 格（Hartenberg） 于1955年提 出了一种矩阵代数方法解决机 器人的运动学问题 D-H方法 其数学基础即是齐次变换 具有直观的几何意义，广泛应 用于动力学、控制算法等方面 的研究 运动学研究运动学研究 运动学正问题运动学正问题 运动学逆问题运动学逆问题 手在手在哪哪里？里？ 手手怎么怎么放那放那 里？里？ 第三章机器人运动学第三章机器人运动学 2021-7-9 2.1 齐次坐标 位置描述：位置矢量位置描述：位置矢量(position

4、vector)(position vector) 空间任意一点空间任意一点 p p 的位置可表示为：的位置可表示为： 矩阵表示 矢量和表示 矢量的模 x y z o p (x,y,z) x y z p xiyjzk p 222 xyzp1p ，单位矢量 第三章机器人运动学第三章机器人运动学 2021-7-9 2.1 齐次坐标 点的齐次坐标点的齐次坐标 一般来说，n 维空间的齐次坐标表示是一个（n+1）维空间实体。有一个 特定的投影附加于 n 维空间，也可以把它看作一个附加于每个矢量的特 定坐标 比例系数。 T x y Px y z w z w 式中式中i, j, k为为x, y, z 轴上的单

5、位矢量，轴上的单位矢量， a= , b= , c= ，w为比例系数为比例系数 w x w y w z 齐次坐标表达并不是唯一的，随齐次坐标表达并不是唯一的，随w值的值的 不同而不同。在计算机图学中，不同而不同。在计算机图学中，w 作为作为 通用比例因子，它可取任意正值，但在通用比例因子，它可取任意正值，但在 机器人的运动分析中，总是取机器人的运动分析中，总是取w=1 。 列矩阵列矩阵 Paib jck 第三章机器人运动学第三章机器人运动学 2021-7-9 2.1 齐次坐标 xA yA zA oA p Ap 1 A AA A x y z p 直角坐标系A, P点的齐次坐标： 点的齐次坐标点的齐

6、次坐标 几个特定意义的齐次坐标：几个特定意义的齐次坐标： 0, 0, 0, nT 坐标原点矢量的齐次坐标，n为任意非零比例系数 1 0 0 0T 指向无穷远处的OX轴 0 1 0 0T 指向无穷远处的OY轴 0 0 1 0T 指向无穷远处的OZ轴 第三章机器人运动学第三章机器人运动学 2021-7-9 2.2 刚体位姿描述 接近矢量 a approach 方位矢量 o orientation 法向矢量 n normal a o n B z B y B x O B , , , BBB n o aijk等价于 手爪坐标系手爪坐标系 第三章机器人运动学第三章机器人运动学 2021-7-9 B z B

7、 y B x O B A y A x A z O o A B p o A B p 位置描述位置描述 2.2 刚体位姿描述 o A Bo AA BoBo A B x Py z 第三章机器人运动学第三章机器人运动学 2021-7-9 自由度自由度 (DOF, Degree of freedom) ： 物体能够相对坐标系进行独立运动的数目称为自 由度。 刚体的自由度数目：刚体的自由度数目： 三个平移自由度 T1, T2, T3 三个旋转自由度 R1, R2, R3 Y X Z T1 T2 T3 R1 R2 R3 位置描述位置描述 2.2 刚体位姿描述 第三章机器人运动学第三章机器人运动学 2021-

8、7-9 利用固定于物体的坐标系描述方位利用固定于物体的坐标系描述方位 (orientation)。方位又称为姿。方位又称为姿 态态 (pose)。 B k B j B i A y A x A z O A k A j A i o A B p 方位描述方位描述 2.2 刚体位姿描述 在刚体在刚体B上设置直角坐标系上设置直角坐标系 B，利用与，利用与B的坐标轴平行的的坐标轴平行的 三个单位矢量表示三个单位矢量表示B的姿态。的姿态。 坐标系坐标系B的三个单位主矢量在坐标系的三个单位主矢量在坐标系A中的描述：中的描述： , AAA BBB ijk 坐标系坐标系B相对于坐标系相对于坐标系A的姿态描述：的姿

9、态描述： , AAAA BBBB R ijk 第三章机器人运动学第三章机器人运动学 2021-7-9 111213 212223 313233 , AAAA BBBB rrr Rrrr rrr ijk A B R 表示刚体B相对于坐标系A的姿态。 112131 122232 132333 A B A B A B r ir jr k r ir jr k r ir jr k i j k 刚体B与坐标系B固接 姿态矩阵（旋转矩阵）姿态矩阵（旋转矩阵） 2.2 刚体位姿描述 9个参量，自由度？个参量，自由度？ 约束方程个数？约束方程个数？ （abs(a)=1;a.b=0) 第三章机器人运动学第三章机器

10、人运动学 2021-7-9 0 0 0 AAAAAA BBBBBB ijjkki 1 , 1 RRRR A B TA B A B B A 旋转变换的逆等于其转置旋转变换的逆等于其转置 11 1221 2231 32 11213121 3222 3112 3111 3211 2212 21 122232 ()()() AA BB AA BB ijr rr rr r ijk ijrrrr rr rir rr rjr rr rk rrr 1 1 1 AAAAAA BBBBBB iijjkk 旋转矩阵中的旋转矩阵中的9个元素只有个元素只有3个独立变量，它满足正交条件个独立变量，它满足正交条件 姿态矩阵

11、（旋转矩阵）姿态矩阵（旋转矩阵） 2.2 刚体位姿描述 第三章机器人运动学第三章机器人运动学 2021-7-9 相对于参考坐标系相对于参考坐标系A，坐标系，坐标系B的原点位置和坐标轴的的原点位置和坐标轴的 方位可以由位置矢量和旋转矩阵描述。刚体方位可以由位置矢量和旋转矩阵描述。刚体B在参考坐标在参考坐标 系系A中的位姿利用坐标系中的位姿利用坐标系B描述。描述。 o AA BB BRp IR A B 0 o A B p 位置与姿态的表示位置与姿态的表示 2.2 刚体位姿描述刚体位姿描述 第三章机器人运动学第三章机器人运动学 2021-7-9 平移坐标变换：平移坐标变换：在坐标系在坐标系B中的位置

12、矢量中的位置矢量Bp在坐标系在坐标系 A中的表示可由矢量相加获得。中的表示可由矢量相加获得。 ABA B ppp xA yA zA oA Ap xB yB zB oB Bp A B ApB xA zA oA Bp A oA xB zB yA yB pRp BA B A pRpRp ATA B AB A B 旋转坐标变换：旋转坐标变换： 坐标系坐标系BB与坐标系与坐标系AA原点原点 相同，则相同，则p p点在两个坐标系中点在两个坐标系中 的描述具有下列关系：的描述具有下列关系： 2.3 齐次变换与齐次变换矩阵齐次变换与齐次变换矩阵 一般变换一般变换 第三章机器人运动学第三章机器人运动学 2021

13、-7-9 分别绕分别绕x,y,z轴的旋转变换轴的旋转变换(基本旋转变换基本旋转变换)：任何旋转变换可以由有限任何旋转变换可以由有限 个基本旋转变换合成得到。个基本旋转变换合成得到。 100 ( , )0 0 R xcs sc ( , ) AB R xpp 2.3 齐次变换与齐次变换矩阵齐次变换与齐次变换矩阵 基本旋转变换基本旋转变换 yB yA xB zB oA Bp B xA zA A P , 0 010 0 ),( cs sc yR 100 0 0 ),( cs sc zR 第三章机器人运动学第三章机器人运动学 2021-7-9 ACAABA CBB pppRpp AABA BB pRpp

14、 yC xA yA zA oA Ap xB yB zB oB Bp A B ApB xC zC C 复合变换：复合变换：平移和旋转构成复合变换。平移和旋转构成复合变换。 CCBAB BB pRpRp 2.3 齐次变换与齐次变换矩阵齐次变换与齐次变换矩阵 基本复合变换基本复合变换 第三章机器人运动学第三章机器人运动学 2021-7-9 B k B j B i A y A x A z O A k A j A i o A B p P A p B p B A o AABA BB Rp =p+p 110 0 01 o AA AB BB R ppp = 2.3 齐次变换与齐次变换矩阵齐次变换与齐次变换矩阵

15、 齐次变换齐次变换 齐次变换齐次变换是在齐次坐标描述基础上的一种矩阵运算方法，齐次变是在齐次坐标描述基础上的一种矩阵运算方法，齐次变 换使齐次坐标作换使齐次坐标作移动移动 、旋转、旋转 、透视、透视 等几何变换。等几何变换。 非齐次 齐次 第三章机器人运动学第三章机器人运动学 2021-7-9 B k B j B i A y A x A z O A k A j A i o A B p P A p B p B A 110 0 01 o AA AB BB R ppp = 1 31 1 o AA BB A B R T fs p = 旋转旋转平移平移 透视透视 比例（缩放）比例（缩放） 计算机图形学计

16、算机图形学 0 0 01 o AA BBA B R T p =齐次变换矩阵齐次变换矩阵 2.3 齐次变换与齐次变换矩阵齐次变换与齐次变换矩阵 齐次变换矩阵齐次变换矩阵 第三章机器人运动学第三章机器人运动学 2021-7-9 透视变换透视变换（Perspective transformation）举例 p p : Px1 PP x1 1 () 1 pp pp T T ppp pppp p ppp pppp ppp pppp p p yz yz yzzf yzzyf yf zxyf zxyyf yyfyf yyfyyff x x y 以光心为原点O，光轴与y轴重合，P为物点， 用齐次坐标表示 求的

17、齐次坐标，即求 根据三角形相似原理： 注意是负值， 是正值，所以实际上为相减关系 又有 设 11 1000 0100 0010 0001 111 pp pp pp p p pp p pp fp pp p yz yz yz fff x xx y yy Tz zz y f 用矩阵表示： z y P yp f o zp f p z P p y 第三章机器人运动学第三章机器人运动学 2021-7-9 p p : Px1 PP x1 1 () 1 pp pp T T ppp pppp ppp pppp p p yz yz ypzpzpf ypzpzpypf ypf zxyf zxyyf yyfyf yy

18、fyyff x x y 以光心为原点O，光轴与y轴重合，P为物点， 用齐次坐标表示 求的齐次坐标，即求 根据三角形相似原理： 注意是负值，是正值，所以实际上为相减关系 又有 设 11 1000 0100 0010 0001 111 pp pp ppp p pp p pp fp pp p yz yz yy fff x xx y yy Tz zz y f 用矩阵表示： 因此，进行机器人运行学计算时，不能省略透视矩阵，有摄像头时，透视矩阵 为 0 - 0，没有摄像头时为0 0 0 。 f 1 p 1000 0100 0010 1 001 111 p pp pp ff p pp p x xx y yy

19、 TTz zz y f f 用矩阵表示： 透视变换透视变换（Perspective transformation）举例 第三章机器人运动学第三章机器人运动学 2021-7-9 , 1000 00 00 0001 ),(Rot cs sc x 平移齐次坐标变换平移齐次坐标变换 旋转齐次坐标变换旋转齐次坐标变换 1000 100 010 001 ),Trans( c b a cba Translation transformation , 1000 00 0010 00 ),(Rot cs sc y 1000 0100 00 00 ),(Rot cs sc z Rotation transform

20、ation 2.4 齐次变换矩阵运算齐次变换矩阵运算 齐次坐标变换齐次坐标变换 注意：注意：平移矩阵间可以交换，平移矩阵间可以交换， 平移和旋转矩阵间不可以交换平移和旋转矩阵间不可以交换 第三章机器人运动学第三章机器人运动学 2021-7-9 对于坐标系对于坐标系A、B ，A是参考坐标系，是参考坐标系， B相对于相对于A的联体坐标系。的联体坐标系。 B相对于相对于 A的描述为：的描述为： A相对于相对于B的描述为：的描述为： 01 o AA BBA B Rp T 1 1 0101 oo AAATAT A BBBBBBA AB RpRRp TT 2.4 齐次变换矩阵运算齐次变换矩阵运算 齐次坐标

21、变换的逆变换齐次坐标变换的逆变换 第三章机器人运动学第三章机器人运动学 2021-7-9 例题例题1：坐标系坐标系 B 的初始位姿与参考坐标系的初始位姿与参考坐标系 A 相同，坐标系相同，坐标系 B 相对于相对于 A 的的 zA 轴旋转轴旋转 30 ，再沿，再沿 A 的的 xA 轴移动轴移动 12，沿沿 A 的的 yA 轴移动轴移动 6。求位置矢量。求位置矢量 ApB 和旋转矩阵和旋转矩阵 。假设。假设 p 点在坐标系点在坐标系 B 的描的描 述为述为 Bp=5 9 0T，求其在坐标系，求其在坐标系 A 的描述。的描述。 R A B 0 294.16 83.11 0 6 12 0 9 5 10

22、0 0866. 05 . 0 05 . 0866. 0 0 6 12 ; 100 0866. 05 . 0 05 . 0866. 0 100 03030 03030 )30,( B ABA B A B AA B ppRp pcs sc zRR 解：解： 2.4 齐次变换矩阵运算齐次变换矩阵运算 第三章机器人运动学第三章机器人运动学 2021-7-9 1101 AA AB BBAABA BB Rppp pRpp pTp BA B A 1 , 10 , 1 p p pR T p p B BB AA BA B A A Ap、 Bp称为点的齐次坐标， 为齐次坐标变换矩阵T A B 1 0 294.16

23、 83.11 1 0 9 5 1000 0100 60866. 05 . 0 1205 . 0866. 0 , 1000 0100 60866. 05 . 0 1205 . 0866. 0 10 pTp pR T BA B AB AA BA B 例题例题2：对于例题1利用齐次坐标求解Ap。 第三章机器人运动学第三章机器人运动学 2021-7-9 纯平移变换与变换次序无关纯平移变换与变换次序无关 旋转变换与变换次序有关旋转变换与变换次序有关 复合变换与变换次序有关复合变换与变换次序有关 ssscccsssccs scccs ssccscscsscc cs sccs sc cs sc xRzRyR

24、 0 0 001 100 0 0 0 010 0 ),(),(),( sssccscsscc csssccccscss csscc cs sc cs sc cs scyRzRxR 0 010 0 100 0 0 0 0 001 ),(),(),( 2.4 齐次变换矩阵运算齐次变换矩阵运算 齐次坐标变换的顺序问题齐次坐标变换的顺序问题 第三章机器人运动学第三章机器人运动学 2021-7-9 绕当前轴绕当前轴 开始开始B、A重合，然后先绕重合，然后先绕XA轴转轴转 得到新坐标系得到新坐标系C ，再绕当，再绕当 前轴前轴YC轴转轴转得到要求的坐标系得到要求的坐标系B 。 1)( ,) CCBB B

25、RRot ypp =p 2)( , ) AACC C RRot xpp =p 3)( , )( ,) AB Rot xRot ypp 绕当前轴（即相对于运动坐标系）绕当前轴（即相对于运动坐标系） 右乘右乘 2.4 齐次变换矩阵运算齐次变换矩阵运算 齐次坐标变换的顺序问题齐次坐标变换的顺序问题 第三章机器人运动学第三章机器人运动学 2021-7-9 绕固定轴绕固定轴 开始开始B、A重合，然后重合，然后B先绕先绕XA轴转轴转 ，再绕，再绕YA轴转轴转。 1)( ,) AB Rot x pp 3)( ,)( ,) ACA Rot yRot ypp =p 4)( ,)( , ) AB Rot yRot

26、 xpp 2）C、A重合，重合， C再绕再绕YA轴转轴转 得到得到B中的矢量在中的矢量在A中的表示中的表示 CA pp和等价， 绕固定轴绕固定轴 （及相对固定坐标系）（及相对固定坐标系） 左乘左乘 2.4 齐次变换矩阵运算齐次变换矩阵运算 齐次坐标变换的顺序问题齐次坐标变换的顺序问题 第三章机器人运动学第三章机器人运动学 2021-7-9 刚体位置描述：刚体位置描述：利用齐次坐标变换可以描述刚体的利用齐次坐标变换可以描述刚体的位置和姿态位置和姿态。刚体上其它。刚体上其它 点在参考坐标系中的位置可以由变换矩阵乘以该点在刚体坐标系中的位置获点在参考坐标系中的位置可以由变换矩阵乘以该点在刚体坐标系中

27、的位置获 得。得。 例题例题3：下图中的物体可以由下图中的物体可以由(1,0,0), (-1,0,0), (-1,0,2), (1,0,2), (1,4,0), (-1,4,0)表示。如果表示。如果 该物体在基坐标系中先绕该物体在基坐标系中先绕z轴旋转轴旋转90，再绕，再绕y轴旋转轴旋转90，再沿，再沿x轴平移轴平移4，求物体，求物体6个个 顶点的位置。顶点的位置。 x y z o o1 选取物体上与选取物体上与o点重合的点点重合的点o1为刚为刚 体坐标系原点，其初始坐标轴体坐标系原点，其初始坐标轴 x1y1z1 方向与方向与 xyz 坐标系相同。坐标系相同。 2.4 齐次变换矩阵运算齐次变换

28、矩阵运算 齐次坐标变换举例齐次坐标变换举例 第三章机器人运动学第三章机器人运动学 2021-7-9 先绕先绕 z 轴旋转轴旋转 90 再绕再绕 y 轴旋转轴旋转 90 再沿再沿 x 轴平移轴平移 4 x y z o o1 y x z o o1 x1 y1 z1x y z o o1 x1 z1 y1 y x z oo1 x1 y1 z1 2.4 齐次变换矩阵运算齐次变换矩阵运算 第三章机器人运动学第三章机器人运动学 2021-7-9 1000 0010 0001 4100 1000 0100 0001 0010 1000 0001 0010 0100 1000 0100 0010 4001 Ro

29、t(z,90)90,(Rot)0 , 0 , 4Trans(yT 1000 4001 0100 0010 1000 0100 0010 4001 1000 0001 0010 0100 1000 0100 0001 0010 )0 , 0 , 4rans(Rot(y,90)T)90,(Rot zT x y z o o1 x y z o o1 x1 z1 y1 x y z o o1 x1 z1 y1 x1 o1 x y z o z1 y1 对于右 乘的结 果： （相当 于在新 坐标系 中变换 ） 2.4 齐次变换矩阵运算齐次变换矩阵运算 第三章机器人运动学第三章机器人运动学 2021-7-9 1

30、11111 440000 111111 446644 111111 002200 440000 111111 1000 0010 0001 4100 刚体的6个顶点在基坐标系中的位置： 2.4 齐次变换矩阵运算齐次变换矩阵运算 第三章机器人运动学第三章机器人运动学 2021-7-9 对于坐标系A、B、C，A是参考坐标系， B 相对于A的坐标以及C相对于B的坐标称为联体 坐标。 设B在A中的表示为T1， C在B中的表示为T2， 刚体在C中的表示为T3，刚体在A中的表示为T，则 T= T1 T2 T3 上式可以理解为：上式可以理解为：从基坐标系变换到联体坐标系，右 乘。 2.4 齐次变换矩阵运算齐

31、次变换矩阵运算 联体坐标系联体坐标系 第三章机器人运动学第三章机器人运动学 2021-7-9 通用旋转变换：通用旋转变换： 设设 f 为坐标系为坐标系C的的z轴上的单位矢量，即：轴上的单位矢量，即： 则绕矢量则绕矢量 f 的旋转等价于绕坐标系的旋转等价于绕坐标系C的的z轴的旋转：轴的旋转： kajaiaf aon aon aon C zyx zzz yyy xxx , 1000 0 0 0 ),(Rot),(Rot z Cf TCS CST 1 设坐标系设坐标系C在基坐标系下的描述为在基坐标系下的描述为C。对于某一刚体，在基坐标系下的描述为。对于某一刚体，在基坐标系下的描述为T，在坐，在坐 标

32、系标系C下的描述为下的描述为S，则：，则： 2.4 齐次变换矩阵运算齐次变换矩阵运算 通用旋转变换通用旋转变换 第三章机器人运动学第三章机器人运动学 2021-7-9 T绕绕 f 轴的旋转等价于轴的旋转等价于S绕坐标系绕坐标系C的的z轴的旋转：轴的旋转： 10 0 0 0 00 1000 0 0 0 1000 0100 00 00 1000 0 0 0 ),(Rot zzzzzzzzzz zyyzyzzyzy zxxzxzzxzx yzzyzyyzyzxzxzzxxzxz yyyyyyyyyyxyxyyxxyxy yxxyxyyxyxxxxxxxxxxx zyx zyx zyx zzz yyy

33、 xxx aacoosonsoncnn aacoosonsoncnn aacoosonsoncnn aacoosonsoncnnaacoosonsoncnn aacoosonsoncnnaacoosonsoncnn aacoosonsoncnnaacoosonsoncnn aaa ooo nnn cs sc aon aon aon f 1111 ),(Rot),(Rot),(Rot),(Rot ),(Rot),(Rot CzCTTCzCSTzCf SzCTf 2.4 齐次变换矩阵运算齐次变换矩阵运算 通用旋转变换通用旋转变换 第三章机器人运动学第三章机器人运动学 2021-7-9 令令vers

34、 =1-c , 有：有： cversff ccaa aacaa aacoonn aacoocnn aacoosonsoncnn xx xx xxxx xxxxxx xxxxxx xxxxxxxxxx )1 ( )1 ( )( krrrrjrrrrirrrr rrr rrr kji yx rrrrrryx B A B A B A B A )()()( 211222113211311231223221 322212 312111 323122211211 kajaiaf aon aon aon C zyx zzz yyy xxx 1000 0 0 0 sfsff sacaa aasacaa aas

35、ononcoonn aacoosonsoncnn zxy zxy xyzxy xyxyyxxyxy xyxyyxxyxy ver )1 ( )()( 2.4 齐次变换矩阵运算齐次变换矩阵运算 第三章机器人运动学第三章机器人运动学 2021-7-9 sfff aasacaa aasononcoonn aacoosonsoncnn yzx xzyxz xzxzzxxzxz xzxzzxxzxz vers )()( sfff aasacaa aasononcoonn aacoosonsoncnn yzx zxyzx zxzxxzxzzx zxxzxzzxzx vers )()( sfff aasac

36、aa aasononcoonn aacoosonsoncnn zzx yxzyx yxyxxyxyyx yxxyxyyxyx vers )()( cff aacaa aacoonn aacoosonsoncnn yy yyyy yyyyyy yyyyyyyyyy vers )1 ( )( sfff aasacaa aasononcoonn aacoosonsoncnn xyz yzxyz yzyzzyzyyz yzzyzyyzyz vers )()( cff aacaa aacoonn aacoosonsoncnn zz zzzz zzzzzz zzzzzzzzzz vers )1 ( )(

37、sfersff aasacaa aasononcoonn aacoosonsoncnn xzy zyxzy zyzyyzyzzy zyyzyzzyzy v )()( 2.4 齐次变换矩阵运算齐次变换矩阵运算 第三章机器人运动学第三章机器人运动学 2021-7-9 通用旋转变换为：通用旋转变换为： 1000 0versversvers 0versversvers 0versversvers ),(Rot cffsfffsfff sfffcffsfff sfffsfffcff f zzxzyyzx xyzyyzyx yxzzxyxx 1000 0versversvers 0versversvers

38、 0versversvers 1000 0 0 0 cffsfffsfff sfffcffsfff sfffsfffcff aon aon aon zzxzyyzx xyzyyzyx yxzzxyxx zzz yyy xxx ccfffaon zyxzyx 213)vers( 222 ) 1( 2 1 zyx aonc 等效转角与转轴等效转角与转轴 给出一任意旋转变换，可由上式求得等效转角与转轴。令： 将对角线三项相加，得： 2.4 齐次变换矩阵运算齐次变换矩阵运算 第三章机器人运动学第三章机器人运动学 2021-7-9 将旋转规定为绕矢量 f 的正向旋转，使得0 180。于是得到旋转角： 旋

39、转矢量为： 222 )()()( 2 1 2 2 2 xyzxxz zxy yzx xyz onnaaos sfon sfna sfao ) 1( )()()( tan 222 zyx xyzxxz aon onnaao )2/()( )2/()( )2/()( sonf snaf saof xyz zxy xzx 2.4 齐次变换矩阵运算齐次变换矩阵运算 多值性：转角和转轴有多组，转角相差多值性：转角和转轴有多组，转角相差360360的整数倍时旋转矩阵相同的整数倍时旋转矩阵相同 病态情况：转角是病态情况：转角是0 0或或180180时，转轴不能确定时，转轴不能确定 第三章机器人运动学第三章机

40、器人运动学 2021-7-9 B 基座坐标系 W 腕坐标系 T 工具坐标系 S 工作站坐标系 G 目标坐标系 BS SG TT 、 、 G T T 机器人控制和规划的目标机器人控制和规划的目标 2.5 变换方程变换方程 第三章机器人运动学第三章机器人运动学 2021-7-9 2.5 变换方程变换方程 第三章机器人运动学第三章机器人运动学 2021-7-9 空间尺寸链空间尺寸链 BBW TWT TT T BBSG TSGT TT T T BWBSG WTSGT T TT T T G T T已知，改变 已知，改变 B W T 1BBSGW WSGTT TT T T T 2.5 变换方程变换方程 第

41、三章机器人运动学第三章机器人运动学 2021-7-9 回转（横滚）:绕Z轴转, Roll 俯仰:绕Y轴转, Pitch 偏转:绕X轴转. Yaw 姿态描述姿态描述 2.6 欧拉角与欧拉角与RPY角角 RPYRPY角角 2.6 欧拉角与欧拉角与RPY角角 RPYRPY角角 第三章机器人运动学第三章机器人运动学 2021-7-9 先绕先绕XA轴转轴转，再绕，再绕YA轴转轴转，最后绕，最后绕ZA轴转轴转。 ( , )( ,)( ,)( , ) 00001000 00010000 00100000 000100010001 RPYRot zRot yRot x cscs sccs scsc 注意注意:

42、 绕固定轴绕固定轴 左乘左乘 2.6 欧拉角与欧拉角与RPY角角 RPYRPY角表示运动姿态角表示运动姿态 第三章机器人运动学第三章机器人运动学 2021-7-9 机器人运动姿态描述机器人运动姿态描述 Z-Y-X欧拉欧拉(Euler)角角：先绕：先绕z轴旋转轴旋转 ，再绕新的，再绕新的y轴轴(y )旋转旋转 ，再，再 绕新的绕新的x轴轴(x )旋转旋转 ，以此表示所有的姿态。，以此表示所有的姿态。 ( , , )Rot( , )Rot( ,)Rot( , ) 00001000 00010000 00100000 000100010001 0 0 A Bzyx Rzyx cscs sccs sc

43、sc c cc s ss cc s cs s s cs s sc cs s cc s sc s 0 0001 c c Z-Y-X欧拉欧拉(Euler)角等价的旋转矩阵角等价的旋转矩阵变换表示为：变换表示为： 2.6 欧拉角与欧拉角与RPY角角 Z-Y-XZ-Y-X欧拉角欧拉角 第三章机器人运动学第三章机器人运动学 2021-7-9 机器人运动姿态描述机器人运动姿态描述 Z-Y-Z欧拉角表示欧拉角表示：先绕：先绕z轴旋转轴旋转，再绕新的，再绕新的y轴轴(y )旋转旋转，再绕新，再绕新 的的z轴轴(z )旋转旋转，以此表示所有的姿态。欧拉变换在基坐标系中的，以此表示所有的姿态。欧拉变换在基坐标系中

44、的 表示为：表示为： ( , , )Rot( , )Rot( ,)Rot( , ) 000000 00010000 0010000010 000100010001 0 0 A Bzyz Rzyz cscscs scsc sc c c cs sc c ss cc s s c cc ss c sc cs s s c 0 0001 s sc Z-Y-ZZ-Y-Z欧拉角欧拉角 2.6 欧拉角与欧拉角与RPY角角 第三章机器人运动学第三章机器人运动学 2021-7-9 柱面坐标表示位置柱面坐标表示位置：先沿基坐标系的：先沿基坐标系的x轴平移轴平移r， 再绕基坐标系的再绕基坐标系的z轴旋转轴旋转 ，再，再沿基坐标系的沿基坐标系的z轴轴 平移平移z。 1000 100 0 0 1000 0100 0010 001 1000 0100