工业机器人课程设计23655

1、工业机器人课程设计基于 Matlab 的工业机器人运动学和雅克比较矩阵求解精选文库目录PUMA560机器人简介 .4PUMA560机器人的正解 .51、确定 D-H 坐标系 .52、确定各连杆 D-H 参数和关节变量 .53、求出两杆间的位姿矩阵.54、求末杆的位姿矩阵 .65、 M ATLAB 编程 .76、验证.7PUMA560机器人的逆解 .81、求1 .82、求3 .83、求2 .94、求4 .105、求5 .106、求6 .117、解的多重性 .118、 M ATLAB 编程 .119、对于机器人解的分析 .12机器人的雅克比矩阵 .121、定义.122、雅可比矩阵的求法 .123、

2、微分变换法求机器人的雅可比矩阵.134、矢量积法求机器人的雅克比矩阵.145、 M ATLAB 编程 .15附录 .161、程序.162、三维图 .24-2精选文库摘要机器人学作为一门高度交叉的前沿学科，引起许多具有不同专业背景人们的广泛兴趣，对其进行深入研究， 并使其获得快速发展。 尤其是近年来各种新兴技术飞速发展 ,机械工业产品的自动化、高精度、重负载等性能指标变得越来越突出。因此在机器人学的计算中就要求更高的精度，计算机技术的发展很好的解决了这一问题。本文将以PUMA560 为例，利用个人电脑平台的Matlab 对其运动学的正解、逆解以及雅克比矩阵进行计算研究。关 键 词PUMA560M

3、atlab正解逆解雅克比矩阵微分变换法矢量积法ABSTRACTAs a highly interspersed subject, the robotics makes many people who major in different subject interest in it, research and develop it. Especially in recent years, with the rapid development of varieties of emerging technologies, mechanical products indexes of automat

4、ion，high precision and the re-load are becoming more and more outstanding. There is a need of greater precision in the calculation of robotics and computer technology makes it possible. In this paper we will use Matlab to research the kinematics problem and Jacobian array of PUMA560.KEY WORDSPUMA560

5、 Matlab Kinematics problem Positive-solution Inverse-solution Jacobian array Differential transformation Vector product transformation-3精选文库PUMA560 机器人简介PUMA560 是属于关节式机器人， 6 个关节都是转动关节，如图11 所示，前三个关节确定手腕参考点的位置，后三个关节确定手腕的方位。 和大多数工业机器人一样，后三个关节轴线交于一点。该点选作为手腕参考点，也选作为4 、5 、6 的原点。关节一的轴线为垂直方向，关节2 和关节 3 的轴线为水

6、平，且平行，距离为 a2 。关节 1 和关节 2 的轴线垂直相交，关节3 和关节 4 的轴线垂直交错。距离为 a3 。各个连杆坐标系如图11 所示，相应的连杆参数列于表12 中。其中，a2431.8mm，a320.32mm，d2149.09mm，d3433.07mm。在更进一步了解PUMA560机器人的转动角度问题时，我们先来定义一下PUMA560机器人的初始位姿。首先,定义机器人的初始位置.取大臂处于某一朝向时,作为腰关节的初始位置.大臂处在水平位置时,作为肩关节的初始位置.小-4精选文库臂处在下垂位置 ,关节轴线 Z4 和 Z0 平行时 ,作为肘关节的初始位置 .关节轴线 Z5和 Z3 平

7、行时 ,作为腕扭转关节的初始位置 .关节轴线 Z6 和 Z4 平行时 ,作为腕弯曲关节的初始位置 .抓手两个指尖的连线与大臂平行时 ,作为腕旋转关节的初始位置 .在上述初始位置的前下 ,各个关节的零点位置得到确定.PUMA560 机器人的正解1、确定 D-H 坐标系PUMA 560 的关节全为转动关节 :Zi 坐标轴 :沿着 i+1 关节的运动轴 ;Xi 坐标轴 :沿着 Zi 和 Zi-1 的公法线 ,指向离开 Zi-1 轴的方向 ;Yi 坐标轴 :按右手直角坐标系法则制定;连杆长度 ai; Zi 和 Zi-1 两轴心线的公法线长度 ;连杆扭角 i: Zi 和 Zi-1 两轴心线的夹角 ;两连

8、杆距离 di: Xi 和 Xi-1 两坐标轴的公法线距离 ; 两杆夹角 i :Xi 和 Xi-1 两坐标轴的夹角 ;2、确定各连杆 D-H 参数和关节变量确定各连杆 D-H 参数和关节变量：连杆 i变量 ad i变量范围ii-1i-1110°00-160 °160 °2-90 °0d2-225 °45 °230°a20-45 °225 °34-90 °a3d4-110 °170 °4590°00-100 °100 °56-90 °00-

9、266 °266 °63、求出两杆间的位姿矩阵第 i 连杆与第 i-1 连杆间的变换矩阵A i =Rot(x, i-1 )trans(ai-1, 0,0)Rot(z, i)trans(0,0,di )-5精选文库c is i0ai 1si ci 1c i c i 1s i 1s i 1di=c i s i 1c i 1c i 1dis i s i 10001相邻两个连杆间的位姿变换矩阵c 1s 100c 2s 20T1s 1c 1001T2000010s 2c 2000100c 3s 300c 4s 42T3s 3c 3003T4000010s 4c 4000100c 5s

10、 500c 6s 64T500105T600s 5c 500s 6c 60001000 01 00 00 10 01 00 00 10 01 00 00 14、求末杆的位姿矩阵0T60T1( 1) 1T2( 2 ) 2T3 ( 3) 3T4( 4) 4T5( 5 ) 5T6 ( 6)nxoxaxpx0T60T1 1T6nyoyaypynzozazpz0001由上面的矩阵，我们可以得到最终结果：-6精选文库nxc1c23c4c5 c6s4 s6s23s5 c6s1 s4c5c6c4 s6nys1c23c4 c5 c6s4 s6s23 s5 c6c1 s4c5c6c4s6nzs23 c4 c5c6

11、s4 s6c23s5c6oxc1c23c4c5s6s4 c6s23 s5 s6s1 c4c6s4c5s6oys1c23c4c5 s6s4 c6s23s5 s6c1 c4c6s4c5c6ozs23 c4c5s6s4 c6c23s5 s6axc1 c23c4 s5s23c5s1s4 s5ays1 c23c4 s5s23c5c1s4 s5azs23c4s5c23c5pxc1 a2c2a3c23d4 s23d2 s1pys1 a2 c2a3c23d4 s23d2c1pza3s23a2 s2d4c235、Matlab 编程运行 zhengjie.m，根据提示输入 16 的值，回车后的出 T 的解如下：T

12、=01.00000-149.0900001.0000864.87001.00000020.32000001.00006、验证190o ,20,390o , 40,5060a2431.8mm,d2149.09mm,a320.32mm,d4 433.07mm, d6 56.25mm010d20T6001a2d4100a30001由课本给出的验证公式进行所编程序的验证，经验证， 编程所得结果与课本给出验证公式得到的结果一致。进一步表明所编程序是正确的。-7精选文库PUMA560 机器人的逆解将 PUMA 560 的运动方程（ 3.64）写为 :nxoxaxpxnyoyaypy0T1( 1)1T2(

13、2)2T3( 3 ) 3T4 ( 4) 4T5( 5) 5T6 ( 6 )0T6ozazpznz0001若末端连杆的位姿已经给定,求关节变量16的值成为运动逆解。1、求 10T1 110T61T222T333T444T555T6 6c1s100nxoxaxpxs1c1 00nyoyaypy10010nzozazpzT600010001sin(1 )d2 /;cos(1 )1(d2 /) 2atan2 d2 ,d22111atan2(py , px)atan2(d2 ,p2xp2yd22式中 ,正、负号对应于1 的两个可能解。2、求3c1 pxs1 pya3c23d4s23a2c2pza3s23

14、d4c23a2 s2由以上两式的平方加上 -s1 px c1 pyd2 的平方可以得到：a3c3d4 s3k （ 2 2）-8精选文库在上式中， kp2xp2yp2za22a32d22d422a2式（ 22）中已经消去2 ，所以可以由三角代换求解得到3 的解。所以：在3 的表达式中正、负号对应于3 的两种可能解。3 atan2(a3 , d4 ) atan2(k, a32d42k 2 )3、求20T3 11,2,30T63T444T5 55T66c1c23s1c23s23a2c3nxoxaxpxc1s23s1s23c23a2 s3nyoyaypy3T6 （ 23）s1c10d2nzozazpz

15、00010001c4c5 c6s4 s6c4c5s6s4s6c4 s5a33T63T4 4T6s5 s6s5s6c5d4s4s5s6c4 s6s4 c5 s6c4c6s4s500001令矩阵方程 (23)两端的元素 (1,4)和 (2,4)分别对应相等 ,则得两方程 :c1 s23 pxs1c23 pys23 pza2c3a3c1s23 pxs1s23 pyc23 pza2 s3d4由以上两式可得23 的表达式：2323a tan2( a3 a2c3 ) pz( c1 px s1 py )(a2 s3 d4 ),( d4a2s3 ) p(c1 pxs1 py )(a2c3a3 )由求得的23

16、，可求出2 ：2233根据1和 3 解的四种可能组合可以得到相应的23 四种可能值，于是可得到的 2 四种可能解。-9精选文库4、求4c1c23s1c23s23a2c3nxoxaxpxc1 s23s1s23c23a2s3nyoyaypy3T6s1c10d2nzozazpz00010001令上式的矩阵方程的两端的元素(1,4)和(2,4)分别对应相等 ,则得两方程：axc1c23ays1c23az s23c4s5axs1ay c1 s4 s5当 s5 041atan2( ax s1ay c1 , axc1c23ay s1c23 azs23 )42atan2( ax s1ayc1 , axc1c2

17、3ays1c23 az s23 )当 S5=0 时,机械手处于奇异形位 .此时 ,关节轴 4 和 6 重合 ,只能解出 4 和 6 的和或差 .奇异形位可以由式 4 的表达式 中的 atan2的两个变量是否接近零来判别 .若都接近零 ,则为奇异形位 ,否则 ,不是奇异形位 .在奇异形位时 ,可任意选取 4 值,再计算相应的 6值。5、求5根据求出的 4 ，可以进一步解出5 ：0T4 11,2,3,40T64T5 55T6 64T65 , 6因为1 ，2 ，3 ，4 在前面均已解出，逆变换0T41(1,2, 3, 4) 为：c1c23c4s1 s4s1c23c4c1s4s23c4a2c3c4d2

18、 s4a3c4c1c23s4s1s4s1c23 s4c1c4s23s4a2c3s4d2c4a3s4c1s23s1s23c23a2s3d40001令矩阵方程两端的元素 (1,3)和(3,3)分别对应相等 ,则得两方程：axc1c23c4s1s4ay s1c23c4c1s4az s23c4s5axc1s23ays1s23azc23c5-10精选文库所以可以得到5 的最终表达式：5atan2( s5 , c5 )6、求60T5 11, 2, 3, 4, 5 0T65T66令矩阵方程两端的元素 (3,1)和(1,1)分别对应相等 ,则得两方程：s6nx (c1c23s4s1c4 )ny (s1c23s

19、4c1c4 )nz (s23 s4 )c6nx c1c23c4s1s4 c5c1s23s5 ny( s1c23c4c1s4 )c5s1 s23s5 nz( s23c4c5c23 s5 )得到最后6 的表达式：6atan2( s6 , c6 )7、解的多重性PUMA560 的运动反解可能存在8 种解，但是，由于结构的限制，例如各关节变量不能在全部360 度范围内运动， 有些解不能实现。 在机器人存在多种解的情况下，应选取其中最满意的一组解，以满足机器人的工作要求。8、Matlab 编程在 Matlab 中运行 nijie.m, 根据提示输入 nx , ox , ax , ny, oy, ay ,

20、nz , oz ,az 的值并回车， 可得i 的 8 组解值如下：90.0000-2.6918-84.6272 -180.00002.6810180.000090.0000-0.0000-90.000000.0000090.0000-2.6918-84.62720.0000-2.6810-0.000090.0000-0.0000-90.00000-0.00000-70.4385180.0000-84.6272104.762920.258174.3103-70.4385182.6918-90.000097.529219.738782.0067-70.4385180.0000-84.6272-75

21、.2371-20.2581-105.6897-70.4385182.6918-90.0000-82.4708-19.7387-97.9933-11精选文库9、对于机器人解的分析通过编程可以知道， 我们最终得到八组解。 然后对八组解进行分析， 对于 1的变化范围为从 1600 1600 ，所以程序中我们得到的两个解都是正确的。然后对2 进行分析，由于2 的角度变化范围是从2250 450 ，所以在我们所得到的结果中，后四组是超出2 的变化范围的， 所以我们可以舍去后四组解。 再逐个对3 、 4 、 5 、 6 进行角度分析，最终可获得适合的解。机器人的雅克比矩阵1、定义机械手的操作速度与关节速度

22、间的线性变换定义为机械手的雅可比矩阵。xx(q)x&J (q)q&Jij (q)xi ( q) , i 1,2,L ,6,j 1,2,L ,nq j2、雅可比矩阵的求法（ 1）矢量积法对移动关节vzi &,Jiziwqi00对转动关节vioiozipnqi , J izipn ,wzizii pnooi Ri pn（2）微分变换法-12精选文库对于转动关节 i, 相对连杆 i-1, 绕坐标系 i的 zi轴所作微分转动 d i ,其微分运动矢量为 (3-117),对应的夹持器的微分运动矢量为(3-118)：T dxpn z00T dypo zTd 0 ,0 d i(3 1

23、17) T dzpa zd i(3 118）01xnzTozyTazzpn于是 ,J(q)的第 i 列如下 :对转动关节 i ： T Jlip o p anz0znzT,Jozaz对移动关节 i ： T Jlioz, T Jai0az03、微分变换法求机器人的雅可比矩阵PUMA560 的 6 个关节都是转动关节 ,所以利用 (3-121)求取雅克比矩阵的列矢量。对于第 1 个关节来说 ,将 1T6 中的 n,o,a,p 向量代入式 (3-121)，得到雅克比矩阵的列矢量。T J1xT J1 yT J1 qT J1 zc23s5 c6s23 c4c5c6s4 s6s23 c4c5s6s4c6c2

24、3s5 s6s23c4 s5c23c5其中 T J1x 、 T T2 x 、 TT3x的表达式如下所示：T J1x(s4 s5s6c4s5 )(a3c23s23d4 )d2c23 c4 c5 c6s4 s6s23s5 s6 T J1y(s csc c)(a c23s d)d c( c c c6s s )ss s 45646323422345462356T J1zs s (a c23s d) d (c c s s c )45323422345235对于第 2 个关节来说 ,将 2T6 中的向量代入式(3-121),得到雅克比矩阵的列矢n,o,a,p-13精选文库量 TJ2qT J2 xT J2

25、y可以得到：TJ2 qT J2 zs4c5c6c4 s6s4c5 s6c4c6s4s5其中 T J2 x 、 T J 2 y 、 T J2 z 三个参数的表达式如下所示：同理 ,可求得：T J2 x s3 c4c5c6s4s6c3s5 s6 ( a3 c3s3d4a2 ) c3 c4c5c6s4 s6s3s5s6 ( a3s3c3d4 )T J2 y s3c4c5 s6s4s6c3 s5 s6 ( a3c3s3 d4a2 ) c3c4c5s6 s4 s6s3 s5 s6 (a3s3c3 d4 )T J2 z(c3c5s3 c4 s5 )(a3c3s3 d4a2 )(c3c4s5s3c5 )(a

26、3s3 c3 d4 )T J3 q , T J4 q , T J5 q , T J 6 qd4 c4c5 c6s4 s6a3 s5c6d4 c4c5s6s4c6a3 s5 s6T J3 qd4c4 s5a3 c6s4 c5c6c4 s6s4c5 s6c4c6s4s5000000T J 4 q00, T J6 q0, T J5 qs5c6s60s5 s6c60c501所以又以上六个 6 1矩阵便最后组成了66 的雅克比矩阵。4、矢量积法求机器人的雅克比矩阵PUMA560 的 6 个关节都是转动关节 ,因而其雅克比矩阵具有下列形式：-14精选文库z11 p60z22 p60Lz66 p60J qz

27、1z2Lz65、Matlab编程（ 1）用微分变换法求解雅克比矩阵在 Matlab 中运行 wfbh.m ，根据提示输入16的值并回车， 得雅克比矩阵如下：-864.870000000-149.090020.320020.32000000-864.8700-433.07000000-1.0000-1.00000-1.000000001.000001.00001.000000000（ 2）用矢量积法求解雅克比矩阵在 Matlab 中运行 slj.m ，根据提示输入16 的值并回车，得雅克比矩阵如下：-864.870000000-149.090020.320020.32000000-864.870

28、0-433.07000000-1.0000-1.00000-1.000000001.000001.00001.000000000从以上结果中我们可以看出其运行结果与用微分变换编程所得到的结果是一致的，进一步证明了所编写程序的正确性。-15精选文库附录1、程序1、运动学正解function zhengjie(c1,c2,c3,c4,c5,c6)clc;a2=431.8;a3=20.32;d2=149.09;d4=433.07;d6=56.25;c1=input('c1=');c2=input('c2=');c3=input('c3=');c4=in

29、put('c4=');c5=input('c5=');c6=input('c6=');T1=cosd(c1) -sind(c1) 0 0;sind(c1),cosd(c1),0,0;0,0,1,0;0 001; T2=cosd(c2),-sind(c2),0,0;0,0,1 d2;-sind(c2) -cosd(c2) 0 0;0 001; T3=cosd(c3) -sind(c3) 0 a2;sind(c3) cosd(c3) 0 0;0 010;-16精选文库0001;T4=cosd(c4) -sind(c4) 0 a3 ;0 0 1 d4;

30、-sind(c4) -cosd(c4) 0 0;0001;T5=cosd(c5) -sind(c5) 0 0;00-10;sind(c5) cosd(c5) 0 0;0001;T6=cosd(c6) -sind(c6) 0 0;0010;-sind(c6) -cosd(c6) 0 0;0001;T=T1*T2*T3*T4*T5*T6;disp('T=');disp(T)2、运动学逆解function nijie(T)clc;nx=input('nx=');ox=input('ox=');ax=input('ax=');ny=inp

31、ut('ny=');oy=input('oy=');ay=input('ay=');nz=input('nz=');oz=input('oz=');az=input('az=');px=input('px=');py=input('py=');pz=input('pz=');a2=431.8;a3=20.32;d2=149.09;d4=433.07;c1=atan2(py,px)-atan2(d2,sqrt(px*px+py*py-d2*d2),atan2(py,px)-atan2(d2,-sqrt(px*px+py*py-d2*d2); %求解 c1c1=c1/pi*180;-17精选文库k=(px*px+py*py+pz*pz-a2*a2-a3*a3-d2*d2-d4*d4)/(2*a2);c3=atan2(a3,d4)-atan2(k,sqrt(a3