三轴串联机器人逆运动解matlab程序
根据足端位置，本体和肩关节位置，反求三个关节角。单片机运行速度可以满足实时性function angle=anglecal(zu1,rc,b) %%
%% zu1 为足端位置， rc为本体中心坐标，b为 髋关节坐标。求解
%% th1 th2 th3 为三个关节角。
%b=[-0.1 0.2 0];
%rc=;
r=;
% 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=;
为什么如此好贴没人顶呢 longwu537 发表于 2012-9-14 10:36 static/image/common/back.gif
为什么如此好贴没人顶呢
谢谢夸奖，不过这种方法有点问题，他那个足端跑到本体下方后得到的角度就不对，需要修正。
陈学东的书里面也没讲，我哪里的if就是根据我的进行的修正，但是还是有问题。
还可以用神经网络模拟，过一段我再发。
页:
[1]