成为控制大师[1]——闭环控制与滤波器设计

介绍

在写了这么多视觉和深度学习的博客之后，我也总算念完了大二了，接下来就要逐步成为控制大师。

在很早之前我尝试过做无人机，但是不知道为什么无人机飞起来特别困难，在之后我尝试了穿越机以及穿越机的PID调参，我忽然明白了些什么但是不多，所以本期博客就要详细的叙述一下。为了有一个良好的参考，这里我选择了一个编码电机用于演示。

1

单片机选择的是f407vet6，控制板是亚博的，编码电机是同学的（硬抢）然后我们需要完成一下最基本的stm32配置。

设备驱动

编码电机的驱动要有两步：

  • 迫使定时器输出PWM波
  • 改变定时器为编码器，对于M测速法的电机，我们必须把定时器改成双触发模式，2分频。

PWM输出定时器，这里我用了TIM1，TIM1在APB1总线上，频率为84Mhz，在20000的分频下可以输出50Hz的PWM波

2

编码器部分使用了TIM2，我们只需要编码，所以分频默认65535即可。编码开启T1和T2，细分为2（Division by 2）

3

然后我们还需要一个串口用于绘制波形，这里选择的是VOFA+上位机（好看，比匿名上位机好看），串口配置过于基础，这里不说了。

测速

大家可能会好奇，为什么我不使用一个TIM中断进行定时测速。那么我们就不得不说一下中断是什么了。

中断是时序恶魔：

我这么说可能击碎了很多人脆弱的心脏，但事实上对于单核CPU或者说单片机而言，中断是噩梦一般的存在。

我们都知道程序保存在连续的内存空间内，CPU加载指令到缓存中可以非常方便的执行。如果产生了一个IRQ打断了当前正在运行的指令的话我们的CPU就不得不跳转到另一段连续的内存空间中了，如果主进程是一个对时间要求非常高的进程，中断就会影响到这个时间。为什么我们在控制的时候一定要做好对时间的把握，原因是外部的干扰需要实时的进行反馈，中断会打断这个实时性，会让控制机错过很多信息。举个例子：

小明驾车出去玩，当车辆行驶在公路（主进程）时忽然来了一个电话（中断），这时会产生两个情况：

  • 小明接电话了，但是在接电话时迎面驶来了一个失控的货车，小明观察不及时而寄了
  • 小明没接电话，而是选择在休息站（主进程空闲）接电话，这时迎面驶来一个失控的货车，小明即使反应而活了下来

没错，这个故事告诉我们一心不可而用，单片机可是如此（当然多核中断随便用，因为可以把任务分散给不同核心，但是单核就不可以了）

单片机控制电机的时候需要实时查看电机情况，无论电机是超前校正还是滞后校正，都不得不实时获取电机状态，一旦超过了调整时间就会造成失控。

如此说来我们就应该实现主程序计时。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
#define TimePeriod 100

uint16_t U_ABS(uint16_t a,uint16_t b){
    return a > b ? a - b : b - a;
}

uint32_t last_system_time = 0;


void Motor_update(TIM_HandleTypeDef *encoder_tim, Motor_t *motor) {
    // HAL_GetTick()可以获取到1ms自增一次的定时
    if(HAL_GetTick() - last_system_time >= TimePeriod){
        motor->encoder = encoder_tim->Instance->CNT;
        // 编码器异常情况需忽略
        if(U_ABS(motor->encoder,motor->last_encoder) <= 10000) {
            motor->speed = U_ABS(motor->encoder, motor->last_encoder);
            last_system_time = HAL_GetTick();
        }
        motor->last_encoder = encoder_tim->Instance->CNT;
    }
}

这样，电机参数更新的函数就实现了，而TimePeriod也可以给实时操作系统使用，保证程序严丝合缝执行。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
typedef struct Motor Motor_t;

struct Motor{
    uint16_t encoder;
    uint16_t last_encoder;

    // 减速比, 用于确定轴位置
    uint16_t output_ratio; // 输出比例
    uint16_t input_ratio;  // 输入比例

    float angle;
    uint16_t speed;
};

#define Motor_INIT(output_ratio_i, input_ratio_i) { \
        .encoder         =      0,                          \
        .last_encoder    =      0,                          \
        .output_ratio    =      output_ratio_i,             \
        .input_ratio     =      input_ratio_i,              \
        .angle           =      0,                          \
        .speed           =      0                           \
};

那么可能会有人问，1ms一次的定时会不会不准确呢？答案很简单，编码器可以计数在10000内进行计算非溢出，在这10000次内，假设两个霍尔编码间距为0.5cm，那么如果要想程序无法进行，电机的速度至少在300倍声速以上。所以这种速度完全足够了。（你在小看单片机的速度）

然后在主程序实时输出速度即可。

4

通过频谱可以发现，它的高频数据异常我们需要进行低通滤波确保获取一个准确的速度

数字信号处理(DSP)

我们把串口数据保存下来，然后利用matlab绘制一下FFT，确定一下频率中心，中央宽度以及采样率。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
% 自己改数据位置
fl = fopen("C://Users/28211/Desktop/Serial.txt", "r");
% 文件的每一行都是速度值
Y = textscan(fl, '%d');
Y = Y{1};
fclose(fl);

% 绘制fft波形
FFT_Y = fft(Y);

FFT_Y_shifted = fftshift(FFT_Y);
 
% 计算频率轴
N = length(FFT_Y);
n = 0:N-1;
f = (n - n/2) * 10 / N; % Fs是采样频率

plot(f, abs(FFT_Y_shifted));
xlabel('Frequency (Hz)');
ylabel('Amplitude');
title('FFT of Signal');

绘制一下图像可以得到：

5

在2.5hz左右数据分布密度最大，其他的集中在2.47627hz到2.5232hz之间，我们就可以确定中心频率为2.5Hz，频率宽度在0.04693Hz，采样率就是我们更新电机速度的频率，我这里的TimePeriod是100，也就是100ms，换算频率为10Hz，所以我们就用10Hz的采样率进行计算。

接下来我们实现滤波器的设计：

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
#define PI 3.14159265358979323846

typedef enum {
    DSP_IIR_FILTER = 0,
    DSP_FIR_FILTER = 1,
    DSP_LPF_FILTER = 2,
    DSP_HPF_FILTER = 3,
    DSP_BPF_FILTER = 4,
    DSP_BSF_FILTER = 5,
    DSP_NOTCH_FILTER = 6,
    DSP_PEAKING_FILTER = 7,
    DSP_ALLPASS_FILTER = 8,
    DSP_DC_REMOVAL_FILTER = 9,
    DSP_COUNT = 10
} DSP_FilterType;

typedef struct {
    double a[3];
    double b[3];
    double w[3];
} DSP_Filter_t;

/*** 滤波器初始化
 * @param filter 滤波器
 * @param type 滤波器类型
 * @param f0 截止频率
 * @param fs 采样频率
 * @param Q 品质因数
 * @param gain 增益
 * */
void DSP_Filter_Init(DSP_Filter_t *filter, DSP_FilterType type, double f0, double fs, double Q, double gain);

double DSP_Filter_Process(DSP_Filter_t *filter, DSP_FilterType type, double x);

然后我们看看C语言如何实现具体功能：

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
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
#include <math.h>
#include "DSP.h"

// IIR
void DSP_IIR_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_IIR_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// FIR
void DSP_FIR_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_FIR_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// LPF
void DSP_LPF_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_LPF_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// HPF
void DSP_HPF_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_HPF_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// BPF
void DSP_BPF_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_BPF_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// BSF
void DSP_BSF_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_BSF_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// NOTCH
void DSP_NOTCH_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_NOTCH_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// PEAKING
void DSP_PEAKING_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_PEAKING_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// ALLPASS
void DSP_ALLPASS_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_ALLPASS_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

// DC_REMOVAL
void DSP_DC_REMOVAL_Filter_Init(DSP_Filter_t *filter, double f0, double fs, double Q, double gain) {
    double w0 = 2 * PI * f0 / fs;
    double alpha = sin(w0) / (2 * Q);
    double a0 = 1 + alpha;
    filter->a[0] = 1 / a0;
    filter->a[1] = -2 * cos(w0) / a0;
    filter->a[2] = (1 - alpha) / a0;
    filter->b[0] = (1 - cos(w0)) / a0;
    filter->b[1] = 2 * (1 - cos(w0)) / a0;
    filter->b[2] = (1 - cos(w0)) / a0;
    filter->w[0] = 0;
    filter->w[1] = 0;
}

double DSP_DC_REMOVAL_Filter_Process(DSP_Filter_t *filter, double x) {
    double y = filter->b[0] * x + filter->w[0];
    filter->w[0] = filter->b[1] * x - filter->a[1] * y + filter->w[1];
    filter->w[1] = filter->b[2] * x - filter->a[2] * y;
    return y;
}

void DSP_Filter_Init(DSP_Filter_t *filter, DSP_FilterType type, double f0, double fs, double Q, double gain){
    switch(type) {
        case DSP_IIR_FILTER:
            DSP_IIR_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_FIR_FILTER:
            DSP_FIR_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_LPF_FILTER:
            DSP_LPF_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_HPF_FILTER:
            DSP_HPF_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_BPF_FILTER:
            DSP_BPF_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_BSF_FILTER:
            DSP_BSF_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_NOTCH_FILTER:
            DSP_NOTCH_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_PEAKING_FILTER:
            DSP_PEAKING_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_ALLPASS_FILTER:
            DSP_ALLPASS_Filter_Init(filter, f0, fs, Q, gain);
            break;
        case DSP_DC_REMOVAL_FILTER:
            DSP_DC_REMOVAL_Filter_Init(filter, f0, fs, Q, gain);
            break;
        default:
            break;
    }
}


double DSP_Filter_Process(DSP_Filter_t *filter, DSP_FilterType type, double x){
    switch(type) {
        case DSP_IIR_FILTER:
            return DSP_IIR_Filter_Process(filter, x);
        case DSP_FIR_FILTER:
            return DSP_FIR_Filter_Process(filter, x);
        case DSP_LPF_FILTER:
            return DSP_LPF_Filter_Process(filter, x);
        case DSP_HPF_FILTER:
            return DSP_HPF_Filter_Process(filter, x);
        case DSP_BPF_FILTER:
            return DSP_BPF_Filter_Process(filter, x);
        case DSP_BSF_FILTER:
            return DSP_BSF_Filter_Process(filter, x);
        case DSP_NOTCH_FILTER:
            return DSP_NOTCH_Filter_Process(filter, x);
        case DSP_PEAKING_FILTER:
            return DSP_PEAKING_Filter_Process(filter, x);
        case DSP_ALLPASS_FILTER:
            return DSP_ALLPASS_Filter_Process(filter, x);
        case DSP_DC_REMOVAL_FILTER:
            return DSP_DC_REMOVAL_Filter_Process(filter, x);
        default:
            return 0;
    }
}

数据初始化也很简单：

1
2
3
4
void Motor_Filter_init(){
    // 中心频率2.5Hz, 采样率10hz, Q值0.04693（通频带?）
    DSP_Filter_Init(&motor_filter, DSP_LPF_FILTER, 2.49, 10, 0.04693, 1);
}

我们将电机更新方法重新实现一下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
void Motor_Filter_init(){
    // 中心频率2.5Hz, 采样率10hz, Q值0.04693
    DSP_Filter_Init(&motor_filter, DSP_LPF_FILTER, 2.49, 10, 0.04693, 1);
}

void Motor_update(TIM_HandleTypeDef *encoder_tim, Motor_t *motor) {
    if(HAL_GetTick() - last_system_time >= TimePeriod){
        motor->encoder = encoder_tim->Instance->CNT;
        if(U_ABS(motor->encoder,motor->last_encoder) <= 10000) {
            motor->speed = U_ABS(motor->encoder, motor->last_encoder);
            // 速度IIR滤波
            motor->speed = (int) DSP_Filter_Process(&motor_filter, DSP_LPF_FILTER, motor->speed);
            last_system_time = HAL_GetTick();
        }
        motor->last_encoder = encoder_tim->Instance->CNT;
    }
}

最后我们看一下速度波形曲线：

6

电机的启动需要时间，所以前期会缓慢上升，最后速度的波形也是相对不错的。

速度控制

速度的控制是控制的关键，我们这里使用了PID控制，为了方便适配不同情况我这里把PID代码发出来大家自行参考：

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
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#ifndef PIDMOTOR_PID_H
#define PIDMOTOR_PID_H

#include "main.h"

#ifndef pid_abs
#define pid_abs(x) x < 0 ? -x : x
#endif

/*** PID结构体, 不需要直接初始化，需要通过PID_config_t进行初始化
 * @param PID PID结构体
 * @brief 保存所有变量
 * */
typedef struct PID PID_t;
// 初始化需要用到的参数
typedef struct PID_config PID_config_t;
// PID状态
typedef struct PID_status PID_status_t;

// PID模式(自行调节, 不影响初始化)
typedef enum{
    PID_IMPROVE_NONE = 0X00,                // 0000 0000 无超级模式
    PID_Integral_Limit = 0x01,              // 0000 0001 积分限幅
    PID_Derivative_On_Measurement = 0x02,   // 0000 0010 微分先行
    PID_Trapezoid_Intergral = 0x04,         // 0000 0100 梯形积分
    PID_Proportional_On_Measurement = 0x08, // 0000 1000
    PID_OutputFilter = 0x10,                // 0001 0000 输出滤波
    PID_ChangingIntegrationRate = 0x20,     // 0010 0000 变速积分
    PID_DerivativeFilter = 0x40,            // 0100 0000 微分滤波器
    PID_ErrorHandle = 0x80,                 // 1000 0000 异常句柄
} PID_Improvement;

typedef enum Error_Type
{
    PID_ERROR_NONE = 0x00U,
    PID_BLOCKED_ERROR = 0x01U
} Error_Type_e;


struct PID_status{
    // 异常测数
    uint16_t ErrorCount;
    // 设备状态
    uint8_t type;
};

struct PID{
    // 基本PID配置
    float Kp;  // P
    float Ki;  // I
    float Kd;  // D
    float Max_out;  // 输出限幅
    float Dead_band;  // 输出死区
    // ------------------

    // 中间变量
    float Measure;
    float Last_Measure;
    float Err;
    float Last_Err;
    float Last_ITerm;

    // 中间运算输出变量
    float P_out;
    float I_out;
    float D_out;
    float ITerm;    // 积分累积

    // 进阶参数
    PID_Improvement Improve;
    float Integral_Limit;     // 积分限幅
    float Coef_A;             // 变速积分
    float Coef_B;             // 变速积分 ITerm = Err*((A-abs(err)+B)/A)  if B<|err|<A+B
    float Output_LPF_RC;     // 输出滤波器 RC = 1/omega
    float Derivative_LPF_RC; // 微分滤波器系数

    // 输出变量
    float Output;
    float Last_Output;
    float Last_Dout;

    // 实际值
    float Ref;

    // PID状态
    PID_status_t pidStatus;

    // 程序计数(用于前反馈)
    uint32_t dt;
};

struct PID_config{
    // 对应PID内的每个参数
    float Kp;
    float Ki;
    float Kd;
    float Max_out;   // 输出限幅
    float Dead_band; // 死区

    // improve parameter
    PID_Improvement Improve;
    float Integral_Limit; // 积分限幅
    float Coef_A;         // AB为变速积分参数,变速积分实际上就引入了积分分离
    float Coef_B;         // ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
    float Output_LPF_RC; // RC = 1/omega
    float Derivative_LPF_RC;
};

/*** 参数配置表
 * @param Kp_val Kp
 * @param Ki_val Ki
 * @param Kd_val Kd
 * @param IntegralLimit_val 积分限幅
 * @param MaxOut_val 输出限幅
 * @param Improve_val PID扩展模式
 * */
#define INIT_PID_CONFIG(Kp_val, Ki_val, Kd_val, IntegralLimit_val, MaxOut_val, Improve_val) \
    {                                                                                \
        .Kp = Kp_val,                                                                \
        .Ki = Ki_val,                                                                \
        .Kd = Kd_val,                                                                \
        .Integral_Limit = IntegralLimit_val,                                          \
        .Max_out = MaxOut_val,                                                        \
        .Improve = Improve_val,                                                      \
    }

void PID_init(PID_t *pid, PID_config_t *config);

float PID_Calculate(PID_t *pid, float measure, float ref);

void PID_clear(PID_t *pid);

#endif //PIDMOTOR_PID_H
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
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
#include <math.h>
#include "PID.h"

// 梯形积分
static void Trapezoid_Intergral(PID_t *pid){
    pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}


// 变速积分(误差小时积分作用更强)
static void Changing_Integration_Rate(PID_t *pid){
    if (pid->Err * pid->I_out > 0){
        // 积分呈累积趋势
        if (pid_abs(pid->Err) <= pid->Coef_B)
            return; // 全积分
        if (pid_abs(pid->Err) <= (pid->Coef_A + pid->Coef_B))
            pid->ITerm *= (pid->Coef_A - pid_abs(pid->Err) + pid->Coef_B) / pid->Coef_A;
        else // 最大阈值,不使用积分
            pid->ITerm = 0;
    }
}

// 积分限幅(宏无法考虑到对积分累积的归零, 很麻烦)
static void Integral_Limit(PID_t *pid){
    static float temp_Output, temp_Iout;
    temp_Iout = pid->I_out + pid->ITerm;
    temp_Output = pid->P_out + pid->I_out + pid->D_out;
    if (pid_abs(temp_Output) > pid->Max_out)
        if (pid->Err * pid->I_out > 0) // 积分却还在累积
            pid->ITerm = 0; // 当前积分项置零

    if (temp_Iout > pid->Integral_Limit){
        pid->ITerm = 0;
        pid->I_out = pid->Integral_Limit;
    }
    if (temp_Iout < -pid->Integral_Limit){
        pid->ITerm = 0;
        pid->I_out = -pid->Integral_Limit;
    }
}

// 微分先行(仅使用反馈值而不计参考输入的微分)
static void Derivative_On_Measurement(PID_t *pid){
    pid->D_out = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}

// 微分滤波(采集微分时,滤除高频噪声)
static void Derivative_Filter(PID_t *pid){
    pid->D_out = pid->D_out * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
                pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}

// 输出滤波
static void Output_Filter(PID_t *pid){
    pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
                  pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}

// 输出限幅
static void Output_Limit(PID_t *pid){
    if (pid->Output > pid->Max_out)
        pid->Output = pid->Max_out;
    if (pid->Output < -(pid->Max_out))
        pid->Output = -(pid->Max_out);
}

// 电机堵转检测
static void PID_status_ErrorHandle(PID_t *pid){
    /*Motor Blocked Handle*/
    if (fabsf(pid->Output) < pid->Max_out * 0.001f || fabsf(pid->Ref) < 0.0001f)
        return;

    if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
        pid->pidStatus.ErrorCount++;
    else
        pid->pidStatus.ErrorCount = 0;


    if (pid->pidStatus.ErrorCount > 500)
        // Motor blocked over 1000times
        pid->pidStatus.type = PID_BLOCKED_ERROR;
}

void PID_init(PID_t *pid, PID_config_t *config){
    // basic parameter
    pid->Kp = config->Kp;
    pid->Ki = config->Ki;
    pid->Kd = config->Kd;
    pid->Max_out = config->Max_out;
    pid->Dead_band = config->Dead_band;

    // improve parameter
    pid->Improve = config->Improve;
    pid->Integral_Limit = config->Integral_Limit;
    pid->Coef_A = config->Coef_A;
    pid->Coef_B = config->Coef_B;
    pid->Output_LPF_RC = config->Output_LPF_RC;
    pid->Derivative_LPF_RC = config->Derivative_LPF_RC;
}

float PID_Calculate(PID_t *pid, float measure, float ref){
    // 堵转检测
    if (pid->Improve & PID_ErrorHandle)
        PID_status_ErrorHandle(pid);

    pid->dt = HAL_GetTick() - pid->dt;

    pid->Measure = measure;
    pid->Ref = ref;
    pid->Err = pid->Ref - pid->Measure;

    // 如果在死区外,则计算PID
    if (pid_abs(pid->Err) > pid->Dead_band){
        // 基本的pid计算,使用位置式
        pid->P_out = pid->Kp * pid->Err;
        pid->ITerm = pid->Ki * pid->Err * pid->dt;
        pid->D_out = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;

        // 梯形积分
        if (pid->Improve & PID_Trapezoid_Intergral)
            Trapezoid_Intergral(pid);
        // 变速积分
        if (pid->Improve & PID_ChangingIntegrationRate)
            Changing_Integration_Rate(pid);
        // 微分先行
        if (pid->Improve & PID_Derivative_On_Measurement)
            Derivative_On_Measurement(pid);
        // 微分滤波器
        if (pid->Improve & PID_DerivativeFilter)
            Derivative_Filter(pid);
        // 积分限幅
        if (pid->Improve & PID_Integral_Limit)
            Integral_Limit(pid);

        pid->I_out += pid->ITerm;                         // 累加积分
        pid->Output = pid->P_out + pid->I_out + pid->D_out; // 计算输出

        // 输出滤波
        if (pid->Improve & PID_OutputFilter)
            Output_Filter(pid);

        // 输出限幅
        Output_Limit(pid);
    }
    else {
        // 死区
        pid->Output = 0;
        pid->ITerm = 0;
    }

    // 保存当前数据,用于下次计算
    pid->Last_Measure = pid->Measure;
    pid->Last_Output = pid->Output;
    pid->Last_Dout = pid->D_out;
    pid->Last_Err = pid->Err;
    pid->Last_ITerm = pid->ITerm;

    return pid->Output;
}

void PID_clear(PID_t *pid){
    pid->Measure = 0;
    pid->Last_Measure = 0;
    pid->Err = 0;
    pid->Last_Err = 0;
    pid->Last_ITerm = 0;
    pid->P_out = 0;
    pid->I_out = 0;
    pid->D_out = 0;
    pid->ITerm = 0;
    pid->Output = 0;
    pid->Last_Output = 0;
    pid->Last_Dout = 0;
    pid->pidStatus.ErrorCount = 0;
    pid->pidStatus.type = 0;
}

这里我们实现了很多额外PID计算的功能，包括梯形积分和变速积分，这里我使用了变速积分器。我们找个位置声明一下PID参数

1
PID_config_t pidConfig = INIT_PID_CONFIG(230, 0.24, 1.9, 5000, 10000, PID_ChangingIntegrationRate);

在stm32的初始化阶段就把PID初始化好，数字滤波器初始化好后就可以把PID的输出放入PWM输出了：

1
2
3
4
5
// 初始化区域
PID_t pid;
PID_config_t pidConfig = INIT_PID_CONFIG(230, 0.24, 1.9, 5000, 10000, PID_ChangingIntegrationRate);
PID_init(&pid, &pidConfig);
Motor_Filter_init();

最后我们完成控制：

1
2
3
4
5
6
7
8
9
10
11
12
13
void Motor_loop(){
    HAL_UART_Receive_DMA(&huart1, Motor_Speed, 2);
    Motor_Speed_Value = Motor_Speed[0] << 8 | Motor_Speed[1];
    Motor_update(&htim2, &motor);
    uint16_t pwm = (uint16_t) PID_Calculate(&pid, motor.speed, Motor_Speed_Value);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwm);
	
    // 串口发送数据(迎合上位机绘制波形)
    Serial_TX_Int(&huart1, motor.speed);
    HAL_UART_Transmit(&huart1, (uint8_t *)",", 1, 0xFFFF);
    Serial_TX_Int(&huart1, Motor_Speed_Value);
    HAL_UART_Transmit(&huart1, (uint8_t *)"\r\n", 2, 0xFFFF);
}

我们看一下PID控制效果：

7

492是PID控制的速度，500是输入的控制速度，可以看到效果非常不错，如果我们用力去捏电机的轴，速度只会小范围移动，这样我们的PID就算控制好了。

总结

本次尝试了一下闭环控制，算是小试牛刀（用时比深度测距用的久），假期会做一个六足机器人，同样会教大家一些好玩的知识，敬请期待哦！

控制大师的修炼手册
#C #嵌入式 #自动控制
成为控制大师[1]——闭环控制与滤波器设计
https://blog.minloha.cn/posts/1130227c2174242024063019.html
作者
Minloha
发布于
2024年6月30日
更新于
2024年9月16日
许可协议