%并聯機構運動學正解模型?輸入三個電機轉角（單位°），輸出位置（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