ESP32 无感FOC - SMO滑模观测器代码解析文档
项目概述
本项目是基于ESP32的无感FOC（Field Oriented Control，磁场定向控制）电机控制实现，使用滑模观测器（Sliding Mode Observer, SMO）进行转子位置和速度估算。项目支持双电机控制，采用电流闭环控制方式。
硬件平台
- MCU: ESP32
- 电流采样: 两相采样（A、B相）
- 位置传感器: AS5600磁编码器（仅用于校准，运行时不使用）
- 功率级: 三相桥驱动
核心特性
- 滑模观测器（SMO）无感位置估算
- 电流闭环FOC控制
- 双电机支持（M0和M1）
- 可配置的PID控制器
- 低通滤波器
文件结构
1
2
3
4
5
6
7
8
| 4_SMO滑模观测器/
├── 4_SMO滑模观测器.ino # 主程序入口
├── DengFOC.h/cpp # FOC核心控制库
├── InlineCurrent.h/cpp # 电流采样模块
├── AS5600.h/cpp # AS5600编码器驱动（校准用）
├── lowpass_filter.h/cpp # 低通滤波器
├── pid.h/cpp # PID控制器
└── esp32_adc_driver.h/cpp # ESP32 ADC底层驱动
|
一、主程序 (4_SMO滑模观测器.ino) 详解
1.1 全局变量定义
1
2
3
4
5
|
float Rs = 8.25;
float Ls = 0.004256;
float h = 0.3;
float SMO_Ualpha, SMO_Ubeta;
|
说明：
Rs, Ls: 电机参数，需要根据实际电机调整h: 滑模增益，影响SMO收敛速度和稳定性
1
2
3
4
5
6
7
|
float Est_Ialpha, Est_Ibeta;
float Ealpha, Ebeta;
float Ealpha_flt, Ebeta_flt;
float SMO_Est_theta;
float SMO_Vel;
float SMO_Theta;
|
1.2 setup()函数 - 初始化
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
| void setup() {
Serial.begin(115200);
DFOC_enable();
DFOC_Vbus(12);
DFOC_M0_alignSensor(7, 1);
DFOC_M1_alignSensor(7, -1);
DFOC_M0_SET_CURRENT_PID(3, 200, 0, 100000);
DFOC_M1_SET_CURRENT_PID(3, 200, 0, 100000);
DFOC_M0_SET_VEL_PID(0.02, 0.1, 0, 100000, 6);
DFOC_M1_SET_VEL_PID(0.02, 0.5, 0, 100000, 6);
}
|
1.3 loop()函数 - 主循环
1
2
3
4
5
| void loop() {
runFOC();
DFOC_M0_setVelocity(80);
serialReceiveUserCommand();
}
|
二、SMO滑模观测器核心算法
2.1 SMO原理简介
滑模观测器是一种基于变结构控制理论的状态观测器，其核心思想是：
- 根据电机电压方程构建电流观测器
- 通过滑模控制律（符号函数）使观测电流收敛到实际电流
- 从滑模控制信号中提取反电动势信息
- 由反电动势计算转子位置
2.2 核心函数：SMO_position_estimate()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
| void SMO_position_estimate(){
uint32_t now_time = micros();
Ts = (now_time - SMO_last_time) * 1e-6f;
SMO_last_time = now_time;
if (Ts < 0 || Ts > 5e-3f) Ts = 1e-3f;
float Ialpha = CS_M0.current_a;
float Ibeta = _1_SQRT3 * CS_M0.current_a + _2_SQRT3 * CS_M0.current_b;
Est_Ialpha += Ts * (-Rs / Ls * Est_Ialpha + 1 / Ls * (SMO_Ualpha - Ealpha));
Est_Ibeta += Ts * (-Rs / Ls * Est_Ibeta + 1 / Ls * (SMO_Ubeta - Ebeta));
float Ialpha_Err = Est_Ialpha - Ialpha;
float Ibeta_Err = Est_Ibeta - Ibeta;
Ealpha = h * sat(Ialpha_Err, 0.5f);
Ebeta = h * sat(Ibeta_Err, 0.5f);
Ealpha_flt = 0.1 * Ealpha_flt + 0.9 * Ealpha;
Ebeta_flt = 0.1 * Ebeta_flt + 0.9 * Ebeta;
SMO_Est_theta = (-atan(Ealpha_flt / Ebeta_flt));
SMO_Est_theta = _normalizeAngle(SMO_Est_theta);
}
|
2.3 饱和函数 sat()
1
2
3
4
5
| float sat(float err, float limits) {
if (err > limits) return 1;
else if (err < -limits) return -1;
else return err / limits;
}
|
作用：减少滑模控制的抖振现象（Chattering）
2.4 SMO速度计算
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
| float getSMOVel(){
float Ts = (SMO_angle_prev_ts - SMO_vel_angle_prev_ts)*1e-6;
if(Ts <= 0) Ts = 1e-3f;
float vel = ((float)(SMO_full_rotations - SMO_vel_full_rotations)*_2PI
+ (SMO_angle_prev - SMO_vel_angle_prev)) / Ts;
vel = vel/14;
SMO_vel_angle_prev = SMO_angle_prev;
SMO_vel_full_rotations = SMO_full_rotations;
SMO_vel_angle_prev_ts = SMO_angle_prev_ts;
return vel;
}
|
三、FOC核心控制 (DengFOC.cpp)
3.1 坐标变换
3.1.1 帕克逆变换（dq → αβ）
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
| void M0_setTorque(float Uq, float angle_el) {
Uq = _constrain(Uq, -(voltage_power_supply)/2, (voltage_power_supply)/2);
float Ud = 0;
angle_el = _normalizeAngle(angle_el);
float Ualpha = -Uq*sin(angle_el);
float Ubeta = Uq*cos(angle_el);
SMO_Ualpha = Ualpha;
SMO_Ubeta = Ubeta;
float Ua = Ualpha + voltage_power_supply/2;
float Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
float Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
M0_setPwm(Ua, Ub, Uc);
}
|
3.1.2 克拉克变换（ABC → αβ）
1
2
3
4
5
6
7
8
9
| float cal_Iq_Id(float current_a, float current_b, float angle_el) {
float I_alpha = current_a;
float I_beta = _1_SQRT3 * current_a + _2_SQRT3 * current_b;
float ct = cos(angle_el);
float st = sin(angle_el);
float I_q = I_beta * ct - I_alpha * st;
return I_q;
}
|
3.2 闭环控制函数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
|
void DFOC_M0_setTorque(float Target) {
M0_setTorque(current_loop_M0(Target - DFOC_M0_Current()), S0_electricalAngle());
}
void DFOC_M0_setVelocity(float Target) {
DFOC_M0_setTorque(DFOC_M0_VEL_PID((Target - DFOC_M0_Velocity())*180/PI));
}
void DFOC_M0_set_Velocity_Angle(float Target) {
DFOC_M0_setTorque(DFOC_M0_VEL_PID(
DFOC_M0_ANGLE_PID((Target - DFOC_M0_Angle())*180/PI) - DFOC_M0_Velocity()
));
}
|
四、电流采样模块 (InlineCurrent.cpp)
4.1 硬件配置
1
2
3
4
5
6
7
8
9
10
11
12
13
14
| CurrSense::CurrSense(int Mot_Num) {
if(Mot_Num == 0) {
pinA = 39;
pinB = 36;
_shunt_resistor = 0.01;
amp_gain = 50;
volts_to_amps_ratio = 1.0f / _shunt_resistor / amp_gain;
gain_a = volts_to_amps_ratio;
gain_b = volts_to_amps_ratio;
}
}
|
4.2 ADC零点校准
1
2
3
4
5
6
7
8
9
10
11
12
13
14
| void CurrSense::calibrateOffsets() {
const int calibration_rounds = 1000;
offset_ia = 0;
offset_ib = 0;
for (int i = 0; i < calibration_rounds; i++) {
offset_ia += readADCVoltageInline(pinA);
offset_ib += readADCVoltageInline(pinB);
delay(1);
}
offset_ia /= calibration_rounds;
offset_ib /= calibration_rounds;
}
|
4.3 读取三相电流
1
2
3
4
5
6
| void CurrSense::getPhaseCurrents() {
current_a = (readADCVoltageInline(pinA) - offset_ia) * gain_a;
current_b = (readADCVoltageInline(pinB) - offset_ib) * gain_b;
current_c = (!_isset(pinC)) ? 0 :
(readADCVoltageInline(pinC) - offset_ic) * gain_c;
}
|
五、PID控制器 (pid.cpp)
5.1 PID算法实现
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
| float PIDController::operator()(float error) {
unsigned long timestamp_now = micros();
float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
float proportional = P * error;
float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
integral = _constrain(integral, -limit, limit);
float derivative = D*(error - error_prev)/Ts;
float output = proportional + integral + derivative;
output = _constrain(output, -limit, limit);
if(output_ramp > 0) {
float output_rate = (output - output_prev)/Ts;
if (output_rate > output_ramp)
output = output_prev + output_ramp*Ts;
else if (output_rate < -output_ramp)
output = output_prev - output_ramp*Ts;
}
integral_prev = integral;
output_prev = output;
error_prev = error;
timestamp_prev = timestamp_now;
return output;
}
|
5.2 PID参数说明
| 参数 |
含义 |
推荐范围 |
作用 |
| P |
比例增益 |
0.01~10 |
快速响应误差 |
| I |
积分增益 |
0~500 |
消除稳态误差 |
| D |
微分增益 |
0~1 |
抑制振荡 |
| ramp |
变化率限制 |
1000~100000 |
平滑输出 |
| limit |
输出限幅 |
根据母线电压 |
保护硬件 |
六、低通滤波器 (lowpass_filter.cpp)
6.1 一阶低通滤波器
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
| float LowPassFilter::operator()(float x) {
unsigned long timestamp = micros();
float dt = (timestamp - timestamp_prev)*1e-6f;
if (dt < 0.0f) dt = 1e-3f;
else if(dt > 0.3f) {
y_prev = x;
timestamp_prev = timestamp;
return x;
}
float alpha = Tf/(Tf + dt);
float y = alpha*y_prev + (1.0f - alpha)*x;
y_prev = y;
timestamp_prev = timestamp;
return y;
}
|
传递函数：H(s) = 1/(1 + s·Tf)
截止频率：fc = 1/(2π·Tf)
| 应用 |
Tf值 |
说明 |
| 速度滤波 |
0.01s |
截止频率约16Hz |
| 电流滤波 |
0.002s |
截止频率约80Hz |
| SMO速度滤波 |
0.05s |
截止频率约3Hz |
七、编码器传感器 (AS5600.cpp)
7.1 AS5600读取
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
| double Sensor_AS5600::getSensorAngle() {
uint8_t angle_reg_msb = 0x0C;
byte readArray[2];
wire->beginTransmission(0x36);
wire->write(angle_reg_msb);
wire->endTransmission(false);
wire->requestFrom(0x36, (uint8_t)2);
for (byte i=0; i < 2; i++) {
readArray[i] = wire->read();
}
int _bit_resolution = 12;
int _bits_used_msb = 11-7;
float cpr = pow(2, _bit_resolution);
int lsb_used = _bit_resolution - _bits_used_msb;
uint8_t lsb_mask = (uint8_t)((2 << lsb_used) - 1);
uint8_t msb_mask = (uint8_t)((2 << _bits_used_msb) - 1);
uint16_t readValue = (readArray[1] & lsb_mask);
readValue += ((readArray[0] & msb_mask) << lsb_used);
return (readValue/(float)cpr) * _2PI;
}
|
7.2 传感器校准
1
2
3
4
5
6
7
8
9
10
11
12
13
| void DFOC_M0_alignSensor(int _PP, int _DIR) {
M0_PP = _PP;
M0_DIR = _DIR;
M0_setTorque(3, _3PI_2);
delay(1000);
S0.Sensor_update();
S0_zero_electric_angle = S0_electricalAngle();
M0_setTorque(0, _3PI_2);
}
|
八、主控制循环 runFOC()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
| void runFOC() {
S0.Sensor_update();
CS_M0.getPhaseCurrents();
CS_M1.getPhaseCurrents();
SMO_position_estimate();
SMOThetaUpdate();
SMO_Vel = SMO_Vel_Flter(getSMOVel());
getSMOTheta();
if (cntt++ > 20) {
cntt = 0;
Serial.printf("%f,%f\n", SMO_Vel, DFOC_M0_Velocity());
}
}
|
九、控制框图
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
| ┌─────────────────────────────────────────────────────────────────┐
│ FOC控制框图 │
└─────────────────────────────────────────────────────────────────┘
目标值
│
▼
[PID控制器] ────► Uq
│
▼
[帕克逆变换] ────► Uα, Uβ ───► [SMO] ───► 估算θ, ω
│ │
▼ │
[克拉克逆变换] ────► Ua, Ub, Uc │
│ │
▼ │
[PWM输出] ────────► [电机] ────────────┘
│ │
▼ ▼
[电流采样] ◄──────── 实际Ia, Ib
│
▼
[克拉克变换] ────► Iα, Iβ
│
▼
[帕克变换] ──────► Iq
│
└───────► 反馈给PID
|
十、参数调试指南
10.1 电机参数（必须根据实际电机修改）
1
2
| float Rs = 8.25;
float Ls = 0.004256;
|
10.2 SMO参数
| 参数 |
推荐值 |
调整说明 |
| h（滑模增益） |
0.1~1.0 |
太小收敛慢，太大抖动大 |
| 反电动势滤波系数 |
0.1~0.3 |
影响位置估算平滑度 |
10.3 PID调试步骤
先调电流环：
- P从0.1开始逐步增大
- I设为0，观察电流响应
- 增大I直到电流跟踪准确
再调速度环：
- 电流环稳定后
- P从小增大，观察速度响应
- 适当增加I消除稳态误差
10.4 常见问题
| 现象 |
可能原因 |
解决方法 |
| 电机不转 |
电流环P太小或电流采样错误 |
检查offset校准，增大P |
| 转速抖动 |
SMO增益h不当，滤波参数不对 |
调整h和滤波系数 |
| 失步 |
速度环响应太慢 |
增大速度环P、I |
| 电流过大 |
PID积分累积过快 |
降低I或增加ramp限制 |
十一、数学公式汇总
11.1 克拉克变换（ABC → αβ）
1
2
| Iα = Ia
Iβ = (1/√3)·Ia + (2/√3)·Ib
|
11.2 帕克变换（αβ → dq）
1
2
| Id = Iα·cosθ + Iβ·sinθ
Iq = Iβ·cosθ - Iα·sinθ
|
11.3 帕克逆变换（dq → αβ）
1
2
| Uα = -Uq·sinθ + Ud·cosθ
Uβ = Uq·cosθ + Ud·sinθ
|
11.4 SMO电流观测器
1
2
3
4
5
| dÎα/dt = (-Rs·Îα + Uα - Eα) / Ls
dÎβ/dt = (-Rs·Îβ + Uβ - Eβ) / Ls
Eα = h·sat(Îα - Iα, limit)
Eβ = h·sat(Îβ - Iβ, limit)
|
11.5 角度估算
十二、参考文献
滑模观测器原理：
- “Sensorless Control of PMSM with Sliding Mode Observer”
- PMSM无传感器控制经典方法
FOC基础：
作者信息：
文档生成日期：2026年3月
适用于：DengFOC V4 SMO滑模观测器例程
