机器人课程辅导材料-SK6机器人运动学_919007295

1    机器人工程基础及应用课程辅导 材料 关于××同志的政审材料调查表环保先进个人材料国家普通话测试材料农民专业合作社注销四查四问剖析材料 MOTOMAN-SK6机器人运动学 1.SK6机器人正运动学 MOTOMAN-SK6机器人，如图 1所示，具有 6个轴，分别为：S轴、L轴、U轴、R轴、 B轴、T轴。 图 1 MOTOMAN-SK6工业机器人 设机器人各轴编码器脉冲数值为 in ， 1,2,...,6i = 。机器人各轴转动角度为 iϕ ， 1,2,...,6i = 。 已知两者的比例系数 ik ， , , , , ,i S L U R B T= 。则有： i i j n k ϕ = , ( 1, 2,...,6i = , , , , , ,j S L U R B T= ) (1) 脉冲坐标值与驱动角度之间的比例系数如 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 1所示。 ik ( , , , , ,i S L U R B T= ) 表 1 SK6机器人各轴脉冲值与关节驱动角比例系数 Sk Lk Uk 1777.77966699 1788.87915786 1788.88968788 Rk Bk Tk 2    909.08585157 727.27685995 415.57775913 机器人的正运动学问题可以表述为：已知：机器人末端到达某点时的六个脉冲值。求：此 时机器人末端坐标系相对于机器人坐标系的转换矩阵。 按照 D-H方法在 SK6机器人的各个关节上建立连杆坐标系，如图 2所示。 图 2 SK6机器人连杆坐标系建立 根据 D-H方法，各关键参数如表 2所示。其中，a表示公共法线距离，即连杆长度； iα 为 连杆扭角， id 为两连杆距离， iθ 为两连杆夹角， iϕ 为关节电机实际转角（相对于零位置）。 表 2 用 D-H方法建立 SK6机器人连杆坐标系的连杆参数 i a /mm id /mm iα /° iθ /° 1 150 0 -90 1ϕ 2 450 0 180 2ϕ -90 3 115 0 -90 2 3ϕ ϕ+ 4 0 -735 90 4ϕ 3    5 0 0 -90 5ϕ -90 6 0 -95 180 6θ 由所建立的机器人连杆坐标系，由于： 1 c c s s s c s c c s c 0 s c 0 0 0 1 i i i i i i i i i i i i ii i i i i a a s A d θ α θ α θ θ θ α θ α θ θ α α − ⎡ − ⎤⎢ ⎥−⎢ ⎥= ⎢ ⎥⎢ ⎥⎢ ⎦⎣ (2) 其中，s代表 sin，c代表 cos，下同。 可以得到机器人的运动学方程： 0 1 2 3 4 5 6 1 2 3 4 5 6T A A A A A A= (3) 2.SK6机器人的逆运动学 SK6机器人的逆运动学问题可以描述为：已知机械手末端坐标系{E}相对于机器人末端 坐标系{R}的转换矩阵 ERT 。求在末端到达末端坐标系中确定一点时，六个轴的脉冲值。将运 动方程写为： 6 1 2 3 4 5 6 0 0 0 1 x x x x y y y y z z z z n o a p n o a p T A A A A A A n o a p ⎡ ⎤⎢ ⎥⎢ ⎥= =⎢ ⎥⎢ ⎥⎢ ⎦⎣ (4) (1) 求 1φ 1 1 1 6 6A T A − −⋅ ⋅ = 1 1 1 1 1 c s 0 0 0 1 0 s c 0 0 0 0 0 1 aφ φ φ φ ⎡ − ⎤⎢ ⎥−⎢ ⎥⎢ ⎥−⎢ ⎥⎢ ⎦⎣ ⎢⎢ ⎢⎢ ⎢ ⎣ ⎡ ⎥⎥ ⎥⎥ ⎦ ⎤ 1000 zzzz yyyy xxxx paon paon paon 6 6 6 6 6 c s 0 0 s c 0 0 0 0 1 0 0 0 1 d φ φ φ φ ⎡ ⎤⎢ ⎥−⎢ ⎥⎢ ⎥−⎢ ⎥⎢ ⎦⎣ (5) 发现第四列为： 1 1 1 6 1 1 6 1 1 6 1 1 c s ( c s ) c s ( c s ) 1 x y x y z z y x y x a p p d a a d a p p p d a a φ φ φ φ φ φ φ φ ⎡− + + + + ⎤⎢ ⎥− −⎢ ⎥⎢ ⎥− + −⎢ ⎥⎢ ⎦⎣ 计算 2 3 4 5A A A A ，发现第 3行、第 4列元素为 0。所以，有方程 4    1 1 6 1 1c s ( c s )y x y xp p d a aφ φ φ φ− + − ＝0 有： 61 6 tan y y x x p d a p d a φ += + ， 1 6 6atan2( , )y y x xp d a p d aφ = + + (6) (2) 求 3φ 的计算值 计算 1 1 12 1 6A A TA− − − ，得到第四列元素为： 6 2 1 1 1 6 1 6 1 2 2 1 1 6 1 6 1 1 2 6 2 1 1 6 1 1 ( ) c ( c s c s )s ( c s c s )c ( )s c s ( c s ) 1 z z x y x y x y x y z z y x y x p d a a p p d a d a a p p d a d a a p d a p p d a a φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ ⎡ + + − + + + + − ⎤⎢ ⎥− − − − + + +⎢ ⎥⎢ ⎥− + + − +⎢ ⎥⎢ ⎦⎣ 计算 3 4 5A A A 得第四列： 3 3 4 3 4 3 3 3 c s s s 0 1 a d d a φ φ φ φ ⎡ − ⎤⎢ ⎥+⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎦⎣ 由上面两列的前两行相等，可得方程组 6 2 1 1 1 6 1 6 1 2 2 3 3 4 3 1 1 6 1 6 1 1 2 6 2 4 3 3 3 ( ) c ( c s c s )s c s ( c s c s ) c ( )s s s z z x y x y x y x y z z p d a a p p d a d a a a d p p d a d a a p d a d a φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ + + − + + +⎧⎪+ − = −⎪⎨ − − − − +⎪⎪ + + = +⎩ (7) 令 2 2 2 2 2 1 2 2 3 4 22 M M a a dM a + − − −= , 2 2 1 3 4r a d= + , 其中， 1 6z zM p d a= + , 2 1 1 1 6 1 6 1c s c sx y x yM a p p d a d aφ φ φ φ= − + + + + , 则 3 3 4 3c sa d Mφ φ− = (8) 于是 3 1 s( ) M r φ β+ = , 2 3 2 1 c( ) 1 M r φ β+ = ± − 其中， 3 4 tan a d β = − . 可以解得： 5    2 3 3 42 1 1 atan2( , 1 ) atan2( , )M M a d r r φ = ± − − − (9) (3) 求 2φ 令 1 3 3 4 3 2c sD a d aφ φ= − + , 2 4 3 3 3s sD d aφ φ= + . 根据上式，有 1 2 2 2 1D c D s Mφ φ+ = 令 2 22 1 2r D D= + ，于是有 2 1 1 2 1 22 2 2 atan2( , 1 ) atan2( , )M M D D r r φ = ± − − (10) (4) 求 4φ 根据 3606103 )( RRR =− ,分别计算左右两式，得 右边＝ 4 6 5 4 6 6 4 4 5 6 4 5 6 4 5 4 6 4 6 4 5 6 5 4 6 5 5 6 5 c c s s s c s c s s c c c s s c s c c s s s c s c c c s s φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ ⎡ − + − ⎤⎢ ⎥− − + −⎢ ⎥⎢ ⎥− − − ⎦⎣ 左边＝ ⎢⎢ ⎢⎢ ⎣ ⎡ ⎥⎥ ⎥⎥ ⎦ ⎤ ′′′ ′′′ ′′′ zzz yyy xxx aon aon aon 令 2 3sin( )S φ φ= − ，以及 2 3cos( )C φ φ= − ，有如下等式： 1 1( c s )x x y zn n n S n Cφ φ′ = + + , 1 1s cy x yn n nφ φ′ = − + , 1 1( c s )z x y zn n n S n Cφ φ′ = − + + , 1 1( c s )x x y zo o o S o Cφ φ′ = + + , 1 1s cy x yo o oφ φ′ = − + , 1 1( c s )z x y zo o o S o Cφ φ′ = − + + , 1 1( c s )x x y za a a S a Cφ φ′ = + + , 1 1s cy x ya a aφ φ′ = − + , 1 1( c s )z x y za a a S a Cφ φ′ = − + + . 根据（1,3）、（2,3）元素对应相等，有 4 5 1 1 5 4 1 1 c c ( c s ) c s s c x x y z y x y a a a S a C a a a φ φ φ φ φ φ φ φ ′⎧− = = + +⎪⎨ ′− = = − +⎪⎩ (11) 6    解得： 4 =atan2( , )y xa aφ ′ ′− − (12) (5) 求 5φ 根据 564536134 )( RRRR =− ，左边计算得到第三列： 4 4 4 4 c s s c x y z x y a a a a a φ φ φ φ ′ ′⎡ ⎤+⎢ ⎥′⎢ ⎥⎢ ⎥′ ′⎢ ⎥− ⎦⎣ . 右边第三列为： 5 5 c s 0 φ φ ⎡− ⎤⎢ ⎥−⎢ ⎥⎢ ⎥⎦⎣ . 对应项相等，所以有 5 4 4atan2( , ( c s ))z x ya a aφ φ φ′ ′ ′= − − + (13) (6) 求 6φ 根据 564536134 )( RRRR =− ，左边计算的第三行： 4 4 4 4 4 4s c s c s cx y x y x yn n o o a aφ φ φ φ φ φ′ ′ ′ ′ ′ ′⎡ ⎤− − −⎢ ⎦⎣ 右边计算的第三行： ][ 0cossin 66 φφ− ，对应项相等，于是有 6 4 4 4 4atan2( s c , s c )x y x yn n o oφ φ φ φ φ′ ′ ′ ′= − + − (14) (7) 求 3φ 的机器人内部数值 U轴实际驱动角应为： 3 3 2φ φ φ′= − (15) 由此，求解得到全部驱动角，再利用式(1),可以计算出各轴的脉冲值。