滑膜观测器代码实现

        滑膜观测器是无传感器控制策略中常用的一种方法，当存在未知信号和不确定性时，SMO 的稳健性极高，鲁棒性强，但也存在对参数不敏感，在实际应用中调参数对电机的控制用处不大，

pmsm数字化模型：

其中增益F和G是一个定值，用万用表可以测量出电阻R和电感L的值，然后把频率带入即可。

滑膜观测器：
位置和速度估算器是基于电流观测器而构建的。 该观测器是电机的一个数字化模型，如公式1 表征。其中的变量和常量包括：
• 电机相电流（is）
• 输入电压（vs）
• 反电动势（es）
• 绕组电阻（R）
• 绕组电感（L）
• 控制周期（Ts）
• 输出校正因子电压（z）

理论推导可以参考知乎的这篇文章https://zhuanlan.zhihu.com/p/416224632，本博客仅仅讲述如何实现滑膜控制。滑动模式控制器（Slide Mode Controller， SMC）在程序中定义了一个限制值 MaxSMCError 。也就是上述文章中的h，

也就是说如果 等式右边部分就是简化的版的滑膜控制的输出Z, 满足了小于和，即在程序中为满足小于最大误差即代表滑膜观测器处于收敛状态， 观测电流就会收敛到实际电流（不论观测电流一开始离实际电流多远），最终可以达到并保持在滑模面上。对于线性范围外的误差值，滑动模式控制器的输出为（+Kslide）或（-Kslide），取决于误差的符号。

如图所示，Z大于0，即误差大于零，说明观测器估计值比实际值大，Z取正值，减小观测器下一次计算值，反之就增大下一次计算值，最终会使Z<MaxSMCError,使SMC处于线性区域，这也是滑模控制抖震的原因。

typedef struct {
    SFRAC16 Valpha;
    SFRAC16 Ealpha;
    SFRAC16 EalphaFinal;   // 滤波后的反电动势，用于角度计算
    SFRAC16 Zalpha;      	// 输出：静止坐标系滑膜控制
    SFRAC16 Gsmopos;    	// 滑膜控制增益 Gsmopos = Ts/L
    SFRAC16 EstIalpha;   	// 变量：观测器的alpha轴电流 
    SFRAC16 Fsmopos;    	// Parameter: Motor dependent plant matrix Fsmopos = 1-Ts*R/L
    SFRAC16  Vbeta;   		// β输入电压 
    SFRAC16  Ebeta;  		// β反电动势
	SFRAC16  EbetaFinal;	// 滤波后的β反电动势
    SFRAC16  Zbeta;      	// 输出：滑膜控制器的输出 
    SFRAC16  EstIbeta;    	// 变量：观测器的alpha轴电流 
    SFRAC16  Ialpha;  		 
    SFRAC16  IalphaError; 	                 
    SFRAC16  Kslide;     	 
    SFRAC16  MaxSMCError;  	
    SFRAC16  Ibeta;  		 
    SFRAC16  IbetaError;  	                 
    SFRAC16  Kslf;       	 //滤波器增益
    SFRAC16  KslfFinal;    	// 参数：用于角度计算的BEMF滤波器
    SFRAC16  FiltOmCoef;   	// 参数：Omega滤波计算的滤波器系数
	SFRAC16  ThetaOffset;	// Output: Offset used to compensate rotor angle
    SFRAC16  Theta;			// Output: Compensated rotor angle 
	SFRAC16  Omega;     	// Output: Rotor speed
	SFRAC16  OmegaFltred;  	// Output: Filtered Rotor speed for speed PI
	} SMC;
#define Q15(Float_Value)	\
        ((Float_Value < 0.0) ? (SFRAC16)(32768 * (Float_Value) - 0.5) \
        : (SFRAC16)(32767 * (Float_Value) + 0.5))
#define _PI 3.1416
 #define SMCGAIN			0.85		
 #define MAXLINEARSMC       0.01

void SMC_Position_Estimation (SMC *s)
{
    PUSHCORCON();
	CORCONbits.SATA = 1;
	CORCONbits.SATB = 1;
	CORCONbits.ACCSAT = 1;

s->EstIalpha = s->Gsmopos * s->Valpha - s->Gsmopos * s->Ealpha\
				 - s->Gsmopos * s->Zalpha + s->Fsmopos * s->EstIalpha//计算Is
s->IalphaError = s->EstIalpha - s->Ialpha;

if (_Q15abs(s->IalphaError) < s->MaxSMCError)
	{
		 s->Zalpha = (s->Kslide * s->IalphaError)/s->MaxSMCError;
	}
	else if (s->IalphaError > 0)
		s->Zalpha = s->Kslide;
	else
		s->Zalpha = -s->Kslide;

	if (_Q15abs(s->IbetaError) < s->MaxSMCError)
	{
		 s->Zbeta = (s->Kslide * s->IbetaError)/s->MaxSMCError
		
	}
	else if (s->IbetaError > 0)
		s->Zbeta = s->Kslide;
	else
		s->Zbeta = -s->Kslide;

	// Sliding control filter -> back EMF calculator
	 s->Ealpha = s->Ealpha + s->Kslf * s->Zalpha -
							   s->Kslf * s->Ealpha
	 s->Ebeta = s->Ebeta + s->Kslf * s->Zbeta -
							 s->Kslf * s->Ebeta
	 s->EalphaFinal = s->EalphaFinal + s->KslfFinal * s->Ealpha
									   - s->KslfFinal * s->EalphaFinal;
	 s->EbetaFinal = s->EbetaFinal + s->KslfFinal * s->Ebeta
									 - s->KslfFinal * s->EbetaFinal;

	// Rotor angle calculator -> Theta = atan(-EalphaFinal,EbetaFinal)
    s->Theta = atan2CORDIC(-s->EalphaFinal,s->EbetaFinal);//Microchip内置求反正切函数

	AccumTheta += s->Theta - PrevTheta;
	PrevTheta = s->Theta;
	
	AccumThetaCnt++;
	if (AccumThetaCnt == IRP_PERCALC)
	{
		s->Omega = AccumTheta;
		AccumThetaCnt = 0;
		AccumTheta = 0;
	}
    s->Kslf = s->KslfFinal = FracMpy(s->OmegaFltred,Q15(_PI / IRP_PERCALC));
    if (s->Kslf < Q15(OMEGA0 * _PI / IRP_PERCALC))
	{
		s->Kslf = Q15(OMEGA0 * _PI / IRP_PERCALC);
		s->KslfFinal = Q15(OMEGA0 * _PI / IRP_PERCALC);
	}
	s->ThetaOffset = CONSTANT_PHASE_SHIFT;
	s->Theta = s->Theta + s->ThetaOffset;

	POPCORCON();

	return;
}
void SMCInit(SMC *s)
{
    
	s->Kslide = Q15(SMCGAIN);
	s->MaxSMCError = Q15(MAXLINEARSMC);
	return;
}

滑动模式控制器或称SMC 用来对数字化电机模型进行补偿。 SMC包含一个求和结点，用于计算电机上的测量电流与数字化电机模型上的估算电流之差的符号。 计算出的差值符号（+1 或-1）乘以SMC 增益（K）。 SMC控制器的输出就是校正因子（Z）。 该增益被加到数字化模型的电压项，在每一个控制周期中都重复执行该过程直到测量电流（is）和估算电流（is*）的差值为零（即，直到测量电流与估算电流相同为止）。

由于在计算theta 期间应用了滤波函数，所以在使用计算得到的角度给电机绕组通电之前需要对相位进行补偿。 theta 补偿量取决于theta 的变化速率或电机的速度。 theta 补偿由以下两步组成：
1. 通过未补偿的theta 来计算电机的速度。
2. 对计算得到的速度进行过滤，并计算补偿量。
通过将m 次采样得到的每相邻两个theta 值的差进行累加，然后与一个常量值相乘，即可得到速度值。