基本框架（从github上抄来的感觉可行）

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
#ifndef PID_H
#define PID_H
typedef struct{
    //所需要调节的参数
    float Kp;
    float Ki;
    float Kd;
    
    float tau;//积分所用到的低通滤波时间常数
    float limMin;//out输出限制幅度
    float limMax;

    float limMinInt;//积分项限制幅度
    float limMaxInt;

    float T;//采样速率
    float integrator;//积分项
    float prevError;//上一次的误差
    float differentiator;//微分项
    float prevMeasurement;//上一次的测量值
    float out;//输出
} PIDController;

float PID_update(PIDController *pid, float setpoint, float measurement);

#endif /* PID_H */
pid.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
#include "pid.h"
PIDController motor_pid;// create a PIDController object for motorspeed control


float PID_update(PIDController *pid, float setpoint, float measurement) {

	/*
	* Error signal
	*/
    float error = setpoint - measurement;


	/*
	* Proportional
	*/
    float proportional = pid->Kp * error;


	/*
	* Integral
	*/
    pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);

	/* Anti-wind-up via integrator clamping */
    if (pid->integrator > pid->limMaxInt) {

        pid->integrator = pid->limMaxInt;

    } else if (pid->integrator < pid->limMinInt) {

        pid->integrator = pid->limMinInt;

    }


	/*
	* Derivative (band-limited differentiator)
	*/
		
    pid->differentiator = -(2.0f * pid->Kd * (measurement - pid->prevMeasurement)	/* Note: derivative on measurement, therefore minus sign in front of equation! */
                        + (2.0f * pid->tau - pid->T) * pid->differentiator)
                        / (2.0f * pid->tau + pid->T);


	/*
	* Compute output and apply limits
	*/
    pid->out = proportional + pid->integrator + pid->differentiator;

    if (pid->out > pid->limMax) {

        pid->out = pid->limMax;

    } else if (pid->out < pid->limMin) {

        pid->out = pid->limMin;

    }

	/* Store error and measurement for later use */
    pid->prevError       = error;
    pid->prevMeasurement = measurement;

	/* Return controller output */
    return pid->out;

}

补充

  1. 对于微分项后面
    1
    2
    3
    pid->differentiator = -(2.0f * pid->Kd * (measurement - pid->prevMeasurement) /* Note: derivative on measurement, therefore minus sign in front of equation! */
                            + (2.0f * pid->tau - pid->T) * pid->differentiator)
                            / (2.0f * pid->tau + pid->T);
  • 代码解释
    1. -(2.0f * pid->Kd * (measurement - pid->prevMeasurement)):
      pid->Kd 是PID控制器中的微分增益（Derivative Gain）。
      • measurement - pid->prevMeasurement 是当前测量值与上一次测量值的差，表示测量值的变化率。
      • 2.0f * pid->Kd * (measurement - pid->prevMeasurement): 计算的是微分项的一部分。
      • 前面的负号 “-”是因为这里计算的是测量值的微分，而不是误差的微分。注释中提到的“therefore minus sign in front of equation!”就是这个意思。
    2. + (2.0f * pid->tau - pid->T) * pid->differentiator:
      • pid->tau 是一个滤波常数，用于限制微分项的高频噪声。
      • pid->T 是采样时间。
      • (2.0f * pid->tau - pid->T) * pid->differentiator 是微分项的递推部分，用于平滑微分输出。
    3. / (2.0f * pid->tau + pid->T):
      • 这是对整个微分项进行归一化处理，以确保微分项的幅值在合理的范围内。
  1. 对于积分项后面
    1
    pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);
  • 代码解释
    1. 0.5f * pid->Ki * pid->T * (error + pid->prevError):
      • error + pid->prevError 是当前误差和上一次误差的和。
      • 0.5f * (error + pid->prevError) 是对误差和进行平均处理，以平滑积分项的计算。
      • pid->Ki * pid->T 是积分增益和采样时间的乘积，表示积分项的单位时间内的累积速率。
      • 因此，0.5f * pid->Ki * pid->T * (error + pid->prevError) 计算的是当前采样周期内积分项的增量。
    2. pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError):
      • 这是将当前采样周期内积分项的增量加到之前的积分项上，以更新积分项的值。