SCARA机械臂的动力学模型

运动学 = "地图导航"
  - 告诉你怎么从A点走到B点
  - 不考虑你走路多累、背多重东西

动力学 = "体能消耗计算"
  - 计算你走这段路需要多少能量
  - 考虑体重、背包重量、路面坡度、风速

高速高精度应用：
  • 循环时间 < 0.5秒
  • 精度要求 < ±0.02mm
  • 加速度 > 2g
  • 负载变化频繁

典型应用：
  • 芯片封装机
  • 高速分拣机器人
  • 精密点胶设备
  • 3C产品组装
  
中低速应用：
  • 速度 < 1 m/s
  • 负载稳定
  • 轨迹简单

典型应用：
  • 普通物料搬运
  • 简单上下料
  • 教学演示

前馈补偿控制控制律：τ = τ_feedforward + τ_feedback
                    ↓
τ_ff = M(q_d)q̈_d + C(q_d, q̇_d)q̇_d + G(q_d) + F(q̇_d)
实现步骤：
1. 轨迹规划器生成期望轨迹 q_d(t), q̇_d(t), q̈_d(t)
2. 实时计算前馈力矩 τ_ff
3. 叠加PID反馈控制：τ_fb = Kp·e + Kd·ė
4. 总控制量：τ = τ_ff + τ_fb

计算力矩控制控制律：τ = M(q)[q̈_d + K_v·ė + K_p·e] + C(q, q̇)q̇ + G(q)
实现流程：
    期望轨迹 → 计算力矩控制器 → 动力学模型补偿 → 机械臂
          ↑                                ↓
      反馈比较 ← 位置/速度传感器 ← 实际运动

自适应控制的参数自适应控制结构
τ = Y(q, q̇, q̇_r, q̈_r) · θ̂ + K_D·s
# 参数更新律
θ̂̇ = -Γ·Yᵀ·s
# 其中：
# Y: 回归矩阵（已知函数）
# θ̂: 估计的参数向量
# s = ė + Λ·e: 滑模变量
# Γ: 自适应增益矩阵

简化策略1：分层控制
  高速层：使用完整动力学模型
  低速层：仅使用摩擦补偿 + PID

简化策略2：增益调度
  if (速度 < 阈值):
      使用低增益PID
  else:
      启用动力学前馈

简化策略3：分区线性化
  将工作空间分为若干区域
  每个区域使用不同的线性化模型

% 1. 精确建模振动模态
% 假设SCARA有主要振动频率 f_vib = 15 Hz
wn = 2*pi*15;  % 自然频率 rad/s

% 2. 设计输入整形器
% 两脉冲整形器消除15Hz振动
A1 = 1/(1+K); A2 = K/(1+K);
t1 = 0; t2 = π/wn;
K = exp(-ξ*π/sqrt(1-ξ^2));  % 阻尼比ξ=0.1

% 3. 前馈控制加入整形后轨迹
qd_shaped = A1*qd(t) + A2*qd(t-t2);
τ_ff = dynamics_model(qd_shaped, dqd_shaped, ddqd_shaped);

1. 逆动力学计算重力补偿
   τ_gravity = G(q)  // 补偿机械臂自身重力

2. 阻抗控制实现柔顺
   τ_impedance = M_d·(q̈ - q̈_d) + D_d·(q̇ - q̇_d) + K_d·(q - q_d)

3. 环境力反馈
   τ_force = Jᵀ·F_desired  // 期望接触力

总控制：τ = τ_gravity + τ_impedance + τ_force

def optimizeEnergy(waypoints):
    # 基于动力学的轨迹优化
    # 目标函数：最小化能量消耗
    cost = ∫(τᵀ·R·τ)dt

    # 约束条件：
    # 1. 动力学方程约束：Mq̈ + Cq̇ + G = τ
    # 2. 位置约束：起点/终点位置
    # 3. 速度/加速度约束：电机极限

    # 求解得到最优轨迹
    return optimal_trajectory

步骤1：静态验证
  • 重力补偿测试：检查在静止时能否平衡自重
  • 摩擦测试：低速运动观察力矩波动

步骤2：动态验证
  • 正弦跟踪测试：不同频率下的跟踪误差
  • 阶跃响应测试：观察超调量和稳定时间

步骤3：对比验证
  • 有无动力学补偿的对比
  • 不同负载下的性能一致性

M(q)q̈ + C(q, q̇)q̇ + G(q) + F(q̇) = τ

惯性矩阵M(q)
# 4×4对称正定矩阵，表示各关节加速度所需的惯性力
# 对于SCARA，M(q)结构相对简单：
M = [[M11, M12, 0,    0],
     [M12, M22, 0,    0],
     [0,   0,   M33,  0],
     [0,   0,   0,   M44]]

其中：
M11 = I1 + I2 + m1*r1² + m2*(a1² + r2² + 2*a1*r2*cosθ2) + I_motor1
M12 = M21 = I2 + m2*(r2² + a1*r2*cosθ2)
M22 = I2 + m2*r2² + I_motor2
M33 = m3 + m_tool  # Z轴移动质量
M44 = I4 + I_tool  # 末端旋转惯量


离心力矩阵 C(q, q̇)
# 由速度引起的耦合项
# 对于SCARA，仅关节1和2之间存在科氏力耦合
C = [[C11, C12, 0, 0],
     [C21, 0,   0, 0],
     [0,   0,   0, 0],
     [0,   0,   0, 0]]

其中：
C11 = -m2*a1*r2*sinθ2 * θ̇2
C12 = -m2*a1*r2*sinθ2 * (θ̇1 + θ̇2)
C21 = m2*a1*r2*sinθ2 * θ̇1


重力向量 G(q)
# SCARA主要受Z轴重力影响
G = [0, 0, (m3 + m_tool)*g, 0]ᵀ
# 注意：某些倾斜安装的SCARA在平面内也有重力分量


摩擦项 F(q̇)
# 实际系统中必须考虑
F(q̇) = Fv*q̇ + Fc*sign(q̇) + Fs*exp(-|q̇|/v_s)*sign(q̇)
其中：
Fv：粘性摩擦系数
Fc：库仑摩擦系数
Fs：静摩擦系数
v_s：Stribeck速度