目录
本文代码在该硬件上测试运行：点此查看硬件
在该FOC框架基础上修改：stm32_foc_2以及openloop
本文完整Keil+stm32f103c8t6+HAL库工程：smo分支
默认你已经了解前文【无感FOC开环拖动V/F和I/F】
电机参数
先准备好电机参数和电路参数和软件参数。由于增加了无感观测器，增加了不少计算耗时，把有感FOC基础框架的重复计数器(点此重复计数器的讲解)改大，给计算留出更多空间。
c
//conf.h
#pragma once
// 电机物理参数
#define POLE_PAIRS 7 // 极对数
#define PSI_F 0.00257f // 磁链
#define MOTOR_RS 5.5f // 相电阻，Ω
#define MOTOR_LS 0.00085f // 相电感，H
// 电机控制参数
#define MOTOR_MAX_CURRENT 1.0f //软件限制的电机最大电流，安培A
#define MOTOR_MAX_SPEED 200.0f // 软件限制的电机最大转速，弧度每秒
// 电路参数
#define U_BUS 12.0f // 母线电压，伏
#define R_SHUNT 0.02f // 电流采样电阻，欧姆
#define OP_GAIN 50.0f // 运放放大倍数
#define ADC_REFERENCE_VOLT 3.3f // 电流采样adc参考电压，伏
#define ADC_BITS 12 // ADC精度，bit
#define ENCODER_BITS 14 // 磁编码器精度，bit
// 单片机配置参数
#define motor_pwm_freq 40000 // 驱动桥pwm频率，Hz
#define motor_pwm_repetition 8 // pwm溢出多少次后，产生一次更新事件（adc触发事件）
#define motor_adc_dt (1.0f / (motor_pwm_freq / (motor_pwm_repetition + 1.0f))) // adc中断周期，秒
#define motor_adc_freq (motor_pwm_freq / (motor_pwm_repetition + 1.0f) / 1000.0f) //adc中断频率，kHz
#define motor_speed_calc_freq 500 // 电机速度计算频率，Hz
// 软件参数
#define sin_table_bit 11 // 三角函数查找表的位数
#define sin_table_size (1 << sin_table_bit) // 三角函数查找表的大小u α , u β u_\alpha,u_\beta uα,uβ计算
u α , u β u_\alpha,u_\beta uα,uβ是用三相电压clark变换来的，svpwm输出的占空比不是相电压，而是相对GND的电压，因此需要先算出中心点电压，再得到相电压，再进行clark变换：
c
void set_pwm_duty(float d_u, float d_v, float d_w)
{
htim1.Instance->CCR1 = d_u * htim1.Instance->ARR;
htim1.Instance->CCR2 = d_v * htim1.Instance->ARR;
htim1.Instance->CCR3 = d_w * htim1.Instance->ARR;
float d_mid = (d_u + d_v + d_w) / 3.0f;
float u_u = (d_u - d_mid) * U_BUS;
float u_v = (d_v - d_mid) * U_BUS;
arm_clarke_f32(u_u, u_v, &motor_u_alpha, &motor_u_beta);
}ADC零漂计算
由于无感观测器是观测电流，特此增加ADC零漂计算，但是实际测试下来发现零漂约等于无，该功能加不加都可以。
c
//adc.c
...
float adc_offset_u;
float adc_offset_v;
// adc零漂计算
void adc_offset_calibration()
{
size_t adc_offset_calib_count = 512;
static float adc_offset_sum_u;
static float adc_offset_sum_v;
for (size_t i = 0; i < adc_offset_calib_count; i++)
{
HAL_ADC_Start(&hadc1);
HAL_ADC_Start(&hadc2);
HAL_ADC_PollForConversion(&hadc1, 10);
HAL_ADC_PollForConversion(&hadc2, 10);
adc_offset_sum_u += (HAL_ADC_GetValue(&hadc1) * (ADC_REFERENCE_VOLT / (1 << ADC_BITS)) - ADC_REFERENCE_VOLT / 2.0f);
adc_offset_sum_v += (HAL_ADC_GetValue(&hadc2) * (ADC_REFERENCE_VOLT / (1 << ADC_BITS)) - ADC_REFERENCE_VOLT / 2.0f);
}
adc_offset_u = adc_offset_sum_u / adc_offset_calib_count;
adc_offset_v = adc_offset_sum_v / adc_offset_calib_count;
}
...零漂计算放在桥臂开启前：
c
//main.c
...
HAL_ADCEx_Calibration_Start(&hadc1);
HAL_ADCEx_Calibration_Start(&hadc2);
adc_offset_calibration();
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
...滑模观测器函数
根据前文【01 观测反电动势】的推导结果：
1.计算 i ~ α = i α − i ^ α \tilde{i}\alpha = i\alpha-\hat{i}_\alpha i~α=iα−i^α
2.计算 z α = − K ⋅ sat ( i ~ α ) z_\alpha = -K \cdot \text{sat}(\tilde{i}_\alpha) zα=−K⋅sat(i~α)
3.计算精确离散化 i α ^ [ k + 1 ] = i α ^ [ k ] e − R s T s L s + ( u α [ k ] − z α [ k ] ) ∗ 1 − e − R s T s L s R s \hat{i_\alpha}[k+1] = \hat{i_\alpha}[k] e^{-\frac{R_sT_s}{L_s}} + (u_\alpha[k] - z_\alpha[k])*\frac{ 1 - e^{-\frac{R_sT_s}{L_s}}}{R_s} iα^[k+1]=iα^[k]e−LsRsTs+(uα[k]−zα[k])∗Rs1−e−LsRsTs
4.将 z α z_\alpha zα输入低通滤波得到并更新 e α ^ \hat{e_\alpha} eα^
滑模函数就是将以上几个步骤翻译为C语言。
先按照【01 观测反电动势】所说的将常数 e − R s T s L s e^{-\frac{R_sT_s}{L_s}} e−LsRsTs和 1 − e − R s T s L s R s \frac{1 - e^{-\frac{R_sT_s}{L_s}}}{R_s} Rs1−e−LsRsTs提前计算好：
c
//smo.c
void smo_var_init()
{
smo_i_decay = expf(-(MOTOR_RS * motor_adc_dt) / MOTOR_LS);
smo_i_drive_gain = (1.0f - smo_i_decay) / MOTOR_RS;
}再定一下几个公式里的常数。K要覆盖住最大的反电动势=最大转速*磁链常数；sat的线性区通常很小，设置为10%的最大电流：
c
#define SMO_K (MOTOR_MAX_SPEED * POLE_PAIRS * PSI_F) // 覆盖住最大的反电动势=最大转速*磁链常数
#define SMO_SAT_A (MOTOR_MAX_CURRENT / 10.0f) // 用1/10的最大电流然后是正式的滑模观测器函数：
c
static void smo()
{
// 对应公式\tilde{i}_\alpha = i_\alpha-\hat{i}_\alpha
smo_tilde_i_alpha = motor_i_alpha - smo_hat_i_alpha;
smo_tilde_i_beta = motor_i_beta - smo_hat_i_beta;
// 对应公式z_\alpha = -K \cdot \text{sat}(\tilde{i}_\alpha)
if (smo_tilde_i_alpha > smo_sat_a)
smo_z_alpha = -smo_k;
else if (smo_tilde_i_alpha < -smo_sat_a)
smo_z_alpha = smo_k;
else
smo_z_alpha = -(smo_k / smo_sat_a) * smo_tilde_i_alpha;
if (smo_tilde_i_beta > smo_sat_a)
smo_z_beta = -smo_k;
else if (smo_tilde_i_beta < -smo_sat_a)
smo_z_beta = smo_k;
else
smo_z_beta = -(smo_k / smo_sat_a) * smo_tilde_i_beta;
// 可以直接用sat函数，但是上述展开后能减少计算量
// smo_z_alpha = -smo_k * sat(smo_tilde_i_alpha / smo_sat_a);
// smo_z_beta = -smo_k * sat(smo_tilde_i_beta / smo_sat_a);
// 对应步骤：精确离散化
smo_hat_i_alpha = smo_i_decay * smo_hat_i_alpha + smo_i_drive_gain * (motor_u_alpha - smo_z_alpha);
smo_hat_i_beta = smo_i_decay * smo_hat_i_beta + smo_i_drive_gain * (motor_u_beta - smo_z_beta);
// 对应步骤将$z_\alpha$输入低通滤波得到并更新$\hat{e_\alpha}$
smo_hat_e_alpha = low_pass_filter(smo_z_alpha, smo_hat_e_alpha, smo_z_lpf_alpha);
smo_hat_e_beta = low_pass_filter(smo_z_beta, smo_hat_e_beta, smo_z_lpf_alpha);
}PLL函数
滑模观测器后立刻接PLL函数，PLL函数本质就是一个pid函数。这里将smo()直接整合到smo_pll()函数中。将前文【02 PLL】最后的控制框图翻译为C语言。
先配置好控制框图中的常量参数，理论上 K p = 2 ζ ∗ 2 π f , K i = ( 2 π f ) 2 K_p = 2\zeta*2\pi f,K_i = (2\pi f)^2 Kp=2ζ∗2πf,Ki=(2πf)2，其中 f f f是设计带宽， ζ \zeta ζ通常设置为0.707，理论带宽通常为FOC计算周期的 1 10 \frac{1}{10} 101到 1 20 \frac{1}{20} 201之间。低通滤波理论公式是 α = 1 − e − 2 π f l p T s \alpha = 1 - e^{-2\pi f_{lp} T_s} α=1−e−2πflpTs，取滤波 f l p = 250 H z f_{lp}=250Hz flp=250Hz。本文硬件使用stm32f103c8t6浮点计算，计算能力有限，实际测试时将PLL带宽设置到了30Hz左右，公式算出的参数也要根据实际情况进行调节。这些理论公式是怎么来的属于另一门学问，这里先不深入。
c
#define SMO_Z_LPF_ALPHA 0.33f
#define SMO_PLL_KP 128.0f
#define SMO_PLL_KI 30000.0f接下来是PLL框图的C语言版本：
c
void smo_pll()
{
smo();
int hat_theta_hex = rad_to_index(smo_hat_theta_e);
float sin_theta = sin_table(hat_theta_hex);
float cos_theta = cos_table(hat_theta_hex);
arm_park_f32(smo_hat_e_alpha,
smo_hat_e_beta,
&smo_hat_e_d,
&smo_hat_e_q,
sin_theta,
cos_theta);
// 对应公式err=\frac{\hat{e_d}}{\omega \psi_f}
smo_pll_err = -smo_hat_e_d / (smo_hat_speed_e * PSI_F + 1e-6f); // 1e-6f是为了防止除以0
//积分项，积分饱和clamp截断
smo_pll_integral =
clamp(smo_pll_integral + smo_pll_ki * smo_pll_err * motor_adc_dt, -SMO_PLL_OMEGA_MAX, SMO_PLL_OMEGA_MAX);
smo_hat_speed_e = clamp(smo_pll_integral + smo_pll_kp * smo_pll_err, -SMO_PLL_OMEGA_MAX, SMO_PLL_OMEGA_MAX);
//角度周期化到0到2pi之间
smo_hat_theta_e = wrap_0_2pi_fast(smo_hat_theta_e + smo_hat_speed_e * motor_adc_dt);
}开环切入
反电动势观测器需要电机达到一定转速后才能切入闭环观测器，因此一开始不能直接使用观测器，但是观测器可以全程在后台跑，只不过切入的时候foc再使用观测器的输出。
这里简单一点，直接使用开环转速进行判断切入，你也可以加上其他条件，比如要判断q轴反电动势smo_hat_e_q要达到某个阈值，或者PLL误差smo_pll_err要小于某个阈值等等。
c
bool smo_is_ready()
{
return fabsf(openloop_speed_e) > SMO_CHECK_READY_SPEED;
}开环切入观测器的实时性要求不高，我们可以将这个判断逻辑放在主函数中，减少adc中断计算压力。切入时，才真正设置FOC控制模式，比如速度环之类的。注意，常规的反电动势观测器由于需要电机在转，所以无法做到位置环。注意，切入时要给观测器赋初值，很多参数的初值并不是0，比如切入时，理论上d轴反电动势是0，q轴反电动势是 ω ∗ ψ f \omega*\psi_f ω∗ψf。
c
//main.c
...
while(1)
{
// 无感开环启动切到闭环观测器，实时性要求不高，计算能力受限，放在while(1)里
if (sensorless_control_state == sensorless_control_state_openloop && smo_is_ready())
{
//观测器很多参数初始不是零，需要赋初值
smo_hat_e_d = 0;
smo_hat_e_q = motor_dir * SMO_CHECK_READY_SPEED * PSI_F;
smo_hat_theta_e = openloop_theta_e;
smo_hat_speed_e = openloop_speed_e;
smo_pll_integral = openloop_speed_e;
sensorless_control_state = sensorless_control_state_closeloop;
//粗调
set_motor_pid(
0.0f, 0.0f, 0.0f,
0.4f, 0.4f, 0.0f,
0.3f, 0.5f, 0.0f,
0.3f, 0.5f, 0.0f);
motor_mode_speed(motor_dir * 100.0f);
// motor_mode_torque(motor_dir * 0.0f, motor_dir * 0.8f);
// 无感串级时，pid给弱，否则电流容易被冲烂
// set_motor_pid(
// 0.0f, 0.0f, 0.0f,
// 0.4f, 0.1f, 0.0f,
// 0.3f, 0.5f, 0.0f,
// 0.3f, 0.5f, 0.0f);
// motor_mode_speed_torque(motor_dir * 100.0f, 0.7f);
arm_pid_reset_f32(&pid_position);
arm_pid_reset_f32(&pid_speed);
arm_pid_reset_f32(&pid_torque_d);
arm_pid_reset_f32(&pid_torque_q);
}ADC中断
滑模观测器代码一定要放在稳定的计算周期里，这里放在ADC中断里刚计算出 i α , i β i_\alpha,i_\beta iα,iβ的后面。用了一个状态机，在切入观测器后，将FOC转子角度从开环转子角度转为滑模观测出的转子角度。
c
//adc.c
...
arm_clarke_f32(motor_i_u, motor_i_v, &motor_i_alpha, &motor_i_beta);
//这里设置smo全程计算，也可以切入观测器后再启动smo计算
smo_pll();
int theta_e_hex_norm = rotor_logic_angle_hex_norm;
if (is_sensorless)
{
switch (sensorless_control_state)
{
case sensorless_control_state_openloop:
if (openloop_type == openloop_type_VF)
openloop_VF();
else
openloop_IF();
theta_e_hex_norm = rad_to_index(openloop_theta_e);
break;
case sensorless_control_state_closeloop:
theta_e_hex_norm = rad_to_index(smo_hat_theta_e);
break;
default:
return;
}
}
arm_park_f32(motor_i_alpha, motor_i_beta, &motor_i_d, &motor_i_q
...失踪监测
无感FOC不像有感FOC，可以知道真实的转子角度，一旦无感FOC失去了转子角度跟踪，它就会把错误的转子角度输入到foc中，导致电机运行异常抖动或发热等，因此我们加上失踪监测功能，一旦监测到失踪，就停止运行。失踪判断条件可以有很多种，这里将 e d ^ , e q ^ \hat{e_d},\hat{e_q} ed^,eq^作为监测量，理论上观测器正常运行时 e d ^ \hat{e_d} ed^在0附近， e q ^ \hat{e_q} eq^具有一定的值，如果 e d ^ \hat{e_d} ed^太大或者 e q ^ \hat{e_q} eq^太小，则说明观测器失踪。还进行了强滤波防止噪声干扰。
c
//smo.c
...
float smo_check_lose_e_d;
float smo_check_lose_e_q;
// 无感观测保护，防止观测器失踪后还在长时间运行电机
bool check_smo_lose()
{
//失踪检测实时性要求不高，给予低通滤波
smo_check_lose_e_d = low_pass_filter(smo_hat_e_d, smo_check_lose_e_d, 0.005f);
smo_check_lose_e_q = low_pass_filter(smo_hat_e_q, smo_check_lose_e_q, 0.005f);
return fabsf(smo_check_lose_e_d) > 0.4f || fabsf(smo_check_lose_e_q) < (SMO_CHECK_READY_SPEED * PSI_F * 0.7f);
}这个功能对实时性要求不高，我们还是放在主函数运行，减轻adc中断计算压力。这个功能在你调pid参数的时候可以暂时关闭。
因为刚切入观测器时，还不稳定，直接监测可能造成误保护，所以延时了500ms再开启监测，因此还需要在切入观测器的逻辑里加上时间标记。
c
//main.c
...
while (1)
{
static uint32_t tik;
// 无感开环启动切到闭环观测器，实时性要求不高，计算能力受限，放在while(1)里
if (sensorless_control_state == sensorless_control_state_openloop && smo_is_ready())
{
//观测器很多参数初始不是零，需要赋初值
smo_hat_e_d = 0;
smo_hat_e_q = motor_dir * SMO_CHECK_READY_SPEED * PSI_F;
smo_check_lose_e_d = smo_hat_e_d;//这个是失踪监测用的
smo_check_lose_e_q = smo_hat_e_q;
smo_hat_theta_e = openloop_theta_e;
smo_hat_speed_e = openloop_speed_e;
smo_pll_integral = openloop_speed_e;
sensorless_control_state = sensorless_control_state_closeloop;
//粗调
set_motor_pid(
0.0f, 0.0f, 0.0f,
0.4f, 0.4f, 0.0f,
0.3f, 0.5f, 0.0f,
0.3f, 0.5f, 0.0f);
motor_mode_speed(motor_dir * 100.0f);
// motor_mode_torque(motor_dir * 0.0f, motor_dir * 0.8f);
// 无感串级时，pid给弱，否则电流容易被冲烂
// set_motor_pid(
// 0.0f, 0.0f, 0.0f,
// 0.4f, 0.1f, 0.0f,
// 0.3f, 0.5f, 0.0f,
// 0.3f, 0.5f, 0.0f);
// motor_mode_speed_torque(motor_dir * 100.0f, 0.7f);
arm_pid_reset_f32(&pid_position);
arm_pid_reset_f32(&pid_speed);
arm_pid_reset_f32(&pid_torque_d);
arm_pid_reset_f32(&pid_torque_q);
tik = ms_tik;//这个是失踪监测用的
}
if(sensorless_control_state == sensorless_control_state_closeloop)
{
static bool check_smo_lose_enable = false;
if ((ms_tik - tik) > 500)//切入观测器500ms后开启监测
check_smo_lose_enable = true;
// 监测观测器是否失踪，实时性要求不高，计算能力受限，放在while(1)里
if (motor_control_context.type != control_type_null
&& check_smo_lose_enable == true
&& check_smo_lose())
{
led_blink_period = 40;
motor_control_context.type = control_type_null;
set_pwm_duty(0.0f, 0.0f, 0.0f);
}
}
...开环启动
在while(1)前面启动开环拖动，实测开环IF需要独立的一套电流环pid，不然开环阶段可能抖动：
c
//main.c
...
set_motor_pid(
0.0f, 0.0f, 0.0f,
1.0f, 0.2f, 0.0f,
0.19f, 0.02f, 0.0f,
0.19f, 0.02f, 0.0f);
motor_dir = -1; //+1代表正转，-1代表反转
motor_control_context.type = control_type_openloop;
openloop_type = openloop_type_IF; // 使用IF开环拖动
// 【无感模式】 在while(1)中从开环切换到无感观测器
is_sensorless = true;
while(1)
{
//适机切入观测器
...滑模成功的数据查看
可以通过多种数据分析滑模是否运行成功。本文使用Ballet软件进行波形查看。
1.查看转子角度追踪情况。本文硬件放置了一个磁编码器，可以进行追踪对比。对比smo_hat_theta_e和index_to_rad(rotor_logic_angle_hex_norm)。可以看到两者基本重合，有时360度跨圈差属于正常。
2.查看dq轴反电动势。查看smo_check_lose_e_d和smo_check_lose_e_q，可以看到smo_check_lose_e_d在0附近，smo_check_lose_e_q具有一定值。这里不直接查看smo_hat_e_d和smo_hat_e_d是因为该二者是滑模开关函数少量低通滤波来的，波动很大。
3.查看转速跟踪情况。本文硬件放置了一个磁编码器，可以进行追踪对比真实的转子转速。查看smo_hat_speed_e和encoder_speed*极对数，可以看到均值是对的上的，这里的波动再加一个低通滤波后就能降低。
4.电机实际运行状态。关闭板载编码器或者将电机拿出来看运行情况。
本文代码在该硬件上测试运行：点此查看硬件
本文完整keil工程：smo分支