梳理总结Aug_week2

电机位置控制模式下整条控制链路是怎样的？¶

不管上位机怎么得到最终电机位置（电机转动到哪个角度，这个一般是在上位机进行IK反解得到的），只讨论在给出电机位置后，怎样让电机运动到想要的位置。

先梳理清楚一个最终目标，控制电机时靠的是给电机绕组一个电磁扭矩，而控制这个扭矩的方法便是控制电流（T≈KTIq）。因此在最底层是电流环，通过扭矩计算出实时电流。再上层探讨的是如何得出想要的扭矩，常见的有MIT控制模式和位置速度模式，MIT 模式的外层本质上是 PD + 扭矩前馈，位置速度模式是位置 PI + 速度 PI + 电流环（位置环 → 速度环 → 电流环，三级串级控制）。求解扭矩的再上层便是如何得到分阶段位置，让从初始位置到目标位置的轨迹变得平滑，因为如果直接给电机一个很大的位置差，在扭矩计算中可能得到的是一个极大的扭矩，不够平滑稳定。

电机控制的流程更直观的如下：

上位机目标关节角
       ↓
STM32：限位、速度斜坡、跟随误差窗口
       ↓ 每1ms发送 pos/vel/Kp/Kd/tor
达妙电机：MIT位置阻抗计算
       ↓ 生成目标力矩/目标电流
电机内部：电流环/FOC
       ↓
实际电机力矩

STM32层¶

这一层不是位置PD，而是决定“交给电机的目标位置允许怎样变化”。

绝对关节限位：防止软件目标越过机械行程。
目标位置速度斜坡（加上速度限制）：目标可能瞬间从0变成1 rad。如果直接把这个阶跃目标交给电机，位置误差会瞬间变成1 rad，而力矩等于误差✖Kp,这会马上进入电机力矩饱和，造成剧烈冲击。代码限制每次最大位置增量 = 最大目标速度 × 时间。
跟随误差窗口（加上位置限制）：限制位置误差，也相当于间接限制最大比例力矩。

电控层¶

如果采用MIT模式，电控层的目标是算出需要多少力矩。
使用MIT模式进行控制的原理：

MIT模式可以通过改变 Kp、Kd 和 Tff来一种模式变成三种控制（位置控制、速度控制和力矩控制），下面着重探讨位置控制。

MIT位置控制¶

核心逻辑就是对位置做PD控制+前馈力矩

p_des = 目标位置
Kp > 0
Kd > 0
v_des 通常 = 0
T_ff 可为0或给前馈
τ_cmd = Kp × (q_des - q) - Kd × dq + τ_ff

在不加t_ff时，就是简单的PD控制，Kp虚拟扭转弹簧，位置误差越大，提供的扭矩越大；Kd虚拟粘性阻尼，产生与关节实际运动方向相反的阻尼。这样控制后依旧会存在误差，即给定目标值和编码器反馈的值存在一定误差。为了尽量减少误差，考虑在τ_ff中增加位置积分补偿和重力补偿。

积分补偿

积分补偿的几个特点：“积分工作带、有限幅、带死区”
积分工作带：积分补偿并不是在整个运动过程中都加上，只在期望目标与实际目标的误差处在一个较小区间的时候才开始工作，同时在这个误差进入到一个十分小的区间（积分死区）的时候停止。误差太大：不积分；误差适中：积分；误差很小：冻结积分。
有限幅：积分具有持续累积的特性。如果机械臂被手按住、卡住或碰到机械限位，松手瞬间，累积力矩可能突然释放，造成机械臂猛冲。
带死区：如果对任何微小误差都积分，导致积分力矩会在目标附近不断调整，来回震荡抖动。

重力补偿

单纯只求解重力补偿的话，有3种方法，其中拉格朗日方法和牛顿-欧拉方法是动力学种2种不同的求解思路。

摩擦补偿

摩擦分为静摩擦、库仑摩擦、粘滞摩擦，其中静摩擦只在从静止到运动这个过程中有，存在一个最大静摩擦。在运动后存在库仑摩擦和粘滞摩擦，其中库仑摩擦大小与转动速度无关，方向与转动速度方向相反，大小几乎是一个定值，粘滞摩擦与转动角速度成正比，需要乘以damp参数。

MIT速度控制¶

MIT力矩控制¶

电机层¶

对于永磁同步电机，目标力矩在常见工作区间近似满足：τ_motor ≈ Kt × Iq
Kt = 电机转矩常数
Iq = 产生转矩的q轴电流
电机实际有三相电流：Ia、Ib、Ic
但三相电流随着转子旋转不断变化，直接控制比较困难。FOC使用转子电角度，把三相电流转换到跟随转子旋转的坐标系中：

三相abc电流
    ↓ Clarke变换
静止αβ坐标
    ↓ Park变换
旋转dq坐标

转换后得到：
Id：主要控制磁场
Iq：主要控制转矩

对于常见永磁同步电机，通常让：
Id_des ≈ 0
Iq_des = 目标转矩对应电流
这样就可以相对独立地控制磁场和转矩。之后分别对Id和Iq进行PI控制。

=================== FOC =======================

iq_ref ──────┐
             ↓
         q轴电流PI ───→ vq
             ↑
          iq反馈

id_ref ──────┐
             ↓
         d轴电流PI ───→ vd
             ↑
          id反馈

        vd / vq
           │
           ↓
       逆Park变换
           │
           ↓
        vα / vβ
           │
           ↓
         SVPWM
           │
           ↓


=================== 功率级 =====================

     六个MOSFET三相逆变桥
           │
           ↓
       Ua / Ub / Uc
           │
           ↓
     三相电机绕组
           │
           ↓
       ia / ib / ic
           │
           ↓


=================== 反馈 =======================

ia ib ic
   │
   ↓
Clarke
   │
   ↓
iα iβ
   │
   ↓       编码器θ
Park  ←──────┘
   │
   ↓
id iq
   │
   └────────→ 电流PI

电流通常使用PI控制，而不是位置控制中的PD。原因是电流控制需要消除稳态电流误差，因此需要积分项，最终计算出需要施加的电压。

补充¶

电控层位置速度模式¶

电控层如果采用的是位置速度模式，而非MIT模式，那么整套逻辑都有变化，MIT模式是直接从位置误差来计算出扭矩，但是位置速度模式路径如下：

位置误差
↓
位置PI
↓
速度目标
↓
限速
↓
速度PI
↓
电流目标
↓
电流环

这种模式可以实现给定一个位置目标，通过位置PI计算出速度目标，然后就会经历一个加速、匀速、减速的过程（中间匀速是因为有速度限制），然后停在目标位置。
位置速度模式比 MIT 更“稳”但响应慢，因为路径更长了。

电控层速度模式¶

速度模式是对速度做PI控制，去得到想要的电流，与MIT的速度模式相对比，MIT的速度模式相当于只做P控制。加上I可以避免长期误差累积，如果目标是恒速旋转，最好采用速度模式。

电控层力位混控模式¶

相当于带最大力矩/电流约束的位置控制(位置速度串级控制 + 速度环输出电流限幅)。具体如下：

p_des
  ↓
位置 PI
  ↓
速度目标
  ↓
速度 PI
  ↓
原始 iq_ref
  ↓
┌───────────────┐
│ 电流/扭矩限幅器 │ ← i_des
└───────┬───────┘
        ↓
    限制后的 iq_ref
        ↓
      电流环
        ↓
       电机

力位混控最适合机械夹爪，没夹到物体时正常移动，夹到物体后力不会一直增大，不会把东西夹坏或损坏电机。

电控层模式总结¶

MIT模式：希望最终在哪、希望最终有多快、有多硬、阻尼多大、主动施加多少力
位置速度模式：去哪里、最多多快
速度模式：一直多快
力位混控模式：去哪里、最多多快、最大出力是多少

MIT 特别适合机器人，而位置速度适合传统自动化。速度模式适合驱动轮，力位混控模式适合夹爪。
因为机器人往往需要上层控制器高速参与，MIT CAN 帧里同时包含 Position、Velocity、Kp、Kd 和 Torque 五个参数。可以实时调节机器人关节特性。
而传统自动化只关注稳定的去到哪里。