FOC 电机控制做到一定程度目标就不再只是让电机转起来而是让电机按照期望的方式出力。尤其在做机器人关节、力控模组、灵巧手这类项目时电机既要有足够的力矩输出又要对外力保持柔顺避免硬碰硬。MIT 五参数阻抗控制就是这类场景里很实用的一套控制策略它把关节简化成由虚拟质量、弹簧和阻尼组成的二阶系统用五个参数决定期望力矩再把期望力矩交给 FOC 电流环执行。本文以自制 FOC 驱动器为背景先拆清楚 FOC 和阻抗控制的层级关系再讲五参数的含义然后给出一套最小可运行实现最后重点展开调参顺序和排错路径。1. 先理清 FOC 与阻抗控制的关系电流环负责力矩阻抗环负责柔顺1.1 FOC 本身是力矩伺服不是位置伺服FOC 全称磁场定向控制核心工作是把三相电流通过 Clarke 变换和 Park 变换拆成 d 轴分量和 q 轴分量。d 轴电流控制磁场励磁q 轴电流控制电磁转矩。对表贴式永磁同步电机来说q 轴电流参考值可以直接换算成期望力矩tau 1.5 * pole_pairs * flux_linkage * iq很多驱动器把这个综合系数叫 Kt单位是 N·m/A。因此 FOC 电流环本质上是力矩环。这里要注意FOC 只负责“让实际电流跟踪期望电流”它并不知道电机当前角度应该停在哪里也不知道电机对外力应该表现出多硬或多软。所有关于位置、速度、柔顺性的判断都需要在 FOC 之外的更高层进行。1.2 控制链的层级关系在自制 FOC 项目中控制结构一般是这样分层的控制层输入输出作用位置环目标角度、当前角度目标速度让角度收敛到目标速度环目标速度、当前速度期望力矩让速度收敛到目标阻抗控制环目标位置、目标速度、当前角度、当前速度期望力矩调节关节的柔顺性电流环 FOC期望 q 轴电流、三相电流占空比让实际电流跟踪期望电流也就是说位置环、速度环、阻抗环都处于 FOC 之上。它们不关心 PWM 怎么生成只负责给出一个力矩或电流指令。1.3 五参数阻抗控制与普通 PD 位置控制的区别普通 PD 位置控制一般是tau kp * (q_des - q) kd * (dq_des - dq)它把目标位置和当前位置的误差换算成力矩目的是“抑制误差、到达目标”。如果位置误差很大PD 控制会输出很大的力矩把电机拉回目标这会让关节看起来很硬。五参数阻抗控制的思路不同。它不仅要让电机回到目标位置还要让电机表现成一个可设计的二阶系统。外界推它时它像弹簧一样有恢复力又像阻尼器一样吸收能量同时还带有一定的惯性感。这样机器人碰到人和物体时不会瞬间输出巨大冲击力。两者的核心区别可以理解为控制方式关心的问题对外表现PD 位置控制位置误差要小硬快速回位阻抗控制位置误差与力矩的关系要可控弹簧加阻尼柔顺五参数阻抗控制同时考虑刚度、阻尼、惯性和参考轨迹柔顺且具有质量感2. MIT 五参数阻抗控制的原理与五个参数的物理含义2.1 质量-弹簧-阻尼模型把电机关节想象成一端连着弹簧、一端连着阻尼器同时关节本身还有一个转动惯量。这样一个系统的动力学方程可以写成tau M * qdd_des Kp * (q_des - q) Kd * (dq_des - dq) tau_ff其中q 是当前角度。dq 是当前角速度。q_des 是目标角度。dq_des 是目标角速度。qdd_des 是目标角加速度。M、Kp、Kd 是设计参数。tau_ff 是前馈力矩一般不属于五参数本身。这个方程就是“MIT 五参数阻抗控制”最常见的工程表达。开源社区把这类实现称为 MIT 五参数是因为相关控制思想来自 MIT 开源四足动物机器人项目中的关节柔顺控制经验后来在 ODrive 等开源 FOC 驱动器社区中被广泛复现。不同代码仓库里名称可能略有差异但核心模型一致。2.2 五个参数分别是什么参数常见符号单位物理含义调整太小的表现调整太大的表现虚拟惯量Mkg·m^2关节对外力加速度响应的“沉重感”参考轨迹加速度大时力矩不足跟不上插补对参考加速度过度放大容易冲击刚度KpN·m/rad目标位置与当前位置差产生的恢复力矩关节很软抗扰动能力弱关节很硬失去柔顺性容易抖动阻尼KdN·m·s/rad目标速度与当前速度差产生的阻尼力矩振荡衰减慢容易来回摆动响应迟钝跟踪滞后目标位置q_desrad虚拟弹簧的平衡位置关节漂向错误位置位置设定有误时持续输出偏置力矩目标速度dq_desrad/s参考轨迹期望速度参与阻尼计算无法跟踪运动目标速度设定过高时力矩突变这里的 Kp 和 Kd 和普通 PD 控制里的 Kp、Kd 写法完全一样。区别在于多了 M 一项并且 q_des、dq_des 是显式参与计算的参考状态而不是简单地把误差换算成扭矩。2.3 计算流程每一步控制周期内阻抗环按以下顺序工作从编码器读取当前角度 q。从编码器差分或速度观测器获得当前角速度 dq。从轨迹规划器拿到目标角度 q_des、目标角速度 dq_des、目标角加速度 qdd_des。计算误差角度误差 q_des - q速度误差 dq_des - dq。计算期望力矩M * qdd_des Kp * 角度误差 Kd * 速度误差 前馈。对力矩做限幅。将期望力矩换算成 q 轴电流参考值交给 FOC 电流环。2.4 为什么不是三个或四个参数如果只看稳态Kp 和 Kd 就能完成位置柔顺控制。但实际运动控制里有一个很重要的问题参考轨迹本身可能带有加速度。机器人关节在做插补运动时如果只靠 Kp、Kd 去追目标加速度越大的轨迹误差就越大。M 这一项正好解决“为了跟上参考加速度需要额外输出多少力矩”这个问题。因此五参数可以理解成2 个目标状态参数q_des、dq_des。3 个机械特性参数M、Kp、Kd。这样组合起来电机不仅能“停在目标位置”还能“按预期的动态特性响应外力”。3. 最小可运行环境自制 FOC 板、ODrive 和上位机脚本3.1 硬件清单在自制项目里复现五参数阻抗控制不一定要从零写 FOC。常见做法有两种一种是使用 ODrive、VESC 这类已经具备 FOC 力矩环的驱动器另一种是在自己的 STM32 工程里把 FOC 电流环和阻抗外环一起写。硬件推荐配置说明电机额定 24V 或 48V 的 BLDC/PMSM优先选带霍尔或编码器的型号编码器AS5047P、MT6816、AMT10 等至少 12 位以上能测角度和速度FOC 驱动器自制 STM32 板或 ODrive必须支持力矩模式和电流环闭环电源带限流的开关电源24V/48V限流保护很重要通信USB 转串口或 CAN 适配器用于上位机下发参数和读取状态上位机笔记本 Python实现调参脚本和运行日志3.2 软件环境如果使用 ODrive需要安装对应的 Python 工具。ODrive 的 API 在 0.5、0.6 等版本之间差异很大比如枚举类型名称、axis0和odrv0.axis0的访问方式都会变。落地前先确认固件版本和 Python 库版本一致。如果使用自制 STM32 FOC 工程建议先用 CubeMX 生成工程把 PWM、ADC 采样、编码器接口、串口分别验证通过再叠加 FOC 电流环。不要一开始就同时写五参数阻抗和电流环排查困难。3.3 环境检查清单检查项预期结果不符时处理编码器角度手动转电机角度单调变化且方向可控检查 ABZ 接法或编码器方向配置编码器零点在机械零位附近角度接近固定值写入校准偏移电机三相顺序速度环小速度指令下电机能正向转动交换任意两相或配置 PWM 反逻辑电流采样相电流波形为近似正弦检查采样电阻、运放增益、偏置通信上位机能连续读取角度和速度检查串口波特率、CAN ID、回环模式力矩模式给固定力矩指令后电机能握住且不过流先在低限幅下测试这一项必须在进入五参数阻抗之前全部确认。否则后面所有现象都分不清是电流环问题还是外环问题。4. 在 FOC 驱动上实现五参数阻抗控制4.1 第一步把底层 FOC 切换到力矩模式如果使用 ODrive先把控制模式设置为力矩模式输入模式设为直通。这样input_torque会直接作为力矩目标送到电流环。import odrive from odrive.enums import AxisState, ControlMode, InputMode odrv odrive.find_any() axis odrv.axis0 axis.requested_state AxisState.CLOSED_LOOP_CONTROL axis.controller.config.control_mode ControlMode.TORQUE axis.controller.config.input_mode InputMode.PASSTHROUGH如果使用自制 STM32 工程应该在电流环外层定义力矩参考变量并将力矩参考换算成 q 轴电流参考float tau_ref 0.0f; /* 阻抗环输出单位 N·m */ float iq_ref tau_ref / Kt; /* Kt 为电机力矩常数单位 N·m/A */这里的关键是 Kt 要准确。Kt 可以通过堵转测试或电机参数表获得。Kt 偏差太大会导致同样的力矩指令实际输出力矩偏大或偏小整个阻抗的弹簧质感就失真。4.2 第二步在控制循环中读取角度和速度FOC 电流环一般运行在 10 kHz 到 20 kHz。阻抗外环不需要这么快常见选择是 1 kHz 或 2 kHz。如果和电流环同频要确保角度和速度在每个周期都有有效值。速度不建议直接对编码器角度做差分因为角度量化噪声会被差分放大。推荐使用一阶低通滤波或者用锁相环速度观测器。float velocity_state 0.0f; float filter_coeff 0.002f; float read_velocity(float angle_now, float angle_prev) { float raw_vel (angle_now - angle_prev) * control_freq; velocity_state filter_coeff * (raw_vel - velocity_state); return velocity_state; }滤波系数要根据控制频率和信号噪声调整。系数太大会引入滞后系数太小速度波动大。4.3 第三步实现五参数计算函数下面是一段结构清晰的 C 代码示例用于自制 STM32 工程。它把五参数封装成结构体每次控制周期调用一次更新函数。typedef struct { float kp; /* 刚度 */ float kd; /* 阻尼 */ float inertia; /* 虚拟惯量 */ float q_des; /* 目标角度 */ float dq_des; /* 目标角速度 */ float qdd_des; /* 目标角加速度 */ float tau_ff; /* 前馈力矩 */ float torque_limit; /* 力矩限幅 */ } ImpedanceCtrl_t; float impedance_update(ImpedanceCtrl_t *c, float q, float dq) { float tau; tau c-inertia * c-qdd_des c-kp * (c-q_des - q) c-kd * (c-dq_des - dq) c-tau_ff; if (tau c-torque_limit) { tau c-torque_limit; } if (tau -c-torque_limit) { tau -c-torque_limit; } return tau; }这段代码有几个点要注意目标角加速度 qdd_des 单独传入而不是写死在结构体里因为轨迹规划器每一拍都会更新它。矩阵形式可以同时加入重力补偿但重力补偿一般算作前馈不属于五参数本身。限幅必须在阻抗环输出处做一次在电流环参考处再做一次双重保护。4.4 第四步把期望力矩送入电流环完成力矩计算后把力矩换算成电流然后交给 FOC 的电流 PI 控制器。float q_current_ref impedance_update(imp_ctl, angle_now, velocity_state) / Kt; foc_set_iq_reference(q_current_ref);在 ODrive 这类驱动器上力矩模式内部会自动完成力矩到电流的换算不需要外部再除以 Kt。此时直接写axis.controller.input_torque即可。4.5 编写上位机调参脚本调参阶段反复烧录固件太耗时。建议把五参数放在上位机脚本里通过串口或 ODrive Python 接口实时修改。下面是一个针对 ODrive 的最小实现import odrive from odrive.enums import AxisState, ControlMode, InputMode odrv odrive.find_any() axis odrv.axis0 def clamp(value, low, high): return max(low, min(high, value)) axis.requested_state AxisState.CLOSED_LOOP_CONTROL axis.controller.config.control_mode ControlMode.TORQUE axis.controller.config.input_mode InputMode.PASSTHROUGH KP 2.0 KD 0.05 Q_DES 0.0 DQ_DES 0.0 TORQUE_LIMIT 3.0 while True: q axis.encoder.pos_estimate dq axis.encoder.vel_estimate tau KP * (Q_DES - q) KD * (DQ_DES - dq) tau clamp(tau, -TORQUE_LIMIT, TORQUE_LIMIT) axis.controller.input_torque tau这里没有加 M 项因为静态目标是 qdd_des 0。如果要测试带参考轨迹的跟踪再把目标速度和目标加速度按轨迹插补函数填入。这个脚本有一个明显问题while True循环在 Python 里会受系统调度影响控制频率不稳定。它适合验证逻辑和调参不适合作为生产控制主循环。生产环境应该把阻抗环放到驱动器的实时中断或实时线程里。5. 参数标定与调参顺序先刚度后阻尼惯量最后5.1 初始参数设置第一次上电不要让电机直接锁在某个目标位置。先把目标位置 q_des 设为当前角度目标速度 dq_des 设为 0这样初始力矩接近 0。参数初始值说明Kp0.1 N·m/rad从小开始避免直接飞车Kd0.01 N·m·s/rad先给一点基础阻尼M0静态测试时不需要惯量q_des当前角度保证初始力矩为零dq_des0静止目标torque_limit1.0 N·m限幅保持较小保护设备和手5.2 刚度 Kp 的标定方法Kp 决定电机像多硬的弹簧。标定步骤让 Kd 保持一个很小的值M 设为 0。将 q_des 从当前角度阶跃偏移 0.1 rad 到 0.2 rad。观察电机是否回位以及是否开始振荡。逐步增大 Kp直到手动推电机时明显感觉“有弹性但没有高频抖动”。如果出现持续振荡把 Kp 回退到振荡前的 70% 左右。这里要特别注意Kp 不能只按“手感”调。要记录角度误差和输出力矩的关系确认稳态时误差不大并且没有极限环振荡。5.3 阻尼 Kd 的标定方法Kd 决定振荡衰减快慢。标定步骤选择前面标定好的 Kp。手动推动电机观察它回位时是否来回摆动超过两次。增大 Kd直到回位过程接近临界阻尼也就是不超调或只超调一次。继续增大 Kd直到电机转动手感变“黏”说明阻尼过大再回退。Kd 过大会让电机响应变迟钝位置跟踪时感觉像拖着阻尼器走。如果负载变化大Kd 适合做增益调度。5.4 惯量 M 的使用场景M 只在参考轨迹有加速度时起作用。做静态柔顺测试时M 不参与也不需要调。当电机需要跟踪一个连续规划的目标角度时目标加速度 qdd_des 会产生一项前馈力矩。M 越大关节越容易“跟上”参考加速度但同时会把参考轨迹里的速度突变放大成力矩冲击。因此 M 应该在 Kp、Kd 调好之后再调整。现象可能原因调整方向跟随参考轨迹时相位滞后大Kp 不足或 Kd 过大增大 Kp 或减小 Kd参考加速度大时力矩不足M 未配置增大 M起步瞬间力矩冲击M 过大或轨迹尖角减小 M 或改用平滑轨迹5.5 调参检查表检查项合格标准初始力矩接近 0电机不漂移手推回位回位过程不振荡或最多振荡一次外力释放关节回到 q_des 周围无持续漂移力矩限幅输出力矩不超过设定值温度连续运行 10 分钟后功率管不明显烫手速度波动静止时速度反馈在 ±1% 额定速度以内6. 常见问题排查抖动、漂移、饱和与失控6.1 现象到根因的排查总表问题现象常见原因检查方式处理建议电机高频抖动电流环增益过高或编码器噪声进入速度环用示波器看 q 轴电流和速度估算波形降低电流环 PI或降低速度低通截止频率手推电机很硬Kp 过大或 q_des 被锁死检查 q_des 是否一直等于错误角度降低 Kp或把 q_des 更新为机械零位关节缓慢漂移编码器零位不准或重力前馈缺失停止电机观察角度是否缓慢变化重新校准编码器偏移加重力补偿松开瞬间飞车力矩方向反了或编码器方向反了手动给一个小力矩看转动方向先解决方向问题再继续调参饱和后失控力矩限幅未生效或限幅值太大检查限幅代码是否在电流环前执行双重限幅电流环参考也要限幅上位机控制卡顿Python 主循环阻塞或通信超时打印循环周期观察丢包率把控制逻辑移入驱动器实时任务6.2 编码器方向或极性错误是最容易忽视的坑很多自制 FOC 项目出现“推力越大越飞”的现象根因不是 Kp 太大而是编码器方向反了。阻抗环里的误差项是q_des - q如果编码器角度方向反了实际角度误差会反过来形成正反馈。排查方法很简单在力矩模式下手动给一个很小的正力矩观察电机是否沿正方向转动。然后对比编码器读数是否也随之正向增加。如果不一致先交换 ABZ 中的 A、B或在软件里配置取反。6.3 力矩饱和导致柔顺失效阻抗控制依赖“输出力矩与误差成正比”这个关系。如果力矩指令达到限幅值误差继续增大输出力矩不再增加关节就会表现出“硬限位”一样的特性。因此要区分两层限幅阻抗环输出限幅保护机械结构。电流环参考限幅保护驱动器和电机。两层限幅都要有日志记录。如果经常触发限幅说明负载超过了当前阻抗参数的许用范围要么减小 Kp要么降低运动速度。6.4 控制频率不足阻抗控制虽然不需要和电流环一样快但也不能太低。1 kHz 以下时手推电机的延迟会很明显Kd 很难发挥作用。常见表现为Kd 调得很大仍然振荡因为每次控制周期之间电机已经积累了过大的位置误差。自制 STM32 工程中建议把五参数阻抗环放在定时器中断里控制频率固定为 1 kHz 或 2 kHz。不要放在while(1)主循环里因为主循环的执行时间会受串口、显示、其他任务影响。6.5 日志关键字段进入排错阶段日志是唯一可信的线索。建议至少记录以下字段字段单位作用timems判断控制周期稳定性qrad当前角度dqrad/s当前速度q_desrad目标角度tau_cmdN·m阻抗环输出的力矩指令iq_refA电流环参考电流status枚举值驱动器状态和限幅标志在 ODrive 上可以用axis.encoder.pos_estimate、axis.encoder.vel_estimate、axis.controller.input_torque直接采集。在自制工程里则要专门写一个日志发送接口通过串口或者 SD 卡保存。7. 从桌面实验到机器人关节的工程化要点7.1 学习环境与生产环境的差异要点学习环境机器人生产环境力矩限幅手动设一个较小值根据机构和电机额定值双重限制供电稳压电源带限流电池供电时要考虑欠压保护控制主循环Python 脚本调试驱动器实时中断或实时线程参数来源手动试凑根据负载模型辨识或离线优化安全策略手能随时停急停、力矩斜坡下掉、看门狗日志调参时打印落盘存储支持离线回放自制桌面实验可以只关注控制逻辑本身。但一旦把关节装到机器人上还要补三件事力限幅、温度保护、故障降级。7.2 安全策略阻抗控制本身是柔顺控制但参数设置错误时依然可能造成机械冲击。以下策略建议在自制项目中完整实现力矩限幅必须由驱动器底层执行不能只依赖上位机。控制周期超时自动切到零力矩或保持当前力矩并置故障标志。编码器断线检测要快。编码器失效时阻抗环会计算出错误力矩。电源电压低于阈值时禁止继续输出大步进力矩。7.3 扩展方向前馈、导纳控制与变阻抗五参数阻抗控制是阻抗控制里的一个基础形态。在机器人关节中常见的扩展方向有扩展方向解决的问题实现要点重力前馈关节爬升时产生额外偏置根据关节角度和负载质量计算前馈力矩摩擦补偿低速时扭矩死区明显增加粘滞摩擦和库仑摩擦前馈导纳控制期望的接触力而不是期望位置用外力估算修改目标轨迹变阻抗控制不同运动阶段需要不同刚度阻尼根据关节状态切换 Kp、Kd、M串联弹性执行器 SEA减少刚性冲击在电机和关节之间加弹性体如果已经跑通了本文的最小实现下一步最值得做的是把“重力前馈”加上。因为很多自制机器人关节在悬臂状态下阻抗环的稳态误差并不只是因为 Kp 不够而是重力力矩没有补偿。7.4 下一步练习建议自己动手做一遍的推荐顺序用一台电机、一块 ODrive 或自制 FOC 板先跑通力矩模式。用 Python 脚本实现不含惯量的三参数阻抗控制也就是 PD 加目标速度。手动推电机感受 Kp 和 Kd 的影响。给电机加一条正弦参考轨迹逐步加入目标加速度和 M 项。把控制逻辑迁移到 STM32 定时器中断固定 1 kHz 控制频率。最后加入编码器零位校准、力矩限幅和日志系统。五参数阻抗控制最大的价值不是让电机停在某个角度而是让电机“质感”可以被设计。参数调好之后手推电机的感觉会介于弹簧和阻尼器之间这种可控的柔顺性才是机器人关节力控的基础。 
长河君 · 2026/9/4 4:34:58 · 阅读 1.2k FOC电机控制实战：MIT五参数阻抗控制原理与调参方法
FOC 电机控制做到一定程度目标就不再只是让电机转起来而是让电机按照期望的方式出力。尤其在做机器人关节、力控模组、灵巧手这类项目时电机既要有足够的力矩输出又要对外力保持柔顺避免硬碰硬。MIT 五参数阻抗控制就是这类场景里很实用的一套控制策略它把关节简化成由虚拟质量、弹簧和阻尼组成的二阶系统用五个参数决定期望力矩再把期望力矩交给 FOC 电流环执行。本文以自制 FOC 驱动器为背景先拆清楚 FOC 和阻抗控制的层级关系再讲五参数的含义然后给出一套最小可运行实现最后重点展开调参顺序和排错路径。1. 先理清 FOC 与阻抗控制的关系电流环负责力矩阻抗环负责柔顺1.1 FOC 本身是力矩伺服不是位置伺服FOC 全称磁场定向控制核心工作是把三相电流通过 Clarke 变换和 Park 变换拆成 d 轴分量和 q 轴分量。d 轴电流控制磁场励磁q 轴电流控制电磁转矩。对表贴式永磁同步电机来说q 轴电流参考值可以直接换算成期望力矩tau 1.5 * pole_pairs * flux_linkage * iq很多驱动器把这个综合系数叫 Kt单位是 N·m/A。因此 FOC 电流环本质上是力矩环。这里要注意FOC 只负责“让实际电流跟踪期望电流”它并不知道电机当前角度应该停在哪里也不知道电机对外力应该表现出多硬或多软。所有关于位置、速度、柔顺性的判断都需要在 FOC 之外的更高层进行。1.2 控制链的层级关系在自制 FOC 项目中控制结构一般是这样分层的控制层输入输出作用位置环目标角度、当前角度目标速度让角度收敛到目标速度环目标速度、当前速度期望力矩让速度收敛到目标阻抗控制环目标位置、目标速度、当前角度、当前速度期望力矩调节关节的柔顺性电流环 FOC期望 q 轴电流、三相电流占空比让实际电流跟踪期望电流也就是说位置环、速度环、阻抗环都处于 FOC 之上。它们不关心 PWM 怎么生成只负责给出一个力矩或电流指令。1.3 五参数阻抗控制与普通 PD 位置控制的区别普通 PD 位置控制一般是tau kp * (q_des - q) kd * (dq_des - dq)它把目标位置和当前位置的误差换算成力矩目的是“抑制误差、到达目标”。如果位置误差很大PD 控制会输出很大的力矩把电机拉回目标这会让关节看起来很硬。五参数阻抗控制的思路不同。它不仅要让电机回到目标位置还要让电机表现成一个可设计的二阶系统。外界推它时它像弹簧一样有恢复力又像阻尼器一样吸收能量同时还带有一定的惯性感。这样机器人碰到人和物体时不会瞬间输出巨大冲击力。两者的核心区别可以理解为控制方式关心的问题对外表现PD 位置控制位置误差要小硬快速回位阻抗控制位置误差与力矩的关系要可控弹簧加阻尼柔顺五参数阻抗控制同时考虑刚度、阻尼、惯性和参考轨迹柔顺且具有质量感2. MIT 五参数阻抗控制的原理与五个参数的物理含义2.1 质量-弹簧-阻尼模型把电机关节想象成一端连着弹簧、一端连着阻尼器同时关节本身还有一个转动惯量。这样一个系统的动力学方程可以写成tau M * qdd_des Kp * (q_des - q) Kd * (dq_des - dq) tau_ff其中q 是当前角度。dq 是当前角速度。q_des 是目标角度。dq_des 是目标角速度。qdd_des 是目标角加速度。M、Kp、Kd 是设计参数。tau_ff 是前馈力矩一般不属于五参数本身。这个方程就是“MIT 五参数阻抗控制”最常见的工程表达。开源社区把这类实现称为 MIT 五参数是因为相关控制思想来自 MIT 开源四足动物机器人项目中的关节柔顺控制经验后来在 ODrive 等开源 FOC 驱动器社区中被广泛复现。不同代码仓库里名称可能略有差异但核心模型一致。2.2 五个参数分别是什么参数常见符号单位物理含义调整太小的表现调整太大的表现虚拟惯量Mkg·m^2关节对外力加速度响应的“沉重感”参考轨迹加速度大时力矩不足跟不上插补对参考加速度过度放大容易冲击刚度KpN·m/rad目标位置与当前位置差产生的恢复力矩关节很软抗扰动能力弱关节很硬失去柔顺性容易抖动阻尼KdN·m·s/rad目标速度与当前速度差产生的阻尼力矩振荡衰减慢容易来回摆动响应迟钝跟踪滞后目标位置q_desrad虚拟弹簧的平衡位置关节漂向错误位置位置设定有误时持续输出偏置力矩目标速度dq_desrad/s参考轨迹期望速度参与阻尼计算无法跟踪运动目标速度设定过高时力矩突变这里的 Kp 和 Kd 和普通 PD 控制里的 Kp、Kd 写法完全一样。区别在于多了 M 一项并且 q_des、dq_des 是显式参与计算的参考状态而不是简单地把误差换算成扭矩。2.3 计算流程每一步控制周期内阻抗环按以下顺序工作从编码器读取当前角度 q。从编码器差分或速度观测器获得当前角速度 dq。从轨迹规划器拿到目标角度 q_des、目标角速度 dq_des、目标角加速度 qdd_des。计算误差角度误差 q_des - q速度误差 dq_des - dq。计算期望力矩M * qdd_des Kp * 角度误差 Kd * 速度误差 前馈。对力矩做限幅。将期望力矩换算成 q 轴电流参考值交给 FOC 电流环。2.4 为什么不是三个或四个参数如果只看稳态Kp 和 Kd 就能完成位置柔顺控制。但实际运动控制里有一个很重要的问题参考轨迹本身可能带有加速度。机器人关节在做插补运动时如果只靠 Kp、Kd 去追目标加速度越大的轨迹误差就越大。M 这一项正好解决“为了跟上参考加速度需要额外输出多少力矩”这个问题。因此五参数可以理解成2 个目标状态参数q_des、dq_des。3 个机械特性参数M、Kp、Kd。这样组合起来电机不仅能“停在目标位置”还能“按预期的动态特性响应外力”。3. 最小可运行环境自制 FOC 板、ODrive 和上位机脚本3.1 硬件清单在自制项目里复现五参数阻抗控制不一定要从零写 FOC。常见做法有两种一种是使用 ODrive、VESC 这类已经具备 FOC 力矩环的驱动器另一种是在自己的 STM32 工程里把 FOC 电流环和阻抗外环一起写。硬件推荐配置说明电机额定 24V 或 48V 的 BLDC/PMSM优先选带霍尔或编码器的型号编码器AS5047P、MT6816、AMT10 等至少 12 位以上能测角度和速度FOC 驱动器自制 STM32 板或 ODrive必须支持力矩模式和电流环闭环电源带限流的开关电源24V/48V限流保护很重要通信USB 转串口或 CAN 适配器用于上位机下发参数和读取状态上位机笔记本 Python实现调参脚本和运行日志3.2 软件环境如果使用 ODrive需要安装对应的 Python 工具。ODrive 的 API 在 0.5、0.6 等版本之间差异很大比如枚举类型名称、axis0和odrv0.axis0的访问方式都会变。落地前先确认固件版本和 Python 库版本一致。如果使用自制 STM32 FOC 工程建议先用 CubeMX 生成工程把 PWM、ADC 采样、编码器接口、串口分别验证通过再叠加 FOC 电流环。不要一开始就同时写五参数阻抗和电流环排查困难。3.3 环境检查清单检查项预期结果不符时处理编码器角度手动转电机角度单调变化且方向可控检查 ABZ 接法或编码器方向配置编码器零点在机械零位附近角度接近固定值写入校准偏移电机三相顺序速度环小速度指令下电机能正向转动交换任意两相或配置 PWM 反逻辑电流采样相电流波形为近似正弦检查采样电阻、运放增益、偏置通信上位机能连续读取角度和速度检查串口波特率、CAN ID、回环模式力矩模式给固定力矩指令后电机能握住且不过流先在低限幅下测试这一项必须在进入五参数阻抗之前全部确认。否则后面所有现象都分不清是电流环问题还是外环问题。4. 在 FOC 驱动上实现五参数阻抗控制4.1 第一步把底层 FOC 切换到力矩模式如果使用 ODrive先把控制模式设置为力矩模式输入模式设为直通。这样input_torque会直接作为力矩目标送到电流环。import odrive from odrive.enums import AxisState, ControlMode, InputMode odrv odrive.find_any() axis odrv.axis0 axis.requested_state AxisState.CLOSED_LOOP_CONTROL axis.controller.config.control_mode ControlMode.TORQUE axis.controller.config.input_mode InputMode.PASSTHROUGH如果使用自制 STM32 工程应该在电流环外层定义力矩参考变量并将力矩参考换算成 q 轴电流参考float tau_ref 0.0f; /* 阻抗环输出单位 N·m */ float iq_ref tau_ref / Kt; /* Kt 为电机力矩常数单位 N·m/A */这里的关键是 Kt 要准确。Kt 可以通过堵转测试或电机参数表获得。Kt 偏差太大会导致同样的力矩指令实际输出力矩偏大或偏小整个阻抗的弹簧质感就失真。4.2 第二步在控制循环中读取角度和速度FOC 电流环一般运行在 10 kHz 到 20 kHz。阻抗外环不需要这么快常见选择是 1 kHz 或 2 kHz。如果和电流环同频要确保角度和速度在每个周期都有有效值。速度不建议直接对编码器角度做差分因为角度量化噪声会被差分放大。推荐使用一阶低通滤波或者用锁相环速度观测器。float velocity_state 0.0f; float filter_coeff 0.002f; float read_velocity(float angle_now, float angle_prev) { float raw_vel (angle_now - angle_prev) * control_freq; velocity_state filter_coeff * (raw_vel - velocity_state); return velocity_state; }滤波系数要根据控制频率和信号噪声调整。系数太大会引入滞后系数太小速度波动大。4.3 第三步实现五参数计算函数下面是一段结构清晰的 C 代码示例用于自制 STM32 工程。它把五参数封装成结构体每次控制周期调用一次更新函数。typedef struct { float kp; /* 刚度 */ float kd; /* 阻尼 */ float inertia; /* 虚拟惯量 */ float q_des; /* 目标角度 */ float dq_des; /* 目标角速度 */ float qdd_des; /* 目标角加速度 */ float tau_ff; /* 前馈力矩 */ float torque_limit; /* 力矩限幅 */ } ImpedanceCtrl_t; float impedance_update(ImpedanceCtrl_t *c, float q, float dq) { float tau; tau c-inertia * c-qdd_des c-kp * (c-q_des - q) c-kd * (c-dq_des - dq) c-tau_ff; if (tau c-torque_limit) { tau c-torque_limit; } if (tau -c-torque_limit) { tau -c-torque_limit; } return tau; }这段代码有几个点要注意目标角加速度 qdd_des 单独传入而不是写死在结构体里因为轨迹规划器每一拍都会更新它。矩阵形式可以同时加入重力补偿但重力补偿一般算作前馈不属于五参数本身。限幅必须在阻抗环输出处做一次在电流环参考处再做一次双重保护。4.4 第四步把期望力矩送入电流环完成力矩计算后把力矩换算成电流然后交给 FOC 的电流 PI 控制器。float q_current_ref impedance_update(imp_ctl, angle_now, velocity_state) / Kt; foc_set_iq_reference(q_current_ref);在 ODrive 这类驱动器上力矩模式内部会自动完成力矩到电流的换算不需要外部再除以 Kt。此时直接写axis.controller.input_torque即可。4.5 编写上位机调参脚本调参阶段反复烧录固件太耗时。建议把五参数放在上位机脚本里通过串口或 ODrive Python 接口实时修改。下面是一个针对 ODrive 的最小实现import odrive from odrive.enums import AxisState, ControlMode, InputMode odrv odrive.find_any() axis odrv.axis0 def clamp(value, low, high): return max(low, min(high, value)) axis.requested_state AxisState.CLOSED_LOOP_CONTROL axis.controller.config.control_mode ControlMode.TORQUE axis.controller.config.input_mode InputMode.PASSTHROUGH KP 2.0 KD 0.05 Q_DES 0.0 DQ_DES 0.0 TORQUE_LIMIT 3.0 while True: q axis.encoder.pos_estimate dq axis.encoder.vel_estimate tau KP * (Q_DES - q) KD * (DQ_DES - dq) tau clamp(tau, -TORQUE_LIMIT, TORQUE_LIMIT) axis.controller.input_torque tau这里没有加 M 项因为静态目标是 qdd_des 0。如果要测试带参考轨迹的跟踪再把目标速度和目标加速度按轨迹插补函数填入。这个脚本有一个明显问题while True循环在 Python 里会受系统调度影响控制频率不稳定。它适合验证逻辑和调参不适合作为生产控制主循环。生产环境应该把阻抗环放到驱动器的实时中断或实时线程里。5. 参数标定与调参顺序先刚度后阻尼惯量最后5.1 初始参数设置第一次上电不要让电机直接锁在某个目标位置。先把目标位置 q_des 设为当前角度目标速度 dq_des 设为 0这样初始力矩接近 0。参数初始值说明Kp0.1 N·m/rad从小开始避免直接飞车Kd0.01 N·m·s/rad先给一点基础阻尼M0静态测试时不需要惯量q_des当前角度保证初始力矩为零dq_des0静止目标torque_limit1.0 N·m限幅保持较小保护设备和手5.2 刚度 Kp 的标定方法Kp 决定电机像多硬的弹簧。标定步骤让 Kd 保持一个很小的值M 设为 0。将 q_des 从当前角度阶跃偏移 0.1 rad 到 0.2 rad。观察电机是否回位以及是否开始振荡。逐步增大 Kp直到手动推电机时明显感觉“有弹性但没有高频抖动”。如果出现持续振荡把 Kp 回退到振荡前的 70% 左右。这里要特别注意Kp 不能只按“手感”调。要记录角度误差和输出力矩的关系确认稳态时误差不大并且没有极限环振荡。5.3 阻尼 Kd 的标定方法Kd 决定振荡衰减快慢。标定步骤选择前面标定好的 Kp。手动推动电机观察它回位时是否来回摆动超过两次。增大 Kd直到回位过程接近临界阻尼也就是不超调或只超调一次。继续增大 Kd直到电机转动手感变“黏”说明阻尼过大再回退。Kd 过大会让电机响应变迟钝位置跟踪时感觉像拖着阻尼器走。如果负载变化大Kd 适合做增益调度。5.4 惯量 M 的使用场景M 只在参考轨迹有加速度时起作用。做静态柔顺测试时M 不参与也不需要调。当电机需要跟踪一个连续规划的目标角度时目标加速度 qdd_des 会产生一项前馈力矩。M 越大关节越容易“跟上”参考加速度但同时会把参考轨迹里的速度突变放大成力矩冲击。因此 M 应该在 Kp、Kd 调好之后再调整。现象可能原因调整方向跟随参考轨迹时相位滞后大Kp 不足或 Kd 过大增大 Kp 或减小 Kd参考加速度大时力矩不足M 未配置增大 M起步瞬间力矩冲击M 过大或轨迹尖角减小 M 或改用平滑轨迹5.5 调参检查表检查项合格标准初始力矩接近 0电机不漂移手推回位回位过程不振荡或最多振荡一次外力释放关节回到 q_des 周围无持续漂移力矩限幅输出力矩不超过设定值温度连续运行 10 分钟后功率管不明显烫手速度波动静止时速度反馈在 ±1% 额定速度以内6. 常见问题排查抖动、漂移、饱和与失控6.1 现象到根因的排查总表问题现象常见原因检查方式处理建议电机高频抖动电流环增益过高或编码器噪声进入速度环用示波器看 q 轴电流和速度估算波形降低电流环 PI或降低速度低通截止频率手推电机很硬Kp 过大或 q_des 被锁死检查 q_des 是否一直等于错误角度降低 Kp或把 q_des 更新为机械零位关节缓慢漂移编码器零位不准或重力前馈缺失停止电机观察角度是否缓慢变化重新校准编码器偏移加重力补偿松开瞬间飞车力矩方向反了或编码器方向反了手动给一个小力矩看转动方向先解决方向问题再继续调参饱和后失控力矩限幅未生效或限幅值太大检查限幅代码是否在电流环前执行双重限幅电流环参考也要限幅上位机控制卡顿Python 主循环阻塞或通信超时打印循环周期观察丢包率把控制逻辑移入驱动器实时任务6.2 编码器方向或极性错误是最容易忽视的坑很多自制 FOC 项目出现“推力越大越飞”的现象根因不是 Kp 太大而是编码器方向反了。阻抗环里的误差项是q_des - q如果编码器角度方向反了实际角度误差会反过来形成正反馈。排查方法很简单在力矩模式下手动给一个很小的正力矩观察电机是否沿正方向转动。然后对比编码器读数是否也随之正向增加。如果不一致先交换 ABZ 中的 A、B或在软件里配置取反。6.3 力矩饱和导致柔顺失效阻抗控制依赖“输出力矩与误差成正比”这个关系。如果力矩指令达到限幅值误差继续增大输出力矩不再增加关节就会表现出“硬限位”一样的特性。因此要区分两层限幅阻抗环输出限幅保护机械结构。电流环参考限幅保护驱动器和电机。两层限幅都要有日志记录。如果经常触发限幅说明负载超过了当前阻抗参数的许用范围要么减小 Kp要么降低运动速度。6.4 控制频率不足阻抗控制虽然不需要和电流环一样快但也不能太低。1 kHz 以下时手推电机的延迟会很明显Kd 很难发挥作用。常见表现为Kd 调得很大仍然振荡因为每次控制周期之间电机已经积累了过大的位置误差。自制 STM32 工程中建议把五参数阻抗环放在定时器中断里控制频率固定为 1 kHz 或 2 kHz。不要放在while(1)主循环里因为主循环的执行时间会受串口、显示、其他任务影响。6.5 日志关键字段进入排错阶段日志是唯一可信的线索。建议至少记录以下字段字段单位作用timems判断控制周期稳定性qrad当前角度dqrad/s当前速度q_desrad目标角度tau_cmdN·m阻抗环输出的力矩指令iq_refA电流环参考电流status枚举值驱动器状态和限幅标志在 ODrive 上可以用axis.encoder.pos_estimate、axis.encoder.vel_estimate、axis.controller.input_torque直接采集。在自制工程里则要专门写一个日志发送接口通过串口或者 SD 卡保存。7. 从桌面实验到机器人关节的工程化要点7.1 学习环境与生产环境的差异要点学习环境机器人生产环境力矩限幅手动设一个较小值根据机构和电机额定值双重限制供电稳压电源带限流电池供电时要考虑欠压保护控制主循环Python 脚本调试驱动器实时中断或实时线程参数来源手动试凑根据负载模型辨识或离线优化安全策略手能随时停急停、力矩斜坡下掉、看门狗日志调参时打印落盘存储支持离线回放自制桌面实验可以只关注控制逻辑本身。但一旦把关节装到机器人上还要补三件事力限幅、温度保护、故障降级。7.2 安全策略阻抗控制本身是柔顺控制但参数设置错误时依然可能造成机械冲击。以下策略建议在自制项目中完整实现力矩限幅必须由驱动器底层执行不能只依赖上位机。控制周期超时自动切到零力矩或保持当前力矩并置故障标志。编码器断线检测要快。编码器失效时阻抗环会计算出错误力矩。电源电压低于阈值时禁止继续输出大步进力矩。7.3 扩展方向前馈、导纳控制与变阻抗五参数阻抗控制是阻抗控制里的一个基础形态。在机器人关节中常见的扩展方向有扩展方向解决的问题实现要点重力前馈关节爬升时产生额外偏置根据关节角度和负载质量计算前馈力矩摩擦补偿低速时扭矩死区明显增加粘滞摩擦和库仑摩擦前馈导纳控制期望的接触力而不是期望位置用外力估算修改目标轨迹变阻抗控制不同运动阶段需要不同刚度阻尼根据关节状态切换 Kp、Kd、M串联弹性执行器 SEA减少刚性冲击在电机和关节之间加弹性体如果已经跑通了本文的最小实现下一步最值得做的是把“重力前馈”加上。因为很多自制机器人关节在悬臂状态下阻抗环的稳态误差并不只是因为 Kp 不够而是重力力矩没有补偿。7.4 下一步练习建议自己动手做一遍的推荐顺序用一台电机、一块 ODrive 或自制 FOC 板先跑通力矩模式。用 Python 脚本实现不含惯量的三参数阻抗控制也就是 PD 加目标速度。手动推电机感受 Kp 和 Kd 的影响。给电机加一条正弦参考轨迹逐步加入目标加速度和 M 项。把控制逻辑迁移到 STM32 定时器中断固定 1 kHz 控制频率。最后加入编码器零位校准、力矩限幅和日志系统。五参数阻抗控制最大的价值不是让电机停在某个角度而是让电机“质感”可以被设计。参数调好之后手推电机的感觉会介于弹簧和阻尼器之间这种可控的柔顺性才是机器人关节力控的基础。