Matlab代码实现六轴机械臂和四轴SCARA机械臂的正逆运动学

一、 六轴机械臂 (PUMA 560)

六轴机械臂是最通用的工业机器人结构，具有6个旋转关节（RRRRRR）。

1. 正运动学 (Forward Kinematics)

正运动学是在已知各关节角度 \(\theta_i\) 的情况下，求取末端执行器在基坐标系下的位姿 \(T \in \mathbb{R}^{4 \times 4}\)。
我们采用 标准 DH 参数法 (Standard DH)。PUMA 560 的经典 DH 参数表如下：

关节 \(i\)	\(\alpha_{i-1}\) (扭角)	\(a_{i-1}\) (杆长)	\(d_i\) (偏距)	\(\theta_i\) (关节角)
1	0	0	0	\(\theta_1\)
2	\(-90^\circ\)	0	0	\(\theta_2\)
3	0	\(a_2\)	\(d_3\)	\(\theta_3\)
4	\(-90^\circ\)	\(a_3\)	\(d_4\)	\(\theta_4\)
5	\(90^\circ\)	0	0	\(\theta_5\)
6	\(-90^\circ\)	0	0	\(\theta_6\)

相邻坐标系的变换矩阵通式为：

\[T_i^{i-1} = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_{i-1} & \sin\theta_i\sin\alpha_{i-1} & a_{i-1}\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_{i-1} & -\cos\theta_i\sin\alpha_{i-1} & a_{i-1}\sin\theta_i \\ 0 & \sin\alpha_{i-1} & \cos\alpha_{i-1} & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

末端位姿 \(T_6^0 = T_1^0 \cdot T_2^1 \cdot T_3^2 \cdot T_4^3 \cdot T_5^4 \cdot T_6^5\)。

2. 逆运动学 (Inverse Kinematics)

逆运动学是根据给定的末端位姿 \(T\)，反解出8组可能的关节角度 \((\theta_1, ..., \theta_6)\)。PUMA 560 存在封闭解（解析解），通常采用 Pieper 准则（当三个相邻关节轴线交于一点时存在解析解）来求解。
其核心思路是矩阵方程两边元素对应相等，通过提取 \(T_6^0\) 中的平移向量和旋转矩阵列向量，利用三角函数恒等式依次求解各个关节角。

3. 工作空间 (Workspace)

六轴机械臂的工作空间通常是一个复杂的三维几何体。工程上最常使用 蒙特卡洛法 (Monte Carlo Method) 进行随机采样估计：在关节极限范围内随机生成大量关节角度组合，通过正运动学计算出末端坐标，绘制三维散点图。

4. Matlab 代码实现 (六轴)

%% 六轴机械臂 (PUMA 560) 正运动学与工作空间分析
clear; clc; close all;

% PUMA 560 DH 参数 [alpha, a, d, theta_offset]
% 这里的 theta 存储的是变量符号，实际计算时会加上输入的角度 q
DH = [0,      0,      0,      0;
      -pi/2,  0,      0,      -pi/2;
      0,      0.4318, 0.15005, 0;
      -pi/2,  0.0203, 0.4318, 0;
      pi/2,   0,      0,      0;
      -pi/2,  0,      0,      pi/2]; 

% --- 1. 单采样正运动学测试 ---
q_test = deg2rad([10, 20, 30, 40, 50, 60]); % 测试关节角
T_end = forward_kinematics(DH, q_test);
disp('测试位姿矩阵 T:');
disp(T_end);

% --- 2. 蒙特卡洛法求工作空间 ---
N = 10000; % 采样点数
workspace_points = zeros(N, 3);

% 关节限位 (示例设定，可根据实际修改)
joint_limits = deg2rad([-160, 160; -225, 45; -45, 225; -110, 170; -100, 100; -266, 266]);

for i = 1:N
    % 在限位内随机生成一组关节角
    q_rand = joint_limits(:,1)' + rand(1,6) .* (joint_limits(:,2)-joint_limits(:,1))';
    T_rand = forward_kinematics(DH, q_rand);
    workspace_points(i,:) = T_rand(1:3,4)';
end

% 绘制工作空间点云
figure('Name', '六轴机械臂工作空间 (蒙特卡洛法)', 'Color', 'w', 'Position', [100 100 800 600]);
scatter3(workspace_points(:,1), workspace_points(:,2), workspace_points(:,3), 2, 'b', 'filled');
grid on; axis equal; view(3);
title('六轴机械臂可达工作空间点云');
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');

%% 正运动学函数
function T = forward_kinematics(DH, q)
    T = eye(4);
    for i = 1:size(DH, 1)
        alpha = DH(i, 1);
        a = DH(i, 2);
        d = DH(i, 3);
        theta = DH(i, 4) + q(i);
        
        Ti = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
              sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
              0,          sin(alpha),            cos(alpha),            d;
              0,          0,                     0,                     1];
        T = T * Ti;
    end
end

---

二、 四轴 SCARA 机械臂

SCARA（Selective Compliance Assembly Robot Arm）机械臂广泛应用于平面装配作业。它拥有3个旋转关节（\(R\)）和1个移动关节（\(P\)），即结构为 \(RRRP\)。前3个关节决定末端位置 \((x, y, z)\)，第4个关节决定末端绕垂直轴的姿态（Yaw角）。

1. 正运动学 (Forward Kinematics)

设大臂长度 \(L_1\)，小臂长度 \(L_2\)，初始状态下大小臂完全伸直沿X轴放置。

• 位置: 纯几何关系即可求出。
  \[x = L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2) \]
  \[y = L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2) \]
  \[z = d_1 - d_3$$ (其中 $d_1$ 是基座总高，$\theta_3$ 对应第三关节的移动量，注意方向) \]
  旋转矩阵 \(R_z(\psi)\)。

2. 逆运动学 (Inverse Kinematics)

给定末端位置 \((x, y, z)\) 和姿态 \(\psi\) (即 yaw 角)，求 \(\theta_1, \theta_2, d_3, \theta_4\)。
这类似于一个平面二连杆的逆解问题：

1. 求 \(\theta_2\): 利用余弦定理。
   \[\cos\theta_2 = \frac{x^2 + y^2 - L_1^2 - L_2^2}{2 L_1 L_2} \]
   \(\theta_2\) 有两组解（肘部向上或向下，即 elbow_up 配置）。
2. 求 \(\theta_1\): 利用几何关系与 \(\theta_2\) 关联求解。
3. 求 \(d_3\): 直接由 \(z\) 坐标线性解算。
4. 求 \(\theta_4\): \(\theta_4 = \psi - \theta_1 - \theta_2\)。

3. 工作空间 (Workspace)

SCARA 的机械结构决定了它的工作空间是一个非常规则的 中空圆柱体（或圆环柱体）。

• 外径: \(R_{max} = L_1 + L_2\)
• 内径: \(R_{min} = |L_1 - L_2|\) (当大臂和小臂折叠时)
• 高度范围: 由 \(d_3\) 的最大和最小行程决定。

4. Matlab 代码实现 (四轴 SCARA)

%% 四轴 SCARA 机械臂 正逆运动学与工作空间分析
clear; clc; close all;

% SCARA 参数设定
L1 = 0.5;   % 大臂长度 (m)
L2 = 0.5;   % 小臂长度 (m)
h_base = 0.5; % 基座高度 (m)
z_min = h_base - 0.3; % Z轴最低点
z_max = h_base + 0.1; % Z轴最高点

% --- 1. 正运动学测试 ---
q_s = [pi/6, pi/6, 0.1, pi/4]; % [theta1, theta2, d3_offset, theta4]
T_s = scara_fk(q_s, L1, L2, h_base);
disp('SCARA 测试位姿 T:');
disp(T_s);

% --- 2. 逆运动学测试 ---
target_pos = T_s(1:3,4)';
target_yaw = atan2(T_s(2,1), T_s(1,1)); % 提取当前yaw角
q_s_inv = scara_ik(target_pos, target_yaw, L1, L2, h_base);
disp('逆运动学反解得到的关节角:');
disp(rad2deg(q_s_inv));

% --- 3. 工作空间绘制 ---
N = 2000;
ws_points = zeros(N, 3);
for i = 1:N
    % 在关节/行程限位内随机取值
    t1 = -pi + 2*pi*rand();
    t2 = -pi + 2*pi*rand();
    d3 = -0.3 + 0.4*rand(); % 第三关节移动量
    
    x = L1*cos(t1) + L2*cos(t1+t2);
    y = L1*sin(t1) + L2*sin(t1+t2);
    z = h_base + d3;
    ws_points(i,:) = [x, y, z];
end

figure('Name', 'SCARA 机械臂工作空间', 'Color', 'w', 'Position', [100 100 800 600]);
scatter3(ws_points(:,1), ws_points(:,2), ws_points(:,3), 4, 'r', 'filled');
hold on; grid on; axis equal; view(3);
% 绘制理论内外径圆
[xe, ye] = pol2cart(linspace(0, 2*pi, 100), L1+L2);
plot3(xe, ye, ones(1,100)*z_min, 'k-', 'LineWidth', 1.5);
plot3(xe, ye, ones(1,100)*z_max, 'k-', 'LineWidth', 1.5);

title('SCARA 机械臂可达工作空间 (中空圆柱体)');
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');

%% SCARA 正运动学函数
function T = scara_fk(q, L1, L2, h)
    t1 = q(1); t2 = q(2); d3 = q(3); t4 = q(4);
    x = L1*cos(t1) + L2*cos(t1+t2);
    y = L1*sin(t1) + L2*sin(t1+t2);
    z = h + d3;
    yaw = t1 + t2 + t4;
    
    T = eye(4);
    T(1,1) = cos(yaw); T(1,2) = -sin(yaw); T(1,4) = x;
    T(2,1) = sin(yaw); T(2,2) = cos(yaw);  T(2,4) = y;
    T(3,3) = 1;        T(3,4) = z;
end

%% SCARA 逆运动学函数 (返回两组解)
function q = scara_ik(pos, yaw, L1, L2, h)
    x = pos(1); y = pos(2); z = pos(3);
    
    % 解 theta2
    D = (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2);
    theta2_1 = atan2(sqrt(1-D^2), D); % 肘向上
    theta2_2 = atan2(-sqrt(1-D^2), D); % 肘向下
    
    % 解 theta1
    beta = atan2(y, x);
    phi = atan2(L2*sin(theta2_1), L1 + L2*cos(theta2_1));
    theta1_1 = beta - phi;
    
    phi = atan2(L2*sin(theta2_2), L1 + L2*cos(theta2_2));
    theta1_2 = beta - phi;
    
    % 解 d3 和 theta4
    d3 = z - h;
    theta4_1 = yaw - theta1_1 - theta2_1;
    theta4_2 = yaw - theta1_2 - theta2_2;
    
    % 返回两组解 [t1, t2, d3, t4]
    q = [theta1_1, theta2_1, d3, theta4_1;
         theta1_2, theta2_2, d3, theta4_2];
end

参考代码 六轴机械臂和4轴SCARA机械臂的正逆运动学解和工作空间 www.youwenfan.com/contentcnu/63389.html

总结与拓展建议

1. 验证代码：你可以直接将上述两段Matlab代码分别复制到 .m 文件中运行，观察生成的三维工作空间点云。
2. 六轴逆解补充：上述六轴代码仅包含了正运动学。如果你需要在Matlab中求六轴逆运动学，推荐使用 Robotics System Toolbox 中的 inverseKinematics 类（数值解，通用性强），或者自行编写基于 Pieper 法的代数求解函数（解析解，速度快但对奇异位形敏感）。
3. 可视化增强：如果你想看到机械臂的实体骨架连线，可以在正运动学的基础上，计算每一个关节的坐标，然后使用 plot3 将其连成线段。