基于matlab七轴可伸缩关节机械臂物品抓取

摘要

设计了一款七自由度的机械臂，其中伸缩关节为第七关节，在指定范围内，随机生成10个规定范围内随机尺寸，随机间隔小球，利用RRT避障算法，使机械臂可以无碰撞的提取小球。最后使用多项式轨迹规划运送小球至目标点。抓取小球的顺序按照小球体积大小，从大到小依次抓取。

连环图展示

机械臂介绍

此处仿真测试是在高4000mm，底面半径1000mm的空心圆柱体内测试，因此各有关距离、位置等参数的单位均以毫米为单位。机械臂运动最大速度为600mm/s，最大加速度为300mm/，旋转关节最大速度180°/s，最大加速度180°/s，末端伸缩关节量为15mm~170mm。机械臂各轴工作范围角度如表Ⅰ所示，共由七个关节，第七关节的旋转范围为-360°至360°并且可伸缩。

关节	工作范围
关节1	180°~180°
关节2	180°~180°
关节3	-245°~65°
关节4	-200°~200°
关节5	-115°~115°
关节6	-180°~180°
关节7	-360°~360° （可伸缩关节）

机械臂构型下图所示

matlab机械臂建模

正运动学使用DH法建模，并绘制坐标系和DH参数表，对于本设计的七轴机械臂，采取如下图所示坐标系。Xi，Zi的下标i表示第i个关节轴的坐标系。

建立DH参数表

	θi	dz	da	αi
T1	pi/2	350	0	pi/2
T2	0	0	400	0
T3	0	0	400	0
T4	0	0	400	0
T5	0	0	400	0
T6	pi/2	0	0	-pi/2
T7	0	L(15~170mm)	0	0

DH参数表输入matlab中，第一个子程序。

% %Build Robot by D_H methods
%建立DH参数表
global D5
ToDeg = 180/pi;%转为角度制
ToRad = pi/180;%转为弧度制
UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';
%J1为关节名称，
%th为绕z轴旋转使x轴平行的角度
%dz为沿z轴平移使x轴共线的移动距离
%dx为沿x轴平移使两个参考坐标系原点重合的移动距离
%alf为绕x轴旋转使z轴重合的角度
Link= struct('name','Body' , 'th',  0, 'dz', 0, 'dx', 0, 'alf',0*ToRad,'az',UZ);     % 结构体数组
Link(1)= struct('name','Base' , 'th',  0*ToRad, 'dz', -450, 'dx', -500, 'alf',0*ToRad,'az',UZ); %Base To 1，世界坐标系
Link(2) = struct('name','J1' , 'th',  90*ToRad, 'dz', 350, 'dx', 0, 'alf',90*ToRad,'az',UZ);       %1 TO 2
Link(3) = struct('name','J2' , 'th',  0*ToRad, 'dz', 0, 'dx', 400, 'alf',0*ToRad,'az',UZ);    %2 TO 3
Link(4) = struct('name','J3' , 'th',  0*ToRad, 'dz', 0, 'dx', 400, 'alf',0*ToRad,'az',UZ);     %3 TO 4
Link(5) = struct('name','J4' , 'th',  0*ToRad, 'dz', 0, 'dx', 400, 'alf',0*ToRad,'az',UZ);      %4 TO 5
Link(6) = struct('name','J5' , 'th',  0*ToRad, 'dz', 0, 'dx',400, 'alf',0*ToRad,'az',UZ);     %5 TO 6
Link(7) = struct('name','J6' , 'th',  90*ToRad, 'dz', 0, 'dx', 0, 'alf',-90*ToRad,'az',UZ);     %6 TO 7
Link(8) = struct('name','J7' , 'th',  0*ToRad, 'dz', D5, 'dx', 0, 'alf',0*ToRad,'az',UZ);     %7 TO E

绘制机械臂

画关节连杆子程序

function Connect3D(p1,p2,option,pt)
h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);
set(h,'LineWidth',pt)

画关节轴子程序

function h = DrawCylinder(pos, az, radius,len, col)
%radius表示圆柱半径，len表示圆柱高，az表示朝向，pos表示位置，col表示颜色
%调用示例：shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);%中心点在（0，0，0），侧面与Y轴平行。
% draw closed cylinder
%
%******** rotation matrix
az0 = [0;0;1];
ax  = cross(az0,az);
ax_n = norm(ax);
if ax_n < eps 
	rot = eye(3);
else
    ax = ax/ax_n;
    ay = cross(az,ax);
    ay = ay/norm(ay);
    rot = [ax ay az];
end
%********** make cylinder
% col = [0 0.5 0];  % cylinder color这里调整颜色
a = 40;    % number of side faces这里调整关节轴圆柱形状由多少曲面围成
theta = (0:a)/a * 2*pi;
x = [radius; radius]* cos(theta);
y = [radius; radius] * sin(theta);
z = [len/2; -len/2] * ones(1,a+1);
cc = col*ones(size(x));
for n=1:size(x,1)
   xyz = [x(n,:);y(n,:);z(n,:)];
   xyz2 = rot * xyz;
   x2(n,:) = xyz2(1,:);
   y2(n,:) = xyz2(2,:);
   z2(n,:) = xyz2(3,:);
end
%************* draw
% side faces
h = surf(x2+pos(1),y2+pos(2),z2+pos(3),cc);
for n=1:2
	patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),cc(n,:));
end	

建立DH齐次变换矩阵子程序

function Matrix_DH_Ln(i) 
% Caculate the D-H Matrix
%根据DH参数生成各关节间的齐次变换矩阵
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];

绘制机器人子程序

第一种写法

function pic=DHfk7Dof_Lnya(th1,th2,th3,th4,th5,th6,th7,fcla)
%%%绘制机器人
% close all

global Link
Build_7DOFRobot_Lnya;%建立DH参数表
%画图配置参数
radius    = 30;%半径
len       = 90;%高
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=th7;%末端滑动关节
 %%

%根据DH参数表及关节变量构造各个关节的齐次变换矩阵
for i=1:8
Matrix_DH_Ln(i);
end
%绘制表示各个关节的圆柱体及关节连杆
for i=2:8
      %计算第i个关节的位姿：根据上一关节的位姿计算下一关节的位姿
      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)];
      %绘制第i-1个关节
      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;
      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([-2500,2500,-2500,2500,-2500,2500]);


xlabel('x');
ylabel('y');
zlabel('z');
drawnow;
pic=getframe;%捕获坐标区或者图窗做影片帧
if(fcla)
    cla;
end

第二种写法

function A=DHfk7Dof_kuka_nodraw(q)
global Link
Build_7DOFRobot_Lnya;%建立DH参数表

%input angle
Link(2).th = q(1)*pi/180;
Link(3).th = q(2)*pi/180;
Link(4).th = q(3)*pi/180;
Link(5).th = q(4)*pi/180;
Link(6).th = q(5)*pi/180;
Link(7).th = q(6)*pi/180;

Link(8).dz = q(7);
%DH matrix
for i=1:8
    Matrix_DH_Ln(i);
end

%position and orientation
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)];
end

A=Link(8).A;%4*4 的矩阵

RRT避障提取小球算法子程序

首先在主程序中生成了小球，小球所有的信息，包括坐标、半径、体积等全部存入ss矩阵，ss矩阵作为参数传入该子程序后，目标点为该小球当前x坐标的负方向，即把小球向x轴负方向提取，使之脱离球群。因此搜索方向上也是向着x轴负方向。这里的代码逻辑是RRT搜素出避障路径，然后把经过的一些点存入矩阵path中，我们设法获得矩阵中的这些点，并调用求逆解程序，对机械臂求逆解，然后绘制机械臂，不断的绘制，擦除，绘制出，擦除就形成了动画。已抓取的小球的坐标也要随着这些点发生变化，就形成了小球机械臂跟着走。另外避障的起点，要设置在小球表面，即每个小球避障起点坐标到小球中心点坐标的距离要大于半径。

% clc;
% clear;
% close all;
function rrt_SanWei(ss,index,xun_huan,ban_jing)
%ss是主函数中的存储小球信息的矩阵，index将小球体积从大到小排列后的矩阵
shui_ping_yuan_zhu([0,0,0]',[0,1,0]',1000,4000,0);%绘制水平侧卧的圆柱
hold on
circleCenter = [ss(1,1),ss(1,2),ss(1,3);ss(2,1),ss(2,2),ss(2,3);ss(3,1),ss(3,2),ss(3,3);ss(4,1),ss(4,2),ss(4,3);ss(5,1),ss(5,2),ss(5,3);ss(6,1),ss(6,2),ss(6,3);ss(7,1),ss(7,2),ss(7,3);ss(8,1),ss(8,2),ss(8,3);ss(9,1),ss(9,2),ss(9,3);ss(10,1),ss(10,2),ss(10,3)];
%将10个小球的x，y，z坐标输入
r=[ss(1,5);ss(2,5);ss(3,5);ss(4,5);ss(5,5);ss(6,5);ss(7,5);ss(8,5);ss(9,5);ss(10,5)];
%输入10个小球的半径
for ii=1:1:10
[xx,yy,zz]= ellipsoid(ss(ii,1),ss(ii,2