⚙ 一体化关节模组 · 中层运动控制详细拆解

                ┌──────────────────────────────────┐
位置指令 θ* →  │  位置环（位置 PID）              │  带宽 50-100 Hz
                │  θ* → θ_err → 输出 ω*           │  输出：速度指令
                └──────────────┬───────────────────┘
                               ↓
                ┌──────────────────────────────────┐
                │  速度环（速度 PID）              │  带宽 200-500 Hz
                │  ω* → ω_err → 输出 T*           │  输出：力矩指令
                └──────────────┬───────────────────┘
                               ↓
                ┌──────────────────────────────────┐
                │  电流环（电流 PI / FOC）         │  带宽 1-2 kHz
                │  T* → I* → 输出 Vq / Vd         │  输出：电压矢量
                └──────────────────────────────────┘
    

设 Ld = Lq = L（隐极电机简化），Rs 为定子电阻
电流环 PI：    u(k) = Kp·e(k) + Ki·Ts·Σe(i) + 1/L·∫(u - Rs·i)dt
前馈解耦项：   ωe·(Ld·iq - Lq·id)  →  对消旋转电动势
抗饱和：       当 u 接近母线电压 ±90% 时，冻结积分项
    

ISR_CurrentLoop() @ 10kHz {
  读 ADC：ia, ib  →  ic = -(ia+ib)
  Clarke：  iα = ia,  iβ = (ia + 2·ib) / √3
  Park：    id = iα·cosθ + iβ·sinθ
            iq = -iα·sinθ + iβ·cosθ
  误差：    ed = id* - id
            eq = iq* - iq
  PI：      vd = Kp_d·ed + Ki_d·∫ed
            vq = Kp_q·eq + Ki_q·∫eq
  前馈：    vd += -ωe·Lq·iq
            vq += ωe·Ld·id + ωe·Ψf
  限幅：    vd, vq  ≤  Vbus / √3
  反 Park： vα = vd·cosθ - vq·sinθ
            vβ = vd·sinθ + vq·cosθ
  SVPWM：  → 比较值 CCR1, CCR2, CCR3
  更新 PWM 寄存器
}
    

┌─────────────┐    ┌─────────────┐    ┌─────────────┐
│ ① ADC 采样  │ →  │ ② Clarke 变换│ →  │ ③ Park 变换 │
│   ia, ib    │    │   iα, iβ    │    │   id, iq   │
└─────────────┘    └─────────────┘    └─────────────┘
                                            ↓
┌─────────────┐    ┌─────────────┐    ┌─────────────┐
│ ⑤ SVPWM    │ ←  │ ④ 反 Park   │ ←  │ ⑥ PI 调节  │
│ CCR1-3     │    │   vα, vβ   │    │   vd, vq   │
└─────────────┘    └─────────────┘    └─────────────┘
    

LuGre 模型：
  dz/dt = v - σ₀·|v|·z / g(v)
  T_fric = σ₀·z + σ₁·dz/dt + σ₂·v

其中：
  z    —— 内部鬃毛形变量（不可直接测量）
  v    —— 速度
  σ₀   —— 鬃毛刚度（决定静摩擦大小）
  σ₁   —— 阻尼系数
  σ₂   —— 黏滞系数
  g(v) = Fc + (Fs - Fc)·exp(-(v/vs)²)  —— Stribeck 函数
  Fc   —— 库仑摩擦
  Fs   —— 静摩擦
  vs   —— Stribeck 速度
    

T_est_current = Kt(θ, T_motor) · iq
  其中 Kt 是电机转矩常数，随温度漂移：
    Kt(T) = Kt_25·[1 + α·(T - 25)]    (α ≈ -0.001 / °C)
  加上磁饱和修正：
    Kt(iq) = Kt_0 / (1 + β·iq²)        (β ≈ 0.01-0.05)
    

电机端 ─┐
        ├─ 扭簧 ─ 输出端
        │
        │      Δθ = θ_m / N - θ_l
        │      T_spring = K_spring · Δθ
        │
电机端编码器 19bit  输出端编码器 19bit
θ_m 解析    →      θ_l 解析
    

                 ┌──────────┐
       复位 →   │  Init    │ 初始化（自检 + 标定加载）
                 └────┬─────┘
                      ↓
              ┌───────────────┐
              │  Disabled     │ 电机不可使能（默认安全状态）
              └───────┬───────┘
                      ↓ 使能
              ┌───────────────┐
              │  Enabled      │ 电机可使能，但 T = 0
              └───────┬───────┘
                      ↓ 接收指令
        ┌─────────────┼─────────────┐
        ↓             ↓             ↓
   ┌────────┐    ┌─────────┐   ┌─────────┐
   │  Position   │  Velocity   │  Torque
   │  位置模式   │  速度模式   │  力矩模式
   └────┬───┘    └────┬────┘   └────┬────┘
        └─────────────┼─────────────┘
                      ↓
              ┌───────────────┐
              │  Fault        │ 故障态（关 PWM + 报警）
              └───────┬───────┘
                      ↓ 清除故障
              ┌───────────────┐
              │  Disabled     │
              └───────────────┘
    

母线电容容量：1000 µF（典型）
单关节峰值电流：30 A × 0.5 ms = 15 mC
22 关节同时启动：22 × 15 = 330 mC
母线电压跌落：ΔV = 330 mC / 1000 µF = 330V → 跌落到 0V（母线崩溃）

→ 解法 1：分时启动（每 5ms 启动一个关节）
→ 解法 2：母线电容加大（成本 + 体积上升）
→ 解法 3：限制峰值电流上升率（dI/dt 限制）
→ 解法 4：电源预加重（前置储能电感）