机器人学入门：从机械臂基础到MATLAB实战（附代码示例）

在工业自动化与智能制造快速发展的今天，机械臂作为核心执行单元，其精准控制能力直接决定了生产线的效率与质量。不同于传统教材的理论堆砌，本文将带您以工程师视角，通过MATLAB代码实现机械臂运动学与动力学的关键算法，让抽象公式转化为可运行的仿真结果。无论您是机器人专业学生还是希望转型自动化的工程师，这套"理论+代码+可视化"的学习路径都能帮助您快速建立直觉理解。

1. 机械臂基础概念与坐标系建立

机械臂的本质是一组通过关节连接的刚性连杆系统。要描述其运动，首先需要建立完整的坐标系体系：

• 基坐标系（Base Frame）：固定在机械臂底座，作为所有运动的参考基准
• 关节坐标系（Joint Frame）：每个关节处建立的局部坐标系
• 工具坐标系（Tool Frame）：末端执行器上的工作坐标系

在MATLAB中，我们可以使用Robotics System Toolbox快速构建这些坐标系：

% 创建SCARA型机械臂模型
robot = rigidBodyTree('DataFormat','column');

% 添加基座和第一个旋转关节
base = robot.Base;
j1 = rigidBody('j1');
jnt1 = rigidBodyJoint('jnt1','revolute');
setFixedTransform(jnt1,trvec2tform([0 0 0.5]));
j1.Joint = jnt1;
addBody(robot,j1,'base');

提示：trvec2tform函数将平移向量转换为齐次变换矩阵，这是机器人学中的标准表示方法

2. 正运动学：从关节角度到末端位姿

正运动学解决"给定各关节角度，计算末端执行器位姿"的问题。以常见的6轴工业机器人为例，其运动链可以用D-H参数法描述：

关节	θ (rad)	d (m)	a (m)	α (rad)
1	q1	0.5	0	π/2
2	q2	0	0.8	0
3	q3	0	0.6	0
4	q4	0.7	0	π/2
5	q5	0	0	-π/2
6	q6	0.2	0	0

对应的MATLAB正运动学计算代码：

function T = forwardKinematics(q)
    % 各关节D-H参数
    dh_params = [0.5 0 0 pi/2;
                 0 0.8 0 0;
                 0 0.6 0 0;
                 0.7 0 0 pi/2;
                 0 0 0 -pi/2;
                 0.2 0 0 0];
    
    T = eye(4);
    for i = 1:6
        ct = cos(q(i)+dh_params(i,1));
        st = sin(q(i)+dh_params(i,1));
        ca = cos(dh_params(i,4));
        sa = sin(dh_params(i,4));
        
        Ti = [ct -st*ca st*sa dh_params(i,3)*ct;
              st ct*ca -ct*sa dh_params(i,3)*st;
              0 sa ca dh_params(i,2);
              0 0 0 1];
        T = T * Ti;
    end
end

可视化验证时，可以随机生成一组关节角度并显示机械臂构型：

q = rand(6,1)*2*pi - pi; % 生成-π到π之间的随机角度
show(robot,q);
T = forwardKinematics(q);
disp('末端执行器位姿：');
disp(T);

3. 逆运动学：从目标位姿反解关节角度

逆运动学是机械臂控制的核心难点，通常有解析法和数值法两种求解方式。对于6自由度机械臂，我们采用基于雅可比矩阵的迭代方法：

function q = inverseKinematics(T_desired, q_init, max_iter)
    q = q_init;
    for k = 1:max_iter
        T_current = forwardKinematics(q);
        error = [T_desired(1:3,4)-T_current(1:3,4);
                 tr2rpy(T_desired(1:3,1:3)) - tr2rpy(T_current(1:3,1:3))];
        
        if norm(error) < 1e-6
            break;
        end
        
        J = computeJacobian(q); % 雅可比矩阵计算函数
        dq = pinv(J)*error;
        q = q + 0.1*dq;
    end
end

实际应用中需要考虑关节限位和奇异点规避。当接近奇异构型时，雅可比矩阵条件数会急剧增大：

function [q, success] = safeIK(T_desired, q_init)
    q = q_init;
    success = false;
    for k = 1:100
        [T, J] = getTransformAndJacobian(q);
        condJ = cond(J);
        
        if condJ > 1e4
            % 奇异点规避策略
            q = q + 0.1*randn(size(q));
            continue;
        end
        
        % 正常迭代过程...
    end
end

4. 动力学建模与轨迹规划

机械臂动力学描述力/力矩与运动之间的关系，常用拉格朗日方程表达：

$$ \tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) $$

在MATLAB中可以通过Symbolic Math Toolbox自动推导动力学方程：

syms q1 q2 q3 dq1 dq2 dq3 ddq1 ddq2 ddq3 real
% 定义动能和势能表达式
T = 0.5*(I1*dq1^2 + I2*dq2^2 + I3*dq3^2);
V = m1*g*l1*cos(q1) + m2*g*(l1*cos(q1)+l2*cos(q2));

% 拉格朗日方程推导
L = T - V;
tau1 = simplify(diff(diff(L,dq1),t) - diff(L,q1));
tau2 = simplify(diff(diff(L,dq2),t) - diff(L,q2));
tau3 = simplify(diff(diff(L,dq3),t) - diff(L,q3));

对于轨迹规划，常用五次多项式插值确保起点和终点的位置、速度、加速度连续：

function [q, dq, ddq] = quinticTraj(q0, qf, t, tf)
    % 计算五次多项式系数
    a0 = q0;
    a1 = 0;
    a2 = 0;
    a3 = (20*(qf-q0) - (8*dqf+12*dq0)*tf - (3*ddq0-ddqf)*tf^2)/(2*tf^3);
    a4 = (30*(q0-qf) + (14*dqf+16*dq0)*tf + (3*ddq0-2*ddqf)*tf^2)/(2*tf^4);
    a5 = (12*(qf-q0) - 6*(dqf+dq0)*tf + (ddqf-ddq0)*tf^2)/(2*tf^5);
    
    % 计算轨迹
    q = a0 + a1*t + a2*t.^2 + a3*t.^3 + a4*t.^4 + a5*t.^5;
    dq = a1 + 2*a2*t + 3*a3*t.^2 + 4*a4*t.^3 + 5*a5*t.^4;
    ddq = 2*a2 + 6*a3*t + 12*a4*t.^2 + 20*a5*t.^3;
end

5. 实际应用中的进阶技巧

在工业现场部署时，还需要考虑以下实际问题：

• 碰撞检测：使用bounding box或精确几何模型

  function collision = checkCollision(robot, q, obstacles)
      % 获取所有连杆的位姿
      [~, allTransforms] = getTransform(robot, q);
      
      % 检查每个障碍物与连杆的距离
      for i = 1:length(obstacles)
          for j = 2:robot.NumBodies
              dist = computeDistance(allTransforms(:,:,j), obstacles{i});
              if dist < safety_margin
                  collision = true;
                  return;
              end
          end
      end
      collision = false;
  end
  

• 力矩饱和处理：当所需力矩超过电机额定值时

  function tau_sat = torqueSaturation(tau_desired, tau_max)
      ratio = min(abs(tau_max)./abs(tau_desired));
      if ratio < 1
          tau_sat = ratio * tau_desired;
          warning('Torque saturated at %.1f%%', ratio*100);
      else
          tau_sat = tau_desired;
      end
  end
  

• 振动抑制：通过输入整形技术减少末端振动

  function shaped_cmd = inputShaping(raw_cmd, freq, damping)
      % 计算输入整形器参数
      K = exp(-damping*pi/sqrt(1-damping^2));
      t_d = pi/(freq*sqrt(1-damping^2));
      
      % 应用两脉冲整形器
      shaped_cmd = [raw_cmd*1/(1+K); 
                    raw_cmd*K/(1+K)];
      time_vector = [0; t_d];
  end