main_3sps_3ccs.m

基于四元数的机构综合建立模型的MATLAB程序

%已有函数 g1=fouryie(x) g=fouradd(x, y) g=fourplus(x, y)；使用函数先定义符号变量。
syms g0 g1 g2 g3 q0 q1 q2 q3 x1 x2 x3;
%%%%%%%%%%%%%%%%%%%%已知数据
A1=[0;0;0];A2=[-1.8301;3.1699;-0.5];A3=[3.1699;11.8301;0.5];
A4=[6.8301;11.8301;-1];A5=[11.8301;3.1699;0];A6=[10;0;-1];
B1=[0;0;0];B2=[-2;3.6049;0.069];B3=[-0.8349; 5.4247;-0.9359];
B4=[3.1651;5.6835;0.03];B5=[4.3302;3.3462;-0.8966];B6=[2.3302;0.2589;0.9659];
V1=[0.965923;-0.25883;0];V2=[0.6;0.8;0];V3=[0.4;-0.6;0];
L1=9.8080;L2=10.8465;L3=11.0578;L4=12.8826;L5=9.6551;L6=12.3934;
%%%%%%%%%%%%%%%%%%%%%
A1=[0;A1];A2=[0;A2];A3=[0;A3];V1=[0;V1];
A4=[0;A4];A5=[0;A5];A6=[0;A6];V2=[0;V2];
B1=[0;B1];B2=[0;B2]; B3=[0;B3];V3=[0;V3];
B4=[0;B4];B5=[0;B5];B6=[0;B6];
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:6
  if i==1
      A=A1;B=B1;L=L1;V0=x1*V1;VJ=V1;
  elseif i==2
      A=A2;B=B2;L=L2;V0=x2*V2;VJ=V2;
  elseif i==3
      A=A3;B=B3;L=L3;V0=x3*V3;VJ=V3;
  elseif i==4
      A=A4;B=B4;L=L4;
  elseif i==5
      A=A5;B=B5;L=L5;
  else
      A=A6;B=B6;L=L6;
  end
  %%%%%%%%%%%%%%%%
  g=[g0; g1; g2; g3];q=[q0; q1;q2;q3];
  Temp0=fourTime(q, fouryie(q));
  Temp1=fourTime(g, fouryie(q));
  Temp2=fourTime(q, fourTime(B, fouryie(q)));
  Temp3=fouradd(Temp1, Temp2);
  Temp4=fourplus(Temp3,A);
  Temp5=fourTime(Temp4, fouryie(Temp4));
  Temp60=fourplus(Temp4,V0);
  Temp6=fourTime(Temp60,fouryie(Temp60));
  Temp7=fourTime(Temp60,fouryie(VJ));
  if A==A1
    f=[Temp0(1)-1;Temp1(1)];
  end
  if A==A1|A==A2|A==A3;
     f=[f;Temp6(1)-L^2;Temp7(1)];
  else
     f=[f;Temp5(1)-L^2];
  end
end
f
%%%%%%%%%%%%%%%%%%%%