机器人技术第3章

1、机器人运动学机器人运动学 Kinematics of Robotics 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 ( (姿态和方向角姿态和方向角 位置和坐标位置和坐标 连杆变换矩阵连杆变换矩阵) ) 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 ( (欧拉变换解欧拉变换解/ /滚仰偏变换解滚仰偏变换解/ /球面变换解球面变换解) ) 3.3 PUMA5603.3 PUMA560机器人运动方程机器人运动方程 ( (运动分析运动分析/ /运动综合运动综合) ) 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 ( (微分运动微分运动/ /雅可比矩阵雅可比矩阵/ /计

2、算实例计算实例) ) Robotics 运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.0 A矩阵和T矩阵 机械手可以看成由一系列关节连接起来的连杆组构成. 用A矩阵描述连杆坐标系间相对平移和旋转的齐次变换. A1表示第一连杆对基坐标的位姿 A2表示第二连杆对第一连杆位姿 则第二连杆对基坐标的位姿为 212 AAT 6543216 AAAAAAT Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.1 运动姿态和方向角 1.运动方向 接近矢量a a:夹持器进入物体的方向;Z轴 方向矢量o:o:指尖互相指向;Y轴 法线矢

3、量n:n:指尖互相指向;X轴 111 aoaaoo aon 1000 6 zzzz yyyy xxxx paon paon paon TT Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.1 运动姿态和方向角 2.用旋转系列表示运动姿态 欧拉角:绕Z轴转,再绕新Y轴转,绕最新Z轴转. (3-3) 注意:坐标变换是右乘.即后面的变 换乘在右边.(绕新轴转,连乘) 1000 0100 00 00 1000 00 0010 00 1000 0100 00 00 ),(),(),(),( cs sc cs sc cs sc zRotyRotzRotEul

4、er Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.1 运动姿态和方向角 3.用滚仰偏转表示运动姿态 横滚:绕Z轴转, 俯仰:绕Y轴转, 偏转:绕X轴转. (3-5) 注意:左乘. 1000 00 00 0001 1000 00 0010 00 1000 0100 00 00 ),(),(),(),( cs sc cs sc cs sc xRotyRotzRotRPY Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.2 运动位置和坐标 1.用柱面坐标表示末端运动位置 由于上述绕Z轴的旋转,使末端执

5、行器的姿态出现变化, 若要执行器姿态不变,则需将其绕执行器Z轴反向旋转. (3-8) 1000 100 010 001 1000 0100 00)()( 00)()( 1000 100 0 0 ),( z rs rc cs sc z rscs rcsc rzCyl Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.2 运动位置和坐标 2.用球面坐标表示末端运动位置 沿Z平移r,绕Y轴转,绕Z轴转. (3-10) 1000 0 1000 100 0010 0001 1000 00 0010 00 1000 0100 00 00 ),0,0(),(),

6、(),( rccs srsssccs srcccscc rcs sc cs sc rTransyRotzRotrSph Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.2 运动位置和坐标 表示物体的位置:笛卡尔坐标、柱面坐标、球面坐标 1.用柱面坐标表示末端运动位置 沿X平移r,绕Z轴转,沿Z轴平移z. (绕原坐标系运动,左乘) (3-7) 1000 100 0 0 1000 0100 0010 001 1000 0100 00 00 1000 100 0010 0001 )0 , 0 ,(),(), 0 , 0(),( z rscs rcsc

7、r cs sc z rTranszRotzTransrzCyl Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.2 运动位置和坐标 2.用球面坐标表示末端运动位置 沿Z平移r,绕Y轴转,绕Z轴转. (3-10) 1000 0 1000 100 0010 0001 1000 00 0010 00 1000 0100 00 00 ),0,0(),(),(),( rccs srsssccs srcccscc rcs sc cs sc rTransyRotzRotrSph Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的

8、表示 3.1.2 运动位置和坐标 2.用球面坐标表示末端运动位置 由于上述两个旋转,使执行器姿态发生变化.为保持姿 态,执行器要绕其自身Y和Z轴反向旋转. (3-11) 1000 100 010 001 ),(),(), 0 , 0(),(),(),( rc srs src zRotyRotrTransyRotzRotrSph AA Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.3 连杆变换矩阵 1.广义连杆(D-H坐标) 全为转动关节: Zi坐标轴; Xi坐标轴; Yi坐标轴; 连杆长度ai; 连杆扭角i; 两连杆距离di; 两杆夹角i Ro

9、botics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.3 连杆变换矩阵 1.广义连杆 全为转动关节: Zi坐标轴:沿着i+1关节的运动轴; Xi坐标轴:沿着Zi和Zi-1的公法线,指向离开Zi-1轴的方向; Yi坐标轴:按右手直角坐标系法则制定; 连杆长度ai; Zi和Zi-1两轴心线的公法线长度; 连杆扭角i: Zi和Zi-1两轴心线的夹角; 两连杆距离di:相邻两杆三轴心线的两条公法线间的距离; 两杆夹角i :Xi和Xi-1两坐标轴的夹角; Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.3 连杆变换矩阵

10、 1.广义连杆(D-H坐标) 含移动关节: Zi坐标轴; Xi坐标轴; Yi坐标轴; 连杆长度ai=0; 连杆扭角i; 两连杆距离di; 两杆夹角i Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.3 连杆变换矩阵 1.广义连杆 含移动关节: Zi坐标轴:沿着i+1关节的运动轴; Xi坐标轴:沿着Zi和Zi-1的公法线,指向离开Zi-1轴的方向; Yi坐标轴:按右手直角坐标系法则制定; 连杆长度ai; Zi和Zi-1两轴心线的公法线长度; 连杆扭角i: Zi和Zi-1两轴心线的夹角; 两连杆距离di:相邻两杆三轴心线的两条公法线间的距离; 两杆夹

11、角i :Xi和Xi-1两坐标轴的夹角; Robotics运动学运动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.3 连杆变换矩阵 2.广义变换矩阵 建立D-H坐标系后,可通过两个旋转、两个平移建立相邻 连杆i-1和i间的相对关系。 1。绕Zi-1轴转i角，使Xi-1转到与Xi同一平面内； 2。沿Zi-1轴平移di，把Xi-1移到与Xi同一直线上； 3。沿i轴平移ai-1,把连杆i-1的坐标系移到使其原点与 连杆i的坐标系原点重合的位置； 4。绕Xi-1轴转i-1角，使Zi-1转到与Zi同一直线上； 这四个齐次变换叫Ai矩阵： Robotics运动学运动学 3.1 3.1

12、 机器人运动方程的表示机器人运动方程的表示 3.1.3 连杆变换矩阵 2.广义变换矩阵 对旋转关节: (3-13) 对棱柱关节: (3-14) 1000 0 ),()0 , 0 ,(), 0 , 0(),( 11 111 111 iii iiiiiii iiiiiii iiiii dcs sascccs casscsc xRotaTransdTranszRotA 1000 0 1 0 ),()0 , 0 ,(), 0 , 0(),( 11 11 11 iii iiiii iiiii iiiii dcs scccs sscsc xRotaTransdTranszRotA Robotics运动学运

13、动学 3.1 3.1 机器人运动方程的表示机器人运动方程的表示 3.1.3 连杆变换矩阵 3.用A矩阵表示T矩阵 T6:机械手末端对其基座 Z:机械手基座对参考坐标系 E:端部工具对机械手末端 X:端部工具对参考坐标系 616 1 AAAT ii i 11 6 6 XEZT EZTX Robotics运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 1)问题:已知手部位姿,求各关节位置 2)意义:是机械手控制的关键 3)没有一种算法可以通用,需要几何设置引导 本节介绍上节的几种特殊变换下的求解算法. Robotics运动学运动学 3.2 3.2 机械手运动方程的求解机械手

14、运动方程的求解 3.2.1欧拉变换解 1.基本隐式方程的解 若上式中T矩阵的各元素已知，即 （3-24） 对应项相等，有 ),(),(),(),(zRotyRotzRotEulerT 1000 0 0 0 1111 csscs ssccscsssccs sccssccssccc paon paon paon zzzz yyyy xxxx Robotics运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 3.2.1欧拉变换解 arccos:符号不定； 特殊点不准确； 0或180时，后 （3-25/33） 两式没定义。 ca ssa sca sso ccscso csscco

15、 csn scccsn sscccn z y x z y x z y x )/arccos( )/arccos( )arccos( sn sa a z x z Robotics运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 3.2.1欧拉变换解 2.用显式方程求各角度 (3-37) (3-39) ),(),(),( ),(),(),( 11 1 zRotTzRotyRot zRotyRotTzRot 1000 0 00 0 11111000 0100 00 00 csscs cs ssccc paon paon paon cs sc zzzz yyyy xxxx Rob

16、otics运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 3.2.1欧拉变换解 其中 (3-40) 1000 0 00 0 1000 )()()()( )()()()( )()()()( 13131313 12121212 11111111 csscs cs ssccc pfafofnf pfafofnf pfafofnf paonu uuf ucusuf usucuf z yx yx , )( )( )( 13 12 11 Robotics运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 3.2.1欧拉变换解 由 有: 即 (3-42/43) 这

17、样,由 ,得 (3-44) 再由 ,得 (3-45) 0)( 12 af0 yx acas )/tan()/tan( xyxy aaaaaa )()( 1311 afcafs )tan( z yx a asac a )()( 1212 ofcnfs )tan( yx yx ocos ncns a Robotics运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 3.2.2 滚、仰、偏变换解 由 （3-47） f定义同前。 ),(),(),( 1 xRotyRotTzRot 1000 0 00 0 1000 )()()()( )()()()( )()()()( 131313

18、13 12121212 11111111 ccscs sc csssc pfafofnf pfafofnf pfafofnf Robotics运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 3.2.2 滚、仰、偏变换解 由 得 （3-48） 这样，由 可得： （3-50） 再由 得 （3-51） 0)( 12 yx ncnsnf )tan( x y n n a snnfcnsncnf zyx )()( 1311 )tan( yx z nsnc n a sacasafcocosof yxyx )()( 1212 )tan( yx yx ocos acas a Roboti

19、cs运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 3.2.3 球面变换解 右列相等： （3-53） ), 0 , 0(),(),( 1 rTransyRotTzRot 1000 0 0010 0 10001000 0100 00 00 rccs rssc paon paon paon cs sc zzzz yyyy xxxx 1 0 1 rc rs p pcps pspc z yx yx Robotics运动学运动学 3.2 3.2 机械手运动方程的求解机械手运动方程的求解 3.2.3 球面变换解 由第二行有： （3-54） （3-56） 用 的右列相等， 可得： （

20、3-57） )tan(0 x y yx p p apcps )tan( z yx zyx p pspc arcprspspc ), 0 , 0(),(),( 11 rTransTzRotyRot zyx pcpspcsr)( Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 已知转角，求各杆位姿 Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 1。确定D-H坐标系 全为转动关节: Zi坐标轴:沿着i+1关节的运动轴; Xi坐标轴:沿着Zi和Z

21、i-1的公法线,指向离开Zi-1轴的方向; Yi坐标轴:按右手直角坐标系法则制定; 连杆长度ai; Zi和Zi-1两轴心线的公法线长度; 连杆扭角i: Zi和Zi-1两轴心线的夹角; 两连杆距离di:相邻两杆三轴心线的两条公法线间的距离; 两杆夹角i :Xi和Xi-1两坐标轴的夹角; Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 2。确定各连杆D-H参数和关节变量 Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 Robotics运动学运动

22、学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 3。求出两杆间的位姿矩阵 1000 0 ),()0 , 0 ,(), 0 , 0(),( 11 111 111 iii iiiiiii iiiiiii iiiii dcs sascccs casscsc xRotaTransdTranszRotA Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析

23、4。求末杆的位姿矩阵 Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 （3-64） Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.1 运动分析 5。验证 mmdmmdmmammdmma oo 25.56,07.433,32.20,09.149,8 .431 00, 0,90, 0,90 64322 654321 Robotics

24、运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.2 运动综合 已知， 求：各转角 1000 0 6 zzzz yyyy xxxx paon paon paon T Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.2 运动综合 由于 交于一点W,点W在基础坐标系中的位置仅 与 有关。据此，可先解出 ，再分离出 ，并逐一求解。 1.求1 654 ,zzz 321 , 321 , 654 , 6 111 11 6 1 6 5 3 2 2 1 61 1 1 10001000 0100 00

25、00 )( T paon paon paon cs sc TTTTTT zzzz yyyy xxxx oo Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.2 运动综合 有两个可能的解。 其他角度可以类似方法求得。 211 dpcps yx )tan()tan( 2 2 22 2 1 dpp d a p p a yx x y Robotics运动学运动学 3.3 PUMA6003.3 PUMA600机器人运动方程机器人运动方程 3.3.2 运动综合 解的多重性 Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人

26、的雅可比公式 3.4.1 机器人的微分运动 1。微分平移和旋转 在基系中的描述： 在坐标系T中描述： TdTIdfRotdddTrans TIdfRotdddTransdT TdfRotdddTransdTT zyx zyx zyx ,),(),( ),(),( ),(),( T zyx T zyx zyx TdTIdfRotdddTrans IdfRotdddTransdT dfRotdddTransdTT ,),(),( ),(),( ),(),( T T Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 1。微分平移和旋转

27、微分平移变换： 微分旋转变换： 因为： 1000 100 010 001 ),( z y x zyx d d d dddTrans 1000 0 0 0 ),( cversffsfversffsfversff sfversffcversffsfversff sfversffsfversffcversff fRot zzxzyyzx xyzyyzyx yxzzxyxx Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 1。微分平移和旋转 有： 0, 1cos,sin limlimlim 000 versd 1000 01 01 01

28、 ),( dfdf dfdf dfdf dfRot xy xz yz Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 1。微分平移和旋转 所以有 （3-87） 0000 0 0 0 1000 0100 0010 0001 1000 01 01 01 1000 100 010 001 ),(),( zxy yxz xyz xy xz yz z y x zyx ddfdf ddfdf ddfdf dfdf dfdf dfdf d d d IdfRotdddTrans Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机

29、器人的雅可比公式 3.4.1 机器人的微分运动 1。微分平移和旋转 因为： (3-88) (3-89) 微分平移和旋转矢量： zzyyxx dfdfdf 0000 0 0 0 , 0000 0 0 0 z T x T y T y T x T z T x T y T z T T zxy yxz xyz d d d d d d kjikdjdid zyxzyx d Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 1。微分平移和旋转 记： （3-90） （3-91） z T y T x T z T y T x T T T T z y

30、x z y x d d d d d d d D d D, Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 1。微分平移和旋转 例3.1:已知坐标系A和其对基系的微分平移和旋转, 求微分变换dA. 解:(3-88) kji kji A 01.00 5.001 , 1000 0010 5001 10100 d 0000 5 . 01 . 000 0000 101 . 00 1000 0010 5001 10100 0000 5 . 0001 . 0 0000 11 . 000 0000 5 . 0001 . 0 0000 11 .

31、000 AdA Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 1。微分平移和旋转 坐标系A的微分变化 Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 2。微分运动的等价变换 目的：把一个坐标系内的位姿变换到另一坐标系内 由 有： T TdTTdT TT T 1 1000 10000000 0 0 0 1 ap op np zyx zyx zyx zzzz yyyy xxxx zxy yxz xyz aaa ooo nnn T paon paon paon d d

32、 d T Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 2。微分运动的等价变换 与（3-89）元素对应相等，有 )943( 0000 )(0 )(0 )(0 1 adapno odopna ndnpoa TT T )963(, )( )953()( )( aon adapd odopd ndnpd z T y T x T z T y T x T Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 2。微分运动的等价变换 z y x z y x zyx zyx zyx

33、 zyxzyx zyxzyx zyxzyx z T y T x T z T y T x T d d d aaa ooo nnn apapapaaa opopopooo npnpnpnnn d d d 000 000 000 )()()( )()()( )()()( Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 2。微分运动的等价变换 d R pSRRd T TT T T 0 )( 0 0 0 )(, xy xz yz zzz yyy xxx pp pp pp pS aon aon aon R Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 2。微分运动的等价变换 例3-2：在例1中，求坐标系A的等价微分平移和旋转 Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 3。变换式中的微分关系 Robotics运动学运动学 3.4 3.4 机器人的雅可比公式机器人的雅可比公式 3.4.1 机器人的微分运动 3。变换式中的微分关系 由上图，有 )1043()( 1111111 AB