七轴分拣机器人设计与MATLAB仿真

一、要求

  1、设计六个自由度以上的机器人，至少包括一个伸缩关节，且使机器人的工作空间能够满足下图所示的分拣工位。

  2、求机器人的完整逆解，基本要求是末端实现位姿逆解。
  3、利用逆解完成末端路径仿真。

二、建立坐标系，给出D-H参数表

三、推导正运动学，并写出七个齐次变换矩阵

  正运动学是指根据关节角求连杆的位姿。
  假设现在位于本地参考坐标系Xn-Zn,那么通过以下4步标准运动即可到达下一个本地参考坐标系Xn+1-Zn+1。
1、绕Z_n轴旋转θ_(n+1)，使得X_n 和X_(n+1)互相平行；
2、沿Z_n 轴平移d_(n+1) 距离，使得X_n 和X_(n+1) 共线；
3、沿已经旋转过的X_n轴平移a_(n+1)的距离，使得X_n 和X_(n+1)的原点重合；
4、将Z_n轴绕X_(n+1)轴旋转α_(n+1)，使得Z_n轴与Z_(n+1)轴对准；

关节n到关节n+1的坐标变换步骤如下图所示。

七个其次变换矩阵分别为：

四、每个关节的活动

五、工作空间

  工作空间可以覆盖整个篮子和传送带的抓取位置。

六、逆运动学

  机器人的逆运动学是求解机器人从工作空间到关节空间的变换。一般可分为数值解和解析解。这里使用的是数值解。即当正运动学的计算结果与目标值有差异时修正关节值，再循环。如下图所示。

其算法如下：
第一步：给定目标连杆的位姿（p_ref,R_ref）；
第二步：定义从躯干到目标连杆之间的关节角矢量q；
第三步：由正运动学计算目标连杆的位姿（p，R）；
第四步：计算目标连杆位姿的误差（△p，△R）=（p_ref-p, R_T*R_ref）； R_T表示矩阵R的转置
第五步：当误差（△p，△R）足够小时候，停止运算；
第六步：当误差（△p，△R）大于设定值时计算关节角的修正量△q；
第七步：q=q+△q，返回第三步。
下图是伪代码：

七、代码

DH_Table.m

% %Build Robot by D_H methods

ToDeg = 180/pi;
ToRad = pi/180;
UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';

Link= struct('name','Body' , 'th',  0, 'dz', 0, 'dx', 0, 'alf',0*ToRad,'az',UZ);     % az 
Link(1)= struct('name','Base' , 'th',  0*ToRad, 'dz', 0, 'dx', 0, 'alf',0*ToRad,'az',UZ);        %BASE to 1
Link(2) = struct('name','J1' , 'th',   0*ToRad, 'dz', 420, 'dx', 0, 'alf',-90*ToRad,'az',UZ);       %1 TO 2   
Link(3) = struct('name','J2' , 'th',  -90*ToRad, 'dz', 0, 'dx', 200, 'alf',0*ToRad,'az',UZ);    %2 TO 3     
Link(4) = struct('name','J3' , 'th',  0*ToRad, 'dz', 60, 'dx', 0, 'alf',-90*ToRad,'az',UZ);          %3 TO 4    
Link(5) = struct('name','J4' , 'th',  0*ToRad, 'dz', 300, 'dx', 0, 'alf',90*ToRad,'az',UZ);          %4 TO 5    
Link(6) = struct('name','J5' , 'th',  180*ToRad, 'dz', 0, 'dx', 0, 'alf',90*ToRad,'az',UZ);          %5 TO 6
Link(7) = struct('name','J6' , 'th',  0*ToRad, 'dz', 100, 'dx', 0, 'alf',0,'az',UZ);          %6 TO 7
Link(8) = struct('name','J7',  'th',  0*ToRad, 'dz',  30,  'dx', 0, 'alf', 0, 'az',UZ);     %7 TO E

Matrix_DH.m

function Matrix_DH(i) 
% Caculate the D-H Matrix
global Link      %声明全局变量

ToDeg = 180/pi;
ToRad = pi/180;


C=cos(Link(i).th);
S=sin(Link(i).th);
Ca=cos(Link(i).alf);
Sa=sin(Link(i).alf);
a=Link(i).dx;    %distance between zi and zi-1
d=Link(i).dz;    %distance between xi and xi-1

Link(i).n=[C,S,0,0]';    %增加结构体中的成员变量
Link(i).o=[-1*S*Ca,C*Ca,Sa,0]';
Link(i).a=[S*Sa, -1*C*Sa,Ca,0]';
Link(i).p=[a*C,a*S,d,1]';

Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
Link(i).A=[Link(i).n,Link(i).o,Link(i).a,Link(i).p];

DHfk_7Dbot.m

function A=DHfk_7Dbot(th1,th2,th3,th4,th5,th6,dz7,fcla)
% close all

global Link

DH_Table;        %构建DH表
radius    = 25;  %25
len       = 60;  %60
joint_col = 0;

%plot3(0,0,0,'ro'); 

 Link(2).th=th1*pi/180;
 Link(3).th=th2*pi/180;
 Link(4).th=th3*pi/180;
 Link(5).th=th4*pi/180;
 Link(6).th=th5*pi/180;
 Link(7).th=th6*pi/180;
 Link(8).dz=dz7+30;   &伸缩关节

p0=[0,0,0]';

for i=1:8
Matrix_DH(i);
end


for i=2:8

      Link(i).A=Link(i-1).A*Link(i).A;
      Link(i).p= Link(i).A(:,4);
      Link(i).n= Link(i).A(:,1);
      Link(i).o= Link(i).A(:,2);
      Link(i).a= Link(i).A(:,3);
      Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
      Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;
      plot3(Link(i).p(1),Link(i).p(2),Link(i).p(3),'rx');hold on;
     
      if i<=8
          DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;
      end 
end
DrawFrame(Link(8).R, [Link(8).p(1),Link(8).p(2),Link(8).p(3)]' );hold on;    %绘制机械臂末端坐标系

axis([-700,700,-700,700,-400,1000]);
xlabel('x');
ylabel('y'); 
zlabel('z');
grid on;
drawnow;

A=Link(8).A;
%判断是否清空坐标系
if(fcla)
    cla;
end
hold on
axis([-1000,1000,-1000,