使用HAL库进行电机测速滤波

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

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

对于M法测速来说，测速的公式如下，其中，k是将速度换算成rpm的比例系数

由于除号后面的都是定值，所以我们只要分析每次采样的脉冲数对速度的影响即可。

我们假设现在测速频率是50Hz，减速比为30，编码器线数为13，那么脉冲数每变化1，速度的变化为

所以我们测得的速度只能是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; //记录这一次的计数值
}

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

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