【程序】【必须】编码器电机测速，原理+代码+滤波_gmr编码器

参考资料：

1. https://blog.csdn.net/lzzzzzzm/article/details/119416134
2. 野火STM32电机开发教程

1. 编码器种类及原理

常见的编码器有两种，分别为霍尔编码器和GMR编码器。

1.1 霍尔编码器

​ 霍尔编码器圆盘上分布有磁极，当圆盘随电机主轴转动时，会输出两路相位差90°的方波，用这两路方波可测出电机的转速和转向。霍尔编码器一般是13线的，就是转一圈每项会输出13个脉冲，这个精度基本能够满足大部分使用场景的要求。

1.2 光电编码器

​ 如图，打孔码盘随电机进行旋转。每当光线穿过圆孔，输出电平就会改变，如此产生方波，测量方波的频率即可测出电机转速。

1.3 GMR编码器

​ GMR编码器利用巨磁阻效应进行测速，GMR编码器一般是500线的，转一圈每项会输出500个脉冲，精度比霍尔编码器高得多，适合对精度要求高的环境或者最求完美的人。

​ 下图来自淘宝店铺轮趣科技

2. 常用测速方法

2.1 倍频技术

​ 编码器会输出两路方波信号，如果只在通道A的上升沿计数，那就是1倍频；通道A的上升、下降沿计数，那就是2倍频；如果在通道A、B的上升、下降沿计数，那就是4倍频。

​ 使用倍频可以最大程度地利用两路信号，提高测速的灵敏度。

​ 下面说的三种测速方法只是在软件计算上的区别，硬件上是没有改变的

2.1 M法测速（周期测量法）

​ 简单地说就是根据单位时间一共有多少个脉冲来计算转速。

​ 设转速为n(r/s)；测量时间为$T_0$(s)；$T_0$时间内的脉冲数为$M_0$；电机转一圈产生的脉冲数为C；则转速计算公式为
$$n=\frac{M_0}{C{T}_0}$$
​ 当$M_0$很大，即转速快时，这个方法测得精度和平稳性都很好，但当$M_0$很小，速度改变带来的$M_0$变化很小，即转速慢时算出的误差就很大。所以M法测速适用于高转速场景

2.2 T法测速（频率测量法）

​ T法测速是这样操作的：是指先建立一个频率已知且固定的高频脉冲，当编码器读到一个信号，开始对高频脉冲进行计数，编码器第二个信号到来后，停止计数。根据对高频脉冲计数的次数、高频脉冲频率和电机转一圈编码器产生的脉冲数进行速度计算。

​ 设转速为n(r/s)；两个脉冲的时间间隔为$T_E$(s)；电机转一圈产生的脉冲数为C；$F_0$(Hz)为编码器输出脉冲的频率；$M_1$为高频脉冲的计数值，则转速计算公式为

$$n=\frac{1}{C{T}_E}=\frac{F_0}{C{M}_1}$$
理解：$C{T}_E$为当前速度下电机转一圈需要的时间，1圈除以1圈所需要的时间即可得到转速

其中$T_E$、$M_1$，$F_0$有如下关系
$$T_E=\frac{M_1}{F_0}$$
​ 当$T_E$很大即转速很慢时，T法测速有较高的精度和平稳度，但当$T_E$很小，即转速很快时，速度改变带来的$T_E$变化很小，算出的误差就很大。所以T法测速适用于低转速场景

2.3 M/T法测速

​ M/T法综合了M法和T法的优势，计算公式如下。

$$n=\frac{F_0{M}_0}{C{M}_1}$$

​ 理解：公式中只有$M_0$（$T_0$时间内的脉冲数）、$M_1$（高频脉冲的计数值）为变量。当转速快时，$M_1$变小，$M_0$变大，相当于M法；当转速慢时，$M_1$变大，$M_0$变小，相当于T法。

3. STM32实现编码器M法测速

​ 下面是使用M法测速的实例代码。

3.1 CubeMax配置

​ 为了进行测速，我们一共需要3个定时器，作用分别是：①输出PWM；②编码器模式进行脉冲计数；③计时，确定每次测速的时间间隔。

​ 其中，用于定时的定时器③可以用输出PWM的定时器①代替，输出PWM的定时器一样有更新中断，只要在更新中断里运行测速程序即可。但由于PWM定时器的频率很快，所以我们会间隔很多个更新中断后进行测速。

​ 具体配置如下：

​ TIM2：编码器输入定时器

​ 这里开启了两个通道计数，即Encoder Mode中设置为Encoder Mode TI1 and TI2。这里就是上文倍频技术的4倍频。

​ 编码器模式下的定时器其实是个计数器，在编码器的脉冲到来时，Counter会相应地加和减，正转时加，反转时减，溢出后到达另一个极端值，比如说向上计数到达20001时会变成0

​ 接下来我们需要设定编码器的两个引脚为上拉，防止误触发。

TIM3：PWM输出定时器

​ STM32F103的定时器时钟来源于APB总线时钟，最高为72MHz，我们一般也配置为72MHz。APB时钟经过PSC分频后得到实际的定时器的计数频率。定时器的计数频率为
$$f_{count}=\frac{f_{APB}}{PSC+1}$$
​ 当计数值达到ARR寄存器的设定值后计数值归零，重新开始计数，完成一个周期。在一个周期中，PWM高电平时间由比较寄存器(ARRARR)的值决定。在设置PWM mode1且向上计数时，计数值小于ARR的值时是高电平，大于ARR值是低电平。所以PWM频率是这样计算的
$$f_{PWM}=\frac{f_{APB}}{(PSC+1)(ARR+1)}$$

​ 上图中设置初始PWM频率为100Hz。但是设置频率最好在20Hz~20000Hz以外，因为这个频率内的PWM波会让电机发出明显的电流声。我们可以将PSC设置为3-1，将ARR设置为1000-1，我这里作为演示就先不管了。

​ TIM4：计时间隔定时器

​ 设定为10Hz即1秒计算10次速度。

​ 最后要开启中断，并保证编码器定时器的中断优先级高于计时间隔定时器的中断优先级，避免编码器输入被间隔计时中断。

​ 其他基础配置不再赘述。

3.2 接线

编码器电机、电机驱动（这里用的L298n）、STM32、电源（可以是12V电池）的接线如下

3.3 代码编写

encoder.h中的内容

#ifndef _ENCODER_H_
#define _ENCODER_H_

#include "stm32f1xx.h"

//电机1的编码器输入引脚
#define MOTO1_ENCODER1_PORT GPIOA
#define MOTO1_ENCODER1_PIN  GPIO_PIN_0
#define MOTO1_ENCODER2_PORT GPIOA
#define MOTO1_ENCODER2_PIN  GPIO_PIN_1

//定时器号
#define ENCODER_TIM htim2
#define PWM_TIM     htim3
#define GAP_TIM     htim4

#define MOTOR_SPEED_RERATIO 45u    //电机减速比
#define PULSE_PRE_ROUND 11 //一圈多少个脉冲
#define RADIUS_OF_TYRE 34 //轮胎半径，单位毫米
#define LINE_SPEED_C RADIUS_OF_TYRE * 2 * 3.14
#define RELOADVALUE __HAL_TIM_GetAutoreload(&ENCODER_TIM)    //获取自动装载值,本例中为20000
#define COUNTERNUM __HAL_TIM_GetCounter(&ENCODER_TIM)        //获取编码器定时器中的计数值

typedef struct _Motor
{
    int32_t lastCount;   //上一次计数值
    int32_t totalCount;  //总计数值
    int16_t overflowNum; //溢出次数
    float speed;         //电机转速
    uint8_t direct;      //旋转方向
}Motor;

#endif
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

encoder.c中的内容

#include "encoder.h"

Motor motor1;

void Motor_Init(void)
{
    HAL_TIM_Encoder_Start(&ENCODER_TIM, TIM_CHANNEL_ALL);      //开启编码器定时器
    __HAL_TIM_ENABLE_IT(&ENCODER_TIM,TIM_IT_UPDATE);           //开启编码器定时器更新中断,防溢出处理
    HAL_TIM_Base_Start_IT(&GAP_TIM);                       //开启100ms定时器中断
    HAL_TIM_PWM_Start(&PWM_TIM, TIM_CHANNEL_2);            //开启PWM
    HAL_TIM_PWM_Start(&PWM_TIM, TIM_CHANNEL_1);            //开启PWM
    __HAL_TIM_SET_COUNTER(&ENCODER_TIM, 10000);                //编码器定时器初始值设定为10000
    motor1.lastCount = 0;                                   //结构体内容初始化
    motor1.totalCount = 0;
    motor1.overflowNum = 0;                                  
    motor1.speed = 0;
    motor1.direct = 0;
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)//定时器回调函数，用于计算速度
{
    if(htim->Instance==ENCODER_TIM.Instance)//编码器输入定时器溢出中断，用于防溢出                   
    {      
        if(COUNTERNUM < 10000) motor1.overflowNum++;       //如果是向上溢出
        else if(COUNTERNUM >= 10000) motor1.overflowNum--; //如果是向下溢出
        __HAL_TIM_SetCounter(&ENCODER_TIM, 10000);             //重新设定初始值
    }
    else if(htim->Instance==GAP_TIM.Instance)//间隔定时器中断，是时候计算速度了
    {
        motor1.direct = __HAL_TIM_IS_TIM_COUNTING_DOWN(&ENCODER_TIM);//如果向上计数（正转），返回值为0，否则返回值为1
        motor1.totalCount = COUNTERNUM + motor1.overflowNum * RELOADVALUE;//一个周期内的总计数值等于目前计数值加上溢出的计数值
        motor1.speed = (float)(motor1.totalCount - motor1.totalCount) / (4 * MOTOR_SPEED_RERATIO * PULSE_PRE_ROUND) * 10;//算得每秒多少转
        //motor1.speed = (float)(motor1.totalCount - motor1.totalCount) / (4 * MOTOR_SPEED_RERATIO * PULSE_PRE_ROUND) * 10 * LINE_SPEED_C//算得车轮线速度每秒多少毫米
        motor1.lastCount = motor1.totalCount; //记录这一次的计数值
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

使用时需要在main.c的循环之前调用Motor_Init函数进行初始化。

如果发现无法进入编码器中断导致totalCount经常溢出归零，可以尝试换一种防溢出的方法，代码如下

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)//定时器回调函数，用于计算速度
{	
    if(htim->Instance==GAP_TIM.Instance)//间隔定时器中断，是时候计算速度了
    {
        motor1.direct = __HAL_TIM_IS_TIM_COUNTING_DOWN(&ENCODER_TIM);//如果向上计数（正转），返回值为0，否则返回值为1
        motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一个周期内的总计数值等于目前计数值加上溢出的计数值
        
        if(motor1.lastCount - motor1.totalCount > 19000) // 在计数值溢出时进行防溢出处理
        {
            motor1.overflowNum++;
            motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一个周期内的总计数值等于目前计数值加上溢出的计数值
        }
        else if(motor1.totalCount - motor1.lastCount > 19000) // 在计数值溢出时进行防溢出处理
        {
            motor1.overflowNum--;
            motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一个周期内的总计数值等于目前计数值加上溢出的计数值
        }
        
        motor1.speed = (float)(motor1.totalCount - motor1.lastCount) / (4 * MOTOR_SPEED_RERATIO * PULSE_PRE_ROUND) * 3000;//算得每秒多少转,除以4是因为4倍频
        motor1.lastCount = motor1.totalCount; //记录这一次的计数值
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

3.4 结果

现在将测得的速度值输出到串口，就可以看到电机的实时转速了

4. 滤波

​ 到这里测速就已经完成了，但是如果后续想要进行PID控制，我们最好对测得的速度进行检查。

​ 如果将测得的速度值用VOFA+上位机画出来，我们可能会看到这样的曲线

​ 从图中我们可以看到，速度值在目标速度附近来回小幅度震荡，始终不稳定。这是因为编码器测速得到的速度值是离散的，如果电机的速度值刚好卡在两个离散值中间，我们测得的速度值就会在这两个离散值中间来回震荡。如果我们想要解决这个问题，最好先对测速的精度进行分析。

​ 对于M法测速来说，测速的公式如下，其中，k是将速度换算成rpm的比例系数
$$speed=\Delta pulse/(4\ast 减速比\ast 编码器线数)\ast k$$
​ 由于除号后面的都是定值，所以我们只要分析每次采样的脉冲数对速度的影响即可。

​ 我们假设现在测速频率是50Hz，减速比为30，编码器线数为500，那么脉冲数每变化1，速度的变化为
$$\Delta speed=\frac{1}{(4\ast 减速比\ast 编码器线数)}\ast 3000=1.923rpm$$
​ 所以我们测得的速度只能是1.923rpm的整数倍。如果想要提高精度，在电机不变的情况下，我们可以使用500线的GMR编码器或者降低测速频率。

​ 在VOFA+中，我们可以测得震荡时波峰和波谷的差值为1.92左右，和我们的计算相符。

​ 为了改善这一现象，我们可以对速度采样值使用平均滤波，即将最近几次的速度采样值存放到数组中，每测得一个新的速度，就将新速度存入数组，将最早测得的速度值从数组中删除，我们使用的速度值是数组中所有速度的平均值。实现代码如下

#define SPEED_RECORD_NUM 20 // 经测试，50Hz个采样值进行滤波的效果比较好

float speed_Record[SPEED_RECORD_NUM]={0};

/*
 * 进行速度的平均滤波
 * 输入新采样到的速度，存放速度的数组，
 * 返回滤波后的速度
 */
float Speed_Low_Filter(float new_Spe,float *speed_Record)
{
    float sum = 0.0f;
    test_Speed = new_Spe;
    for(uint8_t i=SPEED_RECORD_NUM-1;i>0;i--)//将现有数据后移一位
    {
        speed_Record[i] = speed_Record[i-1];
        sum += speed_Record[i-1];
    }
    speed_Record[0] = new_Spe;//第一位是新的数据
    sum += new_Spe;
    test_Speed = sum/SPEED_RECORD_NUM;
    return sum/SPEED_RECORD_NUM;//返回均值
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)//定时器回调函数，用于计算速度
{	
    if(htim->Instance==GAP_TIM.Instance)//间隔定时器中断，是时候计算速度了
    {
        motor1.direct = __HAL_TIM_IS_TIM_COUNTING_DOWN(&ENCODER_TIM);//如果向上计数（正转），返回值为0，否则返回值为1
        motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一个周期内的总计数值等于目前计数值加上溢出的计数值
        
        if(motor1.lastCount - motor1.totalCount > 19000) // 在计数值溢出时进行防溢出处理
        {
            motor1.overflowNum++;
            motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一个周期内的总计数值等于目前计数值加上溢出的计数值
        }
        else if(motor1.totalCount - motor1.lastCount > 19000) // 在计数值溢出时进行防溢出处理
        {
            motor1.overflowNum--;
            motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一个周期内的总计数值等于目前计数值加上溢出的计数值
        }
        
        motor1.speed = (float)(motor1.totalCount - motor1.lastCount) / (4 * MOTOR_SPEED_RERATIO * PULSE_PRE_ROUND) * 3000;//算得每秒多少转,除以4是因为4倍频
        /*******************在这里添加滤波函数************************/
        motor1.speed = Speed_Low_Filter(motor1.speed,speed_Record);
        /**********************************************************/
        motor1.lastCount = motor1.totalCount; //记录这一次的计数值
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

​ 经过滤波后的速度曲线如下。

​ 绿线是原始速度，红线是目标速度，粉线是滤波后的速度。可以看到，滤波后的速度值明显要平滑很多，这对我们后期的PID调试是很有利的。