欢迎光临散文网 会员登陆 & 注册

如何在 MCSDK 5.x 中增加绝对位置编码器

2019-10-28 12:27 作者:电堂科技  | 我要投稿

在“AI电堂”微信公众号回复“SPI”,获得示例代码下载链接


当前MC SDK5.x中支持的关于编码器(Encoder)为增量编码器,即有A,B,Z信号的编码器,主要为光编码器;在市场中有些应用(比如无人机云台,伺服,医疗)已经在广泛使用磁编码器,输出的是绝对位置,而想取得位置信息,或者通过取样绝对位置传感的PWM脉宽,或者需要通过通讯方式进行获取,比如SPI,I2C,UART等等;当前MC SDK5.x还未对这个部分直接支持,需要通过修改程序实现绝对位置编码器的应用,本文使用STM32F303RE-Nucleo控制说明。


绝对位置编码器使用简介


当导体通电后放入磁场中,由于磁场的作用导体上流动的电荷就会因为受到由磁场感应产生的洛伦兹力而发生流通路径的偏移,这样在垂直于电流方向上如果去测量的Y1,Y2点电压,将产生电势差,这个就是所说的霍尔效应;如果使用ADC对Y1,Y2点进行测量标定即可以反推磁场变化;


打开有绝对位置编码器的电机可以看到转子上有一个同步旋转的磁石,同时有检测芯片用于绝对角度输出,绝对位置编码器芯片输出PWM波或者内部做通讯输出,单片机可以读取相关信息得到到当前转子的角度。


1. 准备工作


为了方便调试,首先使用无传感控制(Sensor-less)将电机闭环运行起来,如果电机合适,并且使用的是ST的电机Demo板,可以使用ST的Motor Profiler(30KHzPWM)进行无传感电机参数以及转动参数识别,可以很快将电机无传感转动起来;


如果使用客户自制板子,则按照ST的操作说明,使用MCWorkbench将电机闭环无传感转动起来,电机闭环转动过程可以确保硬件软件的完备性;注意这边生成的CubeMx工程文件*.ioc文件,后面我们添加程序会使用到;


为了便于调试可增加两路DAC,比如使用STM32F303则直接使用两路输出的DAC,如果该芯片上没有DAC模块,可以使用TIMER+RC滤波电路模拟输出DAC;下图为使用DAC配置,需要配置为Userdefined DAC1/2。


绝对编码器芯片角度信息读取


绝对编码器的输出可以为PWM波形输出,也可以为通讯端口输出,比如UART,SPI,I2C等等,本文以SPI输出以及PWM输出为例做说明


2. 角度以及速度标定

2.1 电角度标定


因为我们读取的角度为机械角度,在控制过程中我们用到的是电角度,因此还要做一下变换,将机械角度数据变为电角度数 据;使用公式为:

在程序中需要量化为Q16格式后得到电角度范围[−180°~180°];即[-32768~32767]

2.2 速度计算

3. 编码器SPI输出模式

3.1 CubeMx配置


打开MCWorkbench生成的*.ioc文件,配置SPI模块,这边我们使用SPI3接收,配置参数需要参考编码器芯片的说明,这 边以AS5048A为例,如下配置,配置PB10作为片选信号CS输出



3.2 重新生成代码



3.3 编写SPI读取代码


根据编码器芯片的数据手册编写SPI读取代码,调试代码以便读取到角度信息,因为是外设配置,本文对此部分不做具体说明;可以参考spi_pwm_encoder.c文件,得到角度信息后进行机械角度转变为电角度的计算;

#define ENCODER_SPI_MAX 16384 

#define POLES 11 

/* 

* Calculate electrical angle based on motor, data in Q16 format 

* 16384 stand for 360degree(65536)*motor poles 

*/ 

SPI_EIAngle = (int16_t)((65536*SPI_Angle_Digital*POLES)/ENCODER_SPI_MAX)%65536;


SPI_Angle_Digital为通过SPI读取的电角度数字量,ENCODER_SPI_MAX为最大数字量输出,比如14-bit精度的输出,这个数据为16384。


3.4 创建绝对值编码器的结构体


创建结构体,包含SpeednPosFdbk_Handle_t的通用组件,并且定义私有成员变量,用于绝对位置编码器的控制以及参量计算,在abs_encoder_pos_fdbk.c中进行初始化动作。

/**  

 * @brief Abs encoder component parameters definition  

 */  

 typedef struct


{       

      SpeednPosFdbk_Handle_t _Super;      

      int16_t Encoder_EIAngle; /* Encoder final electrical angle */              int16_t Encoder_MecAngle; /* Encoder final mechanical angle */              int16_t Encoder_AngleD_Pre; /* Encoder previous digital angle */            int16_t Encoder_AngleD_Now; /* Encoder present/now digital angle */          int16_t Encoder_Speed_RPM; /* Speed uint in RPM */       

      int16_t Encoder_Average_Speed_RPM; /* Average speed unit in RPM*/           bool SensorIsReliable;      

      uint8_t mode;             

      int32_t Circle_Counter; /* Count the circle for motro run */              bool Middle_Flag; /* Middle flag, 1--arrived pass middle*/       

} Abs_Encoder_Handle_t;


3.5 绝对值编码器函数编写


参照encoder_speed_pos_fdbk.c文件编写对应的绝对值编码器的代码,接口函数对应起来,这样可以有效的集成扩展, 分别定义:

1) 初始化函数Abs_Encoder_Init

2) 清除函数Abs_Encoder_Clear

3) 绝对值编码器读取电角度函数Abs_Encoder_GetElAngle

4) 读取机械角度函数Abs_Encoder_GetMecAngle

5) 得到电机平均机械速度函数 Abs_Encoder_GetAvrgMecSpeed01Hz

6) 计算电机平均机械速度函数 Abs_Encoder_CalcAvrgMecSpeed01Hz


3.6 添加绝对位置编码器代码

3.6.1 绝对位置编码器速度计算


参照2.2公式进行速度计算

/* Speed_RPM = Deta(angle)*60*f/65536 */ 

pHandle->Encoder_Speed_RPM = (int16_t)((int32_t)(pHandle->Encoder_AngleD_Now - pHandle- >Encoder_AngleD_Pre * 60 * SPEED_LOOP_FREQUENCY_HZ/65536);


3.6.2 添加编码器初始化代码


在mc_task.c文件的MCboot()函数中增加初始化代码

Abs_Encoder_Init(&Abs_Encoder_M1); // Initial absolute encoder sensor Abs_Encoder_M1.mode = SENSOR_SPI;


3.6.3 角度以及速度的代码添加


中频任务TSK_MediumFrequencyTaskM1中加入绝对编码器的角度以及速度计算函数

Abs_Encoder_CalcAvrgMecSpeed01Hz(&Abs_Encoder_M1, &wAux );


3.7 判断绝对位置编码器角度准确性


使用DAC对角度进行判读,在mc_task.c中TSK_HighFrequencyTask()函数中增加UserDAC数据更新,一个DAC输出无 传感观测器输出的电角度,另外一路输出绝对值编码器计算出的电角度。

/* DAC output*/ 

extern UI_Handle_t * pDAC; 

static int16_t temp1; 

static int16_t temp2; 

temp1 = FOCVars[M1].hElAngle; 

temp2 = Abs_Encoder_M1.Encoder_EIAngle; 

DAC_SetUserChannelValue(pDAC, DAC_USER1, temp1); 

DAC_SetUserChannelValue(pDAC, DAC_USER2, temp2);

程序修改完成后编译下载,让电机工作在无传感闭环模式,示波器连接DAC两路输出。

如果出现下面波形,则需要交换其中任意两条电机线,或者在程序中加入负号即可;


下面的图为正确的DAC输出波形,可以看到黄色为无传感器观测器输出电角度,蓝色为计算得到的绝对位置编码器输出计算结果,两者基本重合,如果出现吻合度非常差,波形错开很多情况下,有两种措施,要么和电机厂商沟通做准确的零点校准, 要么自行增加角度补偿。



3.8 绝对位置角度闭环程序修改

3.8.1 修改电角度赋值


修改mc_task.c中TSK_MediumFrequencyTaskM1()函数,由无传感方式获得电角度改为绝对位置编码器的转换完成的电角度,并且返回对应的速度值;

#if defined(ABS_ENCODER_MODE)                         Abs_Encoder_CalcAvrgMecSpeed01Hz(&Abs_Encoder_M1, &wAux );      IsSpeedReliable = 1;

#else      

 Abs_Encoder_CalcAvrgMecSpeed01Hz(&Abs_Encoder_M1, &wAux );      IsSpeedReliable = STO_PLL_CalcAvrgMecSpeedUnit( &STO_PLL_M1, &wAux ); 

#endif


3.8.2 屏蔽无传感电角度计算


修改 mc_task.c 中 TSK_HighFrequencyTask() 函数,只保留MC_FOC_DURATION这个判断,其他都可以屏蔽掉。

hFOCreturn = FOC_CurrController(M1); 

if(hFOCreturn == MC_FOC_DURATION) 

{   

   STM_FaultProcessing(&STM[M1], MC_FOC_DURATION, 0); 

}


3.8.3 修改速度传感器为绝对编码传感器


修改 mc_task.c 中 TSK_MediumFrequencyTaskM1()函数,传感器设定修改为绝对编码器。

case CLEAR:  

  FOCVars[M1].bDriveInput = INTERNAL;  

  STC_SetSpeedSensor( pSTC[M1], &Abs_Encoder_M1._Super );  

  if ( STM_NextState( &STM[M1], START ) == true )  

  {    

   FOC_Clear( M1 );    

   R3_2_SwitchOnPWM( pwmcHandle[M1] );   

   }  

   break;


3.8.4 修改状态跳转


因为开始就直接是闭环运行,因此需要修改mc_task.c中TSK_MediumFrequencyTaskM1()函数,状态START下,只需要一条跳转语句,其他删除或者屏蔽掉。

case START:     

   STM_NextState( &STM[M1], START_RUN );

break ;

同样在状态START_RUN下,也只需要一条跳转语句。

case START_RUN:    

   STM_NextState( &STM[M1], RUN ); 

break ;


3.9 绝对编码器力


以上修改完成后,重新编译下载后就可以直接运行电机了,电机需要工作在力矩模式,如果使用GUI进行控制,则切换到Torque模式,同时设定Iqref数据,500这个数据计算来自于电机最大支持电流数字量(比如5000)的10%,可以逐渐增加这个数据,直到电机能够运转起来。



如果程序控制电机运行则可以写为下面代码

MC_ProgramTorqueRampMotor1(500,1000); 

MC_StartMotor1();

3.10 绝对编码器速度闭环运行电机


速度输出已经在上面的中频任务中计算得到,可以在无传感模式下对编码器速度输出做调试,看输出速度是否和无传感匹配,如果匹配后可以转入绝对编码器的速度闭环控制


如果程序控制电机运行则可以写为下面代码:

MC_ProgramSpeedRampMotor1 (600/6,1000); // 600RPM 

MC_StartMotor1();


3.11 使用绝对编码器做位置环

3.11.1 框架说明


以表贴电机为例,一般的FOC电机控制是两环控制,速度环+电流环的控制方式,速度环为外环,电流环为内环,Idref = 0(d轴电流参考为0)的控制为常见控制;参考速度环输出参考转矩后供后端电流环路;


如果增加位置环最简单和直接的方式即为将速度环换为位置环,即变为位置环+电流环的方式:


3.11.2 计算说明


为了方便控制,以及考虑精度问题,角度单位为rad*10000,比如电机转动10圈,也就是10圈后的位置,则该位置的设定角度为:

           θset=10∗2𝜋∗10000≈628320

位置环采用的是线性系数乘以系数直接输出到速度的参考:

          𝜔re𝑓=𝐾𝑝∗(𝑃𝑠−𝑃𝑐)

           𝐾𝑝−位置环系数

           𝑃𝑠 −设定的位置

           𝑃𝑐−当前的位置

当前位置的角度数字量计算如下:

          𝜃𝑐=𝐶𝑛𝑡∗2∗𝑃𝐼∗10000+𝜃𝑑

         𝐶𝑛𝑡 –绝对值编码器转过的整圈数

         𝜃𝑑−当前的机械角度的数字量

         𝑃𝐼 −取值3.1416


3.11.3 位置控制改进


在三环控制中,不可避免的会涉及到加速度的计算,正常控制过程为:加速、匀速、减速、定位过程。


那么会涉及到加速度以及定位阶段的控制,这边我们可以按照加速度与速度差成反比,而定位阶段可以直接设定速度为0,但是实际使用过程中我们会发现直接直接的速度模式在定位阶段力量有限。


方法一:PID 需要特别设定,更能够定位到给定位置;

方法二:抛开速度环,直接力矩控制;


3.11.4 增加以下变量或函数用于位置环控制


1) 位置环PID 结构体PID_Handle_t PIDAngleHandle_M1

2) 位置控制的结构体Position_Handle_t

3) 位置角度误差Position_GetErrorAngle

4) 位置环速度参考输出计算Position_CalcSpeedReferrence

5) 位置环力矩参考输出计算Position_CalcTorqueReferrence


3.11.5 增加位置控制程序


在mc_task.c的中频任务函数TSK_MediumFrequencyTaskM1中增加位置环差值数据计算,根据差值计算,当差值在阈值之上的话进行速度控制,在RUN下调用下面函数:

/* Get error position/Angle, unit in rad*10000 */ Position_GetErrorAngle(&Abs_Encoder_M1,Target_Angle); 

…… 

Position_CalcSpeedReferrence()

如果差值缩小到一定范围内,则进行力矩控制,在函数FOC_CalcCurrRef中进行调用。

__weak void FOC_CalcCurrRef(uint8_t bMotor) 

{       

      if(FOCVars[bMotor].bDriveInput == INTERNAL)       

      {       

      #if defined(POSITION_CONTROL)             

         /* If in position torque mode, Iqref come from Angle PID*/                if(Position_M1.Mode_Flag == P_TORQUE_MODE)             

         {                   

             FOCVars[bMotor].hTeref = Position_CalcTorqueReferrence();                FOCVars[bMotor].Iqdref.q = FOCVars[bMotor].hTeref;                  }             

         else       

      #endif             

         {                  

            FOCVars[bMotor].hTeref=STC_CalcTorqueReference(pSTC[bMotor]);              FOCVars[bMotor].Iqdref.q = FOCVars[bMotor].hTeref;                    }        

       } 

}


3.11.6 实际运行效果


位置偏差较大时,电机加速到最大速度,偏差小时将进入力矩控制模式,直到目标位置,在目标位置直接力矩锁定。


设定位置角度值在0<- ->15708000(电机转动250圈) 实际速度与参考速度曲线。



3.12 测试说明


spi_pwm_encoder.h中提供了测试宏定义

A,无传感运行,用于测试绝对值编码器,如下宏定义

//#define ABS_ENCODER_MODE 

//#define POSITION_CONTROL 

#define ENCODER_SPI_MODE 

//#define ENCODER_PWM_MODE


B,绝对值编码器,SPI输出模式的闭环运行

#define ABS_ENCODER_MODE 

//#define POSITION_CONTROL 

#define ENCODER_SPI_MODE 

//#define ENCODER_PWM_MODE


C,加入位置环后的三环运行

#define ABS_ENCODER_MODE 

#define POSITION_CONTROL 

 #define ENCODER_SPI_MODE 

//#define ENCODER_PWM_MODE


4. 编码器PWM输出模式


一般编码器的PWM都是以固定频率输出,占空比代表角度,Duty的范围为0—T,也就是0—360度(机械角度);



4.1 CubeMx配置


编码器的PWM输出可以直接使用ST芯片的TIMER PWM捕获功能获取PWM波的频率(用于计算电角度)和占空比(机械角度信息);这边我们使用TIM2;打开MC workbench生成的*.ioc文件,修改TIM2为PWM捕获输入:


将TIM2捕获输入通道1配置为Rising Edge,通道2为间接捕获输入(实际是TIM2_CH1的内部输出),配置为Falling Edge,同时我们看到PA15(TIM2_CH1)已经被配置;通道1为频率捕获,通道2为占空比捕获;



使能TIM2中断



编辑中断优先级


因为TIM2中断不能妨碍电机运行,因此需要修改中断优先级为抢占等级为3


4.2 重新生成代码


在生成代码的main.c中增加通道使能代码,TIM2(32-bit)工作在72MHz,如果默认编码器1K PWM,CCR1计数数值为 72MHz/1KHz = 72000

/* In PWM mode, use Tim2 PWM capture mode to get frequency and duty cycle, 

* CCR1 value is the frequency of PWM, can be used as calibration value. 

* CCR2 value is for digital angle 

* In this demo, TIM2 work under 72MHz 

*/ 

HAL_TIM_Base_Start(&htim2);

HAL_TIM_IC_Start(&htim2,TIM_CHANNEL_1); 

HAL_TIM_IC_Start_IT(&htim2,TIM_CHANNEL_2);


4.3 编写电角度转换读取代码


void PWM_Encoder_EIangle(uint32_t data) 

{      

     PWM_Angle_Digital = data;      

     if(PWM_Angle_Digital > ENCODER_PWM_MAX)            

          PWM_Angle_Digital = ENCODER_PWM_MAX;      

   PWM_EIAngle= (int16_t((65536*PWM_Angle_Digital*POLES)/ENCODER_PWM_MAX)%65536; }

在TIM2的中断服务中读取CCR2寄存器内容,为防止误动作,这边CCR1数据标定为ENCODER_PWM_MAX,可以仿真条件下读取这个数据,如果编码器准确1KHz PWM输出这个数据为72000,如果为其他数据可以进行对应修改;

#define ENCODER_PWM_MAX 67039 //72000 

void TIM2_IRQHandler(void) 

{    

    PWM_Encoder_EIangle(TIM2->CCR2);    

    Encoder_EIAngle = PWM_EIAngle;    

    Encoder_MecAngle = (int16_t)(PWM_Angle_Digital*65536/ENCODER_PWM_MAX); 

}


4.4 判断绝对位置编码器角度准确性

同3.7所描述


4.5 绝对位置角度闭环程序修改

同3.8,3.9,3.10的描述


4.6使用绝对编码器做位置环

参照 3.11 中的具体操作


如何在 MCSDK 5.x 中增加绝对位置编码器的评论 (共 条)

分享到微博请遵守国家法律