【机器人学】2-3.六自由度机器人运动学逆解-工业机器人【附MATLAB代码】

解析解推导

假设有一工业机器人参数如下图所示：

matlab代码

clc;clear;
%带入机器人初始值
d1 = 0.670;
d4 = 1.280;
d6 = 0.215;

a2 = 0.312;
a3 = 1.075;
a4 = 0.225;

Position=[288.1, -433.1, 2832,96.9158,  -29.6162,  109.3547];
px = Position(1)/1000;
py = Position(2)/1000;
pz = Position(3)/1000;
rx = Position(4);
ry = Position(5);
rz = Position(6);

posture = [rx,ry,rz]/180*pi;
a = Euler2RotMat(posture);

px = px - a(1,3)*d6;
py = py - a(2,3)*d6;
pz = pz - a(3,3)*d6;

%目标位置姿态矩阵
nx=a(1,1);ox=a(1,2);ax=a(1,3);
ny=a(2,1);oy=a(2,2);ay=a(2,3);
nz=a(3,1);oz=a(3,2);az=a(3,3);

% 求解关节角1  
    theta1_1 = - atan2(py,-px) ;
    theta1_2 = - atan2(py,-px)+ pi ;  
% 求解关节角3
    c1_1 = cos(theta1_1);
    s1_1 = sin(theta1_1);
    c1_2 = cos(theta1_2);
    s1_2 = sin(theta1_2);
    Q = d4*d4+a4*a4;
    M1 = ((c1_1*px+s1_1*py-a2)^2+(pz-d1)^2-a4*a4-d4*d4-a3*a3)/(2*a3);
    M2 = ((c1_2*px+s1_2*py-a2)^2+(pz-d1)^2-a4*a4-d4*d4-a3*a3)/(2*a3);
    theta3_1 =  atan2(M1,sqrt(Q-M1^2))-atan2(a4,d4);
    theta3_2 =  atan2(M1,-sqrt(Q-M1^2))-atan2(a4,d4);   
    theta3_3 =  atan2(M2,sqrt(Q-M2^2))-atan2(a4,d4);   
    theta3_4 =  atan2(M2,-sqrt(Q-M2^2))-atan2(a4,d4);    
    
% 求解关节角2
    t1_1 = px*c1_1+py*s1_1-a2;
    t1_2 = px*c1_2+py*s1_2-a2;
    t2 = pz-d1;
    
    A1 = a4+a3*cos(theta3_1);
    A2 = a4+a3*cos(theta3_2);
    A3 = a4+a3*cos(theta3_3);
    A4 = a4+a3*cos(theta3_4);
    
    B1 = -d4-a3*sin(theta3_1);
    B2 = -d4-a3*sin(theta3_2);
    B3 = -d4-a3*sin(theta3_3);
    B4 = -d4-a3*sin(theta3_4);
    
    theta2_1 = atan2(-B1,A1) + atan2(t2,t1_1) - theta3_1 ;
    theta2_2 = atan2(-B2,A2) + atan2(t2,t1_1) - theta3_2 ;   
    theta2_3 = atan2(-B3,A3) + atan2(t2,t1_2) - theta3_3 ; 
    theta2_4 = atan2(-B4,A4) + atan2(t2,t1_2) - theta3_4 ;

% 求解关节角5
    k1_1 = -nx*cos(theta1_1)*sin(theta3_1+theta2_1)-ny*sin(theta1_1)*sin(theta3_1+theta2_1)+nz*cos(theta3_1+theta2_1);
    k1_2 = -nx*cos(theta1_1)*sin(theta3_2+theta2_2)-ny*sin(theta1_1)*sin(theta3_2+theta2_2)+nz*cos(theta3_2+theta2_2);
    k1_3 = -nx*cos(theta1_2)*sin(theta3_3+theta2_3)-ny*sin(theta1_2)*sin(theta3_3+theta2_3)+nz*cos(theta3_3+theta2_3);
    k1_4 = -nx*cos(theta1_2)*sin(theta3_4+theta2_4)-ny*sin(theta1_2)*sin(theta3_4+theta2_4)+nz*cos(theta3_4+theta2_4);
    
    k2_1 = -ox*cos(theta1_1)*sin(theta3_1+theta2_1)-oy*sin(theta1_1)*sin(theta3_1+theta2_1)+oz*cos(theta3_1+theta2_1);
    k2_2 = -ox*cos(theta1_1)*sin(theta3_2+theta2_2)-oy*sin(theta1_1)*sin(theta3_2+theta2_2)+oz*cos(theta3_2+theta2_2);
    k2_3 = -ox*cos(theta1_2)*sin(theta3_3+theta2_3)-oy*sin(theta1_2)*sin(theta3_3+theta2_3)+oz*cos(theta3_3+theta2_3);
    k2_4 = -ox*cos(theta1_2)*sin(theta3_4+theta2_4)-oy*sin(theta1_2)*sin(theta3_4+theta2_4)+oz*cos(theta3_4+theta2_4);

    k3_1 = -ax*cos(theta1_1)*sin(theta3_1+theta2_1)-ay*sin(theta1_1)*sin(theta3_1+theta2_1)+az*cos(theta3_1+theta2_1);
    k3_2 = -ax*cos(theta1_1)*sin(theta3_2+theta2_2)-ay*sin(theta1_1)*sin(theta3_2+theta2_2)+az*cos(theta3_2+theta2_2);
    k3_3 = -ax*cos(theta1_2)*sin(theta3_3+theta2_3)-ay*sin(theta1_2)*sin(theta3_3+theta2_3)+az*cos(theta3_3+theta2_3);
    k3_4 = -ax*cos(theta1_2)*sin(theta3_4+theta2_4)-ay*sin(theta1_2)*sin(theta3_4+theta2_4)+az*cos(theta3_4+theta2_4);

    theta5_1 = atan2(sqrt(k1_1^2+k2_1^2),-k3_1);
    theta5_2 = atan2(sqrt(k1_2^2+k2_2^2),-k3_2);
    theta5_3 = atan2(sqrt(k1_3^2+k2_3^2),-k3_3);
    theta5_4 = atan2(sqrt(k1_4^2+k2_4^2),-k3_4);
    
    theta5_5 = -atan2(sqrt(k1_1^2+k2_1^2),-k3_1);
    theta5_6 = -atan2(sqrt(k1_2^2+k2_2^2),-k3_2);
    theta5_7 = -atan2(sqrt(k1_3^2+k2_3^2),-k3_3);
    theta5_8 = -atan2(sqrt(k1_4^2+k2_4^2),-k3_4);

% 求解关节角6
    theta6_1 = atan2(-k2_1/sin(theta5_1),k1_1/sin(theta5_1));
    theta6_2 = atan2(-k2_1/sin(theta5_5),k1_1/sin(theta5_5));
    theta6_3 = atan2(-k2_2/sin(theta5_2),k1_2/sin(theta5_2));
    theta6_4 = atan2(-k2_2/sin(theta5_6),k1_2/sin(theta5_6));
    theta6_5 = atan2(-k2_3/sin(theta5_3),k1_3/sin(theta5_3));
    theta6_6 = atan2(-k2_3/sin(theta5_7),k1_3/sin(theta5_7));
    theta6_7 = atan2(-k2_4/sin(theta5_4),k1_4/sin(theta5_4));
    theta6_8 = atan2(-k2_4/sin(theta5_8),k1_4/sin(theta5_8));
    
% 求解关节角4
    N1_1 = ax*cos(theta1_1)*cos(theta3_1+theta2_1)+ay*sin(theta1_1)*cos(theta3_1+theta2_1)+az*sin(theta3_1+theta2_1);
    N1_2 = ax*cos(theta1_1)*cos(theta3_2+theta2_2)+ay*sin(theta1_1)*cos(theta3_2+theta2_2)+az*sin(theta3_2+theta2_2);
    N1_3 = ax*cos(theta1_2)*cos(theta3_3+theta2_3)+ay*sin(theta1_2)*cos(theta3_3+theta2_3)+az*sin(theta3_3+theta2_3);
    N1_4 = ax*cos(theta1_2)*cos(theta3_4+theta2_4)+ay*sin(theta1_2)*cos(theta3_4+theta2_4)+az*sin(theta3_4+theta2_4);
    
    N2_1 = ax*sin(theta1_1)-ay*cos(theta1_1);
    N2_2 = ax*sin(theta1_2)-ay*cos(theta1_2);
    
    theta4_1 = atan2(N2_1/sin(theta5_1),N1_1/sin(theta5_1));
    theta4_2 = atan2(N2_1/sin(theta5_5),N1_1/sin(theta5_5));
    theta4_3 = atan2(N2_1/sin(theta5_2),N1_2/sin(theta5_2));
    theta4_4 = atan2(N2_1/sin(theta5_6),N1_2/sin(theta5_6));
    theta4_5 = atan2(N2_2/sin(theta5_3),N1_3/sin(theta5_3));
    theta4_6 = atan2(N2_2/sin(theta5_7),N1_3/sin(theta5_7));
    theta4_7 = atan2(N2_2/sin(theta5_4),N1_4/sin(theta5_4));
    theta4_8 = atan2(N2_2/sin(theta5_8),N1_4/sin(theta5_8));
theta_STD = [ 
              theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1;
              theta1_1,theta2_1,theta3_1,theta4_2,theta5_5,theta6_2;
              theta1_1,theta2_2,theta3_2,theta4_3,theta5_2,theta6_3;
              theta1_1,theta2_2,theta3_2,theta4_4,theta5_6,theta6_4;
              theta1_2,theta2_3,theta3_3,theta4_5,theta5_3,theta6_5;
              theta1_2,theta2_3,theta3_3,theta4_6,theta5_7,theta6_6;
              theta1_2,theta2_4,theta3_4,theta4_7,theta5_4,theta6_7;
              theta1_2,theta2_4,theta3_4,theta4_8,theta5_8,theta6_8;
             ]*180/pi
function rotMat=Euler2RotMat(rpy)
    rx=rpy(1);
    ry=rpy(2);
    rz=rpy(3);
    
    cRx=cos(rx);
    sRx=sin(rx);
    cRy=cos(ry);
    sRy=sin(ry);
    cRz=cos(rz);
    sRz=sin(rz);
    rotMat=[cRz*cRy cRz*sRy*sRx-sRz*cRx cRz*sRy*cRx+sRz*sRx
       sRz*cRy sRz*sRy*sRx+cRz*cRx sRz*sRy*cRx-cRz*sRx
       -sRy       cRy*sRx                      cRy*cRx];
end

当输入：Position=[288.1, -433.1, 2832,96.9158,  -29.6162,  109.3547]时（姿态用ZYX欧拉角表示，此数据来自机器人示教器上每个关节为100°时的正解值），结果为：