【机器人学习】六足机器人simulink仿真（运动学分析与步态仿真）

分析内容有：六角画圆，单腿画三角形

整体直行

求逆函数代码：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%输入腿的足尖坐标和ID号即可得到腿的三个关节角%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [q1 ,q2, q3] = ikinematics(x ,y, z, ID)

L0=1.3716;

L1 = 0.66;

L2 = 0.94;

L3 = 1.43;

if ID == 1

   x0 = L0 * cos(1.0728);

   y0 = L0 * sin(1.0728);

   q1 = (atan(-(y-y0)/(x+x0)) - 1.0728) ;

   X=x+x0+L1*cos(q1+1.0728);

   Y= y-y0-L1*sin(q1+1.0728);

   L = (     X^2 + Y^2  + z^2   )^0.5 ;

   q2 =( acos((L2^2+L^2-L3^2)/(2*L2*L)) +  atan(z/(X^2+Y^2)^0.5) );

   q3 = 94/180*pi-acos((L2^2+L3^2-L^2)/(2*L2*L3));

end

if ID == 2

   q1 =atan(-y/(x+1.16)) ;

   X=x+1.16+L1*cos(q1);

   Y= y-L1*sin(q1);

   L = (     X^2 + Y^2  + z^2   )^0.5 ;

   q2 =(acos((L2^2+L^2-L3^2)/(2*L2*L)) +  atan(z/(X^2+Y^2)^0.5)) ;

   q3 = -94/180*pi+acos((L2^2+L3^2-L^2)/(2*L2*L3));

end

if ID == 3

   x0 = L0 * cos(1.0728);

   y0 = L0 * sin(1.0728);

   q1 =atan((x+x0)/(y+y0)) - 0.498 ;

   X=x+x0+L1*sin(q1+0.498);

   Y= y+y0+L1*cos(q1+0.498);

   L = (     X^2 + Y^2  + z^2   )^0.5 ;

   q2 = -( acos((L2^2+L^2-L3^2)/(2*L2*L)) +  atan(z/(X^2+Y^2)^0.5) );

   q3 = 94/180*pi-acos((L2^2+L3^2-L^2)/(2*L2*L3));

end

if ID == 4

   x0 = L0 * cos(1.0728);

   y0 = L0 * sin(1.0728);

   q1 = atan(-(y+y0)/(x-x0)) - 1.0728 ;

   X=x-x0-L1*cos(q1+1.0728);

   Y= y+y0+L1*sin(q1+1.0728);

   L = (     X^2 + Y^2  + z^2   )^0.5 ;

   q2 = acos((L2^2+L^2-L3^2)/(2*L2*L)) +  atan(z/(X^2+Y^2)^0.5) ;

   q3 =-(acos((L2^2+L3^2-L^2)/(2*L2*L3))-94/180*pi);

end

if ID == 5

   q1 =-atan(y/(x-1.16)) ;

   X=x-1.16-L1*cos(q1);

   Y= y + L1*sin(q1);

   L = (     X^2 + Y^2  + z^2   )^0.5 ;

   q2 = -( acos((L2^2+L^2-L3^2)/(2*L2*L)) +  atan(z/(X^2+Y^2)^0.5) );

   q3 = - (acos((L2^2+L3^2-L^2)/(2*L2*L3))-94/180*pi );

end

if ID ==6

   x0 = L0 * cos(1.0728);

   y0 = L0 * sin(1.0728);

   q1 =atan((x-x0)/(y-y0)) - 0.498 ;

   X=x-x0-L1*sin(q1+0.498);

   Y= y-y0-L1*cos(q1+0.498);

   L = (    X^2 + Y^2  + z^2   )^0.5 ;

   q2 = -(acos((L2^2+L^2-L3^2)/(2*L2*L)) +  atan(z/(X^2+Y^2)^0.5)) ;

   q3 = 94/180*pi-acos((L2^2+L3^2-L^2)/(2*L2*L3));

end

end

需要技术服务联系qq2386317960