计算公式 - foc控制算法及原理详解

　　FOC 计算公式

　　1. PMSM.C 中的ADC1Interrupt（）AD中断处理函数中调MeasCurr.S 中MeasCompCurr：

　　读入ACD1，ACD2（带符号小数）

　　校正：ADC1*CarrADC1-offset=qIa ADC2*CarrADC2-offset=qIb

　　2. PMSM.C 中的ADC1Interrupt（）中调CalculateParkAngle（）对相角θ处理

　　先调用smcpos.c 滑模观测器中SMC_Position_Estimation（），之中

　　①又调用smc.s 中的CalcEstI 计算估算电流I ：

　　EstIalpha=Gsmopos*（Valpha-Ealpha-Zalpha）+Fsmopos*EstIalpha

　　---- Isα（n+1）=G*（Vα（n）-Eα（n）-Zα（n））+F*αIs α（n）

　　EstI beta=Gsmopos*（V beta-E beta-Z beta）+Fsmopos*EstI beta

　　---- Isβ（n+1）=G*（Vβ（n）-Eβ（n）-Zβ（n））+F*Isβ（n）

　　②调smc.s 中CalcIError 计算实测电流与估算电流的偏差：

　　IalphaError = EstIalpha – Ialpha

　　Ibeta Error = EstIbeta – Ibeta

　　---- ΔI α= Is α- I α ΔI β= Is β- I β

　　③调smc.s 中CalcZalpha ，CalcZbeta 计算Z α， Zβ：

　　当abs（IalphaError）《 MaxSMCError时 Zalpha = （Kslide * IalphaError） / MaxSMCError

　　当abs（IalphaError）≥ MaxSMCError时

　　Zalpha 为正时Zalpha= Kslide 为负时Zalpha=－Kslide

　　当abs（I betaError）《 MaxSMCError时

　　Zbeta = （Kslide * IbetaError） / MaxSMCError

　　当abs（I beta Error）≥ MaxSMCError时

　　Z beta 为正时Z beta = Kslide 为负时Z beta=－Kslide

　　---- 当|ΔI α|《Δmax 时：Z α= Ks*ΔI α/Δmax

　　当|ΔI α|≥Δmax 时：｜Z α｜= Ks

　　当|ΔI β|《Δmax 时：Z β= Ks*ΔI β/Δmax

　　当|ΔI β|≥Δmax 时：｜Z β｜= Ks

　　④调smc.s 中CalcBEM 按低通滤波器公式计算反电动势： Ealpha = Ealpha + Kslf * （Zalpha - Ealpha）

　　Ebeta = Ebeta + Kslf * （Zbeta – Ebeta）

　　两级滤波，后级滤波系数为KslfFinal

　　---- Eα（n）= Eα（n-1）+Ksf*（Zα（n-1）- Eα（n-1））

　　E β（n）= Eβ（n-1）+Ksf* （Zβ（n-1）- Eβ（n-1））

　　⑤调atan2CORDIC.s 中反正切函数atan2CORDIC 计算相角

　　Theta = atan2CORDIC（-EalphaFinal， EbetaFinal）

　　---- θ=atan（-Eα/ Eβ）

　　⑥在smcpos.c 中的SMC_Position_Estimation函数中进行速度预算

　　PrevTheta = s-》Theta

　　AccumThetaCnt++

　　if （AccumThetaCnt == IRP_PERCALC）

　　Omega = AccumTheta

　　---- 求累加角Σθ+=θ（n）-θ（n-1）

　　更新θ（n-1）= θ（n） 累加次数m（θCnt）+=1

　　当m = M （速度计算需要的PWM 定时次数） 时，速度预算值ω=Σθ

　　此前仅算出ω（Omega），未算 V（速度Speed） ，即未找到V=Kv*ω

　　Kv=60/T（测算速度时间）/磁极对数

　　⑦调smc.s 中CalcOmegaFltred 对ω进行数字滤波

　　OmegaFltred=OmegaFltred+FiltOmCoef*（Omega-OmegaFltred）

　　---- ωfl （n）= ωfl （n-1）+ Kωfl *［ω（n）- ωfl （n-1）］

　　Kωfl：滤波系数=2πFc/Fpwm Fpwm：脉宽调制频率

　　Fc： 滤波器截止频率（电机最低转速*磁极对数/60）

　　⑧在smcpos.c 中的SMC_Position_Estimation函数中计算滤波器系数 ？？

　　Kslf = OMEGA0 * _PI / IRP_PERCALC;

　　KslfFinal = OMEGA0 * _PI / IRP_PERCALC

　　⑨在smcpos.c 中的SMC_Position_Estimation函数中进行相位补偿计算

　　ThetaOffset=OmegaFltred*（SLOPEFRACn+SLOPEINTn）+CONSTANTn

　　不同的速度有不同的补偿，程序分几个频段， 式中3常数取不同值（n 不同）。

　　---- θΔ =ωfl *（Kscn+Kstn）+ Kcn

　　3. PMSM.C 中的ADC1Interrupt（）中调clrkepark.s 的ClarkePark 进行座标变化

　　Ialpha = Ia

　　Ibeta = Ia*dOneBySq3 + 2*Ib*dOneBySq3;

　　Id = Ialpha*cos（Angle） + Ibeta*sin（Angle）

　　Iq = -Ialpha*sin（Angle） + Ibeta*cos（Angle）

　　─ ---- Iα= Ia Iβ=（Ia+2Ib）/√3

　　Id = Iα*Cosθ+Iβ*Sinθ Iq = -Iα*Sinθ+Iβ*Cosθ

　　4. PMSM.C 中的ADC1Interrupt（）中调DoControl（）进行Id，Iq，Speed 的PI 迭代

　　5. 调trig.s 中的SinCos 计算qSin θ q Cosθ值

　　6. 调InvPark.s 中的InvPark 计算 qVα qVβ值

　　7. 调CalcRef.s 中的CalcRefVec 由qV α qVβ计算Vr 1 Vr2 Vr3 值

　　8. 调SVGEN.s 中的CalcSVGen 由Vr 1 Vr2 Vr3计算3相PWM 占空比。

　　强制角ParkParm.qAngle （输出PWM 角）和滑模估算角Smc1.Theta 电机启动时只用强制角线性递增（称斜坡法） Startup_Lock += 1 达到转速后计算强制角和估算角偏差，对强制角进行修正

　　计算保存Δθ：Theta_error = ParkParm.qAngle - smc1.Theta 修正量：对强制角修正：ParkParm.qAngle += Startup_Ramp

　　Δθ每次±0.05°直到≤0.05°：Theta_error += _0_05DEG

　　FOC（或矢量控制）算法的简要步骤

　　以下总结了控制PMSM的FOC算法步骤。

　　1.先测量三相定子电流ia和ib。根据ia+ib+ic=0，计算来自两个电流传感器的电流。

　　2.将三相电流转换到2轴坐标系中。该转换根据测量的ia、ib和ic值得到变量iα和iβ。从定子的角度来说，iα和iβ是时变正交电流值。该步骤称为Park变换。

　　3.采用控制循环上一次迭代时计算的变换角旋转2轴坐标系，使之与转子磁通对齐。该转换根据iα和iβ得到变量id和iq。现在，将正交电流id和iq变换到旋转坐标系中。在稳态条件下，id和iq将保持恒定。该步骤称为Clarke变换。