PID控制算法核心原理
数学表达式：
u(t) = Kp·e(t) + Ki·∫e(t)dt + Kd·de(t)/dt
其中：
- e(t) = 设定值（Target） - 实际值（Feedback）
- Kp、Ki、Kd 分别为比例、积分、微分系数
通俗详解：
比例项P就是让当前测量值与目标值做差，得到的误差越大Kp的控制力度就越强因为它只会死板地判断离目标的距离，距离越大它要用力就越大，它的作用就是要快速响应误差，让系统更快趋近于目标值，但是因为它比较死板，就可能出现在靠近目标值的时候它的控制作用仍旧特别强，导致当前值一下子超出目标值，然后因为超出目标值就又有一个误差，可能让它更用力地控制系统向目标靠近，这时就很可能让系统一直在目标值上下抖动形成震荡。比如下图（1）。
这个振荡是我们所不想要的，我们想要的是像右边的图一样越接近目标值系统输出的变化越平缓，直到达到目标值。为了消除振荡我们引入了微分控制D，通过系统输出变化曲线的切线斜率来判断是否需要D发力，斜率越大D的控制力度就越大。简单说D就相当于一个阻尼，比如说你在水下挥拳，是不是挥拳越快受到的阻力就越大，因为水在阻止你，水的阻力在让你的速度变化更慢一点，加速度更小一点。D在系统控制里起到的作用就是这样，当你的系统离目标值很远的时候，Kp开始发力让系统靠近目标值，正常来说我们想要上图（2），但因为Kp单一控制的缺陷，它可能会在靠近目标的地方曲线斜率仍旧是很大，就像上面左边图的情况，这个时候就该微分项D出场了，它会根据那个斜率的大小来对p起一个抑制的作用，让它在靠近目标值的地方变化更慢一点，准确来说应该是让P整体变化都更平缓了，引入D以后系统就不那么容易在目标值附近振荡，会更平稳地趋近于目标值，让系统响应更平滑，振荡更小更少，类似下图（3）。D也有它的缺点，它很容易受噪声的影响，因为噪声的本质是高频随机波动，即使是很小的噪声，它的瞬时变化率（斜率）也可能是非常大，这个时候Kd就会放大噪声，因为平稳信号叠加高频噪声后其导数会出现剧烈波动，导致系统输出的斜率剧烈振荡，导致微分输出项D也同样剧烈地波动，导致系统变得不稳定，出现剧烈抖动等意外。也就是说微分增益 Kd 调得越大，系统对变化越敏感，但噪声也会被放得更大，导致执行机构（如电机或阀门）频繁抖动，甚至让整个系统失控震荡。为了消除噪声的影响，就要给微分项加一个滤波器，比如一阶低通滤波器，来过滤掉噪声，留下真正有用的，以此减少噪声影响。
仅有p和d还是不够的，因为系统还会因为环境影响，测量装置或方法的缺陷，人为操作因素，以及被测对象自身特性等而形成一个稳态误差，就是永远到达不了目标值，稳定时会和目标值有一个差距。比如说有一个小车，我们设定它目标速度为100，但是由于有摩檫力等的存在，在p和d控制下它进入稳定时的速度可能就只有90，因为电机不得不分出一部分的动力去对抗摩擦力了。为了解决这个误差，让我们的系统更完美，我们引入了I积分控制。它通过不断累计误差，只要存在误差（即使很小），积分的作用也会不断地增大，直到误差被完全消除，核心作用就是消除这个静态误差，但它的反应速度相对比较慢（因为是累计），Ki太大的话会导致系统反应迟缓，甚至会导致超冲。因为初始误差可能比较大，这就导致初始就累计起很大的误差，I的控制力度很大，会导致系统很猛烈地去修正误差，往往就导致系统输出快速地冲过目标值，当系统输出超出目标值，继续累计的误差就是负的（也就是开始减小累计积分），但是它想要让先前巨大的正值的累计误差变为负的就需要时间，这个抵消的过程里它就会和比例项p进行对抗，阻止p的修正，这导致系统会在目标值上面停留很久直到先前庞大的正值累计误差被消耗掉变为负值，I才会来帮助系统回调，体现出来就是系统响应速度变慢，整体变得拖沓迟缓。
总结：
- P - 比例控制 (Proportional Control):
- 作用： 产生与当前误差成比例的控制作用。
- 原理： 误差越大，控制作用越强。就像一个本能反应：离目标越远，用的力越大。
- 效果： 能快速响应误差，减小静态误差（系统最终稳定值与目标值之间的偏差）。
- 缺点： 单纯的比例控制通常无法完全消除静态误差（会有余差）。如果比例作用太强，系统可能会产生振荡（在目标值附近来回摆动）。
- I - 积分控制 (Integral Control):
- 作用： 产生与误差随时间的累积量（积分） 成比例的控制作用。
- 原理： 关注历史表现。只要存在误差（即使很小），积分作用就会持续累积增大，直到误差被完全消除。
- 效果： 核心作用是消除静态误差。对于持续存在的小误差特别有效。
- 缺点： 反应相对较慢（因为是累积效应）。如果太强，会导致系统反应迟缓，超调量增大（系统输出超过目标值），甚至引起振荡。
- D - 微分控制 (Derivative Control):
- 作用： 产生与误差变化的速率（微分/导数） 成比例的控制作用。
- 原理： 预测未来趋势。如果误差正在快速增大，微分作用会施加一个强的反向控制力来“刹车”，阻止误差进一步扩大；如果误差正在快速减小，微分作用会减弱控制力，避免“刹车”过头冲过目标。
- 效果： 提供阻尼作用，抑制振荡，减小超调量，提高系统稳定性。让系统响应更平滑。
- 缺点： 对测量噪声非常敏感（噪声会被放大），可能导致控制量剧烈变化。如果太强，也可能导致系统不稳定。
接下来是代码演示：
1 typedef struct {
2 int setValue; // 设定目标
3 float kp; // 比例常数
4 float ki; // 积分常数
5 float kd; // 微分常数
6
7 int lastError; // Error[-1]
8 int prevError; // Error[-2]
9 int sumError; // 误差累加
10
11 int limit; // 限制值
12
13 } PIDTypeDef;
14
15 /*
16
17 * 函数介绍：位置式PID计算
18 * 输入参数：p（PID调节器参数）nextVaule（采样值）
19 * 输出参数：
20 * 返回值 ：
21
22 */
23 int positionPIDCalc(PIDTypeDef *p, int nextValue) {
24 int dError, error;
25 int out;
26 error = p->setValue - nextValue; // 偏差
27 p->sumError += error; // 积分
28 dError = error - p->lastError; // 当前微分
29 p->prevError = p->lastError; // 更新error[-1]的值
30 p->lastError = error; // 更新error[-2]的值
31 out = p->kp * error // 比例项
32 + p->ki * p->sumError // 积分项
33 + p->kd * dError; // 微分项
34
35 out = limit(out, -p->limit, p->limit);
36
37 return (out);
38 }
39
40 /**
41 * @brief 积分分离PID计算
42 *
43 * @param p pid调节器
44 * @param nextValue 采样值
45 * @param threshold 阈值
46 * @return int
47 * @note 当误差绝对值大于阈值时，清零积分的累计误差，可以消除稳态误差
48 */
49 int positionPiD_IntegralSeparate(PIDTypeDef *p, int nextValue,uint16_t threshold){
50 int16_t dError, error;
51 int out;
52 error = p->setValue - nextValue; // 偏差
53 p->sumError += error; // 积分
54 dError = error - p->lastError; // 当前微分
55 p->prevError = p->lastError; // 更新error[-1]的值
56 p->lastError = error; // 更新error[-2]的值
57 //当误差大于阈值时，清零积分项
58 if(abs16(error)>=threshold){
59 p->sumError = 0;
60 }
61 out = p->kp * error // 比例项
62 + p->ki * p->sumError // 积分项
63 + p->kd * dError; // 微分项
64
65 out = limit(out, -p->limit, p->limit);
66
67 return (out);
68 }
69 /*
70
71 * 函数介绍：增量式PID计算
72 * 输入参数：p（PID调节器参数）nextVaule（采样值）
73 * 输出参数：
74 * 返回值 ：
75
76 */
77 int incrementalPIDcalc(PIDTypeDef *p, int nextVaule) {
78 int error;
79 int ep, ei, ed;
80 int out;
81 error = p->setValue - nextVaule; // 偏差
82 ep = error - p->lastError;
83 ei = error;
84 ed = error - 2 * p->lastError + p->prevError; //控制值的增量，（3-2）-（2-1），抗噪声
85
86 p->prevError = p->lastError; // 更新error[-1]的值
87 p->lastError = error; // 更新error[-2]的值
88
89 out = p->kp * ep // 比例项
90 + p->ki * ei // 积分项
91 + p->kd * ed; // 微分项
92
93 out = limit(out, -p->limit, p->limit);
94
95 return (out);
96 }
97
98 /*
99
100 * 函数介绍：位置式PID计算,并限制积分项
101 * 输入参数：p（PID调节器参数）nextVaule（采样值）
102 * 输出参数：
103 * 返回值 ：
104
105 */
106 int positionPIDCalc_LimitSumError(PIDTypeDef *p, int nextValue) {
107 int dError, error;
108 int out;
109 error = p->setValue - nextValue; // 偏差
110 p->sumError += error; // 积分
111 dError = p->lastError - p->prevError; // 当前微分
112 p->prevError = p->lastError; // 更新error[-1]的值
113 p->lastError = error; // 更新error[-2]的值
114 p->sumError =
115 limit(p->sumError, (float)-p->limit / p->ki,
116 (float)p->limit / p->ki); // 限制积分项的输出最大为limit，可以快速回调
117 out = p->kp * error // 比例项
118 + p->ki * p->sumError // 积分项
119 + p->kd * dError; // 微分项
120
121 out = limit(out, -p->limit, p->limit);
122
123 return (out);
124 }
1 // PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式 2 3 #define PID_IMPROVE_NONE (0x00) // 0000 0000 4 #define PID_Integral_Limit (0x01<<0) // 0000 0001 // 积分限幅 5 #define PID_Derivative_On_Measurement (0x01<<1) // 0000 0010 // 微分先行 6 #define PID_Trapezoid_Intergral (0x01<<2) // 0000 0100 // 梯形积分 7 #define PID_DeltaT_Limit (0x01<<3) // 0000 1000 // 时间间隔限幅 8 #define PID_OutputFilter (0x01<<4) // 0001 0000 // 输出滤波器 9 #define PID_ChangingIntegrationRate (0x01<<5) // 0010 0000 // 变速积分 10 #define PID_DerivativeFilter (0x01<<6) // 0100 0000 // 微分滤波器 11 #define PID_Integral_Separate (0x01<<7) // 1000 0000 // 积分分离
1 /* PID结构体 */
2 typedef struct
3 {
4 //---------------------------------- init config block
5 // config parameter
6 float Kp;
7 float Ki;
8 float Kd;
9 float MaxOut; // 输出限幅
10 float DeadBand; // 死区
11
12 // improve parameter
13 uint8_t Improve;
14 float IntegralLimit; // 积分限幅
15 float CoefA; // 变速积分 For Changing Integral
16 float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
17 float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
18 float Derivative_LPF_RC; // 微分滤波器系数
19 float Intergral_Separate; // 积分分离值
20 float DeltaT_Limit_Max; // 时间间隔限幅
21 float DeltaT_Limit_Min; // 时间间隔限幅
22
23 //-----------------------------------
24 // for calculating
25 float Measure; // 反馈值
26 float Last_Measure; // 上次反馈值
27 float Err; // 误差值
28 float Last_Err; // 上次误差值
29 float Last_ITerm; // 上次积分项
30
31 float Pout; // 比例项输出
32 float Iout; // 积分项输出
33 float Dout; // 微分项输出
34 float ITerm; // 积分项
35
36 float Output; // PID输出
37 float Last_Output; // 上次PID输出
38 float Last_Dout; // 上次微分项输出
39
40 float Ref; // 设定值
41
42 uint32_t lastTime; // 上次计算的时间戳,用于计算时间间隔
43 float dt; // 时间间隔,用于积分和微分计算
44
45 } PIDInstance;
46
47 /* 用于PID初始化的结构体*/
48 typedef struct // config parameter
49 {
50 // basic parameter
51 float Kp;
52 float Ki;
53 float Kd;
54 float MaxOut; // 输出限幅
55 float DeadBand; // 死区
56
57 // improve parameter
58 uint8_t Improve;
59 float IntegralLimit; // 积分限幅
60 float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
61 float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
62 float Output_LPF_RC; // RC = 1/omegac
63 float Derivative_LPF_RC;
64 float Intergral_Separate; // 积分分离值
65 float DeltaT_Limit_Max; // 时间间隔限幅
66 float DeltaT_Limit_Min; // 时间间隔限幅
67 } PID_Init_Config_s;
68
69 //普通是矩形积分，精度低一些，因为会多算一部分超出线的面积
70 // 梯形积分
71 static void f_Trapezoid_Intergral(PIDInstance *pid)
72 {
73 // 计算梯形的面积,(上底+下底)*高/2
74 pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
75 }
76
77 // 变速积分(误差小时积分作用更强)
78 //参数A（积分分离阈值）和B（全速积分阈值）无需精确设定，适应性更强
79 //B：设为静差容忍阈值
80 //A+B：设为系统允许的最大偏差范围
81 static void f_Changing_Integration_Rate(PIDInstance *pid)
82 {
83 if (is_same_sign(pid->Err, pid->Iout))
84 {
85 // 积分呈累积趋势
86 if (abs(pid->Err) <= pid->CoefB)
87 return; // Full integral
88 if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
89 pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
90 else // 最大阈值,不使用积分
91 pid->ITerm = 0;
92 }
93 }
94 static void f_Integral_Separate(PIDInstance *pid)
95 {
96 if (abs(pid->Err) > pid->Intergral_Separate)
97 {
98 pid->Iout = 0;// 误差小于分离值,不使用积分
99 pid->ITerm = 0;
100 }
101
102 }
103 static void f_Integral_Limit(PIDInstance *pid)
104 {
105 static float temp_Output, temp_Iout;
106 temp_Iout = pid->Iout + pid->ITerm;
107 temp_Output = pid->Pout + pid->Iout + pid->Dout;
108 if (abs(temp_Output) > pid->MaxOut)
109 {
110 // if (pid->Err * pid->Iout > 0) // 积分却还在累积
111 // {
112 // pid->ITerm = 0; // 当前积分项置零
113 // }
114 if((pid->Ki>0&&is_same_sign(pid->Err, pid->Iout))
115 ||pid->Ki<0&&(!is_same_sign(pid->Err, pid->Iout))){ //前一个是积分正向发力，后一个是积分反向发力，都是要让输出值超出限度
116 pid->ITerm = 0; // 当前积分项置零
117 }
118 }
119
120 if (temp_Iout > pid->IntegralLimit)
121 {
122 pid->ITerm = 0;
123 pid->Iout = pid->IntegralLimit;
124 }
125 if (temp_Iout < -pid->IntegralLimit)
126 {
127 pid->ITerm = 0;
128 pid->Iout = -pid->IntegralLimit;
129 }
130 }
131
132 // 微分先行(仅使用反馈值而不计参考输入的微分) 目标值会大幅度跳变时可以使用
133 static void f_Derivative_On_Measurement(PIDInstance *pid)
134 {
135 pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
136 }
137
138 // 微分滤波(采集微分时,滤除高频噪声)
139 static void f_Derivative_Filter(PIDInstance *pid)
140 {
141 pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
142 pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
143 }
144
145 // 输出滤波
146 static void f_Output_Filter(PIDInstance *pid)
147 {
148 pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
149 pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
150 }
151
152 //时间间隔限幅
153 static void f_DeltaT_Limit(PIDInstance *pid)
154 {
155 if (pid->dt > pid->DeltaT_Limit_Max)
156 pid->dt = pid->DeltaT_Limit_Max;
157 if (pid->dt < pid->DeltaT_Limit_Min)
158 pid->dt = pid->DeltaT_Limit_Min;
159 }
160
161 // 输出限幅
162 static void f_Output_Limit(PIDInstance *pid)
163 {
164 if (pid->Output > pid->MaxOut)
165 {
166 pid->Output = pid->MaxOut;
167 }
168 if (pid->Output < -(pid->MaxOut))
169 {
170 pid->Output = -(pid->MaxOut);
171 }
172 }
173 /**
174 * @brief PID计算
175 * @param[in] PID结构体
176 * @param[in] 测量值
177 * @param[in] 期望值
178 * @retval 返回空
179 */
180 float PIDCalculate(PIDInstance *pid, float measure, float ref)
181 {
182
183 pid->dt = GetDeltaT(&pid->lastTime); // 获取两次pid计算的时间间隔,用于积分和微分
184 if(pid->Improve & PID_DeltaT_Limit)
185 f_DeltaT_Limit(pid); // 时间间隔限幅
186
187 // 保存上次的测量值和误差,计算当前error
188 pid->Measure = measure;
189 pid->Ref = ref;
190 pid->Err = pid->Ref - pid->Measure;
191
192 // 如果在死区外,则计算PID //死区就是容忍一定范围内的波动
193 if (abs(pid->Err) > pid->DeadBand)
194 {
195 // 基本的pid计算,使用位置式
196 pid->Pout = pid->Kp * pid->Err;
197 pid->ITerm = pid->Ki * pid->Err * pid->dt;
198 pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
199
200 // 梯形积分
201 if (pid->Improve & PID_Trapezoid_Intergral)
202 f_Trapezoid_Intergral(pid);
203 // 变速积分
204 if (pid->Improve & PID_ChangingIntegrationRate)
205 f_Changing_Integration_Rate(pid);
206 // 积分分离
207 if (pid->Improve & PID_Integral_Separate)
208 f_Integral_Separate(pid);
209 // 微分先行
210 if (pid->Improve & PID_Derivative_On_Measurement)
211 f_Derivative_On_Measurement(pid);
212 // 微分滤波器
213 if (pid->Improve & PID_DerivativeFilter)
214 f_Derivative_Filter(pid);
215 // 积分限幅
216 if (pid->Improve & PID_Integral_Limit)
217 f_Integral_Limit(pid);
218
219 pid->Iout += pid->ITerm; // 累加积分
220 pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
221
222 // 输出滤波
223 if (pid->Improve & PID_OutputFilter)
224 f_Output_Filter(pid);
225
226 // 输出限幅
227 f_Output_Limit(pid);
228 }
229 else // 进入死区
230 {
231 pid->Output = pid->Last_Output;
232 pid->ITerm = 0;
233 }
234
235 // 保存当前数据,用于下次计算
236 pid->Last_Measure = pid->Measure;
237 pid->Last_Output = pid->Output;
238 pid->Last_Dout = pid->Dout;
239 pid->Last_Err = pid->Err;
240 pid->Last_ITerm = pid->ITerm;
241
242 return pid->Output;
243 }
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
