当前位置：首页 > news >正文

【嵌入式电机控制#9】编码器滤波算法

news 2025/7/5 6:38:27

一、冒泡排序滤波

问题：如何筛选出一组数据中最大的数？

经常刷leetcode同学肯定会直接qsort......

然而今天需要借用的是一种比快速排序更经典的排序方法——冒泡排序

这个算法中有一步十分具有含金量：

通过一次对数组元素的遍历来确定数组的最大值，或者通过一次遍历来确定最小值。通过这两种过程，我们可以快速确定一组编码器数据的极端值，通过删除这些极端值再取平均数，我们就完成了一套标准的转速平均值滤波。

具体算法流程如下（数组元素个数大于1）：

1. 定义两个动态变量，max_speed和min_speed

2. 指针遍历数组一次，每次循环更新维护max_speed、min_speed

（一次循环找两个极端值）

3. 指针遍历数组一次，用动态变量sum累加所有元素

4. 将sum减去maxspeed和minspeed再除以numsSize-2迭代赋值

5. 返回最终的sum

时间复杂度：O（n）

空间复杂度：O（1）

二、一阶低通滤波

接下来的内容比较抽象，注重理解

滤波算法公式：
Y(n) = q * X(n) + (1 - q) * Y(n - 1)

        Y(n)：本次滤波结果。
q：滤波系数。取值范围为0~1, 值越小越稳定，越大越灵敏。
        X(n)：本次采样值。
        Y(n - 1)：上次滤波结果。

它的作用是什么呢？我们举个例子

设定q为0.6则（1-q）== 0.4

我们在读取编码器信号的时候遇到一个高频尖刺，如下图所示：

刚开始的值为100 突然产生一个200的突变，最后又回稳到150

对200的位置做分析，如果我们对这个点滤波则有：

此点滤波输出 Y(n) = 0.6 * 200 + 0.4 * 100 = 160

所以得到的图像定性表示则如下图：

因而我们可以看出，它的作用也是削弱高频信号，让滤波后的波形变得更加平滑。

有人会对这个算法存在一定的误解，认为如果我的200高频是真的变成200，而不是测量误差该怎么办？

归根结底还是上面那句话，算法的核心在于信号削弱，而不是异常处理。换句话说，它只是一个帮我们过滤对控制系统本身稳定性影响比较大的反馈突变的工具，他没有对信号真正异常与否的辨识能力。

对于一些异常的反馈信号（通常是在控制系统之外，对硬件有严重损害的），我们应该通过合理安排逻辑顺序和调整滤波系数q，来达到滤波与异常检测两个环节都能稳定运行的程度。

从控制系统分析来讲，如果反馈回路中某一刻出现异常值，可能有以下几种情况：

1. 控制系统并未失稳，只是反馈回路输入端引入了噪声，我们低通平滑处理即可

2. 控制系统并未失稳，只是进入了一个新的稳态，后续的变化趋势呈现正常的上升或递减

3. 控制系统暂未进入稳态，出现了短暂的超调，震荡

4. 控制系统暂未进入/失去稳态，并出现了负阻尼现象（发散震荡，此刻如果使用大功率电机，将会极其危险，检测到后必须断电或禁能驱动，必须注意）

5. 控制系统失去稳态，输出量缓慢接近0（此刻需要反思我们的系统设计问题）

这个地方如果看不懂没关系，我们下一章开始会详细分析。

三、滤波系数的调节

根据上图，我们能够得出一个结论：

对于相同的滤波系统，在单位阶跃信号下，q越大，响应越快，但曲线不平滑。q越小，曲线越平滑，但响应越慢。

而实际情况不是一个阶跃信号，而是许多脉冲信号触发的，这张图真正表达的含义是两个状态跃迁时，状态间曲线的响应，平滑程度与q的关系。

四、代码实现

在上一章代码的基础上，我们在速度计算中断中增加滤波环节，此外建议先不要增加控制算法之类的内容进去，我们应该先做开环测试去查看性能。

main函数中我们给一个适当的占空比，然后在不套滤波器的情况下查看反馈波形。

可以很明显看到，响应曲线存在一定噪声

接下来我们编写滤波器测速代码

主函数中，我们增加了一个中断十次做滤波的内容。如果k==10，则先做冒泡滤波+去极端值+平均滤波，然后使用一阶低通滤波算法，注意想办法保存上一次的滤波结果。

/* USER CODE BEGIN 4 */
float data[10];
float prev_filted=0;
uint8_t k=0;
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){if(htim==&htim6){printf("entered");int16_t val = (int16_t)__HAL_TIM_GetCounter(&htim2);__HAL_TIM_SET_COUNTER(&htim2, 0);float RPM =(float)((val/7.0/50.0/4.0)*6000);
//		printf("RPM: %0.3f \r \n", RPM);data[k]=RPM;
//			Vofa_data(RPM);if(k==9){float ave = cut_ave_filter(data, 10);st_lowp_filter(ave, SPEED_Q, &prev_filted);printf("RPM: %.3f \r \n",prev_filted);k=0;}else {k++;}
//    __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1,BINMotor_SpeedtoPulse(BACK,L_AddPID.pwm_add));
//			dc_motor_pwmset(RPM,&spdPID);}}typedef union
{float fdata;unsigned long ldata;
} FloatLongType;int fputc(int ch, FILE *f)
{HAL_UART_Transmit(&huart4 , (uint8_t *)&ch, 1, 0xFFFF);return ch;
}
/* USER CODE END 4 */

在滤波文件filter.c中详细写明了所有滤波算法的过程，有兴趣的同学可以看看，不一定要求能够完全默写出来。

#include "filter.h"
void Swap(float* a, float* b){*a=*a+*b;*b=*a-*b;*a=*a-*b;
}
void BubbleSort(float* arr, int n)
{int end = n;while (end){int flag = 0;for (int i = 1; i < end; ++i){if (arr[i - 1] > arr[i]){Swap(arr+i,arr+i-1);flag = 1;}}if (flag == 0){break;}--end;}
}float cut_ave_filter(float * data, int len){float sum=0;BubbleSort(data, len - 1);for(uint8_t i = 2; i < len-2; i++){sum += data[i];}sum = (float)(sum/(len-4));return sum;
}void st_lowp_filter(float input, float q, float* output){*(output)=(float)(((float)q*input)+((float)(1-(float)q)*(*(output))));
}

原本打算使用快速排序滤波试一试结果，但是发现误差极其严重。希望粉丝们能给出解答，为什么快速排序不如冒泡稳定。

接下来我们变动滤波参数q去跑一下波形，

（1）q = 0.1