解析解推导
假设有一工业机器人参数如下图所示：
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 ;
% 求解关节角3c1_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); % 求解关节角2t1_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 ;% 求解关节角5k1_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);% 求解关节角6theta6_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));% 求解关节角4N1_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*sRxsRz*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°时的正解值），结果为：
