各位好 我的程序修改了 循环结构感觉有问题 求大家指点！
clearq01=1; q02=1; q03=1; q04=1; q05=1; q06=1;
q1i=ones(35,1);
q2i=;
q1d1=ones(35,1);
q2d1=;
q1d2=;
q2d2=;
q1d=;
q2d=;
on=zeros(3,3);
on66=zeros(6,6);on62=zeros(6,2);on63=zeros(6,3);on16=zeros(1,6);
on36=zeros(3,6);on33=zeros(3,3);on13=zeros(1,3);on12=zeros(1,2);
on23=zeros(2,3);on32=zeros(3,2);on26=zeros(2,6);on1818=zeros(18,18);on2929=zeros(29,29);
on188=zeros(18,8);on117=zeros(1,17);on116=zeros(11,6);on611=zeros(6,11);on151=zeros(15,1);on296=zeros(29,6);
on187=zeros(18,7);on1816=zeros(16,16);on216=zeros(2,16);on1816=zeros(18,16);on316=zeros(3,16);on629=zeros(6,29);
e66=eye(6);e22=eye(2);e33=eye(3);e1111=eye(11);e1616=eye(16);e1515=eye(15);e2929=eye(29);on2929=zeros(29,29);
e44=eye(4);on34=zeros(3,4);on256=zeros(25,6);on46=zeros(4,6);on425=zeros(4,25);on254=zeros(25,4);on325=zeros(3,25);e2525=eye(25);
Dheng=;
Mi1heng=zeros(32,32);
Ci1heng=zeros(32,32);
Ki1heng=zeros(32,32);
Qi1heng=zeros(32,1);
Pmidu=7801;%定义一些参数，驱动杆，动平台的参数，密度
E=2.11*10^11;
G=8*10^10;
m0=105.85;%动平台的质量
Ixx=2.74; Ixy=2.34*10^-8; Ixz=1.62*10^-2;%动平台的质量的各轴惯性矩
Iyx=2.34*10^-8; Iyy=1.92; Iyz=3.37*10^-7;
Izx=1.62*10^-2; Izy=3.37*10^-7; Izz=1.85;
Aijdanyuan1=;%18*32
Aijdanyuan2=,on187];
Aijdanyuan3=]; %Aijdanyuan1 2 3为3个单元的支链广义坐标转换关系矩阵；
R01=;%35*151
R02=;%35*151
R03=;%35*151
R04=;%35*151
R05=;%35*151
Mi1heng=zeros(32,32);Mi2heng=zeros(32,32);Mi3heng=zeros(32,32);Mi4heng=zeros(32,32);Mi5heng=zeros(32,32);
Ci1heng=zeros(32,32);Ci2heng=zeros(32,32);Ci3heng=zeros(32,32);Ci4heng=zeros(32,32);Ci5heng=zeros(32,32);
Ki1heng=zeros(32,32);Ki2heng=zeros(32,32);Ki3heng=zeros(32,32);Ki4heng=zeros(32,32);Ki5heng=zeros(32,32);
Qi1heng=zeros(32,1);Qi2heng=zeros(32,1);Qi3heng=zeros(32,1);Qi4heng=zeros(32,1);Qi5heng=zeros(32,1);
M00=;%6*6
M0=];
CJ=zeros(151,151);
K=zeros(151,151);
Q=zeros(151,1); tt=0:0.1:0.1;
for i=1:length(tt)
q=;%放在循环里面去哦
t=tt(i);
u1=0.5;u2=0.25;
dt=0.01;%时间步长取0.01
d0=1/(u2*dt^2);d1=u1/(u2*dt);d2=1/(u2*dt);d3=1/(2*u2)-1;d4=u1/u2-1;d5=(dt/2)*(u1/u2-2);d6=dt*(1-u1);d7=u1*dt;
L1i=0.76;L2i=0.88;%L1i摆动杆长度 L2i伸缩杆长度
g=9.8;
xb=980+t; yb=t; zb=t;a=0;b=0;
a1=0;b1=0;a2=0;b2=0;xb1=1;xb2=0;yb1=1;yb2=0;zb1=1;zb2=0;
w=-717.07;
rA=645;
rB=202;
rsx=-59.11;
rsz=-228.4;
Pu1=.';
Pu2=.';
Pu3=.';
Pu4=.';
Pu5=.';
Ps1=.';
Ps2=.';
Ps3=.';
Ps4=.';
Ps5=.';%3*1矩阵
R=;%3*3矩阵
Po=.';%3*1矩阵
Pu=;
Ps=;
fai=;
for j=1:5
L1=R*Ps(:,j)+Po-Pu(:,j);%3*1矩阵
% L1=R*Ps1+Po-Pu1;%3*1矩阵
% L2=R*Ps2+Po-Pu2;
% L3=R*Ps3+Po-Pu3;
% L4=R*Ps4+Po-Pu4;
% L5=R*Ps5+Po-Pu5;%3*1矩阵
s1=R*Ps(:,j)+Po;%此处是求解动平台上五个铰链点的矢量坐标以方便以后使用
% s2=R*Ps2+Po;
% s3=R*Ps3+Po;
% s4=R*Ps4+Po;
% s5=R*Ps5+Po;
xs1=s1(1,1);ys1=s1(2,1);zs1=s1(3,1);
% xs2=s2(1,1);ys2=s2(2,1);zs2=s2(3,1);
% xs3=s3(1,1);ys3=s3(2,1);zs3=s3(3,1);
% xs4=s4(1,1);ys4=s4(2,1);zs4=s4(3,1);
% xs5=s5(1,1);ys5=s5(2,1);zs5=s5(3,1);
if j==1
l1x=rsx*cos(a)*cos(b)+rsz*sin(a)+xb;
l1x1=-rsx*sin(a)*cos(b)*a1-rsx*cos(a)*sin(b)*b1+rsz*cos(a)*a1+xb1;
l1x2=-rsx*cos(a)*cos(b)*a1^2+2*rsx*sin(a)*sin(b)*a1*b1-rsx*sin(a)*cos(b)*a2-rsx*cos(a)*cos(b)*b1^2-rsx*cos(a)*sin(b)*b2-rsz*sin(a)*a1^2+rsz*cos(a)*a2+xb2;
l1y=rsx*sin(a)*cos(b)-rsz*cos(a)-w+yb;
l1y1=rsx*cos(a)*cos(b)*a1-rsx*sin(a)*sin(b)*b1+rsz*sin(a)*a1+yb1;
l1y2=-rsx*sin(a)*cos(b)*a1^2-2*rsx*cos(a)*sin(b)*a1*b1+rsx*cos(a)*cos(b)*a2-rsx*sin(a)*cos(b)*b1^2-rsx*sin(a)*sin(b)*b2+rsz*cos(a)*a1^2+rsz*sin(a)*a2+yb2;
l1z=-rsx*sin(b)+zb;
l1z1=-rsx*cos(b)*b1+zb1;
l1z2=-rsx*cos(b)*b2+rsx*sin(b)*b1^2+zb2;
l1=(l1x^2+l1y^2+l1z^2)^0.5;
l1d1=(1/l1)*(l1x*l1x1+l1y*l1y1+l1z*l1z1);%支链杆1的1次导数
l1d2=(-l1d1/(l1^2))*(l1x*l1x1+l1y*l1y1+l1z*l1z1)+(1/l1)*(l1x1*l1x1+l1x*l1x2+l1y1*l1y1+l1y*l1y2+l1z1*l1z1+l1z*l1z2);%支链杆1的2次导数
elseif j==2
l1x=rB*sin(2*pi/5)*cos(a)*sin(b)-rB*cos(2*pi/5)*sin(a)+xb;
l1x1=-rB*sin(2*pi/5)*sin(a)*sin(b)*a1+rB*sin(2*pi/5)*cos(a)*cos(b)*b1-rB*cos(2*pi/5)*cos(a)*a1+xb1;
l1x2=-rB*sin(2*pi/5)*cos(a)*sin(b)*a1^2-2*rB*sin(2*pi/5)*sin(a)*cos(b)*a1*b1-rB*sin(2*pi/5)*sin(a)*sin(b)*a2-rB*sin(2*pi/5)*cos(a)*sin(b)*b1^2+rB*sin(2*pi/5)*cos(a)*cos(b)*b2+rB*cos(2*pi/5)*sin(a)*a1^2-rB*cos(2*pi/5)*cos(a)*a2+xb2;
l1y=rB*sin(2*pi/5)*sin(a)*sin(b)+rB*cos(2*pi/5)*cos(a)+yb-rA*cos(pi/4);
l1y1=rB*sin(2*pi/5)*cos(a)*sin(b)*a1+rB*sin(2*pi/5)*sin(a)*cos(b)*b1-rB*cos(2*pi/5)*sin(a)*a1+yb1;
l1y2=-rB*sin(2*pi/5)*sin(a)*sin(b)*a1^2+2*rB*sin(2*pi/5)*cos(a)*cos(b)*a1*b1+rB*sin(2*pi/5)*cos(a)*sin(b)*a2-rB*sin(2*pi/5)*sin(a)*sin(b)*b1^2+rB*sin(2*pi/5)*sin(a)*cos(b)*b2-rB*cos(2*pi/5)*cos(a)*a1^2-rB*cos(2*pi/5)*sin(a)*a2+yb2;
l1z=rB*sin(2*pi/5)*cos(b)+zb-rA*sin(pi/4);
l1z1=-rB*sin(2*pi/5)*sin(b)*b1+zb1;
l1z2=-rB*sin(2*pi/5)*cos(b)*b1^2-rB*sin(2*pi/5)*sin(b)*b2+zb2;
l1=(l1x^2+l1y^2+l1z^2)^0.5;
l1d1=(1/l1)*(l1x*l1x1+l1y*l1y1+l1z*l1z1);%支链杆1的1次导数
l1d2=(-l1d1/(l1^2))*(l1x*l1x1+l1y*l1y1+l1z*l1z1)+(1/l1)*(l1x1*l1x1+l1x*l1x2+l1y1*l1y1+l1y*l1y2+l1z1*l1z1+l1z*l1z2);%支链杆1的2次导数
% l2=(l2x^2+l2y^2+l2z^2)^(1/2);
% l2d1=(1/l2)*(l2x*l2x1+l2y*l2y1+l2z*l2z1);%支链杆2的1次导数
% l2d2=(-l2d1/(l2^2))*(l2x*l2x1+l2y*l2y1+l2z*l2z1)+(1/l2)*(l2x1*l2x1+l2x*l2x2+l2y1*l2y1+l2y*l2y2+l2z1*l2z1+l2z*l2z2);%支链杆2的2次导数
elseif j==3
l1x=rB*sin(4*pi/5)*cos(a)*sin(b)-rB*cos(4*pi/5)*sin(a)+xb;
l1x1=-rB*sin(4*pi/5)*sin(a)*sin(b)*a1+rB*sin(4*pi/5)*cos(a)*cos(b)*b1-rB*cos(4*pi/5)*cos(a)*a1+xb1;
l1x2=-rB*sin(4*pi/5)*cos(a)*sin(b)*a1^2-2*rB*sin(4*pi/5)*sin(a)*cos(b)*a1*b1-rB*sin(4*pi/5)*sin(a)*sin(b)*a2-rB*sin(4*pi/5)*cos(a)*sin(b)*b1^2+rB*sin(4*pi/5)*cos(a)*cos(b)*b2+rB*cos(4*pi/5)*sin(a)*a1^2-rB*cos(4*pi/5)*cos(a)*a2+xb2;
l1y=rB*sin(4*pi/5)*sin(a)*sin(b)+rB*cos(4*pi/5)*cos(a)+yb+rA*cos(pi/4);
l1y1=rB*sin(4*pi/5)*cos(a)*sin(b)*a1+rB*sin(4*pi/5)*sin(a)*cos(b)*b1-rB*cos(4*pi/5)*sin(a)*a1+yb1;
l1y2=-rB*sin(4*pi/5)*sin(a)*sin(b)*a1^2+2*rB*sin(4*pi/5)*cos(a)*cos(b)*a1*b1+rB*sin(4*pi/5)*cos(a)*sin(b)*a2-rB*sin(4*pi/5)*sin(a)*sin(b)*b1^2+rB*sin(4*pi/5)*sin(a)*cos(b)*b2-rB*cos(4*pi/5)*cos(a)*a1^2-rB*cos(4*pi/5)*sin(a)*a2+yb2;
l1z=rB*sin(4*pi/5)*cos(b)+zb-rA*sin(pi/4);
l1z1=-rB*sin(4*pi/5)*sin(b)*b1+zb1;
l1z2=-rB*sin(4*pi/5)*cos(b)*b1^2-rB*sin(4*pi/5)*sin(b)*b2+zb2;
l1=(l1x^2+l1y^2+l1z^2)^0.5;
l1d1=(1/l1)*(l1x*l1x1+l1y*l1y1+l1z*l1z1);%支链杆1的1次导数
l1d2=(-l1d1/(l1^2))*(l1x*l1x1+l1y*l1y1+l1z*l1z1)+(1/l1)*(l1x1*l1x1+l1x*l1x2+l1y1*l1y1+l1y*l1y2+l1z1*l1z1+l1z*l1z2);%支链杆1的2次导数
% l3=(l3x^2+l3y^2+l3z^2)^(1/2);
% l3d1=(1/l3)*(l3x*l3x1+l3y*l3y1+l3z*l3z1);%支链杆3的1次导数
% l3d2=(-l3d1/(l3^2))*(l3x*l3x1+l3y*l3y1+l3z*l3z1)+(1/l3)*(l3x1*l3x1+l3x*l3x2+l3y1*l3y1+l3y*l3y2+l3z1*l3z1+l3z*l3z2);%支链杆3的2次导数
elseif j==4
l1x=rB*sin(6*pi/5)*cos(a)*sin(b)-rB*cos(6*pi/5)*sin(a)+xb;
l1x1=-rB*sin(6*pi/5)*sin(a)*sin(b)*a1+rB*sin(6*pi/5)*cos(a)*cos(b)*b1-rB*cos(6*pi/5)*cos(a)*a1+xb1;
l1x2=-rB*sin(6*pi/5)*cos(a)*sin(b)*a1^2-2*rB*sin(6*pi/5)*sin(a)*cos(b)*a1*b1-rB*sin(6*pi/5)*sin(a)*sin(b)*a2-rB*sin(6*pi/5)*cos(a)*sin(b)*b1^2+rB*sin(6*pi/5)*cos(a)*cos(b)*b2+rB*cos(6*pi/5)*sin(a)*a1^2-rB*cos(6*pi/5)*cos(a)*a2+xb2;
l1y=rB*sin(6*pi/5)*sin(a)*sin(b)+rB*cos(6*pi/5)*cos(a)+yb+rA*cos(pi/4);
l1y1=rB*sin(6*pi/5)*cos(a)*sin(b)*a1+rB*sin(6*pi/5)*sin(a)*cos(b)*b1-rB*cos(6*pi/5)*sin(a)*a1+yb1;
l1y2=-rB*sin(6*pi/5)*sin(a)*sin(b)*a1^2+2*rB*sin(6*pi/5)*cos(a)*cos(b)*a1*b1+rB*sin(6*pi/5)*cos(a)*sin(b)*a2-rB*sin(6*pi/5)*sin(a)*sin(b)*b1^2+rB*sin(6*pi/5)*sin(a)*cos(b)*b2-rB*cos(6*pi/5)*cos(a)*a1^2-rB*cos(6*pi/5)*sin(a)*a2+yb2;
l1z=rB*sin(6*pi/5)*cos(b)+zb+rA*sin(pi/4);
l1z1=-rB*sin(6*pi/5)*sin(b)*b1+zb1;
l1z2=-rB*sin(6*pi/5)*cos(b)*b1^2-rB*sin(6*pi/5)*sin(b)*b2+zb2;
l1=(l1x^2+l1y^2+l1z^2)^0.5;
l1d1=(1/l1)*(l1x*l1x1+l1y*l1y1+l1z*l1z1);%支链杆1的1次导数
l1d2=(-l1d1/(l1^2))*(l1x*l1x1+l1y*l1y1+l1z*l1z1)+(1/l1)*(l1x1*l1x1+l1x*l1x2+l1y1*l1y1+l1y*l1y2+l1z1*l1z1+l1z*l1z2);%支链杆1的2次导数 elseif j==5
l1x=rB*sin(8*pi/5)*cos(a)*sin(b)-rB*cos(8*pi/5)*sin(a)+xb;
l1x1=-rB*sin(8*pi/5)*sin(a)*sin(b)*a1+rB*sin(8*pi/5)*cos(a)*cos(b)*b1-rB*cos(8*pi/5)*cos(a)*a1+xb1;
l1x2=-rB*sin(8*pi/5)*cos(a)*sin(b)*a1^2-2*rB*sin(8*pi/5)*sin(a)*cos(b)*a1*b1-rB*sin(8*pi/5)*sin(a)*sin(b)*a2-rB*sin(8*pi/5)*cos(a)*sin(b)*b1^2+rB*sin(8*pi/5)*cos(a)*cos(b)*b2+rB*cos(8*pi/5)*sin(a)*a1^2-rB*cos(8*pi/5)*cos(a)*a2+xb2;
l1y=rB*sin(8*pi/5)*sin(a)*sin(b)+rB*cos(8*pi/5)*cos(a)+yb-rA*cos(pi/4);
l1y1=rB*sin(8*pi/5)*cos(a)*sin(b)*a1+rB*sin(8*pi/5)*sin(a)*cos(b)*b1-rB*cos(8*pi/5)*sin(a)*a1+yb1;
l1y2=-rB*sin(8*pi/5)*sin(a)*sin(b)*a1^2+2*rB*sin(8*pi/5)*cos(a)*cos(b)*a1*b1+rB*sin(8*pi/5)*cos(a)*sin(b)*a2-rB*sin(8*pi/5)*sin(a)*sin(b)*b1^2+rB*sin(8*pi/5)*sin(a)*cos(b)*b2-rB*cos(8*pi/5)*cos(a)*a1^2-rB*cos(8*pi/5)*sin(a)*a2+yb2;
l1z=rB*sin(8*pi/5)*cos(b)+zb+rA*sin(pi/4);
l1z1=-rB*sin(8*pi/5)*sin(b)*b1+zb1;
l1z2=-rB*sin(8*pi/5)*cos(b)*b1^2-rB*sin(8*pi/5)*sin(b)*b2+zb2;
l1=(l1x^2+l1y^2+l1z^2)^0.5;
l1d1=(1/l1)*(l1x*l1x1+l1y*l1y1+l1z*l1z1);%支链杆1的1次导数
l1d2=(-l1d1/(l1^2))*(l1x*l1x1+l1y*l1y1+l1z*l1z1)+(1/l1)*(l1x1*l1x1+l1x*l1x2+l1y1*l1y1+l1y*l1y2+l1z1*l1z1+l1z*l1z2);%支链杆1的2次导数
end
theta1=atan((l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))/l1x);
% theta2=atan((l2y*cos(pi/4)+l2z*sin(pi/4))/l2x);
% theta3=atan((l3y*cos(-pi/4)+l3z*sin(-pi/4))/l3x);
% theta4=atan((l4y*cos(-3*pi/4)+l4z*sin(-3*pi/4))/l4x);
% theta5=atan((l5y*cos(3*pi/4)+l5z*sin(3*pi/4))/l5x);
sheta1=asin((l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))/l1);
% sheta2=asin((l2y*sin(pi/4)-l2z*cos(pi/4))/l2);
% sheta3=asin((l3y*sin(-pi/4)-l3z*cos(-pi/4))/l3);
% sheta4=asin((l4y*sin(-3*pi/4)-l4z*cos(-3*pi/4))/l4);
% sheta5=asin((l5y*sin(3*pi/4)-l5z*cos(3*pi/4))/l5);
theta11=((l1y1*cos(fai(1,j))+l1z1*sin(fai(1,j)))*l1x-(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))*l1x1)/(l1x^2+(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))^2);
sheta11=((l1y1*sin(fai(1,j))-l1z1*cos(fai(1,j)))*l1-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))*l1d1)/(((l1^2-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))^2)^0.5)*l1);
theta12=(((l1y2*cos(fai(1,j))+l1z2*sin(fai(1,j)))*l1x-(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))*l1x2)*(l1x^2+(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))^2)-((l1y1*cos(fai(1,j))+l1z1*sin(fai(1,j)))*l1x-(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))*l1x1)*(2*l1x*l1x1+2*(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))*(l1y1*cos(fai(1,j))+l1z1*sin(fai(1,j)))))/((l1x^2+(l1y*cos(fai(1,j))+l1z*sin(fai(1,j)))^2)^2);
s101=(l1y2*sin(fai(1,j))-l1z2*cos(fai(1,j)))*l1;s102=l1d2*(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)));
A1=(((l1^2-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))^2)^0.5)*l1);B1=((l1y1*sin(fai(1,j))-l1z1*cos(fai(1,j)))*l1-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))*l1d1);
s103=l1d1*(l1^2-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))^2)^0.5+l1*(l1^2-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))^2)^(-0.5)*(l1*l1d1-(l1y*sin(fai(1,j))-l1z*cos(fai(1,j)))*(l1y1*sin(fai(1,j))-l1z1*cos(fai(1,j))));
ifj==1
sheta12=(-1/2/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(93318/25+6*t)+1/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(46659/25+3*t)-1/2*t/((92089/100+t)^2+(94547/100+t)^2+t^2)^(3/2)*(46659/25+3*t)*(93318/25+6*t)+3*t/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2))/((92089/100+t)^2+(94547/100+t)^2)^(1/2)/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)-1/2*(-((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)+t/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(46659/25+3*t))/((92089/100+t)^2+(94547/100+t)^2)^(3/2)/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(93318/25+4*t)-1/2*(-((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)+t/((92089/100+t)^2+(94547/100+t)^2+t^2)^(1/2)*(46659/25+3*t))/((92089/100+t)^2+(94547/100+t)^2)^(1/2)/((92089/100+t)^2+(94547/100+t)^2+t^2)^(3/2)*(93318/25+6*t);
else
sheta12=((s101-s102)*A1-B1*s103)/A1^2;
end
Aij1=[cos(theta1)*cos(sheta1) -sin(theta1) cos(theta1)*sin(sheta1);
cos(fai(1,j))*sin(theta1)*cos(sheta1)+sin(fai(1,j))*sin(sheta1) cos(fai(1,j))*cos(theta1) cos(fai(1,j))*sin(theta1)*sin(sheta1)-sin(fai(1,j))*cos(sheta1);
sin(fai(1,j))*sin(theta1)*cos(sheta1)-cos(fai(1,j))*sin(sheta1) sin(fai(1,j))*cos(theta1) sin(fai(1,j))*sin(theta1)*sin(sheta1)+cos(fai(1,j))*cos(sheta1)]; %支链1上单元的变换矩阵 先算出一个再说，先往后列出式子 看看
Rij1=Aij1.';
% Rij2=Aij2.';
% Rij3=Aij3.';
% Rij4=Aij4.';
% Rij5=Aij5.';
Rij1heng=;%表示单元广义坐标与单元系统坐标系下的转换矩阵
% Rij2heng=;
% Rij3heng=;
% Rij4heng=;
% Rij5heng=;
Bij1=;
% Bij2=;
% Bij3=;
% Bij4=;
% Bij5=; % 3*1表示Aij5的第一列
%下面的五个矩阵为支链1、2、3、4、5上单元的变换矩阵的对时间t的导数矩阵
Gij1=[-sin(theta1)*cos(sheta1)*theta11-cos(theta1)*sin(sheta1)*sheta11,-cos(theta1)*theta11,-sin(theta1)*sin(sheta1)*theta11+cos(theta1)*cos(sheta1)*sheta11;
cos(fai(1,j))*cos(theta1)*cos(sheta1)*theta11-cos(fai(1,j))*sin(theta1)*sin(sheta1)*sheta11+sin(fai(1,j))*cos(sheta1)*sheta11,-cos(fai(1,j))*sin(theta1)*theta11,cos(fai(1,j))*cos(theta1)*sin(sheta1)*theta11+cos(fai(1,j))*sin(theta1)*cos(sheta1)*sheta11+sin(fai(1,j))*sin(sheta1)*sheta11;
sin(fai(1,j))*cos(theta1)*cos(sheta1)*theta11-sin(fai(1,j))*sin(theta1)*sin(sheta1)*sheta11-cos(fai(1,j))*cos(sheta1)*sheta11,-sin(fai(1,j))*sin(theta1)*theta11,sin(fai(1,j))*cos(theta1)*sin(sheta1)*theta11+sin(fai(1,j))*sin(theta1)*cos(sheta1)*sheta11-cos(fai(1,j))*sin(sheta1)*sheta11];
g11ij1=-sin(theta1)*cos(sheta1)*(theta12)-cos(theta1)*cos(sheta1)*(theta11)^2+sin(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(theta1)*sin(sheta1)*(sheta12)+sin(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(theta1)*cos(sheta1)*(sheta11)^2;
g12ij1=-cos(theta1)*(theta12)+sin(theta1)*(theta11)^2;
g13ij1=-sin(theta1)*sin(sheta1)*(theta12)-cos(theta1)*sin(sheta1)*(theta11)^2-sin(theta1)*cos(sheta1)*(theta11)*(sheta11)+cos(theta1)*cos(sheta1)*(sheta12)-sin(theta1)*cos(sheta1)*(theta11)*(sheta11)-cos(theta1)*sin(sheta1)*(sheta11)^2;
g21ij1=cos(fai(1,j))*cos(theta1)*cos(sheta1)*(theta12)-cos(fai(1,j))*sin(theta1)*cos(sheta1)*(theta11)^2-cos(fai(1,j))*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(fai(1,j))*sin(theta1)*sin(sheta1)*(sheta12)-cos(fai(1,j))*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-cos(fai(1,j))*sin(theta1)*cos(sheta1)*(sheta11)^2+sin(fai(1,j))*cos(sheta1)*(sheta12)-sin(fai(1,j))*sin(sheta1)*(sheta11)^2;
g22ij1=-cos(fai(1,j))*sin(theta1)*(theta12)-cos(fai(1,j))*cos(theta1)*(theta11)^2;
g23ij1=cos(fai(1,j))*cos(theta1)*sin(sheta1)*(theta12)-cos(fai(1,j))*sin(theta1)*sin(sheta1)*(theta11)^2+cos(fai(1,j))*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)+cos(fai(1,j))*sin(theta1)*cos(sheta1)*(sheta12)+cos(fai(1,j))*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)-cos(fai(1,j))*sin(theta1)*sin(sheta1)*(sheta11)^2+sin(fai(1,j))*sin(sheta1)*(sheta12)+sin(fai(1,j))*cos(sheta1)*(sheta11)^2;
g31ij1=sin(fai(1,j))*cos(theta1)*cos(sheta1)*(theta12)-sin(fai(1,j))*sin(theta1)*cos(sheta1)*(theta11)^2-sin(fai(1,j))*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-sin(fai(1,j))*sin(theta1)*sin(sheta1)*(sheta12)-sin(fai(1,j))*cos(theta1)*sin(sheta1)*(theta11)*(sheta11)-sin(fai(1,j))*sin(theta1)*cos(sheta1)*(sheta11)^2-cos(fai(1,j))*cos(sheta1)*(sheta12)+cos(fai(1,j))*sin(sheta1)*(sheta11)^2;
g32ij1=-sin(fai(1,j))*sin(theta1)*(theta12)-sin(fai(1,j))*cos(theta1)*(theta11)^2;
g33ij1=sin(fai(1,j))*cos(theta1)*sin(sheta1)*(theta12)-sin(fai(1,j))*sin(theta1)*sin(sheta1)*(theta11)^2+sin(fai(1,j))*cos(theta1)*cos(sheta1)*(sheta11)*(theta11)+sin(fai(1,j))*sin(theta1)*cos(sheta1)*(sheta12)+sin(fai(1,j))*cos(theta1)*cos(sheta1)*(theta11)*(sheta11)-sin(fai(1,j))*sin(theta1)*sin(sheta1)*(sheta11)^2-cos(fai(1,j))*sin(sheta1)*(sheta12)-cos(fai(1,j))*cos(sheta1)*(sheta11)^2;
Gij12=;%对Gij1的1次求导
% Gij22=;
Rij1heng1=;
% Rij2heng1=;
Rij1heng2=;
% Rij2heng2=;
% Gij1Gij2..Gij5都是3*3矩阵
Dij1=[-sin(theta1)*cos(sheta1)*theta11-cos(theta1)*sin(sheta1)*sheta11;
cos(fai(1,j))*cos(theta1)*cos(sheta1)*theta11-cos(fai(1,j))*sin(theta1)*sin(sheta1)*sheta11+sin(fai(1,j))*cos(sheta1)*sheta11;
sin(fai(1,j))*cos(theta1)*cos(sheta1)*theta11-sin(fai(1,j))*sin(theta1)*sin(sheta1)*sheta11-cos(fai(1,j))*cos(sheta1)*sheta11];
% Dij2=[-sin(theta2)*cos(sheta2)*theta21-cos(theta2)*sin(sheta2)*sheta21;
% cos(pi/4)*cos(theta2)*cos(sheta2)*theta21-cos(pi/4)*sin(theta2)*sin(sheta2)*sheta21+sin(pi/4)*cos(sheta2)*sheta21;
% sin(pi/4)*cos(theta2)*cos(sheta2)*theta21-sin(pi/4)*sin(theta2)*sin(sheta2)*sheta21-cos(pi/4)*cos(sheta2)*sheta21];
GAij1=;%GA矩阵相乘
% GAij2=;
GGij1=[(cos(sheta1))^2*theta11^2+sheta11^2,sin(sheta1)*theta11^2*sheta11^2,sin(sheta1)*cos(sheta1)*theta11^2;sin(sheta1)*theta11^2*sheta11^2,theta11^2,-cos(sheta1)*theta11^2*sheta11^2;sin(sheta1)*cos(sheta1)*theta11^2,-cos(sheta1)*theta11^2*sheta11^2,(sin(sheta1))^2*theta11^2+sheta11^2];
% GGij2=[(cos(sheta2))^2*theta21^2+sheta21^2,sin(sheta2)*theta21^2*sheta21^2,sin(sheta2)*cos(sheta2)*theta21^2;sin(sheta2)*theta21^2*sheta21^2,theta21^2,-cos(sheta2)*theta21^2*sheta21^2;sin(sheta2)*cos(sheta2)*theta21^2,-cos(sheta2)*theta21^2*sheta21^2,(sin(sheta2))^2*theta21^2+sheta21^2];
DDij1=(cos(sheta1))^2*(theta11)^2+(sheta11)^2;
% DDij2=(cos(sheta2))^2*(theta21)^2+(sheta21)^2;
%先算摆动杆上的一个单元上的各个矩阵情况 密度为7801kg/m^3拉压弹性模量E=211GPa ，剪切弹性模量80GPa 。动平台质量105.85kg
%Pij=积分Nr*A.'*A*Nraij1代表摆动杆横截面积4.4*10^(-3)m^2 aij2代表伸缩杆横截面积1.96*10^(-3)m^2
% Pmidu=7801----代表密度
%杆的单位向量
n1=.';
% n2=.';
n1d=[(l1x1*l1-l1x*l1d1)/l1^2,(l1y1*l1-l1y*l1d1)/l1^2,(l1z1*l1-l1z*l1d1)/l1^2].';
% n2d=[(l2x1*l2-l2x*l2d1)/l2^2,(l2y1*l2-l2y*l2d1)/l2^2,(l2z1*l2-l2z*l2d1)/l2^2].';
Js1=];%Js1 Js2 ...Js5为机构系统运动学约束条件矩阵
% Js2=];
R1=;
% R2=;
for k=1:3%循环
syms x%Lb=0.76Ls=0.44表示杆摆动杆 伸缩杆上的单元长度
%对摆动杆上的单元 位移型函数以及一些相关矩阵的相乘
%开始求积分Iijp表示对x轴的惯性矩
aij1=4.4*10^-3;
Iijp1=3.3*10^-2; %单位kg.m^2
Iijy1=1.25;
Iijz1=0.53;
Lb=0.76;
if k==1
Lb=0.44;
aij1=1.96*10^-3; %单位m^2
Iijp1=1.67*10^-3;
Iijy1=1.25;
Iijz1=0.53; %单位kg.m^2
end if k==1
eb=x/Lb;
nb1=1-10*eb^3+15*eb^4-6*eb^5;
nb2=Lb*(eb-6*eb^3+8*eb^4-3*eb^5);
nb3=0.5*Lb^2*(eb^2-3*eb^3+3*eb^4-eb^5);
nb4=10*eb^3-15*eb^4+6*eb^5;
nb5=Lb*(-4*eb^3+7*eb^4-3*eb^5);
nb6=0.5*Lb^2*(eb^3-2*eb^4+eb^5);
nb7=1-3*eb^2+2*eb^3;
nb8=Lb*(eb-2*eb^2+eb^3);
nb9=3*eb^2-2*eb^3;
nb10=Lb*(-eb^2+eb^3);
Nb1=;
Nb2=;
Nb3=;
Nb4=;
Nbr=.'; %3*18
Aij=;
NAANbij1=Nbr.'*Aij*Nbr; %18*18
% NAANbij5=Nbr.'*Aij*Nbr; %18*18Aij代表Aij5.'*Aij5 不管怎么相乘都是单位矩阵 因此这么写
% 为了省时间
NBBNbij1=Nb4.'*Nb4;%18*18Bij1.'*Bij1=1
NGANbij1=Nbr.'*GAij1*Nbr; %18*18GAij1=Gij1.'*Aij1
NDBNbij1=0; %18*18
% NDBNbij5=0; %18*18
NGGNbij1=Nbr.'*GGij1*Nbr; %18*18GGij1=Gij1.'*Gij1
% NGGNbij2=Nbr.'*GGij2*Nbr; %18*18
NDDNbij1=Nb4.'*DDij1*Nb4; %18*18
% NDDNbij5=Nb4.'*DDij5*Nb4; %18*18这些都是列单元动能表达式时的一些矩阵相乘以方便以后用到
nGNbij1=n1d.'*Gij1*Nbr;
% nGNbij5=n5d.'*Gij5*Nbr;
DGNbij1=Dij1.'*Gij1*Nbr;
% DGNbij5=Dij5.'*Gij5*Nbr;
end
%在摆动杆上求之此处的L是摆动杆上的单元长度与伸缩杆上L长度不一样
intNAANbij1=int(NAANbij1,x,0,Lb);
intNBBNbij1=int(NBBNbij1,x,0,Lb);
Pijb1=Pmidu.*(aij1.*intNAANbij1+Iijp1.*intNBBNbij1);
Bijb1pie=Pmidu*(aij1*int(NGANbij1,x,0,Lb)+Iijp1*int(NDBNbij1,x,0,Lb));
Bijb1jian=Pmidu*(aij1*int(NGGNbij1,x,0,Lb)+Iijp1*int(NDDNbij1,x,0,Lb));
%下面是势能表达式中向量矩阵表达 也是运动微分方程=右边的一个矩阵表达式
Obij1=;
%此处以后要把计算的出来的t+t1的下一个值代入进来，是迭代哦h1b等等要回带的 记住了啊！
Kbij1=E*Iijy1*int(diff(Nb2,x,2).'*diff(Nb2,x,2),x,0,Lb)+E*Iijz1*int(diff(Nb3,x,2).'*diff(Nb3,x,2),x,0,Lb)+G*Iijp1*int(diff(Nb4,x,2).'*diff(Nb4,x,2),x,0,Lb);% 弯曲变形刚度矩阵 Kbij1五个支链杆摆动杆都是这个刚度
if k==1
Dhb1=Pmidu*aij1*int((x*DGNbij1),x,0,Lb);%力那边的
% Dhb2=Pmidu*aij1*int((x*DGNbij2),x,0,Lb);
% Dhb3=Pmidu*aij1*int((x*DGNbij3),x,0,Lb);
% Dhb4=Pmidu*aij1*int((x*DGNbij4),x,0,Lb);
% Dhb5=Pmidu*aij1*int((x*DGNbij5),x,0,Lb);
Qxbij1=Dhb1.'-Pmidu*aij1*g*Lb*Obij1.';%单元运动微分等号右边的式子 其中一个力矩阵摆动杆
% Qxbij2=Dhb2.'-Pmidu*aij1*g*Lb*Obij2.';
% Qxbij3=Dhb3.'-Pmidu*aij1*g*Lb*Obij3.';
% Qxbij4=Dhb4.'-Pmidu*aij1*g*Lb*Obij4.';
% Qxbij5=Dhb5.'-Pmidu*aij1*g*Lb*Obij5.';%这是力的
if j==1
h1b=Rij1heng*Aijdanyuan1*R1*q1i;%18*1（18*18）*（18*32）*（32*35）*（35*1）
else
h1b=Rij1heng*Aijdanyuan1*R1*q2i;
end
% h2b=Rij2heng*Aijdanyuan1*R2*q2i;
% h3b=Rij3heng*Aijdanyuan1*R3*q3i;
% h4b=Rij4heng*Aijdanyuan1*R4*q4i;
% h5b=Rij5heng*Aijdanyuan1*R5*q5i;%h1..h5表示五个支链上的单元的广义坐标。此处只算摆动杆上的单元1的刚度2矩阵，q1i..q5i表示五个支链上的系统广义坐标，包括动平台的6个广义坐标；
diffNb1x1=diff(Nb1,x,1);diffNb2x1=diff(Nb2,x,1);diffNb3x1=diff(Nb3,x,1);
Nb123jia=diffNb1x1.'*diffNb1x1+diffNb2x1.'*diffNb2x1+diffNb3x1.'*diffNb3x1;
intkb1ij=(diffNb1x1.'+0.5*Nb123jia*h1b)*(diffNb1x1+0.5*h1b.'*Nb123jia);
% intkb2ij=(diffNb1x1.'+0.5*Nb123jia*h2b)*(diffNb1x1+0.5*h2b.'*Nb123jia);
Kb1ij=E*aij1*int(intkb1ij,x,0,Lb); %Kb1ij中的1代表支链1
% Kb2ij=E*aij1*int(intkb2ij,x,0,Lb); %Kb2ij中的2代表支链2
Kijb1=Kbij1+Kb1ij;%Kijb1=Kbij1+Kb1ij表示摆动杆上的刚度矩阵等于弯曲扭转刚度+拉伸刚度矩阵（计入了几何非线性）
% Kijb2=Kbij1+Kb2ij;
% Kijb5=Kbij1+Kb5ij;%求解的单元1的刚度矩阵=弯曲的+拉伸的
elseif k==2
Dhb1=Pmidu*aij1*((l1-L2i)*int(nGNbij1,x,0,Lb)+int((x*DGNbij1),x,0,Lb));%Dh2s1中间的2代表单元2
% Dhb2=Pmidu*aij1*((l2-L2i)*int(nGNbij2,x,0,Lb)+int((x*DGNbij2),x,0,Lb));
Qxbij1=Dhb1.'-Pmidu*aij1*g*Lb*Obij1.';%单元运动微分等号右边的式子 其中一个力矩阵摆动杆
% Qxbij2=Dhb2.'-Pmidu*aij1*g*Lb*Obij2.';
if j==1
h1b=Rij1heng*Aijdanyuan2*R1*q1i;%18*1（18*18）*（18*32）*（32*35）*（35*1）j2表示第二个单元
else
h1b=Rij1heng*Aijdanyuan2*R1*q2i;%18*1
end diffNb1x1=diff(Nb1,x,1);diffNb2x1=diff(Nb2,x,1);diffNb3x1=diff(Nb3,x,1);
Nb123jia=diffNb1x1.'*diffNb1x1+diffNb2x1.'*diffNb2x1+diffNb3x1.'*diffNb3x1;
intkb1ij=(diffNb1x1.'+0.5*Nb123jia*h1b)*(diffNb1x1+0.5*h1b.'*Nb123jia);
% intkb5ij=(diffNb1x1.'+0.5*Nb123jia*h5b)*(diffNb1x1+0.5*h5b.'*Nb123jia);
Kb1ij=E*aij1*int(intkb1ij,x,0,Lb); %Kb1ij中的1代表支链1
Kijb1=Kbij1+Kb1ij;
% Kijb5=Kbij1+Kb5ij;%求解的单元2的刚度矩阵=弯曲的+拉伸的
else
Dhb1=Pmidu*aij1*((l1-L2i+Lb)*int(nGNbij1,x,0,Lb)+int((x*DGNbij1),x,0,Lb));
% Dhb2=Pmidu*aij1*((l2-L2i+Lb)*int(nGNbij2,x,0,Lb)+int((x*DGNbij2),x,0,Lb));
Qxbij1=Dhb1.'-Pmidu*aij1*g*Lb*Obij1.';%单元运动微分等号右边的式子 其中一个力矩阵摆动杆
% Qxbij2=Dhb2.'-Pmidu*aij1*g*Lb*Obij2.';
if j==1
h1b=Rij1heng*Aijdanyuan3*R1*q1i;%18*1（18*18）*（18*32）*（32*35）*（35*1）
else
h1b=Rij1heng*Aijdanyuan3*R1*q2i;
end
% h5b=Rij5heng*Aijdanyuan3*R5*q5i;%h1..h5表示五个支链上的单元的广义坐标。此处只算摆动杆上的单元1的刚度2矩阵，q1i..q5i表示五个支链上的系统广义坐标，包括动平台的6个广义坐标；
diffNb1x1=diff(Nb1,x,1);diffNb2x1=diff(Nb2,x,1);diffNb3x1=diff(Nb3,x,1);
Nb123jia=diffNb1x1.'*diffNb1x1+diffNb2x1.'*diffNb2x1+diffNb3x1.'*diffNb3x1;
intkb1ij=(diffNb1x1.'+0.5*Nb123jia*h1b)*(diffNb1x1+0.5*h1b.'*Nb123jia);
% intkb2ij=(diffNb1x1.'+0.5*Nb123jia*h2b)*(diffNb1x1+0.5*h2b.'*Nb123jia);
Kb1ij=E*aij1*int(intkb1ij,x,0,Lb); %Kb1ij中的1代表支链1
% Kb2ij=E*aij1*int(intkb2ij,x,0,Lb); %Kb2ij中的2代表支链2
Kijb1=Kbij1+Kb1ij;
% Kijb2=Kbij1+Kb2ij;
% Kijb3=Kbij1+Kb3ij;
% Kijb4=Kbij1+Kb4ij;
% Kijb5=Kbij1+Kb5ij;%求解的单元3的刚度矩阵=弯曲的+拉伸的
end
Mbij1pie=Rij1heng.'*Pijb1*Rij1heng;%摆动杆上的一个小质量矩阵
% Mbij2pie=Rij2heng.'*Pijb2*Rij2heng;
Cbij1pie=2*Rij1heng.'*Pijb1*Rij1heng1-2*Rij1heng.'*Bijb1pie*Rij1heng;%摆动杆上的一个小阻尼矩阵
% Cbij2pie=2*Rij2heng.'*Pijb2*Rij2heng1-2*Rij2heng.'*Bijb2pie*Rij2heng;
Qbij1pie=Rij1heng.'*Qxbij1;%18*1
% Qbij2pie=Rij2heng.'*Qxbij2;
RPb1Rheng2=Rij1heng.'*Pijb1*Rij1heng2; RBb1Rheng1=Rij1heng.'*Bijb1pie*Rij1heng1; RKBb1Rheng=Rij1heng.'*(Kijb1-Bijb1jian)*Rij1heng;
% RPb2Rheng2=Rij2heng.'*Pijb2*Rij2heng2; RBb2Rheng1=Rij2heng.'*Bijb2pie*Rij2heng1; RKBb2Rheng=Rij2heng.'*(Kijb2-Bijb2jian)*Rij2heng;
% RPb3Rheng2=Rij3heng.'*Pijb3*Rij3heng2; RBb3Rheng1=Rij3heng.'*Bijb3pie*Rij3heng1; RKBb3Rheng=Rij3heng.'*(Kijb3-Bijb3jian)*Rij3heng;
% RPb4Rheng2=Rij4heng.'*Pijb4*Rij4heng2; RBb4Rheng1=Rij4heng.'*Bijb4pie*Rij4heng1; RKBb4Rheng=Rij4heng.'*(Kijb4-Bijb4jian)*Rij4heng;
% RPb5Rheng2=Rij5heng.'*Pijb5*Rij5heng2; RBb5Rheng1=Rij5heng.'*Bijb5pie*Rij5heng1; RKBb5Rheng=Rij5heng.'*(Kijb5-Bijb5jian)*Rij5heng;
Kbij1pie=RPb1Rheng2-2*RBb1Rheng1+RKBb1Rheng;%摆动杆上的一个小刚度矩阵此处Kijb1=Kbij1+一个拉伸刚度矩阵
% Kbij2pie=RPb2Rheng2-2*RBb2Rheng1+RKBb2Rheng;
if k==1
Mij1danyuan1jian=Aijdanyuan1.'*Mbij1pie*Aijdanyuan1;
% Mij2danyuan1jian=Aijdanyuan1.'*Mbij2pie*Aijdanyuan1;
Cij1danyuan1jian=Aijdanyuan1.'*Cbij1pie*Aijdanyuan1;
% Cij2danyuan1jian=Aijdanyuan1.'*Cbij2pie*Aijdanyuan1;
Kij1danyuan1jian=Aijdanyuan1.'*Kbij1pie*Aijdanyuan1;
% Kij2danyuan1jian=Aijdanyuan1.'*Kbij2pie*Aijdanyuan1;
Qij1danyuan1jian=Aijdanyuan1.'*Qbij1pie;%各个单元1 2 3上的力矩阵
% Qij2danyuan1jian=Aijdanyuan1.'*Qbij2pie;
elseif k==2
Mij1danyuan1jian=Aijdanyuan2.'*Mbij1pie*Aijdanyuan2;
% Mij2danyuan1jian=Aijdanyuan2.'*Mbij2pie*Aijdanyuan2;
Cij1danyuan1jian=Aijdanyuan2.'*Cbij1pie*Aijdanyuan2;
% Cij2danyuan1jian=Aijdanyuan2.'*Cbij2pie*Aijdanyuan2;
Kij1danyuan1jian=Aijdanyuan2.'*Kbij1pie*Aijdanyuan2;
% Kij2danyuan1jian=Aijdanyuan2.'*Kbij2pie*Aijdanyuan2;
Qij1danyuan1jian=Aijdanyuan2.'*Qbij1pie;%各个单元1 2 3上的力矩阵
% Qij2danyuan1jian=Aijdanyuan2.'*Qbij2pie;
else
Mij1danyuan1jian=Aijdanyuan3.'*Mbij1pie*Aijdanyuan3;
% Mij2danyuan1jian=Aijdanyuan3.'*Mbij2pie*Aijdanyuan3;
Cij1danyuan1jian=Aijdanyuan3.'*Cbij1pie*Aijdanyuan3;
% Cij2danyuan1jian=Aijdanyuan3.'*Cbij2pie*Aijdanyuan3;
Kij1danyuan1jian=Aijdanyuan3.'*Kbij1pie*Aijdanyuan3;
% Kij2danyuan1jian=Aijdanyuan3.'*Kbij2pie*Aijdanyuan3;
Qij1danyuan1jian=Aijdanyuan3.'*Qbij1pie;%各个单元1 2 3上的力矩阵
% Qij2danyuan1jian=Aijdanyuan3.'*Qbij2pie;
end %与 if k==1 对应
% Mi1heng=Mij1danyuan1jian+Mij1danyuan2jian+Mij1danyuan3jian;
Mi1heng=Mij1danyuan1jian+Mi1heng;
% Mi2heng=Mij2danyuan1jian+Mi2heng;%Mij2danyuan2jian+Mij2danyuan3jian;
Ci1heng=Cij1danyuan1jian+Ci1heng;
% Ci2heng=Cij2danyuan1jian+Ci2heng;
Ki1heng=Kij1danyuan1jian+Ki1heng;
% Ki2heng=Kij2danyuan1jian+Ki2heng;
Qi1heng=Qij1danyuan1jian+Qi1heng;
% Qi2heng=Qij2danyuan1jian+Qi2heng;%Qij2danyuan2jian+Qij2danyuan3jian;
end %与 for k=1:3 对应
% M00=;%6*6
M1=R1.'*Mi1heng*R1;%35*35
% M2=R2.'*Mi2heng*R2;
% M3=R3.'*Mi3heng*R3;
% M4=R4.'*Mi4heng*R4;
% M5=R5.'*Mi5heng*R5;%35*35
C1=R1.'*Ci1heng*R1;%35*35
% C2=R2.'*Ci2heng*R2;%35*35
% C3=R3.'*Ci3heng*R3;%35*35
% C4=R4.'*Ci4heng*R4;%35*35
% C5=R5.'*Ci5heng*R5;%35*35
K1=R1.'*Ki1heng*R1;%35*35
% K2=R2.'*Ki2heng*R2;
% K3=R3.'*Ki3heng*R3;
% K4=R4.'*Ki4heng*R4;
% K5=R5.'*Ki5heng*R5;%35*35
Q1=R1.'*Qi1heng+1;%35*1
% Q2=R2.'*Qi2heng+1;
% Q3=R3.'*Qi3heng+1;
% Q4=R4.'*Qi4heng+1;
% Q5=R5.'*Qi5heng+1; %35*1 记住此处的Q1 --Q5只是广义力中的通过微分求导计算出来的部分力
% 相当于方程运算简化整理得出的
M01=R01.'*M1*R01;
% M02=R02.'*M2*R02;
% M03=R03.'*M3*R03;
% M04=R04.'*M4*R04;
% M05=R05.'*M5*R05;
C01=R01.'*C1*R01;
% C02=R02.'*C2*R02;
% C03=R03.'*C3*R03;
% C04=R04.'*C4*R04;
% C05=R05.'*C5*R05;
K01=R01.'*K1*R01;
% K02=R02.'*K2*R02;
% K03=R03.'*K3*R03;
% K04=R04.'*K4*R04;
% K05=R05.'*K5*R05;
Q01=R01.'*Q1;
% Q02=R02.'*Q2;
% Q03=R03.'*Q3;
% Q04=R04.'*Q4;
% Q05=R05.'*Q5;
% M=M01+M02+M03+M04+M05+M0;
M=M01+M0;
CJ=C01+CJ;
K=K01+K;
Q=Q01+Q;
end c1=0;c2=0.00023; %c1 c2为阻尼系数 c1=2.0*10^(-3)c2=3.0*10^(-4)
CJ=vpa(CJ,20);
Kc2=c2.*K;
Kc2=vpa(Kc2,20);
% Mc1=c1.*M;
% MK=Mc1+Kc2;
% C=CJ+MK;%c1 c2为阻尼系数 c1=2.0*10^(-3)c2=3.0*10^(-4)
C=CJ+Kc2;
Kyou=K+d0*M+d1*C;
qqian=q;
if i>1
dddq=d0*q+d2*q1d+d3*q2d;
ddq=d1*q+d4*q1d+d5*q2d;
Mdq=M*dddq;
Cdq=C*ddq;
Q=Q+Mdq+Cdq;
q=KL\Q;
q2d=d0*(q-qqian)-d2*q1d-d3*q2d;
q1d=q1d+d6*q2d+d7*q2d;
xq=xb+q(146);
plot(t,xq,'*');
hold on
end
KL=Kyou;
q1i=;
q2i=;
q3i=;
q4i=;
q5i=;
end 各位高手，你们好，这是我修改之后的程序，第二楼帖子是循环开始，先是时间tt=0:0.1:0.1；为了好检查，这里tt只有两个值;下面紧跟着是j=1:5的循环，然后是k=1:3的内循环，各个循环之中穿插着ifelse语句，请问这些有影响吗？
为什么我最后求和时候，M的结果只有一组解！也就是说只有一个循环的结果，没有走完M的结果是j=1:5循环的求和； 如果程序中这种循环语句和转移语句却是会影响matlab的运行速度，matlab的最大优势就是基于向量化的编程。
至于只有一个循环结果的原因，我个人感觉可能是变量被覆盖了吧！ 回复 8 # meiyongyuandeze 的帖子
谢谢可能是这个原因
页:
[1]