行业知识
永磁同步电机无感FOC（龙伯格观测器）算法技术总结-实战篇
2026-11-05 07:40:26
来源：互联网转载
文章目录
- 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
- 3.1.2 Luenberger.c
- 3.2 三段式启动状态机模块
- 3.2.1 mc_statemachine.h
- 3.2.2 mc_statemachine.c
- 3.3 初始化及函数调用
- 3.3.1 初始化
- 3.3.2 反馈速度处理
- 3.3.3 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 __cplusplusextern "C"{ #endif /* Includes -----------------------------------------------------------------*/ #include #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;ipHandle-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 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) 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 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 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 STO_Speed.hSpeed_Buffer[i];}wAverage_Speed = wAverage_Speed >> BUF_POW2;pHandle->STO_Speed.hRotor_Speed_dpp = (s16)(wAverage_Speed);for (i = 0; i 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 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 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 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 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 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 __cplusplusextern "C"{#endif/* Includes -----------------------------------------------------------------*/#include #include "Luenberger.h"/* Macros -------------------------------------------------------------------*//* Typedefs -----------------------------------------------------------------*/#define OPEN_LOOP 0#define CLOSE_LOOP1#define IDLE_STATE2#define CLOSE_SWITCH3#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 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 inc_iq;FOCVars.Iqdref.q = iq_ref_temp >> 15;}else if(popen->Angle 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 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 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(){floatftempll;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.5us pAxisPar.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.5us pAxisPar.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 result FOCVars.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->plus glDebugTestD[13] = STO_Get_Electrical_Angle(&STO_LBG); glDebugTestD[16] = FOCVars.hElAngle;#endif FOCVars.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）
目前代码还有优化空间：实现正反转（反转先降速切开环，反向开环拖动，最后切闭环）
下一篇：投篮训练服务器怎么选择?
