Delta机器人：运动学正反解分析

一、Delta机构简介

Delta机构是并联机构中的一种典型机构，起原始结构如图1所示。Delta机构由R.Clavel 博士在 1985年发明，是现在并联机器人中使用最多的一种，具备了并联机构所特有的优点，负载能力强、效率高、末端执行器精度高、运动惯性小，可以高速稳定运动等。因此在机器人领域获得了越来越广泛的应用。以实现高速、精准、高效的运动。

  图1 R.Clavel博士发明的Delta机构

二、数学模型建立

     图2 Delta机构简化数学模型

三、运动学正解

 图3 Delta机构几何法正解简化模型

  图4 等效四棱锥

   图5 单支链求解示意图

五、Matlba正反解

1. 正解

function [x,y,z] = DeltaForwardkinematics(theta1,theta2,theta3)
%Delta腿正解
%上固定板半径R=90 脚底半径r=41.57  大腿长度L=85 小腿长度l=140
R=90;
r=41.57;
L=85;
l=140;
a1=[R,0,0];
a2=[-R/2,R*sqrt(3)/2,0];
a3=[-R/2,-R*sqrt(3)/2,0];

a1b1=[L*cos(theta1),0,L*sin(theta1)];
a2b2=[-L/2*cos(theta2),L*cos(theta2)*sqrt(3)/2,L*sin(theta2)];
a3b3=[-L/2*cos(theta3),-L*cos(theta3)*sqrt(3)/2,L*sin(theta3)];

c1o2=[-r,0,0];
c2o2=[r/2,-r*sqrt(3)/2,0];
c3o2=[r/2,r*sqrt(3)/2,0];
o1d1=a1+a1b1+c1o2;
o1d2=a2+a2b2+c2o2;
o1d3=a3+a3b3+c3o2;

o1f=(o1d1+o1d2)/2;

d1d2=o1d2-o1d1;
d1d3=o1d3-o1d1;
d2d3=o1d3-o1d2;
a=norm(d1d2);
b=norm(d1d3);
c=norm(d2d3);
p=(a+b+c)/2;
S=sqrt(p*(p-a)*(p-b)*(p-c));
normd1e=a*b*c/(4*S);
normfe=sqrt(normd1e^2-a^2/4);
dirfe1=cross(cross(d1d2,d1d3),d1d2)/(a*a*b);
dirfe=dirfe1/norm(dirfe1);
fe=normfe*dirfe;
o1e=o1f+fe;
normd1o2=l;
%normd1e=a*b*c/(4*S);
normeo2=sqrt(normd1o2^2-normd1e^2);
direo21=cross(d1d2,d1d3)/(a*b);
direo2=direo21/norm(direo21);
eo2=normeo2*direo2;
o1o2=o1e+eo2;
x=o1o2(1);
y=o1o2(2);
z=o1o2(3);
end

2. 反解

function [theta1,theta2,theta3] = DeltaInversekinematic(x,y,z)
%Delta腿正解
%上固定板半径R=90 脚底半径r=41.57  大腿长度L=85 小腿长度l=140
R=90;
r=41.57;
L=85;
l=140;
z=-z;
m=x^2+y^2+z^2+(R-r)^2+L^2-l^2;
A=[(m-2*x*(R-r))/(2*L)-(R-r-x)
   (m+(R-r)*(x-sqrt(3)*y))/(L)-2*(R-r)-(x-sqrt(3)*y)
   (m+(R-r)*(x+sqrt(3)*y))/(L)-2*(R-r)-(x+sqrt(3)*y)];
B=[2*z
   4*z
   4*z];
C=[(m-2*x*(R-r))/(2*L)+(R-r-x)
   (m+(R-r)*(x-sqrt(3)*y))/(L)+2*(R-r)+(x-sqrt(3)*y)
   (m+(R-r)*(x+sqrt(3)*y))/(L)+2*(R-r)+(x+sqrt(3)*y)];
theta1=2*atan((-B(1)-sqrt(B(1)^2-4*A(1)*C(1)))/(2*A(1)));
theta2=2*atan((-B(2)-sqrt(B(2)^2-4*A(2)*C(2)))/(2*A(2)));
theta3=2*atan((-B(3)-sqrt(B(3)^2-4*A(3)*C(3)))/(2*A(3)));
end

3. 绘制工作空间

clear; clc;
R=90;
r=41.57;
L=85;
l=140;

x0=[0,R,R+L,r,0];
y0=[0,0,0,0,0];
z0=[0,0,0,42.3844,42.3811];
x1=[0,R*cos(2*pi/3),(R+L)*cos(2*pi/3),r*cos(2*pi/3),0];
y1=[0,R*sin(2*pi/3),(R+L)*sin(2*pi/3),r*sin(2*pi/3),0];
z1=[0,0,0,42.3844,42.3811];
x2=[0,R*cos(4*pi/3),(R+L)*cos(4*pi/3),r*cos(4*pi/3),0];
y2=[0,R*sin(4*pi/3),(R+L)*sin(4*pi/3),r*sin(4*pi/3),0];
z2=[0,0,0,42.3844,42.3811];
plot3(x0,y0,-z0,'k','LineWidth',5);
hold on;
plot3(x1,y1,-z1,'k','LineWidth',5);
hold on;
plot3(x2,y2,-z2,'k','LineWidth',5);
hold on;
x=zeros(68*68*68,1);
y=zeros(68*68*68,1);
z=zeros(68*68*68,1);
for i=1:2:135
    for j=1:2:135
        for k=1:2:135
            [x((i-1)/2*68*68+(j-1)/2*68+(k+1)/2),y((i-1)/2*68*68+(j-1)/2*68+(k+1)/2),z((i-1)/2*68*68+(j-1)/2*68+(k+1)/2)] = DeltaForwardkinematics((k-46)/180*pi,(j-46)/180*pi,(i-46)/180*pi);
        end
    end
end
v=[x,y,-z];
% v = unique(v,'rows');
shp = alphaShape(v);
plot(shp)
%  f=convhulln(v);
%  patch('vertices',v,'faces',f,'facecolor','c');
% scatter3(x,y,z,'.');

五、参考文献

[1] Clavel R. Dispositif pour le deplacement et le positionnement d’un element dans l’espace[P].Switzerland: CH1985005348856,1985.
[2] 赵杰,朱延河,蔡鹤皋.Delta型并联机器人运动学正解几何解法[J].哈尔滨工业大学学报,2003(01):25-27.
[3] 伍经纹,徐世许,王鹏,宋婷婷.基于Adams的三自由度Delta机械手的运动学仿真分析[J].软件,2017,38(06):108-112.