梳理总结Aug_week2

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

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

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

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

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

STM32层

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

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

电控层

电控层的目标是算出需要多少力矩。
使用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种不同的求解思路。

MIT速度控制

MIT力矩控制

位置速度模式

电机层

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

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

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

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

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