六自由度机器人逆运动学解析与 MATLAB 实现

clc; clear; 
% 输入已知的位姿矩阵（正解可得）---------------------- 
a = [ 0.0000 0.0000 1.0000 800.0000; 
      -0.0000 -1.0000 0.0000 120.0000; 
       1.0000 -0.0000 -0.0000 10.0000; 
       0        0        0        1.0000 ]; 
%-------------------------------------------------

% 定义连杆的 D-H 参数
% 连杆偏移
d1 = 0; d2 = 120; d3 = 0; d4 = 400; d5 = 0; d6 = 0;
% 连杆长度
a2 = 400; a3 = 10;
% 连杆扭角
alpha1 = 0; alpha2 = -pi/2; alpha3 = 0; alpha4 = -pi/2; alpha5 = pi/2; alpha6 = -pi/2;

% 建立机器人模型
L1=Link([0 d1 0 alpha1],'modified');
L2=Link([0 d2 0 alpha2],'modified');
L3=Link([0 d3 a2 alpha3],'modified');
L4=Link([0 d4 a3 alpha4],'modified');
L5=Link([0 d5 0 alpha5],'modified');
L6=Link([0 d6 0 alpha6],'modified');

% 限制关节空间
L1.qlim = [(-165/180)*pi,(165/180)*pi];
L2.qlim = [(-150/180)*pi, (60/180)*pi];
L3.qlim = [(-150/180)*pi, (90/180)*pi];
L4.qlim = [(-180/180)*pi,(180/180)*pi];
L5.qlim = [(-115/180)*pi,(115/180)*pi];
L6.qlim = [(-360/180)*pi,(360/180)*pi];

% 连接连杆，机器人取名为 robot
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','6Rrobot');
robot.display();
robot.teach();

% 提取位姿矩阵元素
nx=a(1,1); ox=a(1,2); ax=a(1,3); px=a(1,4);
ny=a(2,1); oy=a(2,2); ay=a(2,3); py=a(2,4);
nz=a(3,1); oz=a(3,2); az=a(3,3); pz=a(3,4);

% --- 逆解计算部分 ---

% 解关节 1
theta1_1 = atan2(py,px)-atan2(d2,abs(sqrt(px^2+py^2-d2^2)));
theta1_2 = atan2(py,px)-atan2(d2,-abs(sqrt(px^2+py^2-d2^2)));

% 解关节 3
kk = (px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);
theta3_1 = atan2(a3,d4)-atan2(kk,abs(sqrt(a3^2+d4^2-kk^2)));
theta3_2 = atan2(a3,d4)-atan2(kk,-abs(sqrt(a3^2+d4^2-kk^2)));

% 解关节 2 (组合不同 theta1 和 theta3 的情况)
% 情况 1: theta1_1, theta3_1
ko2_1 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_1)-d4);
kt2_1 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_1)+a3);
theta23_1 = atan2(ko2_1,kt2_1);
theta2_1 = theta23_1 - theta3_1;

% 情况 2: theta1_2, theta3_1
ko2_2 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_1)-d4);
kt2_2 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_1)+a3);
theta23_2 = atan2(ko2_2,kt2_2);
theta2_2 = theta23_2 - theta3_1;

% 情况 3: theta1_1, theta3_2
ko2_3 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_2)-d4);
kt2_3 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_2)+a3);
theta23_3 = atan2(ko2_3,kt2_3);
theta2_3 = theta23_3 - theta3_2;

% 情况 4: theta1_2, theta3_2
ko2_4 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_2)-d4);
kt2_4 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_2)+a3);
theta23_4 = atan2(ko2_4,kt2_4);
theta2_4 = theta23_4 - theta3_2;

% 求解关节 4
% 情况 1
ko4_1=-ax*sin(theta1_1)+ay*cos(theta1_1);
kt4_1=-ax*cos(theta1_1)*cos(theta23_1)-ay*sin(theta1_1)*cos(theta23_1)+az*sin(theta23_1);
theta4_1=atan2(ko4_1,kt4_1);

% 情况 2
ko4_2=-ax*sin(theta1_2)+ay*cos(theta1_2);
kt4_2=-ax*cos(theta1_2)*cos(theta23_2)-ay*sin(theta1_2)*cos(theta23_2)+az*sin(theta23_2);
theta4_2=atan2(ko4_2,kt4_2);

% 情况 3
ko4_3=-ax*sin(theta1_1)+ay*cos(theta1_1);
kt4_3=-ax*cos(theta1_1)*cos(theta23_3)-ay*sin(theta1_1)*cos(theta23_3)+az*sin(theta23_3);
theta4_3=atan2(ko4_3,kt4_3);

% 情况 4
ko4_4=-ax*sin(theta1_2)+ay*cos(theta1_2);
kt4_4=-ax*cos(theta1_2)*cos(theta23_4)-ay*sin(theta1_2)*cos(theta23_4)+az*sin(theta23_4);
theta4_4=atan2(ko4_4,kt4_4);

% 求解关节 5
% 情况 1
ko5_1=-ax*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))-ay*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)-cos(theta1_1)*sin(theta4_1))+az*(sin(theta23_1)*cos(theta4_1));
kt5_1= ax*(-cos(theta1_1)*sin(theta23_1))+ay*(-sin(theta1_1)*sin(theta23_1))+az*(-cos(theta23_1));
theta5_1=atan2(ko5_1,kt5_1);

% 情况 2
ko5_2=-ax*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))-ay*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)-cos(theta1_2)*sin(theta4_2))+az*(sin(theta23_2)*cos(theta4_2));
kt5_2= ax*(-cos(theta1_2)*sin(theta23_2))+ay*(-sin(theta1_2)*sin(theta23_2))+az*(-cos(theta23_2));
theta5_2=atan2(ko5_2,kt5_2);

% 情况 3
ko5_3=-ax*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))-ay*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)-cos(theta1_1)*sin(theta4_3))+az*(sin(theta23_3)*cos(theta4_3));
kt5_3= ax*(-cos(theta1_1)*sin(theta23_3))+ay*(-sin(theta1_1)*sin(theta23_3))+az*(-cos(theta23_3));
theta5_3=atan2(ko5_3,kt5_3);

% 情况 4
ko5_4=-ax*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))-ay*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)-cos(theta1_2)*sin(theta4_4))+az*(sin(theta23_4)*cos(theta4_4));
kt5_4= ax*(-cos(theta1_2)*sin(theta23_4))+ay*(-sin(theta1_2)*sin(theta23_4))+az*(-cos(theta23_4));
theta5_4=atan2(ko5_4,kt5_4);

% 求解关节角 6
% 情况 1
ko6_1=-nx*(cos(theta1_1)*cos(theta23_1)*sin(theta4_1)-sin(theta1_1)*cos(theta4_1))-ny*(sin(theta1_1)*cos(theta23_1)*sin(theta4_1)+cos(theta1_1)*cos(theta4_1))+nz*(sin(theta23_1)*sin(theta4_1));
kt6_1= nx*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))*cos(theta5_1)-nx*cos(theta1_1)*sin(theta23_1)*sin(theta4_1)+ny*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)+cos(theta1_1)*sin(theta4_1))*cos(theta5_1)-ny*sin(theta1_1)*sin(theta23_1)*sin(theta5_1)-nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)+cos(theta23_1)*sin(theta5_1));
theta6_1=atan2(ko6_1,kt6_1);

% 情况 2
ko6_2=-nx*(cos(theta1_2)*cos(theta23_2)*sin(theta4_2)-sin(theta1_2)*cos(theta4_2))-ny*(sin(theta1_2)*cos(theta23_2)*sin(theta4_2)+cos(theta1_2)*cos(theta4_2))+nz*(sin(theta23_2)*sin(theta4_2));
kt6_2= nx*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))*cos(theta5_2)-nx*cos(theta1_2)*sin(theta23_2)*sin(theta4_2)+ny*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)+cos(theta1_2)*sin(theta4_2))*cos(theta5_2)-ny*sin(theta1_2)*sin(theta23_2)*sin(theta5_2)-nz*(sin(theta23_2)*cos(theta4_2)*cos(theta5_2)+cos(theta23_2)*sin(theta5_2));
theta6_2=atan2(ko6_2,kt6_2);

% 情况 3
ko6_3=-nx*(cos(theta1_1)*cos(theta23_3)*sin(theta4_3)-sin(theta1_1)*cos(theta4_3))-ny*(sin(theta1_1)*cos(theta23_3)*sin(theta4_3)+cos(theta1_1)*cos(theta4_3))+nz*(sin(theta23_3)*sin(theta4_3));
kt6_3= nx*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))*cos(theta5_3)-nx*cos(theta1_1)*sin(theta23_3)*sin(theta4_3)+ny*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)+cos(theta1_1)*sin(theta4_3))*cos(theta5_3)-ny*sin(theta1_1)*sin(theta23_3)*sin(theta5_3)-nz*(sin(theta23_3)*cos(theta4_3)*cos(theta5_3)+cos(theta23_3)*sin(theta5_3));
theta6_3=atan2(ko6_3,kt6_3);

% 情况 4
ko6_4=-nx*(cos(theta1_2)*cos(theta23_4)*sin(theta4_4)-sin(theta1_2)*cos(theta4_4))-ny*(sin(theta1_1)*cos(theta23_4)*sin(theta4_4)+cos(theta1_2)*cos(theta4_4))+nz*(sin(theta23_4)*sin(theta4_4));
kt6_4= nx*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))*cos(theta5_4)-nx*cos(theta1_2)*sin(theta23_4)*sin(theta4_4)+ny*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)+cos(theta1_2)*sin(theta4_4))*cos(theta5_1)-ny*sin(theta1_2)*sin(theta23_4)*sin(theta5_4)-nz*(sin(theta23_4)*cos(theta4_4)*cos(theta5_4)+cos(theta23_4)*sin(theta5_4));
theta6_4=atan2(ko6_4,kt6_4);

% 整理得到 4 组运动学非奇异逆解
theta_MOD1 = [ theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1; 
               theta1_2,theta2_2,theta3_1,theta4_2,theta5_2,theta6_2; 
               theta1_1,theta2_3,theta3_2,theta4_3,theta5_3,theta6_3; 
               theta1_2,theta2_4,theta3_2,theta4_4,theta5_4,theta6_4; ]*(180/pi);

% 将操作关节'翻转'可得到另外 4 组解
theta_MOD2 = [ theta1_1,theta2_1,theta3_1,theta4_1+pi,-theta5_1,theta6_1+pi; 
               theta1_2,theta2_2,theta3_1,theta4_2+pi,-theta5_2,theta6_2+pi; 
               theta1_1,theta2_3,theta3_2,theta4_3+pi,-theta5_3,theta6_3+pi; 
               theta1_2,theta2_4,theta3_2,theta4_4+pi,-theta5_4,theta6_4+pi; ]*(180/pi);

% 输出所有解
J = [theta_MOD1;theta_MOD2]

运行上述代码时，只需修改 a 矩阵中的位姿数据。若不确定如何获取该矩阵，可先通过正运动学仿真生成。代入已知位姿后，程序将输出 8 组可能的关节角度解，供后续路径规划选择。

总结

本文详细推导了六自由度机器人逆运动学的解析解过程，并结合 MATLAB 代码进行了验证。通过理解矩阵变换规律和三角代换技巧，可以有效解决多解性问题。这一过程不仅有助于深入理解机器人运动学原理，也为实际工程中的轨迹规划与控制提供了基础参考。