三轴串联机器人逆运动解matlab程序

guilty2468

根据足端位置，本体和肩关节位置，反求三个关节角。单片机运行速度可以满足实时性
function angle=anglecal(zu1,rc,b) %%

%% zu1 为足端位置， rc为本体中心坐标，b为 髋关节坐标。求解

%% th1 th2 th3 为三个关节角。
%b=[-0.1 0.2 0];
%  rc=[0 0.25 0.25];
r=[1,0,0;0,1,0;0,0,1;];
% zu1=[-0.3,0.45,0.45];
  zub1=r*(zu1.'-rc.')-b.';
  x=zub1(1,1);
  y=zub1(2,1);
  z=zub1(3,1);
  L=sqrt(x*x+y*y);

  if y<0&&y/x<-sqrt(3)/3
      L=-sqrt(x*x+y*y);
  end;
  if y<0&&y/x>0
         L=-sqrt(x*x+y*y);
  end;
  if y>0&&y/x>-sqrt(3)/3&&y/x<0
        L=-sqrt(x*x+y*y);
  end;

      
%   if y<0
%        L=-sqrt(x*x+y*y);
%   end;
%   if y>0
%        L=sqrt(x*x+y*y);
%   end;


  l1=50;
  l2=120;
  l3=200;
  th1=atan2(y,x);
  a1=-2*l2*z;
  a2=2*l2*(L-l1);
  a3=z*z+(L-l1)*(L-l1)+l2*l2-l3*l3;
  b1=-2*l3*z;
  b2=2*l3*(L-l1);
  b3=z*z+(L-l1)*(L-l1)+l3*l3-l2*l2;
  t1=(a1-sqrt(abs(a1*a1+a2*a2-a3*a3)))/(a2+a3);
  t2=(b1+sqrt(abs(b1*b1+b2*b2-b3*b3)))/(b2+b3);
  
  t11=(a1+sqrt(abs(a1*a1+a2*a2-a3*a3)))/(a2+a3);
  t21=(b1-sqrt(abs(b1*b1+b2*b2-b3*b3)))/(b2+b3);
  
  th2=2*atan(t1);
  th3=2*atan(t2)-2*atan(t1);
  
  th21=2*atan(t11);
  th31=2*atan(t21)-2*atan(t11);
%     th1=th1*180/3.14;
%     th2=th2*180/3.14;
%     th3=th3*180/3.14;
%     th21=th21*180/3.14;
%     th31=th31*180/3.14;
if th1<-pi/6
    th1=th1+pi;
   
end;
if th1>5*pi/6
    th1=th1-pi;
% if th1<0
%     th1=th1+pi;
   
end;
  angle=[th1 th2 th3 ];

longwu537

为什么如此好贴没人顶呢

guilty2468

longwu537 发表于 2012-9-14 10:36
为什么如此好贴没人顶呢

谢谢夸奖，不过这种方法有点问题，他那个足端跑到本体下方后得到的角度就不对，需要修正。
陈学东的书里面也没讲，我哪里的if就是根据我的进行的修正，但是还是有问题。
还可以用神经网络模拟，过一段我再发。