Finnal6.m

close all
clc
lui=0.378; %质心到动平台胡克铰ui的距离  
lsi=0.452; %质心到动平台铰链si的距离
mui=26.15;%摆动杆的质量  
msi=8.45; %伸缩杆的质量
mB=36.28;%并联机构动平台的质量 
mdj=0;%各种机床参数赋值（mdj为刀头质量）

h=0; 
rA=0.645;%定平台上四个虎克铰u1u2u3u4均匀布置于半径645mm的圆上
rB=0.202;%动平台上五个铰链s1s2s3s4s5均匀布置在本经202mm的圆上
mq=pi/4;%定平台上四个胡克铰的间隔角度
Q=2*pi/5;%动平台上五个铰链的间隔角度
W=0.71707;%球副S中心位于半径717.07mm处
g=[9.800].‘;%重力取值 %铰节点位置参数赋值，1.1支链坐标系的建立

a=0.025;%动平台运动轨迹中的给定参数
syms t f1x f1z f2x f3x f4x f5x  %syms函数：创建符号变量的捷径
tt=0:0.01:4;
for j=1:length（tt）
    t=tt（j）
ms1=0;ms2=0;%对于FEDi、FEmi的叠加运算赋初值 
%ma=（pi/36）*t*t*t;mb=0;mc=-pi/2;%欧拉角赋值 
%ma=（pi/72）*t;mb=（pi/72）*t;mc=-pi/2;%欧拉角赋值
%ma=0;mb=0;mc=（-pi/2）*t;%欧拉角赋值
ma=0;mb=0;mc=-pi/2;%欧拉角赋值
%ma=-（pi/18）*t;mb=0;mc=-pi/2;%欧拉角赋值
%aaq1=（pi/12）*t*t;bbq1=0;ccq1=0;%对欧拉角求一阶导数
%aaq11=（pi/6）*t;bbq11=0;ccq11=0;%对欧拉角求二阶导数
%aaq1=0;bbq1=0;ccq1=-pi/2;%对欧拉角求一阶导数
%aaq11=0;bbq11=0;ccq11=0;%对欧拉角求二阶导数
%aaq1=-pi/18;bbq1=0;ccq1=0;%对欧拉角求一阶导数
aaq1=0;bbq1=0;ccq1=0;%对欧拉角求一阶导数
aaq11=0;bbq11=0;ccq11=0;%对欧拉角求二阶导数
A=[cos（ma） -sin（ma） 0;sin（ma） cos（ma） 0;0 0 1];
B=[cos（mb） 0 sin（mb）;0 1 0;-sin（mb） 0 cos（mb）];    
C=[1 0 0;0 cos（mc） -sin（mc）;0 sin（mc） cos（mc）];
ABR=A*B*C; %欧拉转换矩阵（B相对于A的） 3*3
N=inv（ABR）;%欧拉转换矩阵（A相对于B的）后面有用 3*3
X=0.90+0.025*cos（2*t）;%动系坐标原点在静坐标系下的坐标赋值  式40
Y=-0.10+0.025*sin（2*t）;
Z=0.202*cos（mb）*cos（mc）;%动系坐标原点在静坐标系下的坐标赋值 
P=[X;Y;Z]; %3*1
%AaVpB=ABR*[-0.120*cos（2*t）;-0.120*sin（2*t）;0]%动平台在静坐标下的加速度
%求基本量mni（沿着杆长方向的单位向量）5个杆的，RPS也要用到
ma1=[0;W;0];           %定平台上5个胡克铰在静坐标系下的坐标
ma2=[0;rA*cos（mq）;rA*sin（mq）];
ma3=[0;rA*cos（3*mq）;rA*sin（3*mq）];
ma4=[0;rA*cos（5*mq）;rA*sin（5*mq）];
ma5=[0;rA*cos（7*mq）;rA*sin（7*mq）];  %3*1

lApu1=[ma1（21）.^2].^（1/2）; %定平台上5个胡克铰在静坐标系A下的模
lApu2=[ma2（31）.^2+ma2（21）.^2].^（1/2）;
lApu3=[ma3（31）.^2+ma3（21）.^2].^（1/2）;
lApu4=[ma4（31）.^2+ma4（21）.^2].^（1/2）;
lApu5=[ma5（31）.^2+ma5（21）.^2].^（1/2）;  %1*1

mA1=N*（ma1-P）;          %定平台上5个胡克铰在动坐标系下的坐标
mA2=N*（ma2-P）;
mA3=N*（ma3-P）;
mA4=N*（ma4-P）;
mA5=N*（ma5-P）;          %N=inv（ABR）欧拉转换矩阵（A相对于B的） 3*1

mb1=[0;0;-rB];        %动平台上5个铰在动坐标系下的坐标
mb2=[0;rB*sin（Q）;-rB*cos（Q）];
mb3=[0;rB*sin（2*Q）;-rB*cos（2*Q）];
mb4=[0;rB*sin（3*Q）;-rB*cos（3*Q）];
mb5=[0;rB*sin（4*Q）;-rB*cos（4*Q）];  %3*1

mL1=[（0-mA1（11））^2+（0-mA1（21））^2+（-rB-mA1（31））^2].^（1/2）;%5个杆的长度
mL2=[（0-mA2（11））^2+（rB*sin（Q）-mA2（21））^2+（-rB*cos（Q）-mA2（31））^2].^（1/2）;
mL3=[（0-mA3（11））^2+（rB*sin（2*Q）-mA3（21））^2+（-rB*cos（2*Q）-mA3（31））^2].^（1/2）;
mL4=[（0-mA4（11））^2+（rB*sin（3*Q）-mA4（21））^2+（-rB*cos（3*Q）-mA4（31））^2].^（1/2）;
mL5=[（0-mA5（11））^2+（rB*sin（4*Q）-mA5（21））^2+（-rB*cos（4*Q）-mA5（31））^2].^（1/2）;  %1*1

mn1=（mb1-mA1）/mL1;          %5个杆的单位方向矢量（在动系B中）=动平台上五个铰在动平台下的坐标mb1-定平台上五个胡克铰在动坐标系下的坐标mA1的差值除以五个杆的长度mL1
mn2=（mb2-mA2）/mL2;
mn3=（mb3-mA3）/mL3;
mn4=（mb4-mA4）/mL4;
mn5=（mb5-mA5）/mL5;  %3*1 

An1=ABR*mn1; %5个杆的单位方向矢量（在定系A中）
An2=ABR*mn2;
An3=ABR*mn3;
An4=ABR*mn4;
An5=ABR*mn5;  %3*1  

cs1=An1（11）; %求杆与定系x轴的夹角的余弦值
cs2=An2（11）;                    
cs3=An3（11）;
cs4=An4（11）;
cs5=An5（11）;  %1*1