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

一、Delta机构简介

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

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

二、数学模型建立

建立Delta机构简化数学模型如图2所示，其中圆OΟO所在平面为定平台，圆ppp所在平面为动平台，∆C1C2C3∆C_1C_2C_3∆C1C2C3和∆A1A2A3∆A_1A_2A_3∆A1A2A3为等边三角形，点C1、C2、C3、A1、A2、A3C_1、C_2、C_3、A_1、A_2、A_3C1、C2、C3、A1、A2、A3分别为三个主动臂和三个从动臂与上下两个平台的连接点。

图2Delta机构简化数学模型\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图2\ Delta机构简化数学模型图2 Delta机构简化数学模型
如图2所示，以定平台中心OΟO建立坐标系O−XYZΟ-XYZO−XYZ，以动平台中心ppp建立坐标系p−xyzp-xyzp−xyz。由Delta机构的设计原理可知，三条支链完全对称，因此不妨设第i（i=1,2,3）i（i=1,2,3）i（i=1,2,3）条支链的主动臂∣BiCi∣\left|B_iC_i\right|∣BiCi∣长度为LLL，从动臂∣AiBi∣\left|A_iB_i\right|∣AiBi∣长度为lll，主动臂与定平台夹角为θi\theta_iθi，三条支链与X轴的夹角为φi=(2(i−1)π/3)，i=1,2,3，\varphi_i=\left(2\left(i-1\right)\pi/3\right)，i=1,2,3，φi=(2(i−1)π/3)，i=1,2,3，定平台半径为R，动平台半径为rrr。

三、运动学正解

Delta机构的正解，是在已知三个主动臂转角的情况下求出动平台中心点ppp在定平台所在坐标系中的坐标。Delta机构运动学正解的求法有很多种，此处采取几何解法，将AiBiA_iB_iAiBi分别沿AipA_ipAip平移使其交于点ppp得到DipD_ipDip，连接D1D2D_1D_2D1D2、D2D3D_2D_3D2D3、D3D1D_3D_1D3D1得到四棱锥p−D1D2D3p{-D}_1D_2D_3p−D1D2D3,如图3所示。

图3Delta机构几何法正解简化模型\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图3\ Delta机构几何法正解简化模型                                    图3 Delta机构几何法正解简化模型
根据上图不难得到，定平台三个铰接点C1、C2、C3C_1、C_2、C_3C1、C2、C3的坐标为
[xiyizi]=[Rcos⁡φiRsin⁡φi0]，i=1,2,3(1)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}R\cos{\varphi_i}\\R\sin{\varphi_i}\\0\\\end{matrix}\right]，i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (1)                                                ⎣⎡xiyizi⎦⎤=⎣⎡RcosφiRsinφi0⎦⎤，i=1,2,3                    (1)
向量OBi⃗\vec{OB_i}OBi可表示为
OBi⃗=OCi⃗+CiBi⃗，i=1,2,3(2)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{\ OB_i}=\vec{OC_i}+\vec{C_iB_i}，i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2)                                              OBi=OCi+CiBi，i=1,2,3                       (2)
其中CiBi⃗\vec{C_iB_i}CiBi又可表示为
[xiyizi]=[−Lsin⁡θicos⁡φi−Lsin⁡θisin⁡φi−Lcos⁡θi]，i=1,2,3(3)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}-L\sin{\theta_i}\cos{\varphi_i}\\-L\sin{\theta_i}\sin{\varphi_i}\\-L\cos{\theta_i}\\\end{matrix}\right]，i=1,2,3\ \ \ \ \ \ (3)                        ⎣⎡xiyizi⎦⎤=⎣⎡−Lsinθicosφi−Lsinθisinφi−Lcosθi⎦⎤，i=1,2,3      (3)
又Aip⃗\vec{A_ip}Aip可表示为
[xiyizi]=[−rcos⁡φi−rsin⁡φi0]，i=1,2,3(4)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}-r\cos{\varphi_i}\\-r\sin{\varphi_i}\\0\\\end{matrix}\right]，i=1,2,3\ \ \ \ \ \ \ \ \ \ \ (4)                                                       ⎣⎡xiyizi⎦⎤=⎣⎡−rcosφi−rsinφi0⎦⎤，i=1,2,3           (4)
则ODi⃗\vec{OD_i}ODi可以表示为
ODi⃗=OBi⃗+BiDi⃗(5)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OD_i}=\vec{\ OB_i}+\vec{\ B_iD_i}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (5)                                                       ODi= OBi+ BiDi                               (5)
其中由Delta机构的几何性质可知
BiDi⃗=Aip⃗(6)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{\ B_iD_i}=\vec{A_ip}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (6)                                                               BiDi=Aip                                       (6)
所以
ODi⃗=OCi⃗+CiBi⃗+Aip⃗(7)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OD_i}=\vec{OC_i}+\vec{C_iB_i}+\vec{A_ip}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (7)                                                     ODi=OCi+CiBi+Aip                         (7)
综合式（1）—（7）可得，在坐标系Ο-XYZ中D_i的坐标为
[xiyizi]=[(R−r−Lsin⁡θi)cos⁡φi(R−r−Lsin⁡θi)sin⁡φi−Lcos⁡θi]，i=1,2,3(8)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left[\begin{matrix}x_i\\y_i\\z_i\\\end{matrix}\right]=\left[\begin{matrix}\left(R-r-L\sin{\theta_i}\right)\cos{\varphi_i}\\\left(R-r-L\sin{\theta_i}\right)\sin{\varphi_i}\\-L\cos{\theta_i}\\\end{matrix}\right]，i=1,2,3\ \ \ \ \ \ \ \ \ (8)                                        ⎣⎡xiyizi⎦⎤=⎣⎡(R−r−Lsinθi)cosφi(R−r−Lsinθi)sinφi−Lcosθi⎦⎤，i=1,2,3         (8)
此时不难发现，Delta机构的正运动学解算已经转化为已知三个顶点坐标和各棱的长度求解另外一个顶点坐标的问题。将图2.4中的四棱锥p−D1D2D3p{-D}_1D_2D_3p−D1D2D3取出单独分析，作垂线pEpEpE垂直于平面D1D2D3D_1D_2D_3D1D2D3于点EEE，取D2D3D_2D_3D2D3中点FFF，连接EFEFEF、ED2ED_2ED2，如图3所示。

图4等效四棱锥\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图4\ 等效四棱锥                                                            图4 等效四棱锥
不难证明，点E为三角形D_1D_2D_3的外接圆圆心。
则向量Op⃗\vec{Op}Op可表示为
Op⃗=OE⃗+Ep⃗(9)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{Op}=\vec{OE}+\vec{Ep}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (9)                                                          Op=OE+Ep                            (9)
OE⃗\vec{OE}OE可以表示为
OE⃗=OF⃗+FE⃗(10)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{OE}=\vec{OF}+\vec{FE}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (10)                                                         OE=OF+FE                        (10)

其中，OF⃗=(OD2⃗+OD3⃗)/2\vec{OF}=\left(\vec{OD_2}+\vec{OD_3}\right)/2OF=(OD2+OD3)/2，FE⃗=∣FE⃗∣nFE⃗\vec{FE}=\left|\vec{FE}\right|\vec{n_{FE}}FE=∣∣∣FE∣∣∣nFE。

对于向量FE⃗\vec{FE}FE其长度为
∣FE⃗∣=∣D2E∣2−∣D2F∣2(11)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|\vec{FE}\right|=\sqrt{\left|D_2E\right|^2-\left|D_2F\right|^2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (11) ∣∣∣FE∣∣∣=∣D2E∣2−∣D2F∣2 (11)
其中，∣D2E∣\left|D_2E\right|∣D2E∣为三角形D1D2D3D_1D_2D_3D1D2D3的外接圆半径，可用公式(12)计算

∣D2E∣=abc4S(12)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|D_2E\right|=\frac{abc}{4S}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left(12\right) ∣D2E∣=4Sabc (12)

其中

S=p(p−a)(p−b)(p−c)(13)p=(a+b+c)2(14)S=\sqrt{p\left(p-a\right)\left(p-b\right)\left(p-c\right)}\ \ \ \ \ \ \ \ \left(13\right)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ p=\frac{\left(a+b+c\right)}{2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (14)S=p(p−a)(p−b)(p−c) (13) p=2(a+b+c) (14)

a、b、ca、b、ca、b、c是三角形D1D2D3D_1D_2D_3D1D2D3的边长。

联立（11）—（14）可得

∣FE⃗∣=(a2+b2−c2)c8S(15)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \left|\vec{FE}\right|=\frac{\left(a^2+b^2-c^2\right)c}{8S}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (15)                                                        ∣∣∣FE∣∣∣=8S(a2+b2−c2)c                   (15)
向量FE⃗\vec{FE}FE的单位方向向量为
nFE⃗=D2D1⃗×D2D3⃗×D3D2⃗∣D2D1⃗×D2D3⃗×D3D2⃗∣(16)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{n_{FE}}=\frac{\vec{D_2D_1}\times\vec{D_2D_3}\times\vec{D_3D_2}}{\left|\vec{D_2D_1}\times\vec{D_2D_3}\times\vec{D_3D_2}\right|}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (16)                                                  nFE=∣D2D1×D2D3×D3D2∣D2D1×D2D3×D3D2                  (16)
又向量Ep⃗\vec{Ep}Ep的方向向量
nEp⃗=D2D1⃗×D2D3⃗∣D2D1⃗×D2D3⃗∣(17)\ \ \ \ \ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{n_{Ep}}=\frac{\vec{D_2D_1}\times\vec{D_2D_3}}{\left|\vec{D_2D_1}\times\vec{D_2D_3}\right|}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (17)                                                             nEp=∣D2D1×D2D3∣D2D1×D2D3                    (17)
长度为
Ep⃗=∣D1p⃗∣2−∣D1E⃗∣2(18)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \vec{Ep}=\sqrt{\left|\vec{D_1p}\right|^2-\left|\vec{D_1E}\right|^2}\ \ \ \ \ \ \ \ \ \ \ \ \ \ (18)                                                  Ep=∣∣∣D1p∣∣∣2−∣∣∣D1E∣∣∣2              (18)
其中∣D1p⃗∣=∣B1A1⃗∣=l，D1E⃗\left|\vec{D_1p}\right|=\left|\vec{B_1A_1}\right|=l，\vec{D_1E}∣∣∣D1p∣∣∣=∣∣∣B1A1∣∣∣=l，D1E为外接圆半径。
将（10）—（18）式联立求解带入（9）式即可求得Delta机构正解。

四、运动学反解

运动学反解是在已知机器人ppp点的位置(x,y,z)(x,y,z)(x,y,z)的情况下求解三个主动臂需要转过的角度θ1\theta_1θ1、θ2\theta_2θ2、θ3\theta_3θ3，与串联机器人不同，并联机器人的反解较易求得，此处只需要根据杆长进行约束即可很容易解出，求解过程在文献[3]中有详细的说明，此处不再推导，仅根据图5所示的单支链求解示意图给出最终的求解结果。

图5单支链求解示意图\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 图5\ 单支链求解示意图图5 单支链求解示意图

θi=2arctan(−Bi−Bi2−4AiCi2Ai)，i=1,2,3(19)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \theta_i=2arctan\left(\frac{-B_i-\sqrt{B_i^2-4A_iC_i}}{2A_i}\right)，i=1,2,3\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (19) θi=2arctan(2Ai−Bi−Bi2−4AiCi)，i=1,2,3 (19)
其中
A1=x2+y2+z2+(R−r)2+L2−l2−2x(R−r)2L−(R−r−x)A1=\frac{x^2+y^2+z^2+(R-r)^2+L^2-l^2-2x(R-r)}{2L}-(R-r-x)A1=2Lx2+y2+z2+(R−r)2+L2−l2−2x(R−r)−(R−r−x)
B1=2zB1=2zB1=2z
C1=x2+y2+z2+(R−r)2+L2−l2−2x(R−r)2L+(R−r−x)C1=\frac{x^2+y^2+z^2+(R-r)^2+L^2-l^2-2x(R-r)}{2L}+(R-r-x)C1=2Lx2+y2+z2+(R−r)2+L2−l2−2x(R−r)+(R−r−x)

A2=x2+y2+z2+(R−r)2+L2−l2+(x−3y)(R−r)L−2(R−r)−(x−3y)A2=\frac{x^2+y^2+z^2+(R-r)^2+L^2-l^2+(x-\sqrt{3}y)(R-r)}{L}-2(R-r)-(x-\sqrt{3}y)A2=Lx2+y2+z2+(R−r)2+L2−l2+(x−3y)(R−r)−2(R−r)−(x−3y)
B2=4zB2=4zB2=4z
C2=x2+y2+z2+(R−r)2+L2−l2+(x−3y)(R−r)L+2(R−r)+(x−3y)C2=\frac{x^2+y^2+z^2+(R-r)^2+L^2-l^2+(x-\sqrt{3}y)(R-r)}{L}+2(R-r)+(x-\sqrt{3}y)C2=Lx2+y2+z2+(R−r)2+L2−l2+(x−3y)(R−r)+2(R−r)+(x−3y)

A3=x2+y2+z2+(R−r)2+L2−l2+(x+3y)(R−r)L−2(R−r)−(x+3y)A3=\frac{x^2+y^2+z^2+(R-r)^2+L^2-l^2+(x+\sqrt{3}y)(R-r)}{L}-2(R-r)-(x+\sqrt{3}y)A3=Lx2+y2+z2+(R−r)2+L2−l2+(x+3y)(R−r)−2(R−r)−(x+3y)
B3=4zB3=4zB3=4z
C3=x2+y2+z2+(R−r)2+L2−l2+(x+3y)(R−r)L+2(R−r)+(x+3y)C3=\frac{x^2+y^2+z^2+(R-r)^2+L^2-l^2+(x+\sqrt{3}y)(R-r)}{L}+2(R-r)+(x+\sqrt{3}y)C3=Lx2+y2+z2+(R−r)2+L2−l2+(x+3y)(R−r)+2(R−r)+(x+3y)
至此，Delta机构的运动学正反解均以求解完毕。

五、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*z4*z4*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:135for j=1:2:135for 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);endend
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.