Abstract: PID 控制的软件代码，提供 C 和 Python 的版本

两个版本的功能几乎一致。

C 语言版本

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
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
#include <Arduino.h>
#include "PID.h"

/**
  * @brief  PID绝对值函数
  * @param  X
  * @retval X的绝对值
  */
float PID_abs(float X) {
  if (X >= 0) {
    return X;
  } else {
    return -X;
  }
}

/**
  * @brief  PID控制器初始化
  * @param  PID
  * @retval 无
  */
void PID_Init(PID_Typedef *PID) {
  PID->Kp = 0;
  PID->Ki = 0;
  PID->Kd = 0;

  PID->Pterm = 0;
  PID->Iterm = 0;
  PID->Dterm = 0;

  PID->Measure = 0;
  PID->Last_Mistake = 0;
  PID->Last_Measure = 0;
  PID->Target = 0;
  PID->Output = 0;
}

/**
  * @brief  PID限制器初始化
  * @param  PID_Limit
  * @retval 无
  */
void PID_Limit_Init(PID_LimitTypedef *PID_Limit) {
  PID_Limit->Dterm_Forward_On = 0;
  PID_Limit->Iterm_Max_On = 0;
  PID_Limit->Mistake_EnableIterm_On = 0;

  PID_Limit->Iterm_Max = 0;
  PID_Limit->Mistake_EnableIterm = 0;
  PID_Limit->Mistake_Deadband = 0;
}

/**
  * @brief  PID计算
  * @param  PID,PID_Limit
  * @retval 无
  */
void PID_Update(PID_Typedef *PID, PID_LimitTypedef *PID_Limit, float Feedback) {
  float Mistake = PID->Target - Feedback;
  if (PID_Limit->Mistake_Deadband_Enable != 0) {
    if (PID_abs(Mistake) < PID_abs(PID_Limit->Mistake_Deadband))  //误差在死区范围
    {
      Mistake = 0;
    }
  }
  PID->Last_Measure = PID->Measure;
  PID->Measure = Feedback;

  //P计算
  PID->Pterm = Mistake;

  //I计算
  if (PID_Limit->Mistake_EnableIterm_On != 0)  //启用积分分离
  {
    if (Mistake <= PID_Limit->Mistake_EnableIterm) {
      PID->Iterm += Mistake;
    }
  } else {
    PID->Iterm += Mistake;
  }
  if (PID_Limit->Iterm_Max_On != 0)  //启用积分限幅
  {
    if (PID->Iterm > PID_Limit->Iterm_Max) {
      PID->Iterm = PID_Limit->Iterm_Max;
    }
  }

  //D计算
  if (PID_Limit->Dterm_Forward_On != 0)  //启用微分先行
  {
    PID->Dterm = PID->Measure - PID->Last_Measure;
  } else {
    PID->Dterm = Mistake - PID->Last_Mistake;
  }

  PID->Last_Mistake = Mistake;  //记录误差

  PID->Output = PID->Pterm * PID->Kp + PID->Iterm * PID->Ki + PID->Dterm * PID->Kd;  //计算输出
}

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
#ifndef __PID_H
typedef struct
{
	float Kp; //P参数
	float Ki; //I参数
	float Kd; //D参数
	
	float Pterm; //P计算值
	float Iterm; //I计算值
	float Dterm; //D计算值
	
	float Measure; //当前测量量
	float Last_Mistake; //上次误差
	float Last_Measure; //上次测量量
	float Target; //目标值
	float Output;
} PID_Typedef;

typedef struct
{
	uint8_t Dterm_Forward_On; //微分先行,非0表示启用
	uint8_t Iterm_Max_On; //积分限幅,非0表示启用
	uint8_t Mistake_EnableIterm_On; //积分分离,非0表示启用
	uint8_t Mistake_Deadband_Enable; //误差死区,非0表示启用
	
	float Iterm_Max; //积分最大值
	float Mistake_EnableIterm; //允许积分的误差值
	float Mistake_Deadband; //误差死区
} PID_LimitTypedef;


void PID_Init(PID_Typedef *PID);
void PID_Limit_Init(PID_LimitTypedef *PID_Limit);
void PID_Update(PID_Typedef *PID, PID_LimitTypedef *PID_Limit, float Feedback);

#endif

用在 arduino 上需要更改 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
#include <Arduino.h>

#ifdef __cplusplus
extern "C" {
#endif

#ifndef __PID_H
  typedef struct
  {
    float Kp;  //P参数
    float Ki;  //I参数
    float Kd;  //D参数

    float Pterm;  //P计算值
    float Iterm;  //I计算值
    float Dterm;  //D计算值

    float Measure;       //当前测量量
    float Last_Mistake;  //上次误差
    float Last_Measure;  //上次测量量
    float Target;        //目标值
    float Output;
  } PID_Typedef;

  typedef struct
  {
    uint8_t Dterm_Forward_On;         //微分先行,非0表示启用
    uint8_t Iterm_Max_On;             //积分限幅,非0表示启用
    uint8_t Mistake_EnableIterm_On;   //积分分离,非0表示启用
    uint8_t Mistake_Deadband_Enable;  //误差死区,非0表示启用

    float Iterm_Max;            //积分最大值
    float Mistake_EnableIterm;  //允许积分的误差值
    float Mistake_Deadband;     //误差死区
  } PID_LimitTypedef;


  void PID_Init(PID_Typedef *PID);
  void PID_Limit_Init(PID_LimitTypedef *PID_Limit);
  void PID_Update(PID_Typedef *PID, PID_LimitTypedef *PID_Limit, float Feedback);
#endif

#ifdef __cplusplus
}
#endif

Python 版本

位置式

PID.py

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
class PosPIDClass:
    """位置式PID类"""

    def __init__(self, kp=0.1, ki=0, kd=0):
        """初始化"""
        self.__Kp = kp  # P参数
        self.__Ki = ki  # I参数
        self.__Kd = kd  # D参数

        self.__Iterm_max = None  # Iterm最大值,用于积分限幅,非int或float表示不限制
        self.__mistake_enableIterm = None  # 启用I调节的误差最大值,用于积分分离,非int或float表示不限制
        self.__Dterm_forward = False  # 微分先行,False表示不启用
        self.__mistake_deadband = None  # 误差死区

        self.__Pterm = 0  # P调节数值
        self.__Iterm = 0  # I调节数值
        self.__Dterm = 0  # D调节数值
        self.__output = 0  # 输出量

        self.__measurement = 0  # 测量量
        self.__last_mistake = 0  # 上一次的误差
        self.__last_measurement = 0  # 上一次的测量值
        self.__target = 0  # 目标值

    def __str__(self):
        return "Positional PIDClass"

    def setKp(self, kp):
        """设置P参数"""
        self.__Kp = kp

    def setKi(self, ki):
        """设置I参数"""
        self.__Ki = ki

    def setKd(self, kd):
        """设置D参数"""
        self.__Kd = kd

    def setTarget(self, target):
        """设置目标值"""
        self.__target = target

    def setLimitForIterm(self, max_value, mistake_enable_value):
        """设置I计算的限制参数"""
        self.__Iterm_max = max_value
        self.__mistake_enableIterm = mistake_enable_value

    def setDtermForward(self, state):
        """微分先行设置"""
        if isinstance(state, bool):
            self.__Dterm_forward = state

    def setMistakeDeadband(self, daedband):
        """误差死区设置"""
        self.__mistake_deadband = daedband

    def getPterm(self):
        """获取P计算值"""
        return self.__Pterm

    def getIterm(self):
        """获取I计算值"""
        return self.__Iterm

    def getDterm(self):
        """获取D计算值"""
        return self.__Dterm

    def update(self, feedback):
        """
        迭代计算
        在不考虑积分限幅、积分分离和微分先行时,公式为:
        $$U_n = K_P{\times}E_n + K_I{\times}{\sum_{i=1}^{n}E_i} + K_D{\times}(E_n-E_{n-1})$$
        """
        mistake = self.__target - feedback
        if isinstance(self.__mistake_deadband, int) or isinstance(self.__mistake_deadband, float):
            if abs(mistake) < self.__mistake_deadband:  # 误差在死区,则认为无误差
                mistake = 0
        self.__last_measurement = self.__measurement
        self.__measurement = feedback

        # P计算
        self.__Pterm = mistake

        # I计算
        if isinstance(self.__mistake_enableIterm, int) or isinstance(self.__mistake_enableIterm, float):  # 启用积分分离
            if mistake <= self.__mistake_enableIterm:  # 误差不大于限制值
                self.__Iterm += mistake
        else:
            self.__Iterm += mistake
        if isinstance(self.__Iterm_max, int) or isinstance(self.__Iterm_max, float):  # 启用积分限幅
            self.__Iterm = min(self.__Iterm_max, self.__Iterm)

        # D计算
        if self.__Dterm_forward:  # 启用微分先行
            self.__Dterm = self.__measurement - self.__last_measurement
        else:
            self.__Dterm = mistake - self.__last_mistake

        self.__last_mistake = mistake

        self.__output = self.__Pterm * self.__Kp + self.__Iterm * self.__Ki + self.__Dterm * self.__Kd

    def output(self):
        """获取最终结果"""
        return self.__output

增量式

class IncPIDClass:
    """增量式PID类"""

    def __init__(self, kp=0.1, ki=0, kd=0):
        """初始化"""
        self.__Kp = kp  # P参数
        self.__Ki = ki  # I参数
        self.__Kd = kd  # D参数

        self.__mistake_enableIterm = None  # 启用I调节的误差最大值,用于积分分离,非int或float表示不限制
        self.__mistake_deadband = None  # 误差死区

        self.__Pterm = 0  # P调节数值
        self.__Iterm = 0  # I调节数值
        self.__Dterm = 0  # D调节数值
        self.__output = 0  # 输出量

        self.__mistake = 0  # 误差
        self.__last_mistake = 0  # 上一次的误差
        self.__double_last_mistake = 0  # 上上次的测量值
        self.__target = 0  # 目标值

    def __str__(self):
        return "Increasing PIDClass"

    def setKp(self, kp):
        """设置P参数"""
        self.__Kp = kp

    def setKi(self, ki):
        """设置I参数"""
        self.__Ki = ki

    def setKd(self, kd):
        """设置D参数"""
        self.__Kd = kd

    def setTarget(self, target):
        """设置目标值"""
        self.__target = target

    def setMistakeDeadband(self, daedband):
        """误差死区设置"""
        self.__mistake_deadband = daedband

    def getPterm(self):
        """获取P计算值"""
        return self.__Pterm

    def getIterm(self):
        """获取I计算值"""
        return self.__Iterm

    def getDterm(self):
        """获取D计算值"""
        return self.__Dterm

    def update(self, feedback):
        """
        迭代计算
        $$\Delta u_k = u_k - u_{k-1} =
        K_p \times (e_k-e_{k-1}) + K_i \times e_k + K_d \times (e_k - 2e_{k-1} + e_{k-2})$$
        """
        mistake = self.__target - feedback

        self.__double_last_mistake = self.__last_mistake  # 误差记录更新
        self.__last_mistake = self.__mistake
        self.__mistake = mistake

        self.__Pterm = self.__mistake - self.__last_mistake  # P计算

        if isinstance(self.__mistake_enableIterm, int) or isinstance(self.__mistake_enableIterm, float):  # 启用积分分离
            if mistake <= self.__mistake_enableIterm:  # 误差不大于限制值
                self.__Iterm = mistake  # I计算
        else:
            self.__Iterm = mistake

        self.__Dterm = self.__mistake - 2 * self.__last_mistake + self.__double_last_mistake  # D计算

        self.__output = self.__Kp * self.__Pterm + self.__Ki * self.__Iterm + self.__Kd * self.__Dterm  # 计算输出值

    def output(self):
        """获取最终结果"""
        return self.__output