【转】位置式、增量式PID算法C语言实现

位置式、增量式PID算法C语言实现

芯片：STM32F107VC

编译器：KEIL4

作者：SY

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

概述

PID 算法是一种工控领域常见的控制算法，用于闭环反馈控制。有以下两种分类：

• 增量式

  每次周期性计算出的 PID 为增量值，是在上一次控制量的基础上进行的调整。

• 位置式

  每次周期性计算出的 PID 为绝对的数值，是执行机构实际的位置。

我们使用高级语言的思想去实现两种 PID ，做到对于用户来说，调用相同的接口，内部实现不同的 PID 算法。

代码

pid.h

 enum PID_MODE {
     PID_INC = ,    //增量式
     PID_POS,        //位置式
 };

 struct PID {
     enum PID_MODE mode;

     float kp;                   //比例系数
     float ki;                   //积分系数
     float kd;                   //微分系数

     double targetPoint;         //目标点
     double lastError;           //Error[-1]
     double prevError;           //Error[-2]

     void (*init)(struct PID *this, double targetPoint);         //PID初始化
     double (*outputLimit)(struct PID *this, double output);     //PID输出限制
     void (*setParameter)(struct PID *this, \
         float kp, float ki, float kd);                          //设置PID参数
     double (*calculate)(struct PID *this, double samplePoint);  //计算PID
 };

 /* 增量式PID */
 struct PID_INC {
     struct PID pid;
 };

 /* 位置式PID */
 struct PID_POS {
     struct PID pid;
     double iSum;                //积分和
 };

其中 struct PID 就是我们提供给用户的接口，可以理解为 抽象类 ，增量式和位置式 PID 都去继承该抽象类，然后实现其中的抽象方法。

对于位置式 PID 拥有自己的成员 iSum 。

pid.c

 /*
 *********************************************************************************************************
 *                                           PID
 *********************************************************************************************************
 */
 /*
 *********************************************************************************************************
 * Function Name : PID_Init
 * Description   : PID初始化
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 static void PID_Init(struct PID *this, double targetPoint)
 {
     this->targetPoint = targetPoint;
     this->lastError = ;
     this->prevError = ;
 }

 /*
 *********************************************************************************************************
 * Function Name : PID_OutputLimit
 * Description   : PID输出限制
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 static double PID_OutputLimit(struct PID *this, double output)
 {
     if (output < ) {
         output = ;
     } else if (output > DIGITAL_THROTTLE_VALVE_MAX_DEGREE) {
         output = DIGITAL_THROTTLE_VALVE_MAX_DEGREE;
     }
     return output;
 }

 /*
 *********************************************************************************************************
 * Function Name : PID_SetParameter
 * Description   : PID设置参数
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 static void PID_SetParameter(struct PID *this, float kp, float ki, float kd)
 {
     this->kp = kp;
     this->ki = ki;
     this->kd = kd;
 }

 /*
 *********************************************************************************************************
 * Function Name : PID_SetTargetValue
 * Description   : PID设置目标值
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 void PID_SetTargetValue(struct PID *this, double targetPoint)
 {
     this->targetPoint = targetPoint;
 }

 /*
 *********************************************************************************************************
 * Function Name : PID_GetTargetValue
 * Description   : PID获取目标值
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 double PID_GetTargetValue(struct PID *this)
 {
     return this->targetPoint;
 }

 /*
 *********************************************************************************************************
 *                                           增量式PID
 *********************************************************************************************************
 */
 static double PID_IncCalculate(struct PID *this, double samplePoint);

 struct PID_INC g_PID_Inc = {
     .pid = {
         .mode           = PID_INC,
         .init           = PID_Init,
         .outputLimit    = PID_OutputLimit,
         .setParameter   = PID_SetParameter,
         .calculate      = PID_IncCalculate,
     },
 };

 /*
 *********************************************************************************************************
 * Function Name : PID_IncCalculate
 * Description   : 增量式PID计算
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 static double PID_IncCalculate(struct PID *this, double samplePoint)
 {
     double nowError = this->targetPoint - samplePoint;
     double out = this->kp * nowError +\
                  this->ki * this->lastError +\
                  this->kd * this->prevError;
     this->prevError = this->lastError;
     this->lastError = nowError;

     if (this->outputLimit) {
         out = this->outputLimit(this, out);
     }

     return out;
 }

 /*
 *********************************************************************************************************
 *                                           位置式PID
 *********************************************************************************************************
 */
 static double PID_PosCalculate(struct PID *this, double samplePoint);
 static void PID_PosInit(struct PID *this, double targetPoint);

 struct PID_POS g_PID_Pos = {
     .pid = {
         .mode           = PID_POS,
         .init           = PID_PosInit,
         .outputLimit    = PID_OutputLimit,
         .setParameter   = PID_SetParameter,
         .calculate      = PID_PosCalculate,
     },
 };

 /*
 *********************************************************************************************************
 * Function Name : PID_PosInit
 * Description   : 位置式PID初始化
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 static void PID_PosInit(struct PID *this, double targetPoint)
 {
     PID_Init(this, targetPoint);
     struct PID_POS *pid_Handle = (struct PID_POS *)this;
     pid_Handle->iSum = ;
 }

 /*
 *********************************************************************************************************
 * Function Name : PID_PosCalculate
 * Description   : 位置式PID计算
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 static double PID_PosCalculate(struct PID *this, double samplePoint)
 {
     struct PID_POS *pid_Handle = (struct PID_POS *)this;

     double nowError = this->targetPoint - samplePoint;
     this->lastError = nowError;
     //积分累计误差
     pid_Handle->iSum += nowError;
     double out = this->kp * nowError +\
                  this->ki * pid_Handle->iSum +\
                  this->kd * (nowError - this->prevError);
     this->prevError = nowError;

     if (this->outputLimit) {
         out = this->outputLimit(this, out);
     }

     return out;
 }

对于上述内容：最关键的是两个变量 struct PID_INC g_PID_Inc 和 struct PID_POS g_PID_Pos ，他们针对抽象类实现了各自的算法。

其中有一个很重要的小技巧，举例：

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

该函数的接口是 struct PID *this ，但是我们内部将他强制转换为 struct PID_POS *pid_Handle ，这是两种不同的数据类型，为什么可以进行转换呢？而且转换后的数据是正确的？

因为对于 C 语言来说，结构体内部的第一个成员的地址和该结构体变量的地址是一样的，所以可以相互转换，实现向下转型，这就是 C 语言的强大之处。

测试

app.c

 struct KernelCtrl {
     struct DTV dtv;
     struct PID *dtvPid;
 };

 static struct KernelCtrl g_kernelCtrl;

 /*
 *********************************************************************************************************
 * Function Name : Kernel_Ctrl_Init
 * Description   : 内核控制初始化
 * Input         : None
 * Output        : None
 * Return        : None
 *********************************************************************************************************
 */
 void Kernel_Ctrl_Init(void)
 {
 #if 1
     /* 增量式 */
     g_kernelCtrl.dtvPid = &g_PID_Inc.pid;
 #else
     /* 位置式 */
     g_kernelCtrl.dtvPid = &g_PID_Pos.pid;
 #endif
 }

只要在初始化时，指定使用哪一种 PID 模式，在调用时两种方式可以使用同一个接口，这样对于用户来说就屏蔽了内部细节。

参考

Pid控制算法-变积分的pid算法的C++实现

【来源】