引言
在机器人学中，正运动学主要描述关节变量到末端执行器位姿的映射关系。而逆运动学（Inverse Kinematics, IK）则是已知末端位姿与连杆参数，反求关节变量的过程。由于机械结构的差异，一个末端位姿往往对应多组解，这使得逆运动学本质上是一个非线性超越方程组的求解问题。
本文以经典的 PUMA560 六自由度机器人为例，基于 D-H 参数法与齐次变换矩阵，详细推导其封闭解代数过程，并给出完整的 Matlab 实现代码。通过实际算例验证，帮助读者理解从理论公式到工程代码的完整落地路径。
一、运动学逆解原理
1. 封闭解法概述
对于大多数工业机械臂，若满足 Pieper 准则，则存在解析形式的封闭解。该准则要求：
- 三个相邻关节轴相交于一点；
- 或三个相邻关节轴相互平行。
PUMA560 属于典型的 6R 关节式机器人，前三个关节确定手腕参考点位置，后三个关节确定手腕方位，结构上满足上述准则，因此适合采用代数法进行逆解。
2. 代数推导过程
设末端执行器的齐次变换矩阵为 $T_6^0$，由六个单关节变换矩阵连乘得到： $$T_6^0 = T_1^0( heta_1) T_2^1( heta_2) T_3^2( heta_3) T_4^3( heta_4) T_5^4( heta_5) T_6^5( heta_6)$$
我们的目标是通过左乘逆变换矩阵，逐步分离出各个关节角 $ heta_i$。
(1) 求解 $ heta_1$
将方程两边左乘 $(T_1^0)^{-1}$，使左侧仅含 $ heta_1$。利用矩阵元素对应相等，特别是第 (2,4) 和第 (1,4)、(3,4) 元素的关系，结合三角代换可得： $$ heta_1 = ext{Atan2}(p_y, p_x) - ext{Atan2}ig(d_2, ext{sgn} imes ext{sqrt}(p_x^2 + p_y^2 - d_2^2)ig)$$ 其中正负号代表了两组可能的解（左/右肩构型）。
(2) 求解 $ heta_3$
选定 $ heta_1$ 的一个解后，构建关于 $ heta_2, heta_3$ 的方程组。通过平方和消去 $ heta_2$，可得到仅含 $ heta_3$ 的形式： $$a_3 c_3 - d_4 s_3 = k$$ 其中 $k$ 为已知量。解得： $$ heta_3 = ext{Atan2}(a_3, d_4) - ext{Atan2}ig(k, ext{sgn} imes ext{sqrt}(a_3^2 + d_4^2 - k^2)ig)$$ 同样存在肘部向上/向下的两种构型。
(3) 求解 $ heta_2$
利用已知的 $ heta_1, heta_3$，通过 $(T_3^0)^{-1} T_6^0$ 提取 $ heta_2$ 信息。计算中间变量 $ heta_{23} = heta_2 + heta_3$ 后，即可求得： $$ heta_2 = heta_{23} - heta_3$$
(4) 求解 $ heta_4, heta_5, heta_6$
剩余三个关节主要控制手腕姿态。通过 $(T_4^0)^{-1} T_6^0$ 和 $(T_5^0)^{-1} T_6^0$ 分别提取旋转矩阵元素，利用 Atan2 函数直接解算：
$$ heta_4 = ext{Atan2}(s_4, c_4), heta_5 = ext{Atan2}(s_5, c_5), heta_6 = ext{Atan2}(s_6, c_6)$$
需注意当 $s_5=0$ 时，机械臂处于奇异位置，此时 $ heta_4$ 与 $ heta_6$ 无法唯一确定，通常需特殊处理。
二、Matlab 代码实现
理论推导完成后，我们使用 Matlab 机器人工具箱及自定义脚本进行验证。以下代码实现了从齐次变换矩阵反解全部关节变量的功能。
clc; clear; % 初始化环境
% 输入已知的位姿矩阵（齐次变换矩阵 T6_0）
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 参数
% 连杆偏移 d
[d1, d2, d3, d4, d5, d6] = deal(0, 120, 0, 400, 0, 0);
% 连杆长度 a
[a2, a3] = deal(400, 10);
% 连杆扭角 alpha
alpha = [0, -pi/2, 0, -pi/2, pi/2, -pi/2];
% 建立机器人模型
L1 = Link([0 d1 0 alpha(1)], 'modified');
L2 = Link([0 d2 0 alpha(2)], 'modified');
L3 = Link([0 d3 a2 alpha(3)], 'modified');
L4 = Link([0 d4 a3 alpha(4)], 'modified');
L5 = Link([0 d5 0 alpha(5)], 'modified');
L6 = Link([0 d6 0 alpha(6)], '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 = SerialLink([L1 L2 L3 L4 L5 L6], 'name', '6Rrobot');
robot.display();
% 解析位姿矩阵元素
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. 解关节 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)));
% 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)));
% 3. 解关节 2 (组合 theta1 与 theta3)
% 这里演示 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;
% 注意：实际工程中需遍历所有组合 (2x2=4 种基础解)
% ... (此处省略重复组合代码，逻辑同上)
% 4. 解关节 4, 5, 6
% 基于已解出的前三个关节，利用旋转矩阵元素反解手腕角度
% 示例：theta4
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);
% 示例：theta5
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);
% 示例：theta6
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*(...) ...
- nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)+cos(theta23_1)*sin(theta5_1));
theta6_1 = atan2(ko6_1, kt6_1);
% 整理输出结果（转换为角度制）
J = [theta1_1, theta2_1, theta3_1, theta4_1, theta5_1, theta6_1] * (180/pi);
disp('逆解关节角 (度):');
disp(J);
