代码

SMO.c

  1. #include "Sensorless_SMO.h"
  2. #include "IQ_math.h"
  3. #define PI 3.14159265358979
  5. extern  Angle_SMO   Angle_SMOPare ;
  6. extern  Speed_est   Speed_estPare ;
  7. extern  SMO_Motor   SMO_MotorPare ;
  8. extern  IQAtan     IQAtan_Pare; 
  10. //这段代码实现了角度估计的计算过程，其中包括滑模位置观测器的更新、电流误差的计算、滑动控制量的计算和反电势的估计
  11. void  Angle_Cale(p_Angle_SMO  pV)
  12. {
  13.     /*    Sliding mode current observer    */
  14.     pV->EstIalpha = _IQmpy(pV->Fsmopos,pV->EstIalpha) + _IQmpy(pV->Gsmopos,(pV->Valpha-pV->Ealpha-pV->Zalpha));
  15.     pV->EstIbeta  = _IQmpy(pV->Fsmopos,pV->EstIbeta)  + _IQmpy(pV->Gsmopos,(pV->Vbeta -pV->Ebeta -pV->Zbeta ));
  17.     /*    Current errors    */
  18.     pV->IalphaError = pV->EstIalpha - pV->Ialpha;
  19.     pV->IbetaError  = pV->EstIbeta  - pV->Ibeta;
  21.     /*  Sliding control calculator    */
  22.     /* pV->Zalpha=pV->IalphaError*pV->Kslide/pV->E0) where E0=0.5 here*/
  23.      pV->Zalpha = _IQmpy(IQsat(pV->IalphaError,pV->E0,-pV->E0),_IQmpy2(pV->Kslide));
  24.      pV->Zbeta  = _IQmpy(IQsat(pV->IbetaError ,pV->E0,-pV->E0),_IQmpy2(pV->Kslide));
  26.     /*    Sliding control filter -> back EMF calculator    */
  27.     pV->Ealpha = pV->Ealpha + _IQmpy(pV->Kslf,(pV->Zalpha-pV->Ealpha));
  28.     pV->Ebeta  = pV->Ebeta  + _IQmpy(pV->Kslf,(pV->Zbeta -pV->Ebeta));
  30. }
  32. void  SMO_Pare_init(void )  // 电机参数初始化
  33. {
  34.     SMO_MotorPare.Rs = 0.821;    
  35.     SMO_MotorPare.Ls = 0.00758; 
  36.     SMO_MotorPare.Ib = 6 ;   
  37.     SMO_MotorPare.Vb = 14 ;
  38.     SMO_MotorPare.Ts = 0.00008;
  39.     SMO_MotorPare.POLES=4;
  40.     SMO_MotorPare.BASE_FREQ=135;  
  41.     SMO_MotorPare.Fsmopos = exp((-SMO_MotorPare.Rs/SMO_MotorPare.Ls)*(SMO_MotorPare.Ts));
  42.     SMO_MotorPare.Gsmopos = (SMO_MotorPare.Vb/SMO_MotorPare.Ib)*(1/SMO_MotorPare.Rs)*(1-SMO_MotorPare.Fsmopos);
  44.     Angle_SMOPare.Fsmopos = (int32_t)( SMO_MotorPare.Fsmopos*32768);
  45.     Angle_SMOPare.Gsmopos = (int32_t)( SMO_MotorPare.Gsmopos*32768);
  46.     Angle_SMOPare.Kslide = 4500 ;  //     
  47.     Angle_SMOPare.Kslf =1000;      //      
  48.     Angle_SMOPare.E0= 32670 ;      //    
  49.     Speed_estPare.SpeedK1=355;
  50.     Speed_estPare.SpeedK2=669;
  51.     Speed_estPare.speed_coeff=(500*60)/(SMO_MotorPare.POLES ); // 2毫秒计算一次角度差值 1000/2ms=500   =7500    
  52. }
  54.  // 电机在2ms时间计算角度变化量。即是公式：
  55.  // Speed_estPare.Speed_ele_angleIQ =Speed_estPare.ele_angleIQ -Speed_estPare.old_ele_angleIQ  
  56.  // 防止超过65535和小于0，把差值一阶滤波，插值变化量乘系数就可以得到速度。
  57.  // 速度信号的计算可以简单根据转位置的步进角计算或者直接根据角度在一定周期内的变化量计算
  58.  // 其中移位16是把角度变化量归1的一个系数，变化角度/360度，在乘一个系数得到速度，
  59.  // 可以通过示波器测量一个霍尔周期时间来计算。
  60.  // 假设一个霍尔周期时间15ms，电机极对数为4，速度RPM=1/T*60/p=1000/15*60/4=1000rpm 
  61.  // 然后看在线看角度变化量（或者通讯发出来），速度RPM=变化量/65535*K=1000，求得系数K。 
  62.  // 系数 :Speed_estPare.speed_coeff
  64. void SMO_Speedcale(void)  // 2ms执行一次
  65. {
  66.    Speed_estPare.ele_angleIQ= IQAtan_Pare.IQAngle; 
  67.       Speed_estPare.Speed_ele_angleIQ =Speed_estPare.ele_angleIQ- Speed_estPare.old_ele_angleIQ ;
  69.     //如果速度估计值为负数，则加上一个周期的角度（65536）来获得正数表示的速度值
  70.    if( Speed_estPare.Speed_ele_angleIQ <0)
  71.    Speed_estPare.Speed_ele_angleIQ+=65536;     
  73.    //通过速度滤波器计算平滑的速度估计值
  74.      Speed_estPare.Speed_ele_angleIQFitter= _IQ10mpy(Speed_estPare.SpeedK2, Speed_estPare.Speed_ele_angleIQFitter)+_IQ10mpy(Speed_estPare.SpeedK1,  Speed_estPare.Speed_ele_angleIQ);
  76.    //转换成电机的旋转速度值Speed_RPM 这个是系数角度计算系数speed_coeff
  77.    Speed_estPare.Speed_RPM = (Speed_estPare.Speed_ele_angleIQ*Speed_estPare.speed_coeff)>>16; // 最大角度 2pi是一圈 65536
  79.    //对转速值进行限制。如果转速值超过3000 RPM，则将转速值设为0
  80.      if( Speed_estPare.Speed_RPM>=3000)
  81.       Speed_estPare.Speed_RPM=0;
  82.      Speed_estPare.old_ele_angleIQ = Speed_estPare.ele_angleIQ ;
  83. }

SMO.h

  1. #ifndef  Sensorless_SMO_H
  2. #define  Sensorless_SMO_H
  4. #include "IQ_math.h"
  5. #include "stm32f10x.h" 
  6. #include "math.h"
  9. typedef struct {
  10.          int32_t  Valpha;       //二相静止坐标系alpha-轴电压    
  11.          int32_t  Ealpha;       //二相静止坐标系alpha-轴反电动势
  12.          int32_t  Zalpha;       //alfa轴滑膜观测器的z平面
  13.          int32_t  Gsmopos;      //滑膜常数1
  14.          int32_t  EstIalpha;    //滑膜估算alpha-轴电流
  15.          int32_t  Fsmopos;      //滑膜常数2
  16.          int32_t  Vbeta;        //二相静止坐标系beta-轴电压     
  17.          int32_t  Ebeta;          //二相静止坐标系beta-轴反电动势
  18.          int32_t  Zbeta;        //beta轴滑膜观测器的z平面     
  19.          int32_t  EstIbeta;     //滑膜估算beta-轴电流 
  20.          int32_t  Ialpha;          //二相静止坐标系alpha-轴电流
  21.          int32_t  IalphaError;  //二相静止坐标系beta-轴电流误差
  22.          int32_t  Kslide;       //滤波器系数     
  23.          int32_t  Ibeta;          //二相静止坐标系beta-轴电流 
  24.          int32_t  IbetaError;   //二相静止坐标系beta-轴电流误差
  25.          int32_t  Kslf;         //滤波器系数
  26.          int32_t  E0;              //滑膜的电流误差的限幅值 0.5
  27.        } Angle_SMO, *p_Angle_SMO;
  29. #define Angle_SMO_DEFAULTS {0,0,0,0,0,0,0,0,0,0,0,0} // 初始化参数
  31. typedef struct {
  32.         int32_t  Speed_ele_angleIQ;  //速度电角度值（计算速度）  
  33.         int32_t  old_ele_angleIQ;   // 电机历史电角度
  34.         int32_t  ele_angleIQ;      // 电机电角度
  35.         int32_t  Speed_ele_angleIQFitter;  //速度电角度值（计算速度）         
  36.         uint16_t Speed_RPM;            //电机旋转速度      
  37.         uint32_t speed_coeff;       //计算速度的系数
  38.         uint16_t SpeedK1;           // 速度滤波系数K1
  39.         uint16_t SpeedK2;         // 速度滤波系数K2
  40.        }Speed_est;
  42. #define Speed_est_DEFAULTS {0,0,0,0,0,0,0,0} // 初始化参数
  44. typedef struct{
  45.     float  Rs;             //电机的相电阻     
  46.     float  Ls;            //电机的相电感      
  47.     float  Ib;             //电机控制器的基本相电流       
  48.     float  Vb;            //电机控制器的基本相电压     
  49.     float  Ts;             //采样周期     
  50.     uint32_t   POLES; // 电机的极对数
  51.     uint32_t   BASE_FREQ; // 电机控制器的基本频率
  52.     float  Fsmopos;           //滑膜常数1
  53.     float  Gsmopos;             //滑膜常数2
  54.   }SMO_Motor;
  56. #define SMO_Motor_DEFAULTS {0.0,0.0,0.0,0.0,0.0,0,0,0.0,0.0} // 初始化参数
  58. void  Angle_Cale(p_Angle_SMO  pV); //滑膜电机位置电角度计算
  59. void  SMO_Pare_init (void );   // 滑膜观测器的参数初始化
  60. void SMO_Speedcale(void) ;    //  滑膜的角度计算速度函数
  61. #endif /* Sensorless_SMO*/