六自由度运动方程计算.doc

机器人运动学分析 一 机器人齐次坐标系的建立 D—H坐标图 二 机器人正运动学分析 杆件 变量为转角 n (图示角度值) 偏距dn(mm) 扭角 n 杆长ln(mm) 1 1(0) d1=0 1=90 l1=0 2 2(90 ) d2=0 2=0 l2= 3 3(0) d3=0 3=90 l3=0 4 4(0) d4= 4=-90 l4=0 5 5(0) d5=0 5=90 l5=0 6 6(0) d6=0 6=0 l6=0 q q a q a q éC - S C S S l C ù n n n n n n n ê ú S C C - C S l S q q a q a q ê ú A a a = n n n n n n n n ê 0 S C d ú n n n ê ú 0 0 0 1 ? ? éC 0 S 0ù 1 1 ê ú S 0 - C 0 ê ú A = , 1 1 1 ê 0 0ú 0 1 ê ú 0 0 0 1 ? ? éC - S 0 l C ù 2 2 2 2 ê ú S C 0 l S ê ú A = , 2 2 2 2 2 ê 0 0 1 0 ú ê ú 0 0 0 1 ? ? éC 0 S 0ù 3 3 ê ú S 0 - C 0 ê ú A = , 3 3 3 ê 0ú 0 1 0 ê ú 0 0 0 1 ? ? 1 éC 0 - S 0 ù 4 4 ê ú S 0 C 0 ê ú A = , 4 4 4 ê 0 - 1 0 d ú 4 ê ú 0 0 0 1 ? ? éC 0 S 0ù 5 5 ê ú S 0 - C 0 ê ú A = , 5 5 5 ê 0 1 0 0ú ê ú 0 0 0 1 ? ? éC - S 0 0ù 6 6 ê ú S C 0 0 ê ú A = , 6 6 6 ê 0 1 0ú 0 ê ú 0 0 0 1 ? ? 由连杆 6 开始,依次导出下列方程: 5T 6 = A 6 = éC - S 6 6 ê S C ê 6 6 ê 0 0 ê 0 0 ? 0 0 1 0 0ù ú 0 ú 0ú ú 1 ? 4T 6 = éC C - C S 5 6 5 6 ê S C - S S ê A A = 5 6 5 6 5 S C 6 ê 6 6 ê 0 0 ? - S 5 C 5 0 0 0ù ú 0 ú 0ú ú 1 ? 3T 6 = éC C C - S S - C C S - S C C S 0 4 5 6 4 6 4 5 6 4 6 4 5 ê S C C + C S - S C S + C C S S 0 ê A A A = 4 5 6 4 6 4 5 6 4 6 4 5 4 - S C S S C d 5 6 ê 5 6 5 6 5 4 ê 0 0 0 1 ? ù ú ú ú ú ? 2T 6 = A3 A A A 4 5 6 = éC (C C C - S S ) - S S C 3 4 5 6 4 6 3 5 6 ê S (C C C - S S ) + C S C ê 3 4 5 6 4 6 3 5 6 ê S C C + C S 4 5 6 4 6 ê 0 ? - - C (C C S 3 4 5 6 + + S C ) 4 6 0 S (C C S 3 4 5 6 - S C S 4 5 6 S S S 3 5 6 C S S 3 5 6 C C S + - 3 4 5 S C S 3 4 5 S S 4 5 0 S C 3 5 C C 3 5 S d ù 3 4 ú - C d 3 4 ú 0 ú ú 1 ? 1T 6 = A2 A A A A 3 4 5 6 = éC (C C C - S S )- S S C 23 4 5 6 4 6 23 5 6 ê S (C C C - S S ) + C S C ê 23 4 5 6 4 6 23 5 6 ê S C C +C S 4 5 6 4 6 ê 0 ? - - C (C C S + + 0 S C ) + 4 6 S C ) - 4 6 + C C 4 6 23 4 5 6 S (C C S 23 4 5 6 - S C S 4 5 6 S S S 23 5 6 C S S 23 5 6 C C S + - 23 4 5 S C S 23 4 5 S S 4 5 0 S C 23 5 C C 23 5 l C + S d ù 2 2 23 4 ú l S - C d 2 2 23 4 ú 0 ú ú 1 ? 0T 6 = A1 A A A A A 2 3 4 5 6 é C C C C C S S S S C S S C C 1[ 23( - ) - ] + ( ê S C C