使用HAL庫進行電機測速濾波

如果將測得的速度值用VOFA+上位機畫出來，我們可能會看到這樣的曲線

從圖中我們可以看到，速度值在目標速度附近來回小幅度震蕩，始終不穩(wěn)定。這是因為編碼器測速得到的速度值是離散的，如果電機的速度值剛好卡在兩個離散值中間，我們測得的速度值就會在這兩個離散值中間來回震蕩。如果我們想要解決這個問題，最好先對測速的精度進行分析。

對于M法測速來說，測速的公式如下，其中，k是將速度換算成rpm的比例系數(shù)

由于除號后面的都是定值，所以我們只要分析每次采樣的脈沖數(shù)對速度的影響即可。

我們假設現(xiàn)在測速頻率是50Hz，減速比為30，編碼器線數(shù)為13，那么脈沖數(shù)每變化1，速度的變化為

所以我們測得的速度只能是1.923rpm的整數(shù)倍。如果想要提高精度，在電機不變的情況下，我們可以使用500線的GMR編碼器或者降低測速頻率。

在VOFA+中，我們可以測得震蕩時波峰和波谷的差值為1.92左右，和我們的計算相符。

為了改善這一現(xiàn)象，我們可以對速度采樣值使用平均濾波，即將最近幾次的速度采樣值存放到數(shù)組中，每測得一個新的速度，就將新速度存入數(shù)組，將最早測得的速度值從數(shù)組中刪除，我們使用的速度值是數(shù)組中所有速度的平均值。實現(xiàn)代碼如下

#define SPEED_RECORD_NUM 20 // 經(jīng)測試，50Hz個采樣值進行濾波的效果比較好


float speed_Record[SPEED_RECORD_NUM]={0};


/*
 * 進行速度的平均濾波
 * 輸入新采樣到的速度，存放速度的數(shù)組，
 * 返回濾波后的速度
 */
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--)//將現(xiàn)有數(shù)據(jù)后移一位
    {
        speed_Record[i] = speed_Record[i-1];
        sum += speed_Record[i-1];
    }
    speed_Record[0] = new_Spe;//第一位是新的數(shù)據(jù)
    sum += new_Spe;
    test_Speed = sum/SPEED_RECORD_NUM;
    return sum/SPEED_RECORD_NUM;//返回均值
}


void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)//定時器回調(diào)函數(shù)，用于計算速度
{  
    if(htim- >Instance==GAP_TIM.Instance)//間隔定時器中斷，是時候計算速度了
    {
        motor1.direct = __HAL_TIM_IS_TIM_COUNTING_DOWN(&ENCODER_TIM);//如果向上計數(shù)（正轉(zhuǎn)），返回值為0，否則返回值為1
        motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一個周期內(nèi)的總計數(shù)值等于目前計數(shù)值加上溢出的計數(shù)值
        
        if(motor1.lastCount - motor1.totalCount > 19000) // 在計數(shù)值溢出時進行防溢出處理
        {
            motor1.overflowNum++;
            motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一個周期內(nèi)的總計數(shù)值等于目前計數(shù)值加上溢出的計數(shù)值
        }
        else if(motor1.totalCount - motor1.lastCount > 19000) // 在計數(shù)值溢出時進行防溢出處理
        {
            motor1.overflowNum--;
            motor1.totalCount = COUNTERNUM_1 + motor1.overflowNum * RELOADVALUE_1;//一個周期內(nèi)的總計數(shù)值等于目前計數(shù)值加上溢出的計數(shù)值
        }
        
        motor1.speed = (float)(motor1.totalCount - motor1.lastCount) / (4 * MOTOR_SPEED_RERATIO * PULSE_PRE_ROUND) * 3000;//算得每秒多少轉(zhuǎn),除以4是因為4倍頻
        /*******************在這里添加濾波函數(shù)************************/
        motor1.speed = Speed_Low_Filter(motor1.speed,speed_Record);
        /**********************************************************/
        motor1.lastCount = motor1.totalCount; //記錄這一次的計數(shù)值
}

經(jīng)過濾波后的速度曲線如下。

綠線是原始速度，紅線是目標速度，粉線是濾波后的速度?？梢钥吹?，濾波后的速度值明顯要平滑很多，這對我們后期的PID調(diào)試是很有利的。