基于STM32F4的FOC三环PID串级控制实战详解

开场

做电机控制开发的工程师，尤其是接触无刷直流电机（BLDC）或永磁同步电机（PMSM）时，FOC（Field Oriented Control，磁场定向控制）几乎是绕不开的一环。而FOC控制的核心又落在PID三环串级结构上——位置环、速度环、电流环。很多初学者在STM32F4平台上移植FOC算法时，往往卡在PID参数调优、编码器数据读取、三环切换逻辑这几个关键点上，网上的资料要么偏理论推导，要么只给代码片段，很难形成一套完整的闭环实操方案。

本文围绕STM32F4系列微控制器，结合磁编码器（如MT6701、AS5047P）进行位置/速度反馈，完整实现FOC控制中的电流环、速度环、位置环串级PID控制。从硬件选型、工程配置、核心算法原理、代码实现到参数整定技巧，全部覆盖。无论你是刚接触FOC的学生，还是准备在项目中使用FOC的嵌入式工程师，这篇文章都能帮你快速落地一套可运行的FOC控制方案。

1. 背景与核心概念

1.1 什么是FOC控制

FOC（磁场定向控制）是一种通过控制电机定子电流的幅值和相位，从而实现对转子磁场精确控制的技术。它的核心思想是将三相交流电流通过坐标变换（Clark变换、Park变换）解耦为直轴电流Id和交轴电流Iq，然后分别控制。Id控制磁通，Iq控制转矩。这样，原本复杂的三相交流电机控制就被简化为类似直流电机的控制方式，动态响应快、精度高、效率好。

FOC的典型应用场景包括：机器人关节电机、无人机螺旋桨驱动、电动工具、伺服驱动器、四轴机械臂等。在STM32F4平台上实现FOC，主要依赖其内置的定时器高级定时器（TIM1/TIM8）产生互补PWM、ADC采样相电流、以及DSP指令集（浮点运算单元FPU）加速坐标变换。

1.2 三环串级结构

一个完整的FOC控制通常包含三个控制环，从内到外依次是：

• 电流环（最内环）：控制Iq（转矩电流）和Id（励磁电流），响应最快，通常周期在10~100μs。
• 速度环（中间环）：控制电机转速，输出给电流环的Iq参考值，周期通常1~10ms。
• 位置环（最外环）：控制电机轴的位置（角度），输出给速度环的速度参考值，周期通常10~100ms。

三环串级的好处是：内环响应快，外环响应慢，外环输出作为内环的输入，逐级抑制扰动，系统稳定性好。但调参时需要注意每个环的带宽不能重叠，否则可能引起振荡。

1.3 编码器的作用

在FOC中，转子位置是一个关键量。只有知道转子当前的角度，才能正确执行Park变换和逆Park变换。编码器就是用来测量转子绝对位置（或相对位置）的传感器。对于FOC，推荐使用绝对位置编码器（如磁编码器MT6701、AS5047P），它们能直接输出0~360°的角度值，不需要上电找零。增量式编码器也可以使用，但需要配合上电寻零程序（如搜索Z信号或电感法）。

编码器的分辨率直接影响控制精度。例如12位磁编码器分辨率为0.0879°，16位则可达0.0055°。在STM32F4上，常用SPI或I2C接口读取编码器数据，或者使用定时器编码器模式（适用于正交编码器）。

2. 环境准备与版本说明

2.1 硬件平台

• 主控芯片：STM32F407VGT6（或STM32F4系列其他型号，如STM32F405、STM32F427）
• 电机：低压直流无刷电机（BLDC）或永磁同步电机（PMSM），额定电压12~48V，极对数4~7对
• 驱动板：三相逆变器模块（如DRV8301、IR2104+MOSFET）
• 编码器：磁编码器MT6701（12位，SPI接口）或AS5047P（14位，SPI接口）
• 电流采样：三相低侧电阻采样（或霍尔电流传感器），通过ADC采样
• 调试工具：J-Link或ST-Link，串口（USB转TTL）

2.2 软件环境

• IDE：Keil MDK-ARM V5.38 或 STM32CubeIDE 1.15
• 固件库：HAL库（STM32Cube_FW_F4 V1.28）
• 调试助手：串口调试助手、VOFA+（用于波形显示）、Proteus（可选仿真）

2.3 版本与兼容性说明

本文以下示例基于STM32F407VGT6，HAL库版本1.28，Keil MDK。不同型号的STM32F4在定时器、ADC、DMA资源上略有差异，但基本思路相同。如果使用STM32F405或F427，注意修改定时器通道和ADC引脚。代码中会使用浮点运算，要求开启FPU（硬件浮点单元）。

3. 核心原理拆解：FOC三环控制

3.1 电流环实现

电流环是FOC最内环，直接控制相电流。其流程如下：

1. 采样两相电流（Ia、Ib），通过ADC读取。
2. 计算第三相电流 Ic = -Ia - Ib（假设三相平衡）。
3. 进行Clark变换，将三相电流转换为两相静止坐标系下的Iα、Iβ。
4. 进行Park变换，根据转子电角度θ，将Iα、Iβ转换为旋转坐标系下的Id、Iq。
5. 将Id、Iq与参考值Id_ref、Iq_ref比较，通过PI控制器输出Vd、Vq。
6. 逆Park变换，将Vd、Vq转换为Vα、Vβ。
7. SVPWM（空间矢量脉宽调制）生成三相占空比，控制逆变器开关。

关键点：电流采样需要与PWM同步，通常在PWM中心对齐时触发ADC，避免开关噪声。采样频率通常为PWM频率的1~2倍。PWM频率一般设为10~20kHz。

3.2 速度环实现

速度环的输入是目标转速（单位为rpm或rad/s），输出是电流环的Iq参考值。速度反馈通常通过编码器脉冲数或角度微分计算得到。

常用的速度计算方法：

• M法：在固定时间T内计数编码器脉冲数，然后换算为转速。适用于中高速。
• T法：测量相邻两个脉冲的时间间隔，换算为转速。适用于低速。
• M/T法：结合两者，适用范围更广。

在STM32F4中，可以借助定时器的编码器模式直接读取脉冲数，然后通过定时器中断计算速度。对于磁编码器，可以直接读取角度值，然后计算角度差除以时间得到角速度。

3.3 位置环实现

位置环的输入是目标位置（角度或圈数），输出是速度环的速度参考值。位置反馈直接来自编码器的绝对角度值（或累积角度）。位置环通常采用比例控制（P）或PD控制，因为积分项可能导致位置超调过大。

位置环的一个常见问题是：当目标位置是连续旋转（如多圈绝对位置）时，需要处理角度归一化和换向问题。对于单圈位置（0~360°），需要避免角度跳变（如从359°到0°时，误差计算错误）。通常采用角度差取模或计算最短路径的方法。

3.4 串级控制与参数整定

三环串级控制的核心原则：内环带宽 > 中环带宽 > 外环带宽。带宽越大，响应越快，但抗噪能力越弱。一般整定顺序为：先调电流环，再调速度环，最后调位置环。

整定方法：

1. 电流环：设定Iq_ref为小阶跃，观察Iq实际响应，调整比例系数Kp（先增大Kp直到出现轻微振荡，再减小到0.6倍），积分系数Ki一般取Kp的10~20倍（离散化后，Ki = Kp * Ts / Ti，其中Ti为积分时间常数）。
2. 速度环：给一个目标转速阶跃，观察速度响应，同样先调Kp，再调Ki。注意速度环的积分作用可能引起超调，可加入限幅和抗积分饱和。
3. 位置环：通常只用P，Kp根据系统阻尼需求调整。如果位置有稳态误差，可加入很小的积分项，但要注意积分限幅。

4. 完整实战案例：基于STM32F407+MT6701的FOC控制

4.1 创建项目结构

在Keil中新建项目，选择STM32F407VGT6。在STM32CubeMX中配置：

• 时钟：HSE 8MHz，PLL倍频到168MHz（主频），APB1=42MHz，APB2=84MHz。
• 定时器：TIM1作为高级定时器，输出互补PWM（CH1、CH2、CH3），频率20kHz，PWM模式中央对齐，触发ADC采样。
• ADC：ADC1，注入组或规则组，采样两相电流（PA0、PA1），触发源为TIM1_TRGO。
• SPI：SPI1，全双工模式，速率1MHz，用于读取MT6701编码器。
• 串口：USART1（PA9、PA10），波特率115200，用于调试输出。
• GPIO：使能DRV8301等驱动芯片的ENABLE、FAULT引脚。

生成代码后，在Keil中打开工程，添加以下文件：

• foc.c/foc.h：FOC核心算法（坐标变换、SVPWM、PI控制器）
• encoder.c/encoder.h：MT6701读取驱动
• pid.c/pid.h：通用PID控制器
• main.c：主循环和中断处理

4.2 添加依赖或配置

在CubeMX中配置完成后，关键还要在代码中开启FPU和DSP指令。在Keil的Options for Target -> C/C++ -> Define中加入__FPU_PRESENT=1，ARM_MATH_CM4，__TARGET_FPU_VFP。并添加ARM DSP库（arm_math.h），用于快速坐标变换。

4.3 编写核心代码

4.3.1 编码器读取（MT6701，SPI接口）

// encoder.c #include "encoder.h" #include "spi.h" uint16_t MT6701_ReadAngle(void) { uint16_t data; uint8_t rx[2]; HAL_GPIO_WritePin(MT6701_CS_GPIO_Port, MT6701_CS_Pin, GPIO_PIN_RESET); HAL_SPI_Receive(&hspi1, rx, 2, HAL_MAX_DELAY); // 读取两个字节 HAL_GPIO_WritePin(MT6701_CS_GPIO_Port, MT6701_CS_Pin, GPIO_PIN_SET); data = ((uint16_t)rx[0] << 8) | rx[1]; // MT6701 12位角度，数据格式：高12位为角度，低4位为状态 return (data >> 4) & 0x0FFF; // 0~4095 } float Get_Angle_Rad(void) { uint16_t raw = MT6701_ReadAngle(); float angle_rad = (float)raw * 2.0f * 3.1415926f / 4096.0f; return angle_rad; }

4.3.2 PID控制器模块

// pid.c #include "pid.h" void PID_Init(PID_TypeDef *pid, float kp, float ki, float kd, float limit_out, float limit_i) { pid->Kp = kp; pid->Ki = ki; pid->Kd = kd; pid->limit_out = limit_out; pid->limit_i = limit_i; pid->integral = 0.0f; pid->last_error = 0.0f; } float PID_Calc(PID_TypeDef *pid, float ref, float feedback) { float error = ref - feedback; float p_out = pid->Kp * error; // 积分项，带限幅 pid->integral += pid->Ki * error; if (pid->integral > pid->limit_i) pid->integral = pid->limit_i; else if (pid->integral < -pid->limit_i) pid->integral = -pid->limit_i; float d_out = pid->Kd * (error - pid->last_error); pid->last_error = error; float output = p_out + pid->integral + d_out; if (output > pid->limit_out) output = pid->limit_out; else if (output < -pid->limit_out) output = -pid->limit_out; return output; }

4.3.3 FOC核心算法（坐标变换、SVPWM）

// foc.c #include "foc.h" #include "arm_math.h" // Clark变换 void Clark_Transform(float Ia, float Ib, float *Ialpha, float *Ibeta) { *Ialpha = Ia; *Ibeta = (Ia + 2.0f * Ib) * 0.57735f; // 1/sqrt(3) } // Park变换 void Park_Transform(float Ialpha, float Ibeta, float theta, float *Id, float *Iq) { *Id = Ialpha * arm_cos_f32(theta) + Ibeta * arm_sin_f32(theta); *Iq = -Ialpha * arm_sin_f32(theta) + Ibeta * arm_cos_f32(theta); } // 逆Park变换 void Inverse_Park_Transform(float Vd, float Vq, float theta, float *Valpha, float *Vbeta) { *Valpha = Vd * arm_cos_f32(theta) - Vq * arm_sin_f32(theta); *Vbeta = Vd * arm_sin_f32(theta) + Vq * arm_cos_f32(theta); } // SVPWM，输入Valpha, Vbeta，输出占空比Tu, Tv, Tw（归一化0~1） void SVPWM(float Valpha, float Vbeta, float *Tu, float *Tv, float *Tw) { float Udc = 24.0f; // 母线电压，根据实际调整 float Vmax = Udc / 1.732f; // 最大相电压幅值 // 限幅 if (Valpha > Vmax) Valpha = Vmax; if (Valpha < -Vmax) Valpha = -Vmax; if (Vbeta > Vmax) Vbeta = Vmax; if (Vbeta < -Vmax) Vbeta = -Vmax; // 构建参考电压矢量 float V1 = Vbeta; float V2 = (Valpha * 0.866025f - Vbeta * 0.5f); float V3 = (-Valpha * 0.866025f - Vbeta * 0.5f); // 计算占空比 *Tu = (V1 + 0.5f) * 0.5f; *Tv = (V2 + 0.5f) * 0.5f; *Tw = (V3 + 0.5f) * 0.5f; // 归一化到0~1 if (*Tu > 1.0f) *Tu = 1.0f; if (*Tu < 0.0f) *Tu = 0.0f; if (*Tv > 1.0f) *Tv = 1.0f; if (*Tv < 0.0f) *Tv = 0.0f; if (*Tw > 1.0f) *Tw = 1.0f; if (*Tw < 0.0f) *Tw = 0.0f; }

4.3.4 主循环与中断

// main.c #include "main.h" #include "foc.h" #include "encoder.h" #include "pid.h" PID_TypeDef pid_current_q, pid_current_d; PID_TypeDef pid_speed; PID_TypeDef pid_position; float target_position = 0.0f; // 目标位置，单位rad float current_position = 0.0f; float target_speed = 0.0f; float current_speed = 0.0f; float target_iq = 0.0f; float current_iq = 0.0f; float current_id = 0.0f; float Ia, Ib; // 实际采样值 void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) { // 电流采样完成中断，读取ADC值转成电流 uint32_t adc1 = HAL_ADC_GetValue(&hadc1); uint32_t adc2 = HAL_ADC_GetValue(&hadc2); Ia = (adc1 * 3.3f / 4096.0f - 1.65f) / 0.1f; // 假设采样电阻0.1Ω，增益等 Ib = (adc2 * 3.3f / 4096.0f - 1.65f) / 0.1f; // 触发FOC计算 FOC_Control(); } void FOC_Control(void) { float Ialpha, Ibeta, Id, Iq, Vd, Vq, Valpha, Vbeta; float Tu, Tv, Tw; float theta = current_position; // 电角度，需要换算为电角度（机械角*极对数） // 1. 采样电流 -> Clark Clark_Transform(Ia, Ib, &Ialpha, &Ibeta); // 2. Park变换 Park_Transform(Ialpha, Ibeta, theta, &Id, &Iq); // 3. 电流环PI控制 Vd = PID_Calc(&pid_current_d, 0.0f, Id); // Id_ref通常为0 Vq = PID_Calc(&pid_current_q, target_iq, Iq); // 4. 逆Park Inverse_Park_Transform(Vd, Vq, theta, &Valpha, &Vbeta); // 5. SVPWM SVPWM(Valpha, Vbeta, &Tu, &Tv, &Tw); // 6. 更新PWM占空比 __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (uint32_t)(Tu * (TIM1->ARR))); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, (uint32_t)(Tv * (TIM1->ARR))); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, (uint32_t)(Tw * (TIM1->ARR))); } // 速度环和位置环在主循环中或定时器中断中调用，周期10ms void Control_Loop_Task(void) { // 读取编码器角度 current_position = Get_Angle_Rad(); // 计算速度（差分法） static float last_angle = 0.0f; static uint32_t last_time = 0; uint32_t now = HAL_GetTick(); float dt = (now - last_time) / 1000.0f; if (dt > 0.001f) { float delta = current_position - last_angle; // 处理角度跳变 if (delta > 3.14159f) delta -= 2.0f * 3.14159f; else if (delta < -3.14159f) delta += 2.0f * 3.14159f; current_speed = delta / dt; last_angle = current_position; last_time = now; } // 位置环 target_speed = PID_Calc(&pid_position, target_position, current_position); // 速度环 target_iq = PID_Calc(&pid_speed, target_speed, current_speed); // 电流环的target_iq在FOC_Control中使用 }

4.4 运行与验证

编译下载后，先用串口输出编码器角度值，确认SPI读取正常。然后手动转动电机，观察角度值是否连续变化。接着开环测试：设定一个固定占空比，观察电机是否旋转（注意电流不要太大）。最后逐步加入闭环。

调试技巧：

• 电流环调试：先给一个很小的Iq_ref（如0.1A），观察实际Iq是否跟随。如果电流震荡，减小Kp或增大Ki。
• 速度环调试：固定目标转速100rpm，观察速度响应曲线，调整Kp和Ki。
• 位置环调试：给定目标位置90°，观察位置响应，调整Kp。

4.5 结果说明

通过串口或VOFA+软件，可以实时观察三个环的给定值和反馈值。一个典型的调试效果是：位置环阶跃响应超调量小于10%，稳态误差小于1°；速度环加载后转速恢复时间小于100ms；电流环动态响应时间小于1ms。

5. 常见问题与排查思路

问题现象	常见原因	解决思路
电机不转，但PWM有输出	电流环未闭合，Id/Iq参考值未设置	确认PID初始化，给定非零target_iq
电机抖动或啸叫	电流环PI参数不合适，或PWM频率过低	增加PWM频率至20kHz以上，减小电流环Kp
位置环超调过大	位置环Kp太大，或速度环响应太慢	减小位置环Kp，增大速度环带宽
编码器角度跳变	SPI读取时序不对，或角度处理未取模	检查SPI时钟极性和相位，增加角度归一化处理
电机发热严重	Id不为0，或电流环限幅导致饱和	检查Id_ref是否为0，调整电流环限幅
速度计算不准	角度差分噪声大，或采样时间不均匀	使用低通滤波，或改用M/T法测速

排查清单：

1. 确认编码器数据是否正确（读0x3FFF表示12位最大？MT6701是12位，范围0~4095）。
2. 确认ADC采样值是否随电流变化（给定PWM，观察ADC值变化）。
3. 确认Clark/Park变换输出是否正确（可打印Id，Iq，正常时Id应接近0，Iq跟随参考）。
4. 确认SVPWM输出占空比是否在0~ARR范围内。
5. 确认PID限幅是否合理（电流环输出限幅应小于母线电压/SVPWM最大输出）。

6. 最佳实践与工程建议

6.1 代码结构分层

建议将FOC控制分为三层：

• 硬件抽象层：ADC、PWM、SPI、定时器驱动，与芯片绑定。
• 算法层：坐标变换、PID、SVPWM，与硬件无关，可移植。
• 应用层：三环控制逻辑、运动规划、通信接口。

这样在更换芯片或电机时，只需修改硬件抽象层。

6.2 电流采样同步

电流采样必须在PWM中心对齐时触发，避免开关噪声。STM32F4的定时器可以配置为中央对齐模式，并设置TRGO事件为更新事件，触发ADC注入组。同时，ADC采样时间建议设置为15个周期以上，提高精度。

6.3 抗积分饱和

PID控制器中，积分项必须加限幅，防止积分饱和。当输出达到限幅时，应停止积分，即“积分分离”或“条件积分”。在PID_Calc中已经实现了积分限幅，但还需要考虑：当输出饱和时，积分项应保持不变。

6.4 安全保护

• 过流保护：在ADC中断中检测电流，如果超过阈值立即关闭PWM输出。
• 过温保护：通过NTC或温度传感器，降低电机功率。
• 欠压保护：母线电压低于一定值时，限制功率输出。

6.5 调试手段

推荐使用VOFA+或SerialPlot等上位机，将PID参数、电流、速度、位置等变量通过串口打印，实时绘制波形。这样调参效率远高于逐行打印。

7. 总结与学习路线

本文从FOC三环串级控制的基本原理出发，基于STM32F407和MT6701磁编码器，完整实现了电流环、速度环、位置环的PID控制。你掌握了以下关键点：

• FOC的坐标变换和SVPWM原理。
• 磁编码器的SPI读取方法。
• 三环PID控制器的设计与整定顺序。
• 常见问题排查思路。

下一步可以继续深入学习：

• 无感FOC（基于滑模观测器SMO或扩展卡尔曼滤波器EKF）的位置估算。
• 弱磁控制（提高电机高速区性能）。
• MTPA（最大转矩电流比）控制。
• 使用STM32CubeMX + X-CUBE-SPNx库快速开发。

实际项目中，FOC控制是一个系统工程，不仅要关注算法，还要重视硬件设计（电流采样电路、驱动电路、PCB布局）。建议在调试过程中多做实验，逐步积累经验。如果本文对你有帮助，可以收藏备用，后续我也会分享更多关于FOC调参和工程优化的内容。