永磁同步电机无感FOC（龙伯格观测器）算法技术总结-实战篇

本文主要是介绍永磁同步电机无感FOC（龙伯格观测器）算法技术总结-实战篇，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

龙伯格观测+PLL理论篇： https://blog.csdn.net/qq_28149763/article/details/136346434
说明：关键代码已在本文给出（源码不开源，抱歉）

1、ST龙伯格算法分析（定点数）

1.1 符号说明

1.2 最大感应电动势计算

1.3 系数计算

1.4 龙伯格观测器计算

1.5 锁相环计算

1.6 观测器增益计算

1.7 锁相环PI计算（ST）

1.8 平均速度的用意

2、启动策略

2.1 V/F压频比控制

2.2 I/F压频比控制

3、算法开发

3.1 Luenberger核心算法模块

3.1.1 Luenberger.h

/********************************************************************************* @file    Luenberger.h* @author  hlping* @version V1.0.0* @date    2023-12-28* @brief   ******************************************************************************* @attention********************************************************************************/#ifndef __LUENBERGER_H
#define __LUENBERGER_H/* *INDENT-OFF* */
#ifdef __cplusplus
extern "C"
{#endif/* Includes -----------------------------------------------------------------*/#include <types.h>#include "mc_math.h"#include "foc.h"/* Macros -------------------------------------------------------------------*/#define BUFFER_SIZE (u16)64#define BUF_POW2    LOG2(BUFFER_SIZE)#define F1            (s16)(16384)#define F2            (s16)(8192)#define F1LOG         LOG2(F1)#define F2LOG         LOG2(F2)#define PLL_KP_F      (s16)(16384)#define PLL_KI_F      (u16)(65535)#define KPLOG         LOG2(PLL_KP_F)#define KILOG         LOG2(PLL_KI_F)#define PI 			      3.14159265358979#define VARIANCE_THRESHOLD  0.0625            typedef struct{s32 K1;s32 K2;s16 hC2;s16 hC4;s16 hF1;s16 hF2;s16 hF3;s16 hC1;s16 hC3;s16 hC5;s16 hC6;s32 wIalfa_est;s32 wIbeta_est;s32 wBemf_alfa_est;s32 wBemf_beta_est;s32 hBemf_alfa_est;s32 hBemf_beta_est;}STO_Observer_t;typedef struct{s16 Rs;u16 Rs_factor;s16 Ls;u32 Ls_factor;u16 Pole;u16 pwm_frequency;u32 max_speed_rpm;s16 max_voltage;s16 max_current;s16 motor_voltage_constant;s16 motor_voltage_constant_factor;float motor_voltage_constant_f;u16 max_bemf_voltage;}STO_Parameter_t;typedef struct{bool_t Max_Speed_Out;bool_t Min_Speed_Out;bool_t Is_Speed_Reliable;s16 hSpeed_Buffer[BUFFER_SIZE];u16 bSpeed_Buffer_Index;s32 wMotorMaxSpeed_dpp;u16 hPercentageFactor;s16 hRotor_Speed_dpp;s32 wSpeed_PI_integral_sum;s16 hSpeed_P_Gain;s16 hSpeed_I_Gain;s32 speed_sum;}STO_Speed_t;typedef struct{STO_Observer_t STO_Observer;STO_Speed_t STO_Speed;STO_Parameter_t STO_Parameter;s16 hRotor_El_Angle;s16 hRotor_Speed;s16 hLast_Rotor_Speed;s16 hRotor_avSpeed;}STO_luenberger;/* Typedefs -----------------------------------------------------------------*//* Function declarations ----------------------------------------------------*/void STO_InitSpeedBuffer(STO_luenberger *pHandle);void STO_Init(STO_luenberger *pHandle);void STO_Update_Constant(STO_luenberger *pHandle);void STO_Set_k1k2(STO_luenberger *pHandle,s32 pk1,s32 pk2);void STO_PLL_Set_Gains(STO_luenberger *pHandle,s16 pkp,s16 pki);void STO_Gains_Init(STO_luenberger *pHandle);s16 Speed_PI(STO_luenberger *pHandle,s16 hAlfa_Sin, s16 hBeta_Cos);s16 Calc_Rotor_Speed(STO_luenberger *pHandle,s16 hBemf_alfa, s16 hBemf_beta);void Store_Rotor_Speed(STO_luenberger *pHandle,s16 hRotor_Speed);s16 STO_Get_Speed(STO_luenberger *pHandle);s16 STO_Get_Electrical_Angle(STO_luenberger *pHandle);void STO_Set_Electrical_Angle(STO_luenberger *pHandle,s16 eiAngle);void STO_Calc_Speed(STO_luenberger *pHandle);void STO_CalcElAngle(STO_luenberger *pHandle,FOCVars_t *pfoc, s16 hBusVoltage);/* *INDENT-OFF* */#ifdef __cplusplus
}
#endif
/* *INDENT-ON* */#endif /* __LUENBERGER_H *//**** END OF FILE ****/

3.1.2 Luenberger.c

/********************************************************************************* @file    Luenberger.c* @author  hlping* @version V1.0.0* @date    2023-12-28* @brief   ******************************************************************************* @attention********************************************************************************//* Includes ------------------------------------------------------------------*/
#include "Luenberger.h"/* Private define ------------------------------------------------------------*//**
* @brief 初始化观测器速度缓冲区
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @return 无返回值
*/
void STO_InitSpeedBuffer(STO_luenberger *pHandle)
{u8 i;/*init speed buffer*/for (i=0;i<BUFFER_SIZE;i++){pHandle->STO_Speed.hSpeed_Buffer[i] = 0x00;}pHandle->STO_Speed.bSpeed_Buffer_Index = 0;
}/**
* @brief 初始化观测器
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @return 无返回值
*/
void STO_Init(STO_luenberger *pHandle)
{pHandle->STO_Observer.wIalfa_est = 0;pHandle->STO_Observer.wIbeta_est = 0;pHandle->STO_Observer.wBemf_alfa_est = 0;pHandle->STO_Observer.wBemf_beta_est = 0;pHandle->STO_Speed.Is_Speed_Reliable = FALSE;pHandle->STO_Speed.wSpeed_PI_integral_sum = 0;pHandle->STO_Speed.Max_Speed_Out = FALSE;pHandle->STO_Speed.Min_Speed_Out = FALSE;pHandle->STO_Speed.hRotor_Speed_dpp = 0;pHandle->STO_Speed.speed_sum = 0;pHandle->hRotor_avSpeed=0;pHandle->hRotor_El_Angle = 0;            //could be used for start-up procedurepHandle->hRotor_avSpeed = 0;STO_InitSpeedBuffer(pHandle);//	hSpeed_P_Gain = 1638;  //0.1*16384
//	hSpeed_I_Gain = 0;
}/**
* @brief 设置观测器增益参数
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @param pk1 增益参数1
* @param pk2 增益参数2
* @return 无返回值
*/
void STO_Set_k1k2(STO_luenberger *pHandle,s32 pk1,s32 pk2)
{pHandle->STO_Observer.K1 = pk1;pHandle->STO_Observer.K2 = pk2;
}/**
* @brief 设置观测器PLL增益参数
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @param pkp PLL比例增益参数
* @param pki PLL积分增益参数
* @return 无返回值
*/
void STO_PLL_Set_Gains(STO_luenberger *pHandle,s16 pkp,s16 pki)
{pHandle->STO_Speed.hSpeed_P_Gain = pkp;pHandle->STO_Speed.hSpeed_I_Gain = pki;
}/**
* @brief 更新观测器常数参数
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @return 无返回值
*/
void STO_Update_Constant(STO_luenberger *pHandle)
{float temp_rs;float temp_ls;temp_rs=pHandle->STO_Parameter.Rs/(float)pHandle->STO_Parameter.Rs_factor;temp_ls=pHandle->STO_Parameter.Ls/(float)pHandle->STO_Parameter.Ls_factor;pHandle->STO_Observer.hC1 = (s32)(pHandle->STO_Observer.hF1 * temp_rs/(temp_ls*pHandle->STO_Parameter.pwm_frequency));//pHandle->STO_Observer.hC2 = (s32)(hF1 * k1/(float)(pHandle->STO_Parameter.pwm_frequency));pHandle->STO_Observer.hC2 = (s32)(pHandle->STO_Observer.K1);//�����Ǵ������ģ�����Ҫ�ڳ���Ƶ��pHandle->STO_Observer.hC3 = (s32)(pHandle->STO_Observer.hF1 * pHandle->STO_Parameter.max_bemf_voltage/(temp_ls*pHandle->STO_Parameter.max_current*pHandle->STO_Parameter.pwm_frequency));//pHandle->STO_Observer.hC4 = (s32)(((k2 * max_current/(max_bemf_voltage))*hF2)/(float)pHandle->STO_Parameter.pwm_frequency);//pHandle->STO_Observer.hC4 = (s32)(hF1 * k2/(float)(pHandle->STO_Parameter.pwm_frequency));pHandle->STO_Observer.hC4 = (s32)(pHandle->STO_Observer.K2);//�����Ǵ������ģ�����Ҫ�ڳ���Ƶ��pHandle->STO_Observer.hC5 = (s32)(pHandle->STO_Observer.hF1 * pHandle->STO_Parameter.max_voltage/(temp_ls*pHandle->STO_Parameter.max_current*pHandle->STO_Parameter.pwm_frequency));//	hC1 = pHandle->STO_Observer.hC1;
//	hC2 = pHandle->STO_Observer.hC2;
//	hC3 = pHandle->STO_Observer.hC3;
//	hC4 = pHandle->STO_Observer.hC4;
//	hC5 = pHandle->STO_Observer.hC5;
}/**
* @brief 初始化观测器增益参数
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @return 无返回值
*/
void STO_Gains_Init(STO_luenberger *pHandle)
{s16 htempk;pHandle->STO_Observer.hF3 = 1;htempk = (s16)((100*65536)/(F2*2*PI));	//100 rad/swhile (htempk != 0){htempk /= 2;pHandle->STO_Observer.hF3 *= 2;}pHandle->STO_Observer.hC6 = (s16)((F2 * pHandle->STO_Observer.hF3 * 2 * PI)/65536);//10000pHandle->STO_Observer.hF1 = F1;pHandle->STO_Observer.hF2 = F2;pHandle->STO_Parameter.motor_voltage_constant_f = (float)(pHandle->STO_Parameter.motor_voltage_constant/(float)pHandle->STO_Parameter.motor_voltage_constant_factor);pHandle->STO_Parameter.max_bemf_voltage = (u16)((1.2 * pHandle->STO_Parameter.max_speed_rpm*pHandle->STO_Parameter.motor_voltage_constant_f*SQRT_2)/(1000*SQRT_3));
//	pHandle->STO_Parameter.max_current = (u16)(pHandle->STO_Parameter.max_current);
//	pHandle->STO_Parameter.max_voltage = (s16)(pHandle->STO_Parameter.max_voltage);STO_Update_Constant(pHandle);//	hSpeed_P_Gain = PLL_KP_GAIN;
//	hSpeed_I_Gain = PLL_KI_GAIN;pHandle->STO_Speed.wMotorMaxSpeed_dpp = (s32)((1.2 * pHandle->STO_Parameter.max_speed_rpm*65536*pHandle->STO_Parameter.Pole)/(float)(pHandle->STO_Parameter.pwm_frequency*60));pHandle->STO_Speed.hPercentageFactor = (u16)(VARIANCE_THRESHOLD*128);
}/**
* @brief 计算电机旋转速度的PID控制器
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @param hAlfa_Sin e_alpha*sin
* @param hBeta_Cos e_beta*cos
* @return 返回计算得到的电机旋转速度
*/
s16 Speed_PI(STO_luenberger *pHandle,s16 hAlfa_Sin, s16 hBeta_Cos)
{s32 wSpeed_PI_error, wOutput;s32 wSpeed_PI_proportional_term, wSpeed_PI_integral_term;wSpeed_PI_error = hBeta_Cos - hAlfa_Sin;
#if 0		//????if(wSpeed_PI_error > 50)wSpeed_PI_error = 50;else if(wSpeed_PI_error < -50)wSpeed_PI_error = -50;
#endifwSpeed_PI_proportional_term = pHandle->STO_Speed.hSpeed_P_Gain * wSpeed_PI_error;  // !!!pwSpeed_PI_integral_term = pHandle->STO_Speed.hSpeed_I_Gain * wSpeed_PI_error;      // !!!iif ( (pHandle->STO_Speed.wSpeed_PI_integral_sum >= 0) && (wSpeed_PI_integral_term >= 0) && (pHandle->STO_Speed.Max_Speed_Out == FALSE) ){if ((s32)(pHandle->STO_Speed.wSpeed_PI_integral_sum + wSpeed_PI_integral_term) < 0){pHandle->STO_Speed.wSpeed_PI_integral_sum = S32_MAX;}else{pHandle->STO_Speed.wSpeed_PI_integral_sum += wSpeed_PI_integral_term;  //integral}}else if ( (pHandle->STO_Speed.wSpeed_PI_integral_sum <= 0) && (wSpeed_PI_integral_term <= 0) && (pHandle->STO_Speed.Min_Speed_Out == FALSE) ){if((s32)(pHandle->STO_Speed.wSpeed_PI_integral_sum + wSpeed_PI_integral_term) > 0){pHandle->STO_Speed.wSpeed_PI_integral_sum = -S32_MAX;}else{pHandle->STO_Speed.wSpeed_PI_integral_sum += wSpeed_PI_integral_term;   //integral}}else{pHandle->STO_Speed.wSpeed_PI_integral_sum += wSpeed_PI_integral_term;   //integral}wOutput = (wSpeed_PI_proportional_term >> KPLOG) + (pHandle->STO_Speed.wSpeed_PI_integral_sum >> KILOG);if (wOutput > pHandle->STO_Speed.wMotorMaxSpeed_dpp){pHandle->STO_Speed.Max_Speed_Out = TRUE;wOutput = pHandle->STO_Speed.wMotorMaxSpeed_dpp;}else if (wOutput < (-pHandle->STO_Speed.wMotorMaxSpeed_dpp)){pHandle->STO_Speed.Min_Speed_Out = TRUE;wOutput = -pHandle->STO_Speed.wMotorMaxSpeed_dpp;}else{pHandle->STO_Speed.Max_Speed_Out = FALSE;pHandle->STO_Speed.Min_Speed_Out = FALSE;}return ((s16)wOutput);
}/**
* @brief 锁相环计算电机控制器旋转速度
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @param hBemf_alfa BEMF alpha轴反电动势观测值
* @param hBemf_beta BEMF beta轴反电动势观测值
* @return 返回计算得到的电机旋转速度
*/
s16 Calc_Rotor_Speed(STO_luenberger *pHandle,s16 hBemf_alfa, s16 hBemf_beta)
{s32 wAlfa_Sin_tmp, wBeta_Cos_tmp;s16 hOutput;Trig_Components Local_Components;Local_Components = Trig_Functions(pHandle->hRotor_El_Angle);/* Alfa & Beta BEMF multiplied by hRotor_El_Angle Cos & Sin*/wAlfa_Sin_tmp = (s32)(hBemf_alfa * Local_Components.hSin);wBeta_Cos_tmp = (s32)(hBemf_beta * Local_Components.hCos);//alfa_sin_test = wAlfa_Sin_tmp >> 15;//beta_cos_test = wBeta_Cos_tmp >> 15;/* Speed PI regulator */hOutput = Speed_PI(pHandle,(s16)(wAlfa_Sin_tmp >> 15), (s16)(wBeta_Cos_tmp >> 15));return (hOutput);
}/**
* @brief 将电机旋转速度存储数组中
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @param hRotor_Speed 要存储的电机旋转速度
* @return 无返回值
*/
void Store_Rotor_Speed(STO_luenberger *pHandle,s16 hRotor_Speed)
{static s32 start_flag;pHandle->STO_Speed.hSpeed_Buffer[pHandle->STO_Speed.bSpeed_Buffer_Index] = hRotor_Speed;pHandle->STO_Speed.speed_sum += pHandle->STO_Speed.hSpeed_Buffer[pHandle->STO_Speed.bSpeed_Buffer_Index];if(++(pHandle->STO_Speed.bSpeed_Buffer_Index) >= BUFFER_SIZE) //16{pHandle->STO_Speed.bSpeed_Buffer_Index = 0;start_flag = 1;}if(start_flag == 0){pHandle->hRotor_avSpeed = pHandle->STO_Speed.speed_sum / pHandle->STO_Speed.bSpeed_Buffer_Index;}else{pHandle->hRotor_avSpeed = pHandle->STO_Speed.speed_sum >> BUF_POW2;pHandle->STO_Speed.speed_sum -= pHandle->STO_Speed.hSpeed_Buffer[pHandle->STO_Speed.bSpeed_Buffer_Index];}pHandle->STO_Speed.hRotor_Speed_dpp = pHandle->hRotor_avSpeed;
/*bSpeed_Buffer_Index++;if (bSpeed_Buffer_Index == BUFFER_SIZE) //64{bSpeed_Buffer_Index = 0;STO_Calc_Speed();}hSpeed_Buffer[bSpeed_Buffer_Index] = hRotor_Speed;
*/
}/**
* @brief 获取电机旋转速度
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @return 返回电机旋转速度
*/
s16 STO_Get_Speed(STO_luenberger *pHandle)
{return (pHandle->hRotor_avSpeed);
}/**
* @brief 获取电机转子的电角度
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @return 返回电机电角位
*/
s16 STO_Get_Electrical_Angle(STO_luenberger *pHandle)
{return (pHandle->hRotor_El_Angle);
}/**
* @brief 设置电机转子的电角度
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @param eiAngle 设置的电机电角位
* @return 无返回值
*/
void STO_Set_Electrical_Angle(STO_luenberger *pHandle,s16 eiAngle)
{pHandle->hRotor_El_Angle = eiAngle;
}/**
* @brief 计算观测速度
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @return 无返回值
*/
void STO_Calc_Speed(STO_luenberger *pHandle)
{s32 wAverage_Speed = 0;s32 wError;s32 wAverageQuadraticError = 0;u8 i;for (i = 0; i < BUFFER_SIZE; i++){wAverage_Speed += pHandle->STO_Speed.hSpeed_Buffer[i];}wAverage_Speed = wAverage_Speed >> BUF_POW2;pHandle->STO_Speed.hRotor_Speed_dpp = (s16)(wAverage_Speed);for (i = 0; i < BUFFER_SIZE; i++){wError = pHandle->STO_Speed.hSpeed_Buffer[i] - wAverage_Speed;wError = (wError * wError);wAverageQuadraticError += (u32)(wError);}//It computes the measurement variancewAverageQuadraticError= wAverageQuadraticError >> BUF_POW2;//The maximum variance acceptable is here calculated as ratio of average speedwAverage_Speed = (s32)(wAverage_Speed * wAverage_Speed);wAverage_Speed = (wAverage_Speed >> 7) * pHandle->STO_Speed.hPercentageFactor;#if 0 // for debug onlyQuadraticError = wAverageQuadraticError;AverageSpeed = wAverage_Speed;
#endifif (wAverageQuadraticError > wAverage_Speed){pHandle->STO_Speed.Is_Speed_Reliable = FALSE;}else{pHandle->STO_Speed.Is_Speed_Reliable = TRUE;}
}/**
* @brief 观测器观测电角度
* @param pHandle 指向STO_luenberger结构体的指针，用于存储控制器的状态信息
* @param pfoc 指向FOCVars_t结构体的指针，用于存储电压和电流信息
* @param hBusVoltage 输入电压
* @return 无返回值
*/
void STO_CalcElAngle(STO_luenberger *pHandle,FOCVars_t *pfoc, s16 hBusVoltage)
{s32 wIalfa_est_Next, wIbeta_est_Next;s32 wBemf_alfa_est_Next, wBemf_beta_est_Next;s16 hValfa, hVbeta;s16 hIalfa_err, hIbeta_err;s32 bDirection;s16 hRotor_Speed;if (pHandle->STO_Observer.wBemf_alfa_est > (s32)(S16_MAX * pHandle->STO_Observer.hF2)){pHandle->STO_Observer.wBemf_alfa_est = S16_MAX * pHandle->STO_Observer.hF2;}else if (pHandle->STO_Observer.wBemf_alfa_est <= (s32)(S16_MIN * pHandle->STO_Observer.hF2)){pHandle->STO_Observer.wBemf_alfa_est = -S16_MAX * pHandle->STO_Observer.hF2;}if (pHandle->STO_Observer.wBemf_beta_est > (s32)(S16_MAX * pHandle->STO_Observer.hF2)){pHandle->STO_Observer.wBemf_beta_est = S16_MAX * pHandle->STO_Observer.hF2;}else if (pHandle->STO_Observer.wBemf_beta_est <= (s32)(S16_MIN * pHandle->STO_Observer.hF2)){pHandle->STO_Observer.wBemf_beta_est = -S16_MAX * pHandle->STO_Observer.hF2;}if (pHandle->STO_Observer.wIalfa_est > (s32)(S16_MAX * pHandle->STO_Observer.hF1)){pHandle->STO_Observer.wIalfa_est = S16_MAX * pHandle->STO_Observer.hF1;}else if (pHandle->STO_Observer.wIalfa_est <= (s32)(S16_MIN * pHandle->STO_Observer.hF1)){pHandle->STO_Observer.wIalfa_est = -S16_MAX * pHandle->STO_Observer.hF1;}if (pHandle->STO_Observer.wIbeta_est > S16_MAX * pHandle->STO_Observer.hF1){pHandle->STO_Observer.wIbeta_est = S16_MAX * pHandle->STO_Observer.hF1;}else if (pHandle->STO_Observer.wIbeta_est <= S16_MIN * pHandle->STO_Observer.hF1){pHandle->STO_Observer.wIbeta_est = -S16_MAX * pHandle->STO_Observer.hF1;}hIalfa_err = (s16)((pHandle->STO_Observer.wIalfa_est >> F1LOG)- pfoc->Ialphabeta.alpha);hIbeta_err = (s16)((pHandle->STO_Observer.wIbeta_est >> F1LOG)- pfoc->Ialphabeta.beta);hValfa = (s16)((pfoc->Valphabeta.alpha * hBusVoltage) >> 15);   //�������ߵ�ѹĿ���Ǽ�С���ߵ�ѹ������ϵͳ��Ӱ��hVbeta = (s16)((pfoc->Valphabeta.beta * hBusVoltage) >> 15);    //�������ߵ�ѹĿ���Ǽ�С���ߵ�ѹ������ϵͳ��Ӱ��/*alfa axes observer*/wIalfa_est_Next = (s32)(pHandle->STO_Observer.wIalfa_est - (s32)(pHandle->STO_Observer.hC1 * (s16)(pHandle->STO_Observer.wIalfa_est >> F1LOG))+(s32)(pHandle->STO_Observer.hC2 * hIalfa_err)+(s32)(pHandle->STO_Observer.hC5 * hValfa)-(s32)(pHandle->STO_Observer.hC3 * (s16)(pHandle->STO_Observer.wBemf_alfa_est >> F2LOG)));//I(n+1)=I(n)-rs*T/Ls*I(n)+K1*(I(n)-i(n))+T/Ls*V-T/Ls*emfwBemf_alfa_est_Next = (s32)(pHandle->STO_Observer.wBemf_alfa_est + (s32)(pHandle->STO_Observer.hC4 * hIalfa_err)+(s32)(pHandle->STO_Observer.hC6 * pHandle->STO_Speed.hRotor_Speed_dpp * (pHandle->STO_Observer.wBemf_beta_est / (pHandle->STO_Observer.hF2 * pHandle->STO_Observer.hF3))));//(wBemf_beta_est>>20)));//emf(n+1)=emf(n)+K2*(I(n)-i(n))+p*w*emfb*T/*beta axes observer*/wIbeta_est_Next = (s32)(pHandle->STO_Observer.wIbeta_est - (s32)(pHandle->STO_Observer.hC1 * (s16)(pHandle->STO_Observer.wIbeta_est >> F1LOG))+(s32)(pHandle->STO_Observer.hC2 * hIbeta_err)+(s32)(pHandle->STO_Observer.hC5 * hVbeta)-(s32)(pHandle->STO_Observer.hC3 * (s16)(pHandle->STO_Observer.wBemf_beta_est >> F2LOG)));wBemf_beta_est_Next = (s32)(pHandle->STO_Observer.wBemf_beta_est + (s32)(pHandle->STO_Observer.hC4 * hIbeta_err)-(s32)(pHandle->STO_Observer.hC6 * pHandle->STO_Speed.hRotor_Speed_dpp * (pHandle->STO_Observer.wBemf_alfa_est / (pHandle->STO_Observer.hF2 * pHandle->STO_Observer.hF3))));//(wBemf_alfa_est>>20)));/* Extrapolation of present rotation direction, necessary for PLL */if (pHandle->STO_Speed.hRotor_Speed_dpp >= 0){bDirection = -1;}else{bDirection = 1;}/*Calls the PLL blockset*/pHandle->STO_Observer.hBemf_alfa_est = pHandle->STO_Observer.wBemf_alfa_est >> F2LOG;pHandle->STO_Observer.hBemf_beta_est = pHandle->STO_Observer.wBemf_beta_est >> F2LOG;pHandle->hRotor_Speed = Calc_Rotor_Speed(pHandle,(s16)(pHandle->STO_Observer.hBemf_alfa_est * bDirection),(s16)(-pHandle->STO_Observer.hBemf_beta_est * bDirection));if(pfoc->Vqd.q > 0){if(pHandle->hRotor_Speed < 0){pHandle->hRotor_Speed = -pHandle->hRotor_Speed;}}else //MotorCtrl.Dir == CCW{if(pHandle->hRotor_Speed > 0){pHandle->hRotor_Speed = -pHandle->hRotor_Speed;}}Store_Rotor_Speed(pHandle,pHandle->hRotor_Speed);pHandle->hRotor_El_Angle = (s16)(pHandle->hRotor_El_Angle + pHandle->hRotor_Speed);/*storing previous values of currents and bemfs*/pHandle->STO_Observer.wIalfa_est = wIalfa_est_Next;pHandle->STO_Observer.wBemf_alfa_est = wBemf_alfa_est_Next;pHandle->STO_Observer.wIbeta_est = wIbeta_est_Next;pHandle->STO_Observer.wBemf_beta_est = wBemf_beta_est_Next;
}
/**** END OF FILE ****/

3.2 三段式启动状态机模块

3.2.1 mc_statemachine.h

mc_statemachine.h定义相关变量和结构体以及函数申明：

/********************************************************************************* @file    mc_statemachine.h* @author  hlping* @version V1.0.0* @date    2022-11-28* @brief   ******************************************************************************* @attention********************************************************************************/#ifndef __MC_STATEMACHINE_H
#define __MC_STATEMACHINE_H/* *INDENT-OFF* */
#ifdef __cplusplus
extern "C"
{
#endif/* Includes -----------------------------------------------------------------*/
#include <types.h>
#include "Luenberger.h"/* Macros -------------------------------------------------------------------*//* Typedefs -----------------------------------------------------------------*/
#define OPEN_LOOP		  	0
#define CLOSE_LOOP			1
#define IDLE_STATE			2
#define CLOSE_SWITCH		3#define OPENLOOPTIMEINSEC  8.0typedef enum{MOTOR_STOP=0,			MOTOR_INIT=1,			MOTOR_START=2,		MOTOR_RUN=3,			MOTOR_FAULT=4,			MOTOR_BRAKE=5			
} MCStatus_t;//typedef struct
//{
//	MCStatus_t Status;
//	u32 StatusMacCnt;
//	u32 Dir;				
//	bool_t DirChangeFlag;	
//	bool_t StartupFlag;		//	s16 SpdRampRef;		
//	s16 SpdRef;			
//}mc_control_t;typedef struct
{u32 State;u32 Angle;s16 LockCnt;u16 pole;u32 pwm_frequency;float looptimeinsec;//u32 time;u32 locktime;u16 initialSpeedinRpm;u16 start_iq;u16 start_iq_max;u16 min_iq;u16 start_vq;u16 endSpeedOpenloop;u16 inc_iq;u32 ramp_time;u32 delta_startup_ramp;s16 ElangleError;bool_t speed_loop_enable;bool_t current_loop_enable;
}mc_openloop_t;/* Function declarations ----------------------------------------------------*/
void mc_statemachine_init(mc_openloop_t *ploop);
void mc_statemachine_process(mc_openloop_t *popen,STO_luenberger *pHandle,FOCVars_t *pfoc,s16 hBusVoltage);
u16 mc_get_state(mc_openloop_t *ploop);
bool_t mc_get_speed_loop_enable(mc_openloop_t *ploop);
void mc_set_speed_loop_enable(mc_openloop_t *ploop,bool_t state);
bool_t mc_get_current_loop_enable(mc_openloop_t *ploop);
void mc_set_current_loop_enable(mc_openloop_t *ploop,bool_t state);
void mc_set_vf_iqRef(FOCVars_t *pfoc,int16_t piqref);
void mc_parameter_init(mc_openloop_t *ploop);/* *INDENT-OFF* */
#ifdef __cplusplus
}
#endif
/* *INDENT-ON* */#endif /* __MC_STATEMACHINE_H *//**** END OF FILE ****/

3.2.2 mc_statemachine.c

/********************************************************************************* @file    mc_statemachine.c* @author  hlping* @version V1.0.0* @date    2023-01-08* @brief   ******************************************************************************* @attention********************************************************************************//* Includes ------------------------------------------------------------------*/
#include "mc_statemachine.h"
#include "public_global.h"/* Variable definitions ------------------------------------------------------*/
/**
* @brief 初始化电机启动控制器状态机
* @param ploop 指向mc_openloop_t结构体的指针，用于存储控制器的状态信息
* @return 无返回值
*/
void mc_statemachine_init(mc_openloop_t *ploop)
{ploop->speed_loop_enable = FALSE;ploop->current_loop_enable = FALSE;
//	 locktime = ploop->time;
//   ploop->Angle = 0;
//	 ploop->State = IDLE_STATE;
//	 ploop->LockCnt = 0;
//	 endSpeedOpenloop = ploop->endSpeedOpenloop;ploop->looptimeinsec = 1/(float)ploop->pwm_frequency;ploop->inc_iq = 32767 * (ploop->start_iq_max - ploop->start_iq)/ploop->locktime;ploop->ramp_time = (u32)(ploop->endSpeedOpenloop * ploop->pole * 65536 * ploop->looptimeinsec * 65536 /60 );ploop->delta_startup_ramp = (u32)((ploop->ramp_time/OPENLOOPTIMEINSEC)/(float)ploop->pwm_frequency);
}/**
* @brief 处理电机启动控制器状态机
* @param popen 指向mc_openloop_t结构体的指针，用于存储控制器的状态信息
* @param pHandle 指向STO_luenberger结构体的指针，用于存储观测器状态
* @param pfoc 指向FOCVars_t结构体的指针，用于存储FOC相关变量
* @param hBusVoltage 输入电压，单位为V
* @return 无返回值
*/
void mc_statemachine_process(mc_openloop_t *popen,STO_luenberger *pHandle,FOCVars_t *pfoc,s16 hBusVoltage)
{if(popen->LockCnt >= popen->locktime && popen->State != IDLE_STATE)STO_CalcElAngle(pHandle,pfoc, hBusVoltage);if(popen->State == OPEN_LOOP){if(popen->LockCnt < popen->locktime) 			//LOCK{static s32 iq_ref_temp = 0;if (popen->LockCnt == 0)iq_ref_temp = 0;popen->LockCnt++;//			if( FOCVars.Vqd.q > 0)       //��ת
//				iq_ref_temp += inc_iq;
//			else if(FOCVars.Vqd.q < 0)   //��ת
//				iq_ref_temp -= inc_iq;
//			else                         //ֹͣ  
//				iq_ref_temp = 0;iq_ref_temp += popen->inc_iq;FOCVars.Iqdref.q = iq_ref_temp >> 15;}else if(popen->Angle < popen->ramp_time) 			//SPEED RAMP{if (popen->Angle == 0){	//FOCVars.Vqd.q = popen->start_vq * 32767 ;FOCVars.Vqd.q = popen->start_vq ;FOCVars.Vqd.d = 0 ;}popen->Angle += popen->delta_startup_ramp;		if( FOCVars.Vqd.q > 0)       //正转FOCVars.hElAngle += (popen->Angle >> 16);else if(FOCVars.Vqd.q < 0)   //反转FOCVars.hElAngle -= (popen->Angle >> 16);		}else{popen->State = CLOSE_LOOP;
//#ifndef NDEBUG
//			OpenLoopSpeed = STO_Get_Speed();	// for test only
//#endif#if 0  //just for test,openloop for observation anglepopen->speed_loop_enable = FALSE;popen->current_loop_enable = FALSE;#elsepopen->speed_loop_enable = TRUE;popen->current_loop_enable = TRUE;#endifpopen->ElangleError = STO_Get_Electrical_Angle(pHandle) - FOCVars.hElAngle;//FOCVars.hElAngle = STO_Get_Electrical_Angle(pHandle);}}else if(popen->State == CLOSE_LOOP){FOCVars.hElAngle = STO_Get_Electrical_Angle(pHandle) - popen->ElangleError;
//		FOCVars.hElAngle = STO_Get_Electrical_Angle(pHandle) - (popen->ElangleError>>2);
//		//s16 err = popen->ElangleError>>2;
//		if(popen->ElangleError > 0)
//			popen->ElangleError--;
//		else if(popen->ElangleError < 0)
//			popen->ElangleError++;}	
}/**
* @brief 初始化电机控制器参数
* @param ploop 指向mc_openloop_t结构体的指针，用于存储控制器的状态信息
* @return 无返回值
*/
void mc_parameter_init(mc_openloop_t *ploop)
{ploop->State = OPEN_LOOP;     //开环启动ploop->Angle = 0;           //如等于RAMP_TIME就意味着跳过RAMP阶段，直接速度闭环ploop->LockCnt = 0;ploop->speed_loop_enable = FALSE;  ploop->current_loop_enable = FALSE;
}/**
* @brief 设置电机控制器的中间变量
* @param pfoc 指向FOCVars_t结构体的指针，用于存储电机控制器的中间变量
* @param piqref 输入的iq参考值
* @return 无返回值
*/
void mc_set_vf_iqRef(FOCVars_t *pfoc,int16_t piqref)
{pfoc->Iqdref.q = piqref;pfoc->Iqdref.d = 0;
}/**
* @brief 获取电机控制器的状态
* @param ploop 指向mc_openloop_t结构体的指针，用于存储控制器的状态信息
* @return 返回控制器的状态
*/
u16 mc_get_state(mc_openloop_t *ploop)
{return (ploop->State);
}/**
* @brief 获取电机控制器是否启用速度环
* @param ploop 指向mc_openloop_t结构体的指针，用于存储控制器的状态信息
* @return 返回true或false，表示是否启用速度环
*/
bool_t mc_get_speed_loop_enable(mc_openloop_t *ploop)
{return (ploop->speed_loop_enable);
}/**
* @brief 设置电机控制器是否启用速度环
* @param ploop 指向mc_openloop_t结构体的指针，用于存储控制器的状态信息
* @param state 设置为true或false，表示是否启用速度环
* @return 无返回值
*/
void mc_set_speed_loop_enable(mc_openloop_t *ploop,bool_t state)
{ploop->speed_loop_enable = state;
}/**
* @brief 获取电机控制器是否启用电流环
* @param ploop 指向mc_openloop_t结构体的指针，用于存储控制器的状态信息
* @return 返回true或false，表示是否启用电流环
*/
bool_t mc_get_current_loop_enable(mc_openloop_t *ploop)
{return (ploop->current_loop_enable);
}/**
* @brief 设置电机控制器是否启用电流环
* @param ploop 指向mc_openloop_t结构体的指针，用于存储控制器的状态信息
* @param state 设置为true或false，表示是否启用电流环
* @return 无返回值
*/
void mc_set_current_loop_enable(mc_openloop_t *ploop,bool_t state)
{ploop->current_loop_enable = state;
}/**** END OF FILE ****/

3.3 初始化及函数调用

定义全局变量：

STO_luenberger        STO_LBG;     //龙伯格观测器相关变量
mc_openloop_t         mc_openloop; //三段式启动相关变量

3.3.1 初始化

当编码器类型为ENCODER_TYPE_UNKNOWN时为无感运行模式：

if(sensor_peripheral.Encoder_Sensor.encType == ENCODER_TYPE_UNKNOWN)//just for sensorless{sensor_peripheral.Encoder_Sensor.encRes=65535;/* init Luenberger parameter */STO_Parameter_t Parameter={.Rs = 55,                                         //0.055 pMotorParSet.tBasePar.resist[A_AXIS].Rs_factor = 1000,.Ls = 21,                                         //2.1e-4 pMotorParSet.tBasePar.inductance[A_AXIS].Ls_factor = 100000,.Pole = pMotorParSet.tBasePar.poles[A_AXIS],      //4.pwm_frequency = SAMPLE_FREQUENCY,                //10000//.max_speed_rpm = pMotorParSet.tBasePar.ratedVel[A_AXIS]*60/sensor_peripheral.Encoder_Sensor.encRes,//rpm.max_speed_rpm = 3000,//rpm.max_voltage = (s16)(ProtectPar.regenOn/1000),           //36000 mV//.max_current = pMotorParSet.tBasePar.maxPhaseCurr[A_AXIS]/1000,  //A.max_current = 31,                                //A.motor_voltage_constant = 4,                      //4v/1000rpm.motor_voltage_constant_factor = 1,};memcpy(&STO_LBG.STO_Parameter,&Parameter,sizeof(STO_Parameter_t));STO_Init(&STO_LBG);STO_Set_k1k2(&STO_LBG,-24225,25925);STO_Gains_Init(&STO_LBG);STO_PLL_Set_Gains(&STO_LBG,638,45);  mc_openloop_t   openloop={.State= IDLE_STATE,.Angle = 0,.LockCnt = 0,.pole = pMotorParSet.tBasePar.poles[A_AXIS],.pwm_frequency = SAMPLE_FREQUENCY,.looptimeinsec = 1/(float)SAMPLE_FREQUENCY,.locktime = SAMPLES_PER_50MSECOND,.start_iq = 0,             //Q轴启动电流.start_iq_max = 3000,      //mA.endSpeedOpenloop = 300,   //rpm.start_vq = 4000,         //VF启动电压};memcpy(&mc_openloop,&openloop,sizeof(mc_openloop_t));mc_statemachine_init(&mc_openloop);}

3.3.2 反馈速度处理

void AxisVelocityCalc()
{float	ftempll;	if(sensor_peripheral.Encoder_Sensor.encType != ENCODER_TYPE_UNKNOWN){pAxisPar.vel[A_AXIS][2] = sensor_peripheral.Encoder_Sensor.deltaPos * SAMPLE_FREQUENCY; //TODO: ftempll = (float) pAxisPar.vel[A_AXIS][2];ftempll =_filterPar._velFdk(&ftempll,&_filterPar);//250 point  1.5uspAxisPar.vel[A_AXIS][1] = (long) ftempll;pAxisPar.vel[A_AXIS][0] = pAxisPar.vel[A_AXIS][1];}else{pAxisPar.vel[A_AXIS][2] = STO_Get_Speed(&STO_LBG)*sensor_peripheral.Encoder_Sensor.encRes/(pMotorParSet.tBasePar.poles[A_AXIS] * 2 * PI);//we->rpm->plus; //TODO: ftempll = (float) pAxisPar.vel[A_AXIS][2];ftempll =_filterPar._velFdk(&ftempll,&_filterPar);//250 point  1.5uspAxisPar.vel[A_AXIS][1] = (long) ftempll;pAxisPar.vel[A_AXIS][0] = pAxisPar.vel[A_AXIS][1];}
}

3.3.3 FOC模块处理

/*** @brief  FOC function* @param  None* @retval None
**/
void FOC_Model(void)
{FOCVars.Iqdref.q = pMotorParSet.currRef[A_AXIS];//*1.414; // RMS resultFOCVars.Iqdref.d = 0;
//    FOC_Cal(&FOCVars);FOCVars.Ialphabeta = Clark(FOCVars.Iab);FOCVars.Iqd = Park(FOCVars.Ialphabeta, FOCVars.hElAngle);	FOCVars.IqdErr.d = FOCVars.Iqdref.d - FOCVars.Iqd.d;FOCVars.IqdErr.q = FOCVars.Iqdref.q - FOCVars.Iqd.q;if(sensor_peripheral.Encoder_Sensor.encType != ENCODER_TYPE_UNKNOWN){FOCVars.Vqd.d = PI_Controller(&PidIdHandle, FOCVars.IqdErr.d);PidIqHandle.hKpGain = PidIdHandle.hKpGain;PidIqHandle.hKiGain = PidIdHandle.hKiGain;FOCVars.Vqd.q = PI_Controller(&PidIqHandle, FOCVars.IqdErr.q);}else{if(mc_get_current_loop_enable(&mc_openloop) == TRUE){FOCVars.Vqd.d = PI_Controller(&PidIdHandle, FOCVars.IqdErr.d);PidIqHandle.hKpGain = PidIdHandle.hKpGain;PidIqHandle.hKiGain = PidIdHandle.hKiGain;FOCVars.Vqd.q = PI_Controller(&PidIqHandle, FOCVars.IqdErr.q);}//mc_statemachine_process(&mc_openloop,&STO_LBG,&FOCVars,sensor_peripheral.pVbusPar.glVBus);//放在这里主要是可以重置FOCVars.Vqd.qmc_statemachine_process(&mc_openloop,&STO_LBG,&FOCVars,24000);//放在这里主要是可以重置FOCVars.Vqd.q}#if 1//STO_CalcElAngle(&FOCVars,sensor_peripheral.pVbusPar.glVBus);glDebugTestD[12] = STO_Get_Speed(&STO_LBG)*sensor_peripheral.Encoder_Sensor.encRes/(pMotorParSet.tBasePar.poles[A_AXIS] * 2 * PI);//we->rpm->plusglDebugTestD[13] = STO_Get_Electrical_Angle(&STO_LBG);glDebugTestD[16] = FOCVars.hElAngle;
#endifFOCVars.Vqd = Circle_LimitationFunc(&FOCVars.CircleLimitationFoc, FOCVars.Vqd); //340 pointFOCVars.Valphabeta = Rev_Park(FOCVars.Vqd,  FOCVars.hElAngle);								//TODO: USING COSA COSB  calc infront.....FOCVars.DutyCycle = SVPWM_3ShuntCalcDutyCycles(&FOCVars);						pMotorParSet.va[A_AXIS] = MID_PWM_CLK_PRD - FOCVars.DutyCycle.CntPhA;pMotorParSet.vb[A_AXIS] = MID_PWM_CLK_PRD - FOCVars.DutyCycle.CntPhB;pMotorParSet.vc[A_AXIS] = MID_PWM_CLK_PRD - FOCVars.DutyCycle.CntPhC;
}

实际效果（约100rpm）

目前代码还有优化空间：实现正反转（反转先降速切开环，反向开环拖动，最后切闭环）

这篇关于永磁同步电机无感FOC（龙伯格观测器）算法技术总结-实战篇的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---