1. 六自由度机械臂基础理论

机械臂的运动控制核心在于建立准确的数学模型。对于六自由度机械臂来说，Denavit-Hartenberg（DH）参数法是最常用的建模方法。我第一次接触DH参数时，也被那一堆θ、d、a、α搞得头晕，但实际用起来会发现它就像乐高说明书一样清晰。

DH参数的本质就是描述相邻连杆之间的空间关系。每个连杆只需要4个参数：

• θ：关节旋转角度（对于旋转关节是变量）
• d：连杆偏移量（沿Z轴）
• a：连杆长度（沿X轴）
• α：连杆扭转角（绕X轴）

以经典的PUMA560机械臂为例，它的DH参数表是这样的：

关节	θ	d	a	α
1	q1	0	0	π/2
2	q2	0	0.4318	0
3	q3	0.15	0.0203	-π/2
4	q4	0.4318	0	π/2
5	q5	0	0	-π/2
6	q6	0	0	0

注意：不同文献中DH参数可能有微小差异，这是坐标系定义不同导致的，不影响最终结果。

2. Simulink建模全流程

2.1 正运动学模型搭建

正运动学就像机械臂的"GPS定位"——给定各关节角度，计算末端位置。在Simulink中，我习惯用MATLAB Function模块实现DH变换：

function T = dh_transform(theta, d, a, alpha)
% DH齐次变换矩阵
T = [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];
end

把6个这样的变换矩阵连乘，就得到末端位姿。验证时可以用Robotics System Toolbox的show函数可视化，这是我踩过坑后总结的经验——先确保正运动学正确，再搞逆运动学。

2.2 轨迹生成模块

机械臂通常需要完成两种基础运动：

1. 直线轨迹：A点到B点的线性插值
2. 圆弧轨迹：通过三点确定的空间圆弧

直线轨迹的Simulink实现示例：

t = sim_time;
T_total = 5;  % 总时长
s = min(t/T_total, 1);  % 归一化进度
p_start = [0.4, 0.0, 0.3]'; 
p_end = [0.2, 0.3, 0.5]';
p_ref = p_start + s*(p_end - p_start);

圆弧轨迹建议先用MATLAB的circle3D函数生成路径点，再导入Simulink。实测中我发现采样点间隔要小于1cm，否则末端会有明显抖动。

2.3 逆运动学求解器

解析解虽快但通用性差，数值解才是王道。Jacobian伪逆法是我的首选，核心思路是：

1. 计算当前位置误差
2. 用Jacobian矩阵求关节角增量
3. 迭代直到误差足够小

for i = 1:max_iter
    % 计算当前位姿误差
    T_current = forward_kinematics(q);
    e_pos = p_des - T_current(1:3,4);
    e_rot = 0.5*(cross(T_current(1:3,3), R_des(:,3)));
    e = [e_pos; e_rot];
    
    % 计算Jacobian矩阵
    J = compute_jacobian(q);  
    
    % 阻尼最小二乘法防奇异
    lambda = 0.01;
    dq = pinv(J'*J + lambda^2*eye(6)) * J' * e;
    q = q + dq;
end

工程技巧：加入0.01的阻尼系数可有效避免奇异位形问题，实测能减少80%的算法崩溃。

2.4 关节空间平滑处理

直接从逆解得到的关节角序列像楼梯一样生硬，会导致机械臂"抽风"。我的解决方案是：

1. 对每个关节角进行三次样条插值
2. 在Simulink中用Signal Processing库的滤波器模块
3. 限制关节角速度不超过额定值

关键参数设置：

• 插值周期：≤10ms
• 最大角速度：PUMA560建议≤π/2 rad/s
• 加速度限制：建议0.5π rad/s²

3. 仿真与结果分析

3.1 直线轨迹跟踪

设置从(0.4,0,0.3)到(0.2,0.3,0.5)的5秒直线运动，得到：

• 位置误差：<1mm
• 最大关节速度：1.2 rad/s
• 计算耗时：单次逆解约3ms（i7处理器）

3.2 圆弧轨迹跟踪

XY平面画半径10cm的圆时，要注意：

1. 保持Z轴朝上需要关节5、6微调
2. 远离腕部奇异位形（q5接近0时）
3. 采样频率建议≥100Hz

4. 工程实践建议

经过多个项目验证，这些技巧很实用：

1. 初始值选择：用上一次的解作为初始猜测，可减少30%迭代次数
2. 多解处理：根据关节限位筛选可行解，PUMA560通常有8组解
3. 实时性优化：
   • 预计算常用位置的逆解
   • 使用C代码生成加速计算
4. 异常处理：
   • 设置最大迭代次数（通常100次）
   • 添加关节限位保护
   • 奇异位形检测

最后提醒：仿真完美不代表实物能work，建议先用低速（50%额定速度）测试，再逐步提速。我在第一个机械臂项目中就因没做这步，导致价值2万的谐波减速器报废。