PUMA560机器人运动学分析

PUMA560机器人运动学分析

——基于matlab程序的运动学求解及仿真

求解PUMA560正向运动学解。 求解PUMA560逆向运动学解。 求解PUMA560的雅克比矩阵。 利用GUI创建运动分析界面。

姓 名： 学 号： 学 院： 专 业： 年 级 指导教师：

***

201100800*** 机电与信息工程学院 机械设计制造及其自动化 2011 **

求解PUMA560正向运动学解

在已知PUMA560各关节连杆DH参数，以及给定相应的关节变量之后，可以通过正向运动学求解出机械手末端抓手在基系内的位姿。从而利用输入不同的关节变量组合，实现对PUMA560机器人的准确控制。

以下是利用matlab编写的求解PUMA560正向运动学解的函数zhenjie.m:

function T=zhenjie(c1,c2,c3,c4,c5,c6) %求puma560正解

a2=431.8;a3=20.32;d2=149.09;d4=433.07;

c1=c1/180*pi;c2=c2/180*pi;c3=c3/180*pi;c4=c4/180*pi; c5=c5/180*pi;c6=c6/180*pi; A1=[cos(c1),-sin(c1),0,0; sin(c1),cos(c1),0,0; 0,0,1,0;0,0,0,1]; A2=[cos(c2),-sin(c2),0,0; 0,0,1,d2;-sin(c2),-cos(c2),0,0; 0,0,0,1];

A3=[cos(c3),-sin(c3),0,a2; sin(c3),cos(c3),0,0;

0,0,1,0;0,0,0,1]; A4=[cos(c4),-sin(c4),0,a3; 0,0,1,d4;-sin(c4),-cos(c4),0,0; 0,0,0,1];

A5= [cos(c5),-sin(c5),0,0; 0,0,-1,0;sin(c5),cos(c5),0,0; 0,0,0,1];

A6=[cos(c6),-sin(c6),0,0; 0,0,1,0;-sin(c6),-cos(c6),0,0; 0,0,0,1];

T=A1*A2*A3*A4*A5*A6 end

其中c1,c2,c3,c4,c5,c6，为分别输入的各关节变量，即连杆1、连杆2、连杆3、连杆4、连杆5、连杆6的关节转角，直接利用关节矩阵相乘得到机械手末端抓手在基系内的位姿。

当在matlab命令窗口输入：zhenjie(90,0,-90,0,0,0)

可得输出结果：

求解PUMA560逆向运动学解

在已知PUMA560各关节连杆DH参数以及末端抓手相对基系的齐次变换矩阵时，通过各连杆矩阵之间的关系，可以求解出各连杆的关节变量，即在控制PUMA560末端抓手到达空间某一位置时，可以通过逆向运动学求出所需输入的各关节角度。

通过程序计算，可以得到8组解，以下即为可以求出8组解的程序；在实际求解时，可通过设定各关节变量的取值范围，过滤掉一些解，从而得到所需的解。

以下是利用matlab编写的求解PUMA560逆向运动学解的程序fanjie.m: clc;

%T06=input('Enter

x=>[nx,ox,ax,px;ny,oy,ay,py;nz,oz,az,pz;0,0,0,1]'); nx=0;ox=1;ax=0;px=-149.09; ny=0;oy=0;ay=1;py=864.87;

matrix

nz=1;oz=0;az=0;pz=20.32; d2=149.09;%input('d2=?'); d4=433.07;%input('d4=?'); a2=431.8;%input('a2=?'); a3=20.32;%input('a3=?');

x11=atan2(py,px)-atan2(d2,-sqrt(px^2+py^2-d2^2)); x12=atan2(py,px)-atan2(d2,sqrt(px^2+py^2-d2^2)); k=(px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2); x31=atan2(a3,d4)-atan2(k,-sqrt(a3^2+d4^2-k^2)); x32=atan2(a3,d4)-atan2(k,sqrt(a3^2+d4^2-k^2));

y231=atan2(-(a3+a2*cos(x31))*pz+(cos(x11)*px+sin(x11)*py)*(a2*sin(x31)-d4),(-d4+a2*sin(x31))*pz+(cos(x11)*px+sin(x11)*py)*(a2*cos(x31)+a3));

y232=atan2(-(a3+a2*cos(x31))*pz+(cos(x12)*px+sin(x12)*py)*(a2*sin(x31)-d4),(-d4+a2*sin(x31))*pz+(cos(x12)*px+sin(x12)*py)*(a2*cos(x31)+a3));

y233=atan2(-(a3+a2*cos(x32))*pz+(cos(x11)*px+sin(x11)*py)*(a2*sin(x32)-d4),(-d4+a2*sin(x32))*pz+(cos(x11)*px+sin(x11)*py)*(a2*cos(x32)+a3));

y234=atan2(-(a3+a2*cos(x32))*pz+(cos(x12)*px+sin(x12)*py)*(a2*sin(x32)-d4),(-d4+a2*sin(x32))*pz+(cos(x12)*px+sin(x12)*py)*(a2*cos(x32)+a3));

本文来源：https://www.bwwdw.com/article/0yhd.html