Delta机器人正逆解

并联机器人，Delta机器人正逆解，MATLAB程序，验证可行

function [position] = forward_delta（thetasgeometry）
R=geometry（1）; 
r=geometry（2）;
l1=geometry（3）;
l2=geometry（4）;
theta1=thetas（1）*pi/180;
theta2=thetas（2）*pi/180;
theta3=thetas（3）*pi/180;

beta1=（1-1）*2*pi/3-pi/6;
beta2=（2-1）*2*pi/3-pi/6;
beta3=（3-1）*2*pi/3-pi/6;

OB1=R*[cos（beta1）;sin（beta1）;0];
OB2=R*[cos（beta2）;sin（beta2）;0];
OB3=R*[cos（beta3）;sin（beta3）;0];

B1C1=l1*[cos（beta1）*cos（theta1）;sin（beta1）*cos（theta1）;-sin（theta1）];
B2C2=l1*[cos（beta2）*cos（theta2）;sin（beta2）*cos（theta2）;-sin（theta2）];
B3C3=l1*[cos（beta3）*cos（theta3）;sin（beta3）*cos（theta3）;-sin（theta3）];

OC1=OB1+B1C1;
OC2=OB2+B2C2;
OC3=OB3+B3C3;

O1A1=r*[cos（beta1）;sin（beta1）;0];
O1A2=r*[cos（beta2）;sin（beta2）;0];
O1A3=r*[cos（beta3）;sin（beta3）;0];

OB=OC1-O1A1;
OC=OC2-O1A2;
OD=OC3-O1A3;

BC=OB-OC;
CD=OC-OD;
DB=OD-OB;

a=norm（BC）;
b=norm（CD）;
c=norm（DB）;

p=（a+b+c）/2;
s=sqrt（p*（p-a）*（p-b）*（p-c））;
N_BF=a*b*c/（4*s）;
N_BE=a/2;
N_EF=sqrt（N_BF^2-N_BE^2）;
n_EF=cross（cross（BCCD）BC）/a*b*a;
EF=N_EF*n_EF;
OE=（OB+OC）/2;
OF=OE+EF;
N_FO1=sqrt（l2^2-N_BF^2）;
n_FO1=cross（BCCD）/a*b;
FO1=N_FO1*n_FO1;
OO1=OF+FO1;
position=OO1;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1155  2013-08-01 15:53  forward_delta.m

     文件        997  2013-08-01 15:53  inverse_delta.m

----------- ---------  ---------- -----  ----

                 2152                    2