Delta并联机构运动学逆解MATLAB程序

Delta并联机器人逆解程序，自己写的，正逆解完全可以对照上。

%并联机构运动学正解模型 输入三个电机转角（单位°），输出位置（xyz）单位cm。
%%%%由于在静平台上建立坐标系，因此，当三个电机的输入均为0时，动平台的z轴坐标为462cm。
%%%%电机转动一圈产生的脉冲数是512*66个
function [Z] = FK（a1a2a3）
R=120;    %静平台半径
r=60;     %动平台半径
l1=265;   %主动臂长度
l2=565;   %从动臂长度

%将输入的角度换算成MATLAB中使用的弧度
theta1 = a1*pi/180;
theta2 = a2*pi/180;
theta3 = a3*pi/180;

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

OC1=R*[cos（beta1）;sin（beta1）;0];
OC2=R*[cos（beta2）;sin（beta2）;0];
OC3=R*[cos（beta3）;sin（beta3）;0];

C1B1=l1*[cos（beta1）*cos（theta1）;sin（beta1）*cos（theta1）;sin（theta1）];
C2B2=l1*[cos（beta2）*cos（theta2）;sin（beta2）*cos（theta2）;sin（theta2）];
C3B3=l1*[cos（beta3）*cos（theta3）;sin（beta3）*cos（theta3）;sin（theta3）];

OB1=OC1+C1B1;
OB2=OC2+C2B2;
OB3=OC3+C3B3;

%下面三个公式与参考文献不太对应
A1A=r*[cos（beta1）;sin（beta1）;0];
A2A=r*[cos（beta2）;sin（beta2）;0];
A3A=r*[cos（beta3）;sin（beta3）;0];

OB=OB1-A1A;%z之前为减号，03.07号修改为加号
OC=OB2-A2A;
OD=OB3-A3A;

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

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）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%发现问题，单位矢量不正确%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n1=cross（BCCD）;
n_EF1=cross（n1BC）;
d=norm（n_EF1）;
n_EF = n_EF1/d;
EF=N_EF*n_EF;
OE=（OB+OC）/2; 
OF=OE+EF;

N_FA=sqrt（l2^2-N_BF^2）;
n_FA1=cross（BCCD）;%cross（）返回向量积
e=norm（n_FA1）;
n_FA=n_FA1/e;
FA=N_FA*n_FA;
OA=OF+FA;
P=OA;
e1=[1;0;0];
e2=[0;1;0];
e3=[0;0;1];
x=P‘*e1;
y=P‘*e2;
z=P‘*e3;
Z=[x;y;z-462.16]

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1694  2018-04-14 14:43  FK.m
     文件        1141  2018-03-10 08:57  IK.m