之前在自研 FOC 驱动板时一直把重心放在电流环、速度环和位置环的“标准三环”上。后来接触机器人关节控制发现单纯的位置环或速度环在遇到外力交互时会显得很僵硬——要么对外力完全没有“退让”要么在位置跟踪和外力扰动之间反复震荡。后来把 MIT 开源四足机器人中常用的五参数阻抗控制补进了自制的 FOC 控制链路整个关节的柔顺性、抗冲击能力和调试体验提升了一大截。这篇文章就从“为什么要在 FOC 里补一层阻抗控制”开始讲清楚 MIT 五参数阻抗控制的原理、五个参数分别代表什么再结合完整的 FOC 电机控制代码示例展示如何把这套控制策略集成到自制驱动器上。内容覆盖硬件选型、控制链路、代码实现、参数整定和常见排错适合正在自制 FOC 驱动器、想把电机关节做成“可力控”的开发者阅读。1. 从 FOC 电机控制到机器人关节阻抗控制1.1 自制 FOC 项目中最容易忽略的一层很多自制 FOC 项目的控制架构是固定的电流环在最内层速度环在中间位置环在最外层。位置环的输出通常是目标速度速度环的输出通常是目标电流电流环的输出最终映射成 SVPWM 占空比。这套架构在纯位置跟踪、匀速运动、速度控制等场景下完全够用。但一旦电机要装在机器人的关节上比如四足机器人的腿部、机械臂的关节整个控制系统面对的不再是“给定位置跟踪位置”而是“在未知外力环境中既要保持位置稳定性又要允许可控的形变”。这时标准三环控制会暴露一个问题位置环的刚性由位置环 PID 增益直接决定增益大了会震荡增益小了又会软绵绵而且没有办法显式地控制“外力作用下关节会退让多少”。这就是需要在 FOC 外环补充阻抗控制的原因。阻抗控制不直接控制位置或力而是控制位置与力之间的动态关系让关节表现出“弹簧—阻尼—质量”的虚拟机械特性。1.2 什么是阻抗控制阻抗控制最早由 Neville Hogan 在 1985 年提出核心思想是不直接控制末端或关节的位置也不直接控制接触力而是控制“运动偏差”与“接触力”之间的动态关系。以机器人腿部落地为例当足端撞到障碍物理想的行为不是死死顶住也不是完全放弃位置跟随而是像一个“带阻尼的弹簧”一样吸收冲击。这个虚拟弹簧的刚度和阻尼是可以实时调节的。刚度大关节更硬位置跟踪精度高刚度小关节更柔顺外力作用下错位更大。在 FOC 电机驱动器上实现阻抗控制意味着电机不仅要能输出力矩还要能根据当前角度和角速度实时计算目标力矩。这要求驱动器的电流环足够快、力矩输出足够准确所以 FOC 电流环是这套方案的基础。1.3 MIT 五参数阻抗控制与常规 FOC 三环的区别MIT 五参数阻抗控制与常规 FOC 三环最大的区别在于控制目标。常规三环中位置环内部隐含了一个位置 PID输出速度指令速度环再做一次 PID输出电流指令。两层 PID 串联等效刚度、等效阻尼很难直观理解而且外层 PID 的参数往往只能在固定负载下整定遇到外力交互很难做到既稳又柔。MIT 五参数阻抗控制则直接写成一个力矩计算表达式tau kp * (q_des - q) kd * (qd_des - qd) tau_ff这个表达式中只有五个关键参数q_des期望位置、qd_des期望速度、kp刚度系数、kd阻尼系数、tau_ff前馈力矩。它把位置环、速度环和力矩前馈合并成一步直接输出目标力矩交给电流环执行。可以这样理解这两种方案的区别控制方式输出量中间环节外力交互行为常规 FOC 三环位置环 PID 输出速度位置环 → 速度环 → 电流环刚性由多级 PID 决定不直观MIT 五参数阻抗控制直接输出力矩阻抗计算 → 电流环等效为“弹簧—阻尼”系统参数直观需要注意的是五参数阻抗控制在数学上仍然是一种 PD 控制加前馈的结构但由于它放在了力矩层电流环作为内环让它具备了真正的力控能力这是它与普通位置环的本质差异。2. 硬件平台与软件环境准备2.1 硬件方案清单在自制 FOC 驱动器中集成五参数阻抗控制对硬件本身没有额外要求因为阻抗控制只是上层算法底层仍然是 FOC 电流控制。但为了保证阻抗控制效果对硬件有几点基本要求。模块推荐方案说明主控 MCUSTM32F405 / STM32F407 / STM32G4 系列需要足够算力跑 20kHz 以上电流环驱动芯片DRV8301、TMC6300、FD6288 等与 MOS 管组合使用功率 MOS根据电机电压/电流选择 N-MOS注意死区设置相电流采样低阻值采样电阻 运放放大采样电阻阻值常在 1mΩ~10mΩ转子角度传感器AS5047P、MA730、MT6701 等磁编码器建议使用 SPI/ABZ 接口输出绝对角度通信接口UART、CAN、USB 均可用于参数下发和状态上传电机低压无刷电机 / 云台电机 / 盘式电机建议已知 KV 值和极对数如果手头已经有跑通的 FOC 驱动板那么只需要在现有的电流环之上增加阻抗控制层不需要额外改硬件。这也是本文选型如此“宽松”的原因。2.2 软件环境说明本文的代码示例以自制 FOC 驱动器的通用控制框架为基础不限定具体的 STM32 工程生成工具。常见的组合STM32CubeIDE 或 Keil MDK 生成工程定时器触发 ADC 采样频率 20kHz定时器输出互补 PWM 驱动三相桥SPI 读取磁编码器角度串口/CAN 上报状态、下发参数不同芯片的 ADC、定时器、SPI 外设差异较大所以本文后续给出的算法代码是平台无关的 C 语言实现实际移植时只需要把底层接好即可。2.3 整体控制链路接入五参数阻抗控制后整个系统的控制链路如下期望位置 q_des 期望速度 qd_des 前馈力矩 tau_ff ↓ 五参数阻抗计算: tau kp*(q_des-q) kd*(qd_des-qd) tau_ff ↓ 目标力矩 tau_ref ↓ 电流环计算: iq_ref tau_ref / (Kt * cos(electrical_angle)) 更常见的是在 dq 坐标系中直接给定 iq_ref ↓ Park 逆变换 → SVPWM ↓ 三相逆变桥 → 电机 ↓ 编码器角度反馈 / 相电流反馈这里有一个容易困惑的点阻抗控制计算出的目标力矩如何转换为电流环的iq指令对表贴式永磁同步电机SPMSM电磁转矩与 q 轴电流近似成正比tau ≈ 1.5 * p * phi * iq Kt * iq所以iq_ref tau_ref / Kt。如果电机参数未知可以通过离线辨识粗略估算力矩常数或者直接以“力矩指令”的形式下发在调试功率级做标定。3. FOC 电流环基础回顾3.1 相电流采样与重构FOC 电流环需要知道至少两相电流第三相可以根据基尔霍夫电流定律计算得到。常见采样方式有两种低端采样电阻 运放放大在 PWM 下桥臂导通时刻采样高端采样或者模块内置电流放大器采样电阻的阻值和放大倍数需要根据电机相电流范围选择。假设电机堵转电流峰值为 30A采样电阻取 5mΩ那采样电阻上的峰值压降为 0.15V经 10 倍运放放大后为 1.5V在 ADC 量程 3.3V 内还留有裕量。ADC 采样触发时机非常重要必须在 PWM 中心对齐的“下桥臂全部导通”时刻采样才能采到真实的相电流。使用 STM32 时一般配置定时器更新事件或 TRGO 触发 ADC 注入组转换。3.2 Clarke/Park 变换采样得到三相电流ia、ib、ic后先经过 Clarke 变换得到两相静止坐标系电流i_alpha、i_betai_alpha ia i_beta (ia 2 * ib) / sqrt(3)再根据转子电角度theta_e做 Park 变换得到 dq 轴电流id i_alpha * cos(theta_e) i_beta * sin(theta_e) iq -i_alpha * sin(theta_e) i_beta * cos(theta_e)其中theta_e必须乘以电机极对数不能直接用机械角度。3.3 电流环 PID 与 SVPWM 输出电流环通常在两处使用 PI 控制器一个控制id目标是 0一个控制iq目标是外部给定的力矩电流指令。输出分别是vd、vq经过 Park 逆变换得到v_alpha、v_beta再送入 SVPWM 模块生成三相占空比。下面是一个电流环计算的核心示例属于 FOC 项目中最常见的写法// 文件路径app/foc_current_loop.c #include foc.h // 电流环 PI 参数需要根据电机和硬件调试 static float id_kp 0.2f; static float id_ki 0.01f; static float iq_kp 0.2f; static float iq_ki 0.01f; static float id_integral 0.0f; static float iq_integral 0.0f; // 输入目标 iq/id当前角度电角度三相电流 // 输出v_alpha, v_beta void foc_current_loop(float iq_ref, float id_ref, float theta_e, float ia, float ib, float ic, float *v_alpha, float *v_beta) { float i_alpha, i_beta; float id, iq; float vd, vq; float id_err, iq_err; // Clarke 变换 i_alpha ia; i_beta (ia 2.0f * ib) * 0.57735026919f; // 1/sqrt(3) // Park 变换 id i_alpha * cosf(theta_e) i_beta * sinf(theta_e); iq -i_alpha * sinf(theta_e) i_beta * cosf(theta_e); // id 轴 PI目标通常为 0 id_err id_ref - id; id_integral id_ki * id_err; id_integral clamp_f32(id_integral, -1.0f, 1.0f); vd id_kp * id_err id_integral; // iq 轴 PI目标是外部力矩对应的电流 iq_err iq_ref - iq; iq_integral iq_ki * iq_err; iq_integral clamp_f32(iq_integral, -1.0f, 1.0f); vq iq_kp * iq_err iq_integral; // Park 逆变换 *v_alpha vd * cosf(theta_e) - vq * sinf(theta_e); *v_beta vd * sinf(theta_e) vq * cosf(theta_e); }注意id_integral和iq_integral是积分项状态需要在每次电流环中断中持续更新不能局部清零。实际项目里还应该根据母线电压做vd、vq限幅避免 SVPWM 过调制。4. MIT 五参数阻抗控制原理拆解4.1 从 PD 控制说起先看最简单的 PD 位置控制器tau kp * (q_des - q) kd * (qd_des - qd)这个公式大家应该很熟悉就是位置误差乘以比例增益速度误差乘以微分增益。它能让电机跟随期望位置同时通过阻尼项抑制速度振荡。但 PD 控制器在实际机器人关节中有一个问题它只能跟随位置无法主动输出力矩。比如腿部在摆动过程中需要克服重力PD 控制器会通过持续的位置误差产生一个偏置力矩但这个偏置量不是显式控制的而是“误差换来的”。4.2 五参数公式MIT 五参数阻抗控制在 PD 基础上增加了一个前馈力矩项tau kp * (q_des - q) kd * (qd_des - qd) tau_ff其中五个参数分别是参数含义单位q_des期望关节角度radqd_des期望关节角速度rad/skp刚度系数虚拟弹簧刚度N·m/radkd阻尼系数虚拟阻尼N·m·s/radtau_ff前馈力矩N·m需要注意的是q和qd是当前反馈的关节角度和角速度不是 FOC 内部的机械角度和电角速度。在接入时必须分清反馈角度是机械角而 FOC 电流环需要的是电角度。下面给出阻抗控制计算的 C 语言实现// 文件路径app/imp_ctrl.c #include imp_ctrl.h typedef struct { float kp; // 刚度 float kd; // 阻尼 } imp_param_t; typedef struct { imp_param_t param; float q_des; // 期望位置 float qd_des; // 期望速度 float tau_ff; // 前馈力矩 } imp_ref_t; // 五参数阻抗控制输入当前角度和角速度输出目标力矩 float impedance_control(imp_ref_t *ref, float q, float qd) { float tau; tau ref-param.kp * (ref-q_des - q) ref-param.kd * (ref-qd_des - qd) ref-tau_ff; // 输出力矩限幅防止电流环饱和 tau clamp_f32(tau, -MAX_TORQUE, MAX_TORQUE); return tau; }从公式可以看出调节kp可以改变关节的“硬度”调节kd可以改变关节的“晃动抑制能力”而tau_ff可以直接补偿重力或者提供持续推力。4.3 参数物理含义与整定思路很多人拿到这个公式之后第一反应是把kp、kd当成 PID 参数来整定但阻抗控制的参数物理含义更加直观。kp决定了外力作用下的稳态误差在外力F作用下关节稳态偏差为F / kp。想让关节“硬”就把kp调大想让关节柔性就调小。kd决定了关节运动的阻尼阻尼越大运动越平缓冲击吸收能力越强但响应越慢。阻尼太小会引起振荡。tau_ff前馈力矩可以抵消重力、摩擦力等已知干扰分量让零误差时也能输出一个期望力矩。整定建议先把tau_ff设为 0kd设为一个较小值从kp 0开始慢慢增加。观察关节在自由状态下的响应如果缓慢振荡增加kd如果跟随迟缓增加kp。加入负载后通过tau_ff补偿重力力矩观察q_des与实际q的差值是否减小。最终测试外力扰动下的恢复行为刚度越大恢复越快但越容易振荡需要在kp和kd之间找到平衡。4.4 前馈力矩的作用前馈力矩是实现高性能力控的关键但经常被忽略。它在形式上等于在 PD 控制器输出上加一个偏置但这个偏置不需要位置误差来产生。举个例子当一个关节轴需要克服一个恒定的重力矩G在纯 PD 控制下稳定后位置误差会收敛到G / kp。如果G比较大而kp不能太大这就是一个明显的误差。加入tau_ff G后即使位置误差很小控制器也能维持输出从而提升静态精度。在 FOC 中前馈力矩最终也要折算成iq指令所以tau_ff的单位必须和kp*(q_des-q)项保持一致。5. 实战把五参数阻抗控制接入自制 FOC5.1 系统状态机设计在实际驱动器中不能一上电就进入阻抗控制模式需要一个简单的状态机BOOT → CALIBRATION → IDLE → RUN(IMPEDANCE) → FAULTCALIBRATION完成编码器零位校准和相电感/电阻辨识。IDLE电流环、速度环关闭电机自由。RUN(IMPEDANCE)五参数阻抗控制工作电机输出力矩。FAULT出现过流、过温、编码器异常时进入输出关闭。状态机实现// 文件路径app/main_ctrl.c typedef enum { MOTOR_BOOT 0, MOTOR_CALIBRATION, MOTOR_IDLE, MOTOR_RUN, MOTOR_FAULT } motor_state_t; motor_state_t motor_state MOTOR_BOOT; void motor_state_machine(void) { switch (motor_state) { case MOTOR_BOOT: if (encoder_calib_done) { motor_state MOTOR_IDLE; } break; case MOTOR_CALIBRATION: // 执行电角度对齐和相电阻辨识 break; case MOTOR_IDLE: // 力矩输出为 0等待上位机指令 break; case MOTOR_RUN: // 执行阻抗控制 break; case MOTOR_FAULT: // 急停输出 break; } }5.2 位置/速度模式下的五参数阻抗实现下面给出一个完整的融合代码思路。它把五参数阻抗控制、电流环和 SVPWM 串在一起形成完整的控制链路。假设电流环中断频率为 20kHz阻抗控制不需要那么高的频率通常 1kHz ~ 5kHz 即可。这里可以设计一个计数器每 20 次电流环中断执行一次阻抗计算其余周期直接沿用上一次的iq_ref。// 文件路径app/control.c #include foc.h #include imp_ctrl.h #define CURRENT_LOOP_FREQ_HZ 20000 #define IMP_CTRL_DIVIDER 20 // 1kHz 阻抗控制频率 static int imp_div_counter 0; static float iq_ref 0.0f; // 五参数阻抗控制的参考值 static imp_ref_t imp_ref { .param { .kp 1.0f, // 初始刚度 .kd 0.05f, // 初始阻尼 }, .q_des 0.0f, .qd_des 0.0f, .tau_ff 0.0f, }; // 电机当前机械角度rad和角速度rad/s extern float motor_q; extern float motor_qd; extern float motor_theta_e; extern float motor_ia, motor_ib, motor_ic; extern float motor_v_bus; // 每 50us 调用一次 void current_loop_isr(void) { float v_alpha, v_beta; float iq_ref_tmp; // 按分频执行阻抗控制计算 imp_div_counter; if (imp_div_counter IMP_CTRL_DIVIDER) { imp_div_counter 0; // 根据运行状态决定是否启用阻抗控制 if (motor_state MOTOR_RUN) { float tau_ref; tau_ref impedance_control(imp_ref, motor_q, motor_qd); iq_ref tau_ref / MOTOR_KT; } else { iq_ref 0.0f; } } iq_ref_tmp iq_ref; // 电流环计算 foc_current_loop(0.0f, iq_ref_tmp, motor_theta_e, motor_ia, motor_ib, motor_ic, v_alpha, v_beta); // SVPWM 输出 svpwm_set_voltage(v_alpha, v_beta, motor_v_bus); }这里的MOTOR_KT是力矩常数需要根据电机参数进行估算。若不确定可以把它作为标定系数在调试界面实时调整。5.3 力矩模式与保护逻辑除阻抗控制外驱动器通常还需要一个纯力矩模式直接给定iq_ref。这个模式用于机器人腿部落地瞬间的力控或力位混合控制。力矩模式下的保护逻辑非常重要。因为纯力矩模式没有位置约束一旦给定过大的iq_ref电机可能直接飞车。必须做以下处理力矩指令限幅iq_ref clamp_f32(iq_ref, -I_MAX, I_MAX)速度限制如果机械转速超过安全值自动将力降低位置软限位如果关节角度超出允许范围撤销力矩5.4 运行验证与数据观察接好硬件后可以通过串口或者 CAN 上报数据来验证控制效果。最简单的验证方式是“手掰测试”将q_des设为 0qd_des设为 0tau_ff设为 0。用手缓慢扭转电机输出轴感受阻力大小。增大kp再次扭转手感应该变“硬”。增大kd快速扭转时应该能感受到阻尼效果。设置一个偏置tau_ff观察输出轴是否会向一个方向慢慢移动。如果条件允许在电机输出轴上安装力矩传感器或六维力传感器可以把实际力矩与tau_ref做对比这一步对校准MOTOR_KT非常有帮助。6. 常见问题与调试排错在实际调试中我最常遇到的问题基本都集中在电流采样、角度反馈和参数突变这三个方向。下面按排查顺序列出。6.1 电流采样噪声导致力矩抖动问题现象常见原因解决思路电机静止时啸叫手摸有高频抖动电流采样噪声被电流环放大检查采样电阻布局和运放滤波力矩输出在小电流附近跳变ADC 采样点落在 PWM 开关噪声区调整采样触发时刻避开死区高速时电流波形毛刺多采样电阻寄生电感过大改用低感采样电阻或增加 RC 滤波排查建议先把电流环关闭给定开环电压用示波器观察三相电流波形。如果 PWM 切换边缘有明显的尖峰需要在 ADC 采样前加 RC 低通滤波或者在软件中做多次采样平均。6.2 编码器角度漂移导致“越用力越偏”五参数阻抗控制对角度反馈质量的要求比普通速度环更高。如果编码器零位不准或者安装存在偏心会导致角度反馈带有周期性误差这个误差经过kp放大后会变成额外的力矩波动。问题现象常见原因解决思路静止时输出轴缓慢爬行电角度与机械角度不对齐重新执行电角度校准转动时出现周期性的“一顿一顿”磁编码器偏心或磁场干扰检查磁铁安装位置做偏心校准外力撤去后回不到原点零位漂移更换高精度编码器或使用增量编码器 限位如果在电流环中用到了电角度theta_e它是由机械角度乘以极对数得到的。编码器零位漂移会直接影响 FOC 的Park变换结果必须先处理底层角度问题再谈上层阻抗控制。6.3 kp/kd 参数突变引起系统发散调试过程中直接修改kp和kd导致系统发散是常见现象尤其是从高刚度切到低刚度或者从低阻尼切到高阻尼时。原因是阻抗控制的目标力矩突然变化电流环已经进入饱和状态电机在“全速追误差”。问题现象常见原因解决思路参数切换瞬间电机猛跳目标力矩突变参数平滑过渡不要直接改参数系统高频振荡阻尼不足优先增大kd位置跟踪超调大刚度太高、阻尼不匹配降低kp或增大kd实现参数平滑过渡的思路是增加一个“参数斜坡”用一段有限时间把当前参数逐步过渡到目标参数// 文件路径app/imp_ctrl.c void impedance_param_ramp(imp_param_t *target) { static float current_kp 0.0f; static float current_kd 0.0f; float step 0.001f; // 每周期变化步长 current_kp clamp_f32(target-kp - current_kp, -step, step); current_kd clamp_f32(target-kd - current_kd, -step, step); imp_ref.param.kp current_kp; imp_ref.param.kd current_kd; }6.4 电机发热与保护长时间高刚度阻抗控制下电机处于“持续顶住”状态实际电流很大发热明显。尤其是kp调大后微小位置误差也会产生很大力矩这种情况下必须考虑热保护。问题现象常见原因解决思路电机外壳烫手持续大电流输出减小kp或增加散热驱动器 MOS 温度过高开关损耗和导通损耗过大检查 PWM 频率和死区配置长时间运行后力矩下降温度导致磁钢退磁风险增加过温保护阈值建议在驱动器中加入母线电流积分估算当输出的电流积分超过设定阈值时主动降低阻抗控制的刚度而不是直接断电。7. 工程化建议与安全注意事项7.1 控制频率一致性阻抗控制直接输出目标力矩这个力矩最终由电流环执行。如果阻抗控制频率和电流环频率不同步或者阻抗控制周期内没有更新iq_ref就会出现输出力矩滞后、电流环目标跳变的问题。推荐的做法是电流环固定高频运行如 20kHz ~ 40kHz阻抗控制通过分频方式运行如 1kHz ~ 5kHz并在每次电流环中断中沿用上一次的iq_ref确保输出连续。7.2 参数下发与校准五参数阻抗控制的上位机调试至少需要能实时下发这五个参数q_des、qd_des、tau_ff、kp、kd。建议使用结构体打包成统一的参数帧通过串口或 CAN 下发。每次下发后驱动板应该回传当前实际状态q、qd、tau_ref、iq_ref、fault_flag方便在上位机界面上画曲线。7.3 安全限幅与急停任何力矩控制项目都必须考虑失控场景。五参数阻抗控制虽然比纯速度控制更稳定但依然可能出现编码器损坏、CAN 通信中断、上位机卡死等情况。安全措施建议iq_ref限幅必须存在且可配置。通信超时后自动切换为MOTOR_FAULT而不是保持上一次的力矩。关节角度软限位和硬限位都要有硬限位靠机械结构软限位靠软件判断。急停按钮直接切断PWM输出不要依赖软件状态机。下面是一个简单的通信超时保护示例// 文件路径app/safety.c void safety_check(void) { static uint32_t last_cmd_time 0; uint32_t now get_tick_ms(); if (motor_state MOTOR_RUN) { if (now - last_cmd_time 100) { // 100ms 未收到指令 motor_state MOTOR_FAULT; pwm_disable(); } } }7.4 状态反馈与日志在开发阶段一定要把关键状态参数记录下来。推荐在驱动器中保留一个环形缓冲区记录最近几千组q_des、q、qd_des、qd、tau_ref、iq_ref数据。出现异常后通过上位机读取这段日志比现场猜测问题要高效得多。日志格式建议时间戳(us), 状态, q_des, q, qd_des, qd, tau_ref, iq_ref, v_bus, temp8. 总结与下一步学习路线写到这里整个“FOC MIT 五参数阻抗控制”的最小闭环已经完整了。可以看到这套方案并没有改变 FOC 底层电流环的架构而是在位置环和电流环之间插入了一个“力矩计算层”用kp、kd、tau_ff三个可调参数加上期望位置、期望速度两个参考值直接决定了关节的柔顺特性。在你自己的自制 FOC 项目中建议按下面顺序推进先跑通 FOC 电流环确保iq跟踪稳定这是所有力矩控制的基础。在电流环之上实现五参数阻抗控制先用手掰测试感受刚度、阻尼的变化。加入上位机参数下发和状态反馈让参数可以实时调整否则每次改参数都要重新编译会非常痛苦。最后再接入机器人腿部或机械臂结构重点观察落地瞬间、外力扰动下的响应逐步调整kp和kd。下一步可以继续研究的内容包括无感 FOC 中如何通过反电动势估算转子角度、SMO 滑模观测器在低速下的补偿、ADRC 自抗扰控制在电流环中的应用以及从五参数阻抗控制扩展到六维力传感器加持的力位混合控制。每一条线展开下去都能研究很久。