图书分拣报告

图书分拣实验报告

一、实验要求

二、实验过程

  Stanford机械手臂是斯坦福大学研发一款机器人，斯坦福机械手臂是一个拥有6个自由度的球坐标手臂，即头两个关节是旋转的，第三个关节是滑动的，最后三个腕关节全是旋转关节。斯坦福机器人如下图所示：

2.1 建立坐标系

采用D-H法建立坐标系，简述如下：
  （1）确定Z轴：对于旋转关节，z轴按照右手螺旋定则旋转的方向，旋转角θ为关节变量；对于滑动关节，z轴为沿着直线运动的方向，连杆长度是关节变量；
  （2）确定x轴：定义x_n轴的方向为z_(n-1)和z_n轴的公垂线方向；有以下特别情况：
   若z_(n-1)和z_n平行，此时有无数条公垂线，选取与前一关节的公垂线共线的公垂线；
  若z_(n-1)和z_n相交，此时没有公垂线，选取两条z轴叉积的方向作为x轴的方向。
 按照上述方法，建立如下坐标系：

2.2 D-H参数表

  D-H参数表分两种，一种是标准的D-H表，一种是改进的D-H表。以下采用的是标准的D-H表，根据上述信息，按照如下四个变换规则，可以确定D-H表中的值：

  Stanford机械手臂的旋转轴的技术参数经过调试得出关节极限，如下。可计算的D-H表为：

可知，由于存在5个旋转关节，1个伸缩关节，因此存在六个未知变量，即θ₁ ~ θ₆（除去θ₃），d3也是一个变量。

2.3 斯坦福机器人正运动学推导

  分析可知,依次进行坐标系之间的变换 ⁿ T_(n+1)称为A_(n+1)是四个运动变换矩阵的乘积

化简结果为

故关节0与关节1之间的变换可以简化为：

余下关节为：





得到以上变换的A矩阵，在机器人的基座与手之间的总变换则为：

代入数据可得：

2.4 斯坦福机器人逆运动学推导

  机器人逆运动学是求解机器臂从工作空间到关节空间的变换，一般可分为数值解法或者封闭解法（解析解）。由于解析解求解较快，目前设计的机器人多采用特定结构(正交关节轴有多个α为0或者±90°)，可以进行解析解的求解。本文采用解析解。
  为了求这些角度利用逆变换A₁^-1左乘方程 (1) 两边, 得到：

  分别把计算出的4*4矩阵参数代入，不断利用逆变换A_n^(-1)左乘方程两边，再分析比较，调用matlab中的solve函数进行参数运算求解，可得出其余角度，可得出左式为：

由于右边式子过长，故不做展示。





即所有求解结果如下：

2.5 绘制机器人工作空间

  使用MATLAB进行编程，参照D-H表计算旋转矩阵，利用三个for循环，可得仿真效果图为：

2.6 路径仿真

 通过设置书本与AGV之间的位置，两点确定一条直线，求出该直线的点，让机器人沿着此路径进行移动，可实现路径仿真。
效果图如下：

三、代码展示

主函数

clear all

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

th1=0;th2=0;th3=0;th4=0;th5=0;th6=0;th7=0;

stp=30;
d3=200;
%% 关节移动
%%关节1移动
for i=90:stp:360  
     [Link]=DHfk3Dof_Lnya(th1+i,th2,th3,th4,th5,th6,d3,1)
end
%%关节2移动
for i=150:-stp:-90 
     [Link]=DHfk3Dof_Lnya(th1,th2+i,th3,th4,th5,th6,d3,1)
end
%%关节3伸缩移动
for i=0:stp+100:1800  
     [Link]=DHfk3Dof_Lnya(th1,th2,th3,th4,th5,th6,d3+i,1)
end
%%关节4移动
for i=180:-stp:0  
     [Link]=DHfk3Dof_Lnya(th1,th2,th3,th4+i,th5,th6,d3,0)
end
%%关节5移动
for i=180:-stp:0  
     [Link]=DHfk3Dof_Lnya(th1,th2,th3,th4,th5+i,th6,d3,1)
end
%%关节6移动
for i=180:-stp:0  
     [Link]=DHfk3Dof_Lnya(th1,th2,th3,th4,th5,th6+i,d3,1)
end
%%

%% 绘制工作空间
x=[];y=[];z=[];
for i=90:stp:360
   for j=150:-stp:-90
       for k=0:stp+100:1800
           [Link]=DHfk3Dof_Lnya(th1+i,th2+j,th3,th4,th5,th6,d3+k,1)
           x=[x Link(7).p(1,1)];
           y=[y Link(7).p(2,1)];
           z=[z Link(7).p(3,1)];
       end
   end
end
plot3(x,y,z,"o");
           

%% 逆解路径运动
num=1;
for i=1:20
    x(num)=600;
    y(num)=-500;
    z(num)=100*i;
    [th1,th2,th3,th4,th5,th6,d3]=IK_3DOF_Rob_Lnya(100,x(num),y(num),z(num));
    num=num+1;
    th1=th1*ToDeg;th2=th2*ToDeg;th3=th3*ToDeg;
    th4=th4*ToDeg;th5=th5*ToDeg;th6=th6*ToDeg;
    plot3(x,y,z,'o'); hold on;
    DHfk3Dof_Lnya(th1,th2,th3,th4,th5,th6,d3,0);
    th1=0;th2=0;th3=0;th4=0;th5=0;th6=0;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 移动书本
num=1;
for t=1:-0.1:0  
    x=(750-155)*t+155;
    y=100*t-100;
    z=(1000-300)*t+250;
    [th1,th2,th3,th4,th5,th6,d3]=IK_3DOF_Rob_Lnya(100,x,y,z); 
    th1=th1*ToDeg;th2=th2*ToDeg;th3=th3*ToDeg;
    th4=th4*ToDeg;th5=th5*ToDeg;th6=th6*ToDeg;
    DrawBook(x,y,z+100);
    DHfk3Dof_Lnya(th1,th2,th3,th4,th5,th6,d3,1) 
    th1=0;th2=0;th3=0;th4=0;th5=0;th6=0;
end

正运动

function [Link]=DHfk3Dof_Lnya(th1,th2,th3,th4,th5,th6,d3,fcla)
%% 求正解，进行姿态的转换，画图，可修改参数为Matrix_DH_Ln(i);的i和转换for循环里的i
global Link
ToDeg = 180/pi;
ToRad = pi/180;
UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';

Build_3DOFRobot_Lnya
radius    = 20;
len       = 80;
joint_col = 0;

 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(4).dz=d3;


for i=1:7
    Matrix_DH_Ln(i);
end
for i=2:7
      Link(i).A=Link(i-1).A*Link(i).A;%右乘4*4变换位姿矩阵
      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;
      DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;
end
grid on;
view(134,12);
axis([-2000,2000,-2400,2400,-200,2200]);

%% 绘制书架、书和坐标轴
PlotCuboid([-100,-100,0],[200 200 10]);%% 绘制机器人底座
PlotCuboid([600,-500,0],[300 1000 2000/6]);
PlotCuboid([600,-500,2000*1/6],[300 1000 2000/6]);
PlotCuboid([600,-500,2000*2/6],[300 1000 2000/6]);
PlotCuboid([600,-500,2000*3/6],[300 1000 2000/6]);
PlotCuboid([600,-500,2000*4/6],[300 1000 2000/6]);
PlotCuboid([600,-500,2000*5/6],[300 1000 2000/6]);
PlotCuboid([10,-500,0],[300 300 300]);%% 绘制AGV

DrawFrame(Link(7).R, Link(7).p(1:3,1),0)
%%

xlabel('X');
ylabel('Y');
zlabel('Z');
drawnow;
pause(0.001);
% pic=getframe;
if(fcla)
    cla;
end
end

逆运动

function [th1,th2,th3,th4,th5,th6,d3]=IK_3DOF_Rob_Lnya(d2,px,py,pz)
%%作用：求逆解算出各个th
ToDeg = 180/pi;
ToRad = pi/180;

nx=1;ny=0;nz=0;
ox=0;oy=1;oz=0;
ax=0;ay=0;az=1;

r=sqrt((px)^2+(py)^2);
th1=atan2(py,px)-atan2(d2,sqrt(r^2-(d2)^2));   
C1=cos(th1);
S1=sin(th1);
th2=atan2(C1*px+S1*py,pz);
C2=cos(th2);
S2=sin(th2);
d3=S2*(C1*px+S1*py)+C2*pz;
th3=0;
th4=atan2(-S1*ax+C1*ay,C2*(C1*ax+S1*ay)-S2*az);
C4=cos(th4);
S4=sin(th4);
x5=C4*(C2*(C1*ax+S1*ay)-S2*az)+S4*(-S1*ax+C1*ay);
y5=S2*(C1*ax+S1*ay)+C2*az;
th5=atan2(x5,y5);
C5=cos(th5);
S5=sin(th5);
x6=C4*(C2*(C1*ox+S1*oy)-S2*oz)+S4*(-S1*ox+C1*oy);
y6=S2*(C1*ox+S1*oy)+C2*oz;
S6=-C5*x6+S5*y6;
C6=-S4*(C2*(C1*ox+S1*oy)-S2*oz)+C4*(-S1*ox+C1*oy);
th6=atan2(S6,C6);

end

建立DH表

%Build Robot by D_H methods
%%用DH表构建机器人模型，存到link结构体里面
% function aa=Build_3DOFRobot_Lnya(a)
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', 50, 'dx', 0, 'alf',0*ToRad,'az',UZ