odefun1.m

function  dx=odefun1（tx）
global c2  Cn KK2 deltav1 delta  Qdf qk2jx Force Ae Ae1 shendu
global txl ti
% global xjiasudu yjiasudu zjiasudu ajiaojiasudu bjiaojiasudu cjiaojiasudu%（需修改）
% global AA  AXBO AYBO AZBO alpha beta gamma AXBO1 AYBO1 AZBO1 alpha1 beta1 gamma1 AXBO11 AYBO11 AZBO11 alpha11 beta11 gamma11  %（需修改）

format long  %（将数据显示为长整型科学计数）

m0=36.28;%动平台的质量
% mss=14.80;%（伸缩杆质量矩阵中的表达）
mss=8.45;
mb=26.15;%摆动秆的质量


lb2=0.378;%UPS摆动杆质心到定平台S副的距离
ls2=0.452;%UPS杆质心到运动副中心的距离
Ixx=0.93202;  Iyy=0.67346; Izz=0.68159;   %动平台的质量的各轴惯性矩  （和柔性惯性量有点差别）
r1=0.71707;%定平台的转动副R1在Y轴正方向的距离
r2=0.6450;                                                                                                                                                                         
f=pi/4;
r3=0.202;  %U副
r4=0.202;% 动平台半径
% d=0.06711;%虎克铰与YOZ平面的距离
g=3*pi/10;
h=2*pi/5;
alpha=0;
beta=0;
gamma=pi/2;

alpha1=0;beta1=0;gamma1=0;%对欧拉角求一阶导数
alpha11=0;beta11=0;gamma11=0;%对欧拉角求二阶导数

% TT1（end+1）=t
% 
% save TT1 TT1

AXBO=1.150+0.03*sin（2*ti）;%动系坐标原点在静坐标系下的坐标赋值
AYBO=-0.03+0.03*cos（2*ti）;
AZBO=0;
AXBO1=0.03*2*cos（2*ti）;
AYBO1=-0.03*2*sin（2*ti）;
AZBO1=0;
AXBO11=-0.03*2*2*sin（2*ti）;
AYBO11=-0.03*2*2*cos（2*ti）;
AZBO11=0;


Rb2=0.015;
RS2=Rb2+c2;
n=1.5;
ce=0.9;%恢复系数
ud=0.15;%摩擦因数
V0=0.0001;%给定的速度误差
V1=0.5;

qk1=[AXBO1 AYBO1 AZBO1 alpha1 beta1 gamma1]‘;
qk2=[AXBO11 AYBO11 AZBO11 alpha11 beta11 gamma11]‘; 
APBO=[AXBO AYBO AZBO]‘;%
AVBO=[AXBO1 AYBO1 AZBO1]‘;
APBOjx=[x（1） x（3） x（5）]‘;
AVBOjx=[x（2） x（4） x（6）]‘;
qk1jx=[x（2） x（4） x（6） x（8） x（10） x（12）]‘;
p11=[0 r1 0]‘;                          %定平台虎克铰在定系下坐标
p12=[0 r2*cos（f） r2*sin（f）]‘;
p13=[0 r2*cos（3*f） r2*sin（3*f）]‘;
p14=[0 r2*cos（5*f） r2*sin（5*f）]‘;
p15=[0 r2*cos（7*f） r2*sin（7*f）]‘;

p21=[0 0 -r3]‘;                         %动平台球铰在动系下坐标
p22=[0 r4*sin（h） -r4*cos（h）]‘;
p23=[0 r4*cos（g） r4*sin（g）]‘;
p24=[0 -r4*cos（g） r4*sin（g）]‘;
p25=[0 -r4*sin（h） -r4*cos（h）]‘;

ABR=[cos（alpha）*cos（beta） cos（alpha）*sin（beta）*sin（gamma）-sin（alpha）*cos（gamma） cos（alpha）*sin（beta）*cos（gamma）+sin（alpha）*sin（gamma）; sin（alpha）*cos（beta） sin（alpha）*sin（beta）*sin（gamma）+cos（alpha）*cos（gamma） sin（alpha）*sin（beta）*cos（gamma）-cos（alpha）*sin（gamma）;-sin（beta） cos（beta）*sin（gamma） cos（beta）*cos（gamma）];%欧拉转换矩阵



ABR1=[-sin（alpha）*cos（beta）*alpha1-cos（alpha）*sin（beta）*beta1（-sin（alpha）*sin（beta）*sin（gamma）-cos（alpha）*cos（gamma））*alpha1+cos（alpha）*cos（beta）*sin（gamma）*beta1+（cos（alpha）*sin（beta）*cos（gamma）+sin（alpha）*sin（gamma））*gamma1（-sin（alpha）*sin（beta）*cos（gamma）+cos（alpha）*sin（gamma））*alpha1+cos（alpha）*cos（beta）*cos（gamma）*beta1+（-cos（alpha）*sin（beta）*sin（gamma）+sin（alpha）*cos（gamma））*gamma1;
    cos（alpha）*cos（beta）*alpha1-sin（alpha）*sin（beta）*beta1（cos（alpha）*sin（beta）*sin（gamma）-sin（alpha）*cos（gamma））*alpha1+sin（alpha）*cos（beta）*sin（gamma）*beta1+（sin（alpha）*sin（beta）*cos（gamma）-cos（alpha）*sin（gamma））*gamma1（cos（alpha）*sin（beta）*cos（gamma）+sin（alpha）*sin（gamma））*alpha1+sin（alpha）*cos（beta）*