main_6sps.asv

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

%已有函数 g1=fouryie(x) g=fouradd(x, y) g=fourplus(x, y)；使用函数先定义符号变量。
syms g0 g1 g2 g3 q0 q1 q2 q3;
%%%%%%%%%%%%%%%%%%%%已知数据(刘安心专著P212）
%A1=[0;0;0];A2=[1;0;0]; A3=[3;1;0];
%A4=[4;3;2];A5=[3;4;-2];A6=[-1;2;3];
%B1=[0;0;0];B2=[1;0;0];B3=[2;2;0];
%B4=[3;2;3];B5=[3;3;-1];B6=[-2;-2;2];
%L1=10;L2=10;L3=10;L4=10;L5=10;L6=10;
%%%%%%%%%%%%%%%%%%%%%%%%%已知数据(罗佑新农机学报08年）
A1=[0;0;0];A2=[2;0;0]; A3=[3;1;0];
A4=[5;3;1];A5=[3;5;-1];A6=[-1;2;2];
B1=[0;0;0];B2=[1;0;0];B3=[2;2;0];
%B4=[3;2;3];B5=[3;3;-1];B6=[-2;-2;2];
%L1=10;L2=10;L3=10;L4=10;L5=10;L6=10;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%55
A1=[0;A1];A2=[0;A2];A3=[0;A3];
A4=[0;A4];A5=[0;A5];A6=[0;A6];
B1=[0;B1];B2=[0;B2]; B3=[0;B3];
B4=[0;B4];B5=[0;B5];B6=[0;B6];
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:6
  if i==1
      A=A1;B=B1;L=L1;
  elseif i==2
      A=A2;B=B2;L=L2;
  elseif i==3
      A=A3;B=B3;L=L3;
  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));
  %%%%%%%%%%%%%
  if i==1
    f=[Temp0(1)-sym(1);Temp1(1);Temp5(1)-L^2];
  else
    f=[f;Temp5(1)-L^2];
  end
 end
f=f
%%%%%%%%%%%%%%%%%%%%