FOC控制算法滑膜观测器实现

C——Coder

已于 2025-04-11 09:38:37 修改

阅读量2.1k

点赞数 13

文章标签：算法 c语言嵌入式

于 2024-09-21 15:01:10 首次发布

本文链接：https://blog.youkuaiyun.com/javacode123456/article/details/142391339

版权

继续自己手写FOC控制算法之滑膜观测器的实现。

滑膜观测主要就是对电机参数没那么敏感，而且数学公式推导比较简单，就是基本的电机的数学哦公式加上一个滑模面函数，相较于龙伯格来说需要调试的参数很少。龙伯格我自己测试好了再发上来。

滑膜观测器的我理解的核心原理主要是构造滑膜面函数，然后根据李雅普诺夫定理来设定合适的增益实现滑模面函数的收敛。一般构造的滑膜面函数就是这个α-β坐标系下的电流。具体的电机公式及数学推导网络上有很多资料，自己去查阅。我这里直接贴代码。

这个是我自己对滑膜观测器的梳理。

滑膜观测器实现代码

结构体及宏定义

#define BEMF_VOL_FACTOR		(5.0f)		//电机反电势常数的电压
#define BEMF_FREQ_FACTOR		(50)		//电机反电势常数的电转速频率


struct SMO_Obs
{
		float spd_filter;		//滤波后的电转速
		float spd;	//电转速
		float theta_Est;	//估算的电角度
		float ialpha;		//电流α-β坐标系的α轴分量
		float ibeta;		//电流α-β坐标系的β轴分量
		float ialpha_Est;	//估算的电流α-β坐标系的α轴分量
		float ibeta_Est;	//估算的电流α-β坐标系的β轴分量
		float ialpha_Err;	//估算的和实际值误差
		float ibeta_Err;	//估算的和实际值误差
		float ualpha;	//电压α-β坐标系的α轴分量
		float ubeta;	//电压α-β坐标系的β轴分量
		float Ts;		//采样时间，FOC执行周期
		float Rs;		//电机内阻
		float Ld;		//电机D轴电感
		float Lq;		//电机Q轴电感
		float gain;	//滑膜增益
		float bemfalpha;	//反电动势α-β坐标系的α轴分量
		float bemfbeta;		//反电动势α-β坐标系的β轴分量
};

void SMO_Init(void);
void SMO_Handle(struct SMO_Obs* ptr);

函数实现，这里滑膜观测器的增益需要结合自己的工程调试，我这里为了实现原理是随便给的。

//滑膜控制器结构体
struct SMO_Obs SMO_Observer;	

//锁相环
extern struct PLL_Handle  PllCtrl;

static int8_t FuncSign(float in);

/*!
*@prototype void SMO_Init(void)
*@param[ih] none
*@return none
*@brief 滑膜观测器初始化
*/
void SMO_Init(void)
{
		SMO_Observer.spd_filter = 0.0f;
		SMO_Observer.spd = 0.0f;
		SMO_Observer.theta_Est = 0.0f;
		SMO_Observer.ialpha = 0.0f;
		SMO_Observer.ibeta = 0.0f;
		SMO_Observer.ialpha_Est = 0.0f;
		SMO_Observer.ibeta_Est = 0.0f;
		SMO_Observer.ialpha_Err = 0.0f;
		SMO_Observer.ibeta_Err = 0.0f;
		SMO_Observer.ualpha = 0.0f;
		SMO_Observer.ubeta = 0.0f;
		SMO_Observer.Ts = 0.00005f;
		SMO_Observer.Rs = 0.005;
		SMO_Observer.Ld = 0.0005f;
		SMO_Observer.Lq = 0.0005f;
		SMO_Observer.Ld = 0.0005f;
		SMO_Observer.gain = BEMF_VOL_FACTOR / (BEMF_FREQ_FACTOR * 6.28318530717958 * 1.73205 * 2) * 400.0f;
		SMO_Observer.bemfalpha = 0.0f;
		SMO_Observer.bemfbeta = 0.0f;
}	

/*!
*@protoype void SMO_Handle(struct SMO_Obs* ptr)
*@param[in] Pointer To SMO_Obs Structure
*@retrun none
*@brief 滑膜观测器控制
*/
void SMO_Handle(struct SMO_Obs* ptr)
{
		//计算Eα
		ptr->ialpha_Err = ptr->ialpha_Est - ptr->ialpha;
		ptr->bemfalpha = ptr->gain * FuncSign(ptr->ialpha_Err);
		//这里我们默认有Ld等于Lq
		ptr->ialpha_Est += ptr->Ts * (0 - (ptr->Rs / ptr->Ld) * ptr->ialpha_Est + (ptr->ualpha - ptr->bemfalpha) / ptr->Ld);
	
		//计算Eβ
		ptr->ibeta_Err = ptr->ibeta_Est - ptr->ibeta;
		ptr->bemfbeta = ptr->gain * FuncSign(ptr->ibeta_Err);
		//这里我们默认有Ld等于Lq
		ptr->ibeta_Est += ptr->Ts * (0 - (ptr->Rs / ptr->Ld) * ptr->ibeta_Est + (ptr->ubeta - ptr->bemfbeta) / ptr->Ld);
	
		//用锁相环开始提取角度
		PllCtrl.e = (0 - ptr->bemfalpha) * cosf(ptr->theta_Est) - ptr->bemfbeta * sinf(ptr->theta_Est);
			
		Pll_Calulate(&PllCtrl);
		
		ptr->spd = PllCtrl.EstWe;
		
		ptr->theta_Est += ptr->spd * ptr->Ts;
		
		
		if(ptr->theta_Est >= 6.28318530717958f )
		{
				ptr->theta_Est -= 6.28318530717958f;
		}
	
}

/*!
*@prototype static int8_t FuncSign(float in)
*@param[in] Input data
*@return none
*@brief 符号函数
*/
static int8_t FuncSign(float in)
{
		int8_t flag = 0;
		if(in > 0)
		{
				flag = 1;
		}
		else if(in < 0)
		{
				flag = -1;
		}
		else
		{
				flag = 0;
		}
		return flag;
}

测试代码，中间那个Test SVPWM函数看我的前面的文章，手写SVPWM算法

//滑膜观测器及锁相环测试
extern struct SMO_Obs SMO_Observer;	
extern struct PLL_Handle  PllCtrl;
struct DQ_Aix Idq;
struct AlphaBeta_Aix Iab;

void TestSMO_PLL(void)
{
		AntiParkTransfer(Idq ,&Iab);
		Idq.theta = dq_Aix.theta;
	
		SMO_Observer.ialpha = Iab.V_Alpha;
		SMO_Observer.ibeta = Iab.V_Beta;
		SMO_Observer.ualpha = ab_Aix.V_Alpha;
		SMO_Observer.ubeta = ab_Aix.V_Beta;
	
		SMO_Handle(&SMO_Observer);
	
}


//50us定时
if(flag == 1)
{
	//线性差值
	//Foc_HallAngle_Calc(&Foc_Hall_Ctrl , 1);
	TestSMO_PLL();
	TestSVPMW();
	flag = 0;
}

测试结果，最上面的角度是虚构的开环角度，然后假定电机的q轴电流为1A，实际滑膜观测器的输出。下面两个正弦波是滑膜观测器跟踪α-β坐标系的电流。毛刺大是因为滑膜增益选取，我实际试了改变滑膜增益是会产生影响的，这个需要和自己实际的电机来制定。

接下来就是锁相环来解析出角度，锁相环就是这个系统，这个我实话实说是网络上找的，我本来是打算直接反正切得到角度然后用一个位置式的PI系统去跟踪输出角度，但是网上找了一圈都没有这样做的，找了一圈全都是锁相环然后配这个图。

锁相环结构体及宏定义

这里说明一下，我的锁相环代码开始有问题，通过Matlab仿真，改了一下代码的逻辑。

/*
 * @Author: 可达鸭 
 * @Date: 2025-03-16 17:14:27
 * @LastEditors: 可达鸭 
 * @LastEditTime: 2025-03-22 20:47:27
 * @FilePath: \Src\Lib\Lib_PLL.h
 */


#ifndef LIB_PLL_H
#define LIB_PLL_H

#include "stm32f4xx.h"
#include "Lib_PID.h"
#include "MotorCfg.h"

struct PLL_Handle
{
		float KP;		//比例系数
		float KI;		//积分系数
		float Ts;		//FOC执行周期, s
		float WeMaxOut;	//最大电转速，rad/s
		float WeMinOut;	//最小电转速，rad/s
		
		float e;	//误差
	
		float EstWe;	//电转速
		float EstWe_Flt;	//滤波后的电转速
		float EstTheta;		//电角度
		float PreEstTheta;	
	
		float Iout;	//输出
	
};
 
void Pll_Init(void);
void Pll_Calulate(struct PLL_Handle* ptr);
float AngleErrStandlization(float ErrAngle);

#endif

/*
 * @Author: 可达鸭 
 * @Date: 2025-03-16 17:14:27
 * @LastEditors: 可达鸭 
 * @LastEditTime: 2025-03-24 21:11:57
 * @FilePath: \Src\Lib\Lib_PLL.c
 */


#include "Lib_PLL.h"
#include "Lib_Filters.h"
 
 
//锁相环结构体
struct PLL_Handle  PllCtrl;
 
struct Pid_Control AbsoltePid;

struct IIR_Filters PllFilters;

 
 
/*!
*@prototype void Pll_Init(void)
*@param[in] none
*@retruen none
*@brief 锁相环初始化
*/
void Pll_Init(void)
{
		PllCtrl.KP = PLL_KP;
		PllCtrl.KI = PLL_KI;
		PllCtrl.Ts = HW_FOC_CONTROL_PEROID / 1000000.0f;
		PllCtrl.WeMaxOut = PLL_MAX_WE;
		PllCtrl.WeMinOut = PLL_MIN_WE;
		
		PllCtrl.e = 0;
 
		PllCtrl.EstWe = 0;
		PllCtrl.EstWe_Flt = 0;
		PllCtrl.EstTheta = 0;
		PllCtrl.PreEstTheta = 0;
	
		PllCtrl.Iout = 0;

		PllFilters.factor = 0.02f;
		PllFilters.newData = 0.0f;
		PllFilters.oldData = 0.0f;
 
		//PID
		AbsoltePid.Error = 0;
		AbsoltePid.PreError = 0;
		AbsoltePid.LastError = 0;
		AbsoltePid.KP = PllCtrl.KP;
		AbsoltePid.KI = PllCtrl.KI;
		AbsoltePid.KD = 0.0f;
		AbsoltePid.I_AllErr = 0;
		AbsoltePid.P_Parten = 0;
		AbsoltePid.I_Parten = 0;
		AbsoltePid.D_Parten = 0;
		AbsoltePid.Max = MOTOR_MAXSPEED * MOTOR_POLEPAIRS;
		AbsoltePid.Min = 0 - MOTOR_MAXSPEED * MOTOR_POLEPAIRS;
		AbsoltePid.out = 0;
 
}	
 
/*!
*@prototype void Pll_Calulate(struct PLL_Handle* ptr)
*@param[in] ptr: struct PLL_Handle Pointer
*@return none
*@brief 锁相环计算
*/
void Pll_Calulate(struct PLL_Handle* ptr)
{	
	#if 0
	AbsoltePid.Error = ptr->e;
	Absolute_PID_Calculate(&AbsoltePid);
	ptr->EstWe = AbsoltePid.out;
	#else
	AbsoltePid.I_Parten += ptr->e * ptr->KI * ptr->Ts;
	AbsoltePid.P_Parten = ptr->e * ptr->KP;

	ptr->EstWe = AbsoltePid.I_Parten + AbsoltePid.P_Parten;
	if(ptr->EstWe >= ptr->WeMaxOut)
	{
		ptr->EstWe = ptr->WeMaxOut;
	}
	else if(ptr->EstWe <= ptr->WeMinOut)
	{
		ptr->EstWe = ptr->WeMinOut;
	}

	PllFilters.newData = ptr->EstWe;
	ptr->EstWe_Flt = LPF_1_Filter_1(&PllFilters);
	PllFilters.oldData = ptr->EstWe_Flt;


	#endif
		
}

测试结果，第二个那个黑色的就是锁相环提取的角度，这可以说明我们的滑膜观测器是成功完成了跟踪。