【轉】位置式、增量式PID算法C語言實現

位置式、增量式PID算法C語言實現

芯片：STM32F107VC

編譯器：KEIL4

做者：SY

日期：2017-9-21 15:29:19

概述

PID 算法是一種工控領域常見的控制算法，用於閉環反饋控制。有如下兩種分類：

• 增量式

  每次週期性計算出的 PID 爲增量值，是在上一次控制量的基礎上進行的調整。

• 位置式

  每次週期性計算出的 PID 爲絕對的數值，是執行機構實際的位置。

咱們使用高級語言的思想去實現兩種 PID ，作到對於用戶來講，調用相同的接口，內部實現不一樣的 PID 算法。

代碼

pid.h

 1 enum PID_MODE {
 2     PID_INC = 0,    //增量式
 3     PID_POS,        //位置式
 4 };
 5 
 6 struct PID {
 7     enum PID_MODE mode;
 8 
 9     float kp;                   //比例係數
10     float ki;                   //積分系數
11     float kd;                   //微分系數
12 
13     double targetPoint;         //目標點
14     double lastError;           //Error[-1]
15     double prevError;           //Error[-2]
16 
17     void (*init)(struct PID *this, double targetPoint);         //PID初始化
18     double (*outputLimit)(struct PID *this, double output);     //PID輸出限制
19     void (*setParameter)(struct PID *this, \
20         float kp, float ki, float kd);                          //設置PID參數
21     double (*calculate)(struct PID *this, double samplePoint);  //計算PID
22 };
23 
24 /* 增量式PID */
25 struct PID_INC {
26     struct PID pid;
27 };
28 
29 /* 位置式PID */
30 struct PID_POS {
31     struct PID pid;
32     double iSum;                //積分和
33 };

其中 struct PID 就是咱們提供給用戶的接口，能夠理解爲 抽象類 ，增量式和位置式 PID 都去繼承該抽象類，而後實現其中的抽象方法。

對於位置式 PID 擁有本身的成員 iSum 。

pid.c

  1 /*
  2 *********************************************************************************************************
  3 *                                           PID
  4 *********************************************************************************************************
  5 */
  6 /*
  7 *********************************************************************************************************
  8 * Function Name : PID_Init
  9 * Description   : PID初始化
 10 * Input         : None
 11 * Output        : None
 12 * Return        : None
 13 *********************************************************************************************************
 14 */
 15 static void PID_Init(struct PID *this, double targetPoint)
 16 {
 17     this->targetPoint = targetPoint;
 18     this->lastError = 0;
 19     this->prevError = 0;    
 20 }
 21 
 22 /*
 23 *********************************************************************************************************
 24 * Function Name : PID_OutputLimit
 25 * Description   : PID輸出限制
 26 * Input         : None
 27 * Output        : None
 28 * Return        : None
 29 *********************************************************************************************************
 30 */
 31 static double PID_OutputLimit(struct PID *this, double output)
 32 {
 33     if (output < 0) {
 34         output = 0;
 35     } else if (output > DIGITAL_THROTTLE_VALVE_MAX_DEGREE) {
 36         output = DIGITAL_THROTTLE_VALVE_MAX_DEGREE;
 37     }
 38     return output;
 39 }
 40 
 41 /*
 42 *********************************************************************************************************
 43 * Function Name : PID_SetParameter
 44 * Description   : PID設置參數
 45 * Input         : None
 46 * Output        : None
 47 * Return        : None
 48 *********************************************************************************************************
 49 */
 50 static void PID_SetParameter(struct PID *this, float kp, float ki, float kd)
 51 {
 52     this->kp = kp;
 53     this->ki = ki;
 54     this->kd = kd;
 55 }
 56 
 57 /*
 58 *********************************************************************************************************
 59 * Function Name : PID_SetTargetValue
 60 * Description   : PID設置目標值
 61 * Input         : None
 62 * Output        : None
 63 * Return        : None
 64 *********************************************************************************************************
 65 */
 66 void PID_SetTargetValue(struct PID *this, double targetPoint)
 67 {
 68     this->targetPoint = targetPoint;
 69 }
 70 
 71 /*
 72 *********************************************************************************************************
 73 * Function Name : PID_GetTargetValue
 74 * Description   : PID獲取目標值
 75 * Input         : None
 76 * Output        : None
 77 * Return        : None
 78 *********************************************************************************************************
 79 */
 80 double PID_GetTargetValue(struct PID *this)
 81 {
 82     return this->targetPoint;
 83 }
 84 
 85 /*
 86 *********************************************************************************************************
 87 *                                           增量式PID
 88 *********************************************************************************************************
 89 */
 90 static double PID_IncCalculate(struct PID *this, double samplePoint);
 91 
 92 struct PID_INC g_PID_Inc = {
 93     .pid = {
 94         .mode           = PID_INC,
 95         .init           = PID_Init,
 96         .outputLimit    = PID_OutputLimit,
 97         .setParameter   = PID_SetParameter,
 98         .calculate      = PID_IncCalculate,
 99     },
100 };
101 
102 /*
103 *********************************************************************************************************
104 * Function Name : PID_IncCalculate
105 * Description   : 增量式PID計算
106 * Input         : None
107 * Output        : None
108 * Return        : None
109 *********************************************************************************************************
110 */
111 static double PID_IncCalculate(struct PID *this, double samplePoint)
112 {   
113     double nowError = this->targetPoint - samplePoint;
114     double out = this->kp * nowError +\
115                  this->ki * this->lastError +\
116                  this->kd * this->prevError;
117     this->prevError = this->lastError;
118     this->lastError = nowError;
119 
120     if (this->outputLimit) {
121         out = this->outputLimit(this, out);
122     }
123 
124     return out;
125 }
126 
127 /*
128 *********************************************************************************************************
129 *                                           位置式PID
130 *********************************************************************************************************
131 */
132 static double PID_PosCalculate(struct PID *this, double samplePoint);
133 static void PID_PosInit(struct PID *this, double targetPoint);
134 
135 struct PID_POS g_PID_Pos = {
136     .pid = {
137         .mode           = PID_POS,
138         .init           = PID_PosInit,
139         .outputLimit    = PID_OutputLimit,
140         .setParameter   = PID_SetParameter,
141         .calculate      = PID_PosCalculate,
142     },
143 };
144 
145 /*
146 *********************************************************************************************************
147 * Function Name : PID_PosInit
148 * Description   : 位置式PID初始化
149 * Input         : None
150 * Output        : None
151 * Return        : None
152 *********************************************************************************************************
153 */
154 static void PID_PosInit(struct PID *this, double targetPoint)
155 {
156     PID_Init(this, targetPoint);
157     struct PID_POS *pid_Handle = (struct PID_POS *)this;
158     pid_Handle->iSum = 0;
159 }
160 
161 /*
162 *********************************************************************************************************
163 * Function Name : PID_PosCalculate
164 * Description   : 位置式PID計算
165 * Input         : None
166 * Output        : None
167 * Return        : None
168 *********************************************************************************************************
169 */
170 static double PID_PosCalculate(struct PID *this, double samplePoint)
171 {
172     struct PID_POS *pid_Handle = (struct PID_POS *)this;
173 
174     double nowError = this->targetPoint - samplePoint;
175     this->lastError = nowError;
176     //積分累計偏差
177     pid_Handle->iSum += nowError;
178     double out = this->kp * nowError +\
179                  this->ki * pid_Handle->iSum +\
180                  this->kd * (nowError - this->prevError);   
181     this->prevError = nowError;
182 
183     if (this->outputLimit) {
184         out = this->outputLimit(this, out);
185     }
186 
187     return out;
188 }

對於上述內容：最關鍵的是兩個變量 struct PID_INC g_PID_Inc 和 struct PID_POS g_PID_Pos ，他們針對抽象類實現了各自的算法。

其中有一個很重要的小技巧，舉例：

1 static double PID_PosCalculate(struct PID *this, double samplePoint)
2 {
3     struct PID_POS *pid_Handle = (struct PID_POS *)this;
4 }

該函數的接口是 struct PID *this ，可是咱們內部將他強制轉換爲 struct PID_POS *pid_Handle ，這是兩種不一樣的數據類型，爲何能夠進行轉換呢？並且轉換後的數據是正確的？

由於對於 C 語言來講，結構體內部的第一個成員的地址和該結構體變量的地址是同樣的，因此能夠相互轉換，實現向下轉型，這就是 C 語言的強大之處。

測試

app.c

 1 struct KernelCtrl {
 2     struct DTV dtv;
 3     struct PID *dtvPid;
 4 };
 5 
 6 static struct KernelCtrl g_kernelCtrl;
 7 
 8 /*
 9 *********************************************************************************************************
10 * Function Name : Kernel_Ctrl_Init
11 * Description   : 內核控制初始化
12 * Input         : None
13 * Output        : None
14 * Return        : None
15 *********************************************************************************************************
16 */
17 void Kernel_Ctrl_Init(void)
18 {
19 #if 1
20     /* 增量式 */
21     g_kernelCtrl.dtvPid = &g_PID_Inc.pid;
22 #else
23     /* 位置式 */
24     g_kernelCtrl.dtvPid = &g_PID_Pos.pid;
25 #endif  
26 }

只要在初始化時，指定使用哪種 PID 模式，在調用時兩種方式可使用同一個接口，這樣對於用戶來講就屏蔽了內部細節。

參考

Pid控制算法-變積分的pid算法的C++實現

 

 

【來源】