四足机器人关节电机FOC控制：磁编码器校准与SVPWM调试实战

1. 四足机器人关节电机的控制核心：为什么FOC是绕不开的选择

四足机器人这两年热度一直居高不下，从高校实验室到商业公司，做四足平台的团队越来越多。但真正动手搭过四足的人都知道，机械结构只是骨架，真正决定机器人能不能站稳、走顺、跳得起来的关键，在于关节电机的控制品质。而提到关节电机控制，FOC（Field-Oriented Control，磁场定向控制）几乎是绕不开的技术路线。

我接触四足机器人关节驱动大概有几年时间，从最早用现成的ODrive模块，到后来自己画板子、写固件、调参数，踩过的坑可以说能写一本小册子。这篇文章想聊的，就是围绕四足机器人关节电机的FOC控制，把从磁编码器校准到SVPWM调试这一整条链路里，那些真正影响成败的细节拆开来讲。核心关键词包括四足机器人、FOC控制、磁编码器校准、SVPWM调试、关节电机，这些不是孤立的技术点，而是一条完整的工程链路。

先说清楚这篇文章适合谁看。如果你正在做四足机器人的关节电机驱动，不管是选用现成驱动器还是自研方案，只要你需要面对电机转起来、转得准、转得稳这三个问题，那这篇内容就对你有用。如果你刚接触FOC，只知道要调电流环、速度环、位置环，但不知道为什么磁编码器校准没做好后面全白搭，那更应该往下看。我会尽量用从业者之间交流的方式来讲，不堆公式，但关键原理和参数计算会给到位。

四足机器人对关节电机的要求和普通云台、轮式底盘完全不一样。四足关节需要频繁正反转、需要大力矩输出、需要在低速甚至堵转状态下保持力矩稳定，还要能快速响应步态切换带来的动态负载变化。这些需求叠加在一起，对FOC控制的电流环带宽、编码器精度、SVPWM调制质量都提出了很高的要求。很多团队在台架上调得好好的电机，装到四足腿上跑起来就出问题，根源往往就在这几个环节的细节没抠到位。

2. 关节电机FOC控制的整体方案设计与选型思路

2.1 四足关节电机的特殊需求拆解

普通工业伺服电机跑的是连续旋转工况，而四足关节电机本质上是往复摆动工况。以常见的12自由度四足为例，每条腿三个关节：髋关节负责前后摆动，大腿关节负责抬腿，小腿关节负责伸展。这三个关节在行走过程中都在做高频往复运动，频率大概在1到3赫兹，但瞬时加速度很大。

这就带来几个特殊需求。第一是低速力矩稳定性，四足站立时关节几乎不转，但需要持续输出力矩来支撑体重，这时候如果FOC的电流环有纹波，机器人就会抖。第二是快速动态响应，步态切换时关节力矩指令可能在几毫秒内从正最大值跳到负最大值，电流环带宽不够就会跟不上。第三是编码器分辨率要求高，因为低速时反电动势很小，无感FOC基本没法用，必须上磁编码器做有感控制，而且分辨率要足够高才能保证低速平稳。

我实测下来，四足关节电机用磁编码器的话，14位以上分辨率是底线，16位比较稳妥。低于14位的话，低速时位置量化误差会导致电流环出现周期性抖动，机器人站着都能看出来在颤。

2.2 为什么选FOC而不是六步换相

有人可能会问，BLDC的六步换相也能转，为什么非要用FOC。这个问题我在早期也纠结过。六步换相实现简单，对MCU要求低，但它的转矩脉动大，低速时尤其明显。四足机器人站立时如果转矩脉动大，力控就会很粗糙，足端力控精度上不去，步态就会显得很僵硬。

FOC的核心优势在于它把三相电流解耦成励磁分量Id和转矩分量Iq，然后分别控制。对于表贴式PMSM，Id参考值设为0，所有电流都用来产生转矩，效率高、脉动小。而且FOC可以做到电流环带宽很高，动态响应快，这对四足关节的往复运动至关重要。

具体到四足应用，我一般会把电流环带宽做到1.5kHz到2kHz，速度环带宽做到100Hz到200Hz，位置环带宽做到20Hz到50Hz。这个带宽分配不是随便定的，电流环必须比速度环快至少5到10倍，速度环比位置环快5到10倍，这样才能保证级联控制的稳定性。

2.3 硬件方案选型：MCU、驱动芯片与编码器

硬件选型上，MCU我推荐至少用带FPU的Cortex-M4，主频100MHz以上。STM32F405或者G431都是常见选择，G431自带硬件CORDIC和FMAC，做三角函数和滤波很方便。如果预算够，上M7核更从容，但四足关节控制其实M4就够了，关键是把中断优先级和计算时间分配好。

驱动芯片方面，小功率关节可以用DRV8323这类集成方案，大功率就得用分立MOSFET加栅极驱动。我个人的经验是，四足关节电机峰值电流一般在20A到60A之间，母线电压24V到48V。MOSFET选低导通电阻的，比如3毫欧以下，栅极驱动要保证死区时间可调，死区时间设大了会畸变，设小了会直通。

编码器就是磁编码器，常见的有AS5047P、AS5600、MT6701等。AS5047P是14位，支持SPI和ABZ输出，性价比高。MT6701是14位但便宜很多，适合批量。如果要16位，可以考虑AS5048A或者TLE5012B。安装的时候磁铁和芯片的间距很关键，一般1到3毫米，偏心要控制在0.5毫米以内，否则线性度会变差。

3. 磁编码器校准：FOC控制的第一道生死关

3.1 磁编码器为什么必须校准

很多人拿到磁编码器，读出来角度就直接用，结果电机转起来电流很大但力矩很小，或者干脆抖得厉害。根本原因在于，磁编码器输出的机械角度和电机电角度之间有一个固定的偏移量，这个偏移量不校准，FOC的Park变换和反Park变换就会用错角度，导致Id和Iq解耦失败。

电角度和机械角度的关系是：电角度 = 极对数 × 机械角度 + 偏移量。这个偏移量来自编码器安装位置和电机转子磁极位置的相对关系，每个电机都不一样，甚至同一个电机拆装一次编码器就会变。所以校准是必须的，而且每次重新安装编码器后都要重新校准。

我见过不少团队在这个环节偷懒，用示波器看反电动势过零点来估算偏移量，精度很差。正确做法是用驱动器自己跑一个校准流程，让电机开环旋转，同时记录编码器角度和电流响应，通过算法算出偏移量。

3.2 磁编码器校准的实操步骤

校准流程我一般分几步走。第一步，确保电机可以自由旋转，不带负载或者负载很小。第二步，驱动器进入开环电流模式，给定一个固定的Id电流，Iq设为0，然后慢慢增加电角度，让电机像步进电机一样一步一步转。

具体操作是：设定电角度从0到2π递增，每步停留几十毫秒，同时读取磁编码器的机械角度。因为极对数是已知的，把机械角度乘以极对数就得到编码器测得的电角度。然后比较给定电角度和编码器电角度，两者的差值就是偏移量。

实际操作中，我会采集一整圈的数据，然后用最小二乘法拟合。因为磁编码器本身有非线性误差，单点校准不够准。采集点一般取256个以上，覆盖完整电角度周期。拟合出来的偏移量精度可以做到0.01弧度以内。

校准完成后，把偏移量写入驱动器参数，然后验证：给定Iq电流，Id为0，电机应该平稳旋转，电流波形应该是干净的正弦。如果电流波形有畸变或者电机有顿挫，说明校准没做好，需要重新来。

注意：校准过程中电机必须空载或轻载，如果带重载，开环旋转可能失步，校准数据就废了。另外校准电流不要给太大，一般额定电流的20%到30%就够了，太大电机会发热。

3.3 校准中的常见坑与排查

第一个坑是编码器方向搞反。磁编码器安装方向不同，读出的角度可能是反向的。如果校准时发现偏移量一直在变，或者电机转起来电流异常大，先检查编码器方向。解决方法是在软件里加一个方向标志位，或者把编码器读数取反。

第二个坑是磁铁偏心或间距不对。磁编码器对磁铁的同心度很敏感，偏心超过0.5毫米，角度误差就会明显增大。表现是校准出来的偏移量在不同角度下不一致，拟合残差大。这时候要重新调整磁铁位置，用非磁性夹具保证同心。

第三个坑是SPI读取延迟。磁编码器通过SPI读取有延迟，如果电流环频率很高，读取延迟会导致角度滞后。我一般会把编码器读取放在电流环中断的最前面，或者用DMA自动读取，减少CPU干预。AS5047P的SPI时钟可以跑到10MHz，读取一次大概几微秒，影响不大，但MT6701的SPI慢一些，要注意。

第四个坑是校准时的电流环没关。校准必须在开环模式下做，如果电流环还在运行，它会试图调节电流，干扰校准数据。所以校准前一定要把电流环、速度环、位置环全部禁用。

4. SVPWM调试：从能转到转得好的关键一跃

4.1 SVPWM的基本原理与实现要点

SVPWM（Space Vector Pulse Width Modulation，空间矢量脉宽调制）是FOC里把电压矢量转换成三相PWM占空比的环节。相比SPWM，SVPWM的母线电压利用率高约15%，而且谐波特性更好。对于四足关节电机这种需要大力矩输出的场景，15%的电压利用率提升很实在。

SVPWM的核心思想是把三相逆变器的八个开关状态对应到六个有效矢量和两个零矢量，然后在一个PWM周期内用相邻两个有效矢量和零矢量来合成目标电压矢量。具体计算就是判断目标矢量在哪个扇区，然后算两个有效矢量的作用时间，剩下的时间给零矢量。

实现上，我一般用七段式SVPWM，因为谐波最小。计算过程包括：扇区判断、作用时间计算、占空比生成。扇区判断可以用简单的比较，也可以用查表。作用时间计算涉及三角函数，如果MCU有CORDIC硬件，直接调用，没有的话用查表加插值也够快。

4.2 死区时间与死区补偿

死区时间是SVPWM调试里最容易被忽视但影响很大的参数。死区是为了防止上下桥臂直通，但死区会导致输出电压畸变，尤其在电流过零附近，畸变最严重。表现是电流波形在过零点附近出现平顶或者台阶，低速时电机有轻微顿挫。

死区时间一般设0.5微秒到1微秒，具体看MOSFET的开关速度和栅极驱动能力。设好死区后，如果发现电流畸变还是明显，就要做死区补偿。补偿的思路是根据电流方向，在PWM占空比上叠加一个补偿量。电流为正时补偿量加在正方向，电流为负时加在负方向。

我实测下来，死区补偿能把低速电流THD从8%降到3%左右，效果很明显。补偿量的大小和死区时间、PWM频率、母线电压有关，可以理论计算，但更实用的方法是实验标定：给定一个小电流，观察电流波形，调整补偿量直到波形对称。

4.3 PWM频率与电流环带宽的匹配

PWM频率的选择要平衡几个因素。频率高，电流纹波小，电流环带宽可以做高，但开关损耗大，MCU计算压力也大。频率低，开关损耗小，但电流纹波大，带宽受限。

四足关节电机我一般用20kHz到40kHz的PWM频率。20kHz是听感阈值以上，不会啸叫。如果MCU性能够，上40kHz更好，电流纹波能减半。但要注意，PWM频率和电流环频率的关系：电流环频率一般等于PWM频率，或者PWM频率的一半。我习惯让电流环和PWM同频，在PWM中断里做电流采样和FOC计算，这样时序最干净。

电流环带宽的理论上限是PWM频率的十分之一左右。比如20kHz PWM，电流环带宽最多做到2kHz。实际我会留余量，做到1.5kHz。带宽再高，相位裕度不够，容易振荡。

4.4 电流采样时机与噪声抑制

电流采样是FOC的输入，采样不准，后面全白搭。采样时机很关键：必须在PWM周期的特定时刻采样，避开开关噪声。一般是在下桥臂导通的中点采样，这时候电流流过采样电阻，噪声最小。

如果是三电阻采样，要注意采样窗口够不够。PWM占空比接近0%或100%时，采样窗口会消失，这时候要么用两电阻采样，要么做采样窗口补偿。我一般用两电阻采样，虽然算法复杂一点，但全占空比范围都能采到。

噪声抑制方面，采样电阻的走线要短且对称，运放滤波截止频率设到电流环带宽的5到10倍。如果噪声还是大，可以在软件里加一阶低通滤波，但滤波会引入相位滞后，截止频率不能设太低。

5. 常见问题与排查技巧实录

5.1 电机转起来但力矩小、电流大

这个问题我遇到太多次了，九成以上是编码器校准没做好。排查步骤：先检查偏移量是否在合理范围，一般应该在0到2π/极对数之间。如果偏移量异常，重新校准。如果偏移量正常但力矩还是小，检查极对数设置对不对。极对数设错，电角度就全错了。

还有一个可能是电流采样极性反了。如果Iq给定正，电机应该正转，如果反转，说明电流方向定义反了。把任意两相电流采样互换，或者把Iq符号取反。

5.2 低速时电机抖动或爬行

低速抖动通常有几个原因。第一是编码器分辨率不够，位置量化误差导致电流环周期性扰动。第二是死区补偿没做好，电流过零畸变。第三是电流环带宽太低，跟不上低速时的反电动势变化。

排查方法：先看电流波形，如果过零点有明显畸变，做死区补偿。如果电流波形干净但还抖，提高编码器分辨率或者加位置观测器。如果都不行，检查机械安装，联轴器不同心也会导致低速爬行。

5.3 高速时电流波形畸变、电机发热

高速时反电动势增大，如果母线电压不够，电流环会饱和，波形就畸变。这时候要么提高母线电压，要么做弱磁控制。四足关节电机一般不需要跑很高转速，但如果步态需要快速摆动，瞬时转速可能很高，要留够电压余量。

另一个原因是SVPWM的扇区判断或作用时间计算有误，导致输出电压矢量不对。检查SVPWM代码，用示波器看相电压波形，应该是马鞍波。如果波形不对，逐步排查扇区判断和作用时间计算。

5.4 常见问题速查表

5.5 独家避坑经验

第一个经验：校准和调试要分步做，不要跳步。我见过有人编码器没校准就直接调电流环，调了半天调不好，回头发现是校准问题。正确的顺序是：编码器校准→电流环→速度环→位置环，每一步验证通过再下一步。

第二个经验：保留原始数据。校准时采集的角度数据、调试时的电流波形，都存下来。出问题的时候，对比正常和异常的数据，很快能找到差异。

第三个经验：用信号注入法测带宽。电流环调好后，给定一个正弦小信号，看响应幅值和相位，画出伯德图，确认带宽和相位裕度。比单纯看阶跃响应更准。

第四个经验：温度会影响磁编码器。磁编码器的角度输出会随温度漂移，虽然不大，但四足关节电机发热后，如果发现力矩有变化，可以考虑做温度补偿。不过一般应用影响不大，高精度力控才需要。

6. 从台架到整机：四足关节FOC的联调要点

台架上调好的关节电机，装到四足上跑，往往会遇到新问题。第一个问题是通信延迟。四足的主控和关节驱动器之间通过CAN或者EtherCAT通信，通信周期和延迟会影响力控精度。我一般把CAN通信周期做到1ms，EtherCAT可以做到250微秒甚至更快。通信延迟要补偿，否则足端力控会滞后。

第二个问题是多关节耦合。四足站立时，三个关节的力矩是耦合的，单独调好每个关节，合起来可能不稳定。这时候要在主控层面做力分配和协调控制，关节驱动器只管把力矩指令执行好。

第三个问题是电源干扰。四个关节同时大力矩输出时，母线电流波动很大，如果电源滤波不够，会导致母线电压跌落，影响FOC控制。我一般会在母线加足够大的电容，每个关节驱动器本地再加小电容，减少相互干扰。

第四个问题是热管理。四足关节电机连续工作发热严重，磁编码器和MOSFET都会受温度影响。要做好散热，电机外壳加散热片，驱动器布局时MOSFET分散开，必要时加温度监控，过热降额。

联调的时候，我习惯先让四足站着，给每个关节一个固定的力矩指令，看机器人能不能稳住。然后慢慢加步态，从原地踏步到慢走再到快走。每一步都观察电流波形和关节温度，有问题及时停下来排查。

7. 写在最后的一些实操体会

做四足机器人关节电机FOC控制，说到底是一个抠细节的活。磁编码器校准、SVPWM调试、电流环整定，每一个环节都有很多小坑，但每一个坑填好了，机器人的表现就会上一个台阶。我自己的体会是，不要迷信现成的参数，每个电机、每个编码器、每块板子都不一样，必须自己动手校准和调试。

另外，工具很重要。一个好用的上位机软件能省很多时间，实时看电流波形、角度波形、PID响应，比盲调快得多。我一般会用串口或者CAN把数据传到上位机，用Python或者MATLAB画图分析。数据可视化做得好，问题定位就快。

最后分享一个小技巧：调试的时候，先用手转动电机，感受一下齿槽力矩和摩擦力，心里有个底。然后给一个小电流，看电机能不能平滑转动。如果手转都感觉顿挫，那肯定是机械或者编码器问题，先解决硬件再调软件。这个习惯帮我省了很多时间，避免在软件里绕圈子。