【机器人学习】六足机器人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

咨询链接：matlab正逆运动学分析与轨迹规划