Abstract: PID 控制的软件代码，提供 C 和 Python 的版本
两个版本的功能几乎一致。
C 语言版本
PID.c
#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
#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
#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
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