文章目录
- 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 __cplusplus extern "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;i3.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#include "Luenberger.h" /* Macros -------------------------------------------------------------------*/ /* Typedefs -----------------------------------------------------------------*/ #define OPEN_LOOP 0 #define CLOSE_LOOP 1 #define IDLE_STATE 2 #define CLOSE_SWITCH 3 #define OPENLOOPTIMEINSEC 8.0 typedef 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 angle popen->speed_loop_enable = FALSE; popen->current_loop_enable = FALSE; #else popen->speed_loop_enable = TRUE; popen->current_loop_enable = TRUE; #endif popen->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.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.q mc_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 point FOCVars.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）
目前代码还有优化空间：实现正反转（反转先降速切开环，反向开环拖动，最后切闭环）