PID 算法

1.1 概述

比例（Proportion）积分（Integral）微分（Differential）控制器（PID控制器或三项控制器）是一种采用反馈的控制回路机制，广泛应用于工业控制系统和需要连续调制控制的各种其他应用。

PID控制器连续计算误差值 e(t) 作为所需设定点(SP) 和测量过程变量(PV)之间的差值，并应用基于比例、积分和导数项（分别表示为P、I和D）的校正，因此得名。

r(t) 是期望的过程值或设定点(SP)，y(t) 是测量的过程值(PV)。

1.2 历史发展

1911年，第一个PID控制器是由Elmer Sperry开发的。

1922 年，俄裔美国工程师尼古拉斯·米诺斯基 ( Nicolas Minorsky)才首次利用理论分析制定了我们现在所说的 PID 或三项控制的正式控制律。米诺斯基当时正在为美国海军研究和设计自动船舶转向系统，他的分析基于对舵手的观察。

他指出，舵手不仅根据当前航向误差，还根据过去的误差以及当前的变化率来驾驶船舶；然后 Minorsky 对此进行了数学处理。他的目标是稳定，而不是一般控制，这大大简化了问题。

1933年，TIC(泰勒仪器公司)实现了完全可调节的前气动控制器。几年后，控制工程师通过将末端返回到一些假值，直到误差不为零，消除了比例控制器中发现的稳态误差。这个返回包含了误差，这被称为比例积分控制器。

1940年，第一个气动PID控制器通过导数动作开发，以减少超调问题。

1942年，Ziegler & Nichols引入了调谐规则，由工程师发现和设置PID控制器的合适参数。

20世纪50年代中期，自动PID控制器在工业上得到了广泛的应用。工业中大多数现代 PID 控制都是作为DCS、PLC 或单片机程序来实现的。

1.3 应用

•火箭的姿态控制

•无人机悬停控制等

•相机稳定器、相机云台

•平衡小车

•汽车的定速巡航控制、转向控制

•发动机转速控制

•3D打印机上的温度控制器

•工业自动化领域，大约95%的闭环操作使用PID控制器。

1.4 与 ON/OFF 型控制器对比

像PID控制器这样的闭环系统包括一个反馈控制系统。该系统利用一个固定点对反馈变量进行评估，从而产生误差信号。在此基础上，它改变系统输出。这个过程将继续，直到误差达到零，否则反馈变量的值就等于一个固定点。

与ON/OFF型控制器相比，该控制器提供了良好的效果。在开/关型控制器中，只需两个条件即可管理系统。大多数暖通空调系统、冰箱都采用这种方法。

例如，在冰箱中，它会冷却内部直到达到所需温度，然后关闭冷却器，直到达到高于所需温度的设定值。一旦工艺值低于固定点，则开启。

类似地，一旦该值高于固定值，它将关闭。这种控制器的输出不稳定，在不动点的区域内振荡频繁。然而，与ON/OFF型控制器相比，PID 控制器更加稳定和准确。

1.6 响应类型

Introduction to PID：

https://docs.wpilib.org/en/stable/docs/software/advanced-controls/introduction/introduction-to-pid.html

由PID控制器驱动的系统通常具有三种类型的响应：欠阻尼、过阻尼和临界阻尼。

•欠阻尼响应在稳定之前围绕参考值振荡。

•过阻尼响应上升缓慢并且不会超过参考值。

•临界阻尼响应具有最快的上升时间，且不会超过参考值。

公式

2.1 PID 系统定义与公式

r(t) setpoint, reference，是期望的过程值或设定值(SP)；

y(t) output, process variable，是测量的过程值，输出值(PV)；

e(t) error，是偏差；

u(t) control effort，是控制量；

PID控制器的显着特点是能够利用比例、积分和微分这三个控制项对控制器输出的影响来进行精确和最优的控制。

PID 控制器，不断计算误差值e(t) 作为所需设定点之间的差异SP=r(t) 和测量的过程变量PV=y(t):e(t)=r(t)−y(t) ,并应用基于比例、积分和导数项的修正。

控制器尝试通过调整控制变量来最小化随时间变化的误差u(t)。manipulated variable (MV)。

From：

http://matlab.fei.tuke.sk/orhs/subory/podklady/pid_controller_slidy.pdf

2.2 PID 数字公式

由于计算机控制是一种采样控制，它只能根据采样时刻的偏差计算控制量，而不能像模拟控制那样连续输出控制量，进行连续控制。由于这一特点，（式 1-1）中的积分项和微分项不能直接使用，必须进行离散化处理。

离散化处理的方法为：以τ作为采样周期，k作为采样序号，则离散采样时间kτ对应着连续时间t，用矩形法数值积分近似代替积分，用一阶后向差分近似代替微分，可作如下近似变换：

2.3 位置式 PID 算法

将（式 2-1）代入（式 1-1），就可以得到离散的 PID 表达式为

将（式 2-1）代入（式 1-2），就可以得到离散的PID 表达式为

积分系数、微分系数做如下替换：

注意：必须使τ为定值，或者变化小到可以忽略，这样P、I、D才是固定常数，才可能调节

2.4 增量式 PID 算法

增量式 PID 控制算法可以通过（式 2－2）推导出。由（式 2－2）可以得到控制器的第 k－1 个采样时刻的输出值为：

由（式 2－3）可以得到控制器的第 k－1 个采样时刻的输出值为：

用（式 2-3）减去（式 2-7）相减并整理，就可以得到增量式 PID 控制算法公式：

由（式 2－8）可以看出，如果计算机控制系统采用恒定的采样周期τ，一旦确定 A、 B、 C，只要使用前后三次测量的偏差值，就可以由（式 2－8）求出控制量。

增量式 PID 控制算法与位置式 PID 算法（式 2－3）相比，只需要保持当前时刻以前三个时刻的偏差值即可，累计误差较小，计算量小的多，因此在实际中得到广泛的应用。

而位置式 PID 控制算法也可以通过增量式控制算法推出递推计算公式：

（式 2-9）就是目前在计算机控制中广泛应用的数字递推 PID 控制算法。

调试技巧

代码实现

python

From：

https://blog.csdn.net/weixin_43863487/article/details/124604299

import numpy as npimport matplotlib.pyplot as plt
class PositionPID(object):    """位置式PID算法实现"""
    def __init__(self, target, cur_val, dt, max, min, p, i, d) -> None:        self.dt = dt  # 循环时间间隔        self._max = max  # 最大输出限制，规避过冲        self._min = min  # 最小输出限制        self.k_p = p  # 比例系数        self.k_i = i  # 积分系数        self.k_d = d  # 微分系数
        self.target = target  # 目标值        self.cur_val = cur_val  # 算法当前PID位置值，第一次为设定的初始位置        self._pre_error = 0  # t-1 时刻误差值        self._integral = 0  # 误差积分值    def calculate(self):        """        计算t时刻PID输出值cur_val        """        error = self.target - self.cur_val  # 计算当前误差        # 比例项        p_out = self.k_p * error          # 积分项        self._integral += (error * self.dt)        i_out = self.k_i * self._integral        # 微分项        derivative = (error - self._pre_error) / self.dt        d_out = self.k_d * derivative
        # t 时刻pid输出        output = p_out + i_out + d_out
        # 限制输出值        if output > self._max:            output = self._max        elif output < self._min:            output = self._min                self._pre_error = error        self.cur_val = output        return self.cur_val
    def fit_and_plot(self, count = 200):        """        使用PID拟合setPoint        """        counts = np.arange(count)        outputs = []
        for i in counts:            outputs.append(self.calculate())            print('Count %3d: output: %f' % (i, outputs[-1]))
        print('Done')        # print(outputs)                plt.figure()        plt.axhline(self.target, c='red')        plt.plot(counts, np.array(outputs), 'b.')        plt.ylim(min(outputs) - 0.1 * min(outputs), max(outputs) + 0.1 * max(outputs))        plt.plot(outputs)        plt.show()
pid = PositionPID(10, -5, 0.5, 100, -100, 0.2, 0.1, 0.01)pid.fit_and_plot(150)

c/c++

From：

https://blog.csdn.net/skythinker616/article/details/123019829

//首先定义PID结构体用于存放一个PID的数据typedef struct{     float kp,ki,kd;//三个系数    float error,lastError;//误差、上次误差    float integral,maxIntegral;//积分、积分限幅    float output,maxOutput;//输出、输出限幅}PID; //用于初始化pid参数的函数void PID_Init(PID *pid,float p,float i,float d,float maxI,float maxOut){    pid->kp=p;    pid->ki=i;    pid->kd=d;    pid->maxIntegral=maxI;    pid->maxOutput=maxOut;} //进行一次pid计算//参数为(pid结构体,目标值,反馈值)，计算结果放在pid结构体的output成员中void PID_Calc(PID *pid,float reference,float feedback){   //更新数据    pid->lastError=pid->error;//将旧error存起来    pid->error=reference-feedback;//计算新error    //计算微分    float dout=(pid->error-pid->lastError)*pid->kd;    //计算比例    float pout=pid->error*pid->kp;    //计算积分    pid->integral+=pid->error*pid->ki;    //积分限幅    if(pid->integral > pid->maxIntegral) pid->integral=pid->maxIntegral;    else if(pid->integral < -pid->maxIntegral) pid->integral=-pid->maxIntegral;    //计算输出    pid->output=pout+dout+pid->integral;    //输出限幅    if(pid->output > pid->maxOutput) pid->output=pid->maxOutput;    else if(pid->output < -pid->maxOutput) pid->output=-pid->maxOutput;} PID mypid;//创建一个PID结构体变量 int main(){    //...这里有些其他初始化代码    PID_Init(&mypid,10,1,5,800,1000);//初始化PID参数    while(1)//进入循环运行    {        float feedbackValue=...;//这里获取到被控对象的反馈值        float targetValue=...;//这里获取到目标值        PID_Calc(&mypid,targetValue,feedbackValue);//进行PID计算，结果在output成员变量中        设定执行器输出大小(mypid.output);        delay(10);//等待一定时间再开始下一次循环    }}

单环效果

串级PID的C语言代码

//此处需要插入上面的单级PID相关代码 //串级PID的结构体，包含两个单级PIDtypedef struct{    PID inner;//内环    PID outer;//外环    float output;//串级输出，等于inner.output}CascadePID; //串级PID的计算函数//参数(PID结构体,外环目标值,外环反馈值,内环反馈值)void PID_CascadeCalc(CascadePID *pid,float outerRef,float outerFdb,float innerFdb){    PID_Calc(&pid->outer,outerRef,outerFdb);//计算外环    PID_Calc(&pid->inner,pid->outer.output,innerFdb);//计算内环    pid->output=pid->inner.output;//内环输出就是串级PID的输出} CascadePID mypid;//创建串级PID结构体变量 int main(){    //...其他初始化代码    PID_Init(&mypid.inner,10,0,0,0,1000);//初始化内环参数    PID_Init(&mypid.outer,5,0,5,0,100);//初始化外环参数    while(1)//进入循环运行    {        float outerTarget=...;//获取外环目标值        float outerFeedback=...;//获取外环反馈值        float innerFeedback=...;//获取内环反馈值        PID_CascadeCalc(&mypid,outerTarget,outerFeedback,innerFeedback);//进行PID计算        设定执行机构输出大小(mypid.output);        delay(10);//延时一段时间    }}

双环效果

双环控制

串联

from：

https://blog.csdn.net/weixin_43058521/article/details/115503356

如果电机控制既要控制速度又要控制位置，因为速度和位置相关，所以需要串联。

并联

from：

https://blog.csdn.net/weixin_43058521/article/details/115503356

姿态角度与速度间无相关性，各自单独算一路控制

示例

循迹小车

https://blog.csdn.net/m0_38106923/article/details/109545445

可见小车的循迹效果。

野火中步进电机位置速度双环控制

https://doc.embedfire.com/motor/motor_tutorial/zh/latest/improve_part/step_motor_double_loop_control.html

步进电机速度环控制实现和 10. 步进电机位置环控制实现介绍了单环控制已经能很好地提高电机的性能了，但是仍有其局限性。

使用速度环精确控制了电机的转速，但是停止的位置难以精确控制；

使用位置环精确控制了电机转过的角度，却不得不人为限制速度来防止堵转。

位置环和速度环双环控制，既实现位置的精确调节又实现速度的自动控制。

该控制下，编码器不仅起到了反馈位置的作用，也起到了反馈速度的作用。

调参技巧：在PID参数整定时，采取先内环再外环的方法，也就是先单独使用速度环控制，得到满意的参数后， 再把位置环套在外面，整定位置环参数，最后根据整体效果对速度环参数进行微调。

bsp_pid.h

/*pid*/typedef struct{  float target_val;     //目标值  float actual_val;     //实际值  float err;            //定义当前偏差值  float err_next;       //定义下一个偏差值  float err_last;       //定义上一个偏差值  float Kp, Ki, Kd;     //定义比例、积分、微分系数}_pid;

bsp_stepper_ctrl.h

/*宏定义*//*******************************************************/#define TIM_STEP_FREQ     (SystemCoreClock/TIM_PRESCALER) // 频率ft值
/*电机单圈参数*/#define STEP_ANGLE                          1.8f                 //步进电机的步距角 单位：度#define FSPR              (360.0f/STEP_ANGLE)  //步进电机的一圈所需脉冲数
#define MICRO_STEP        32                                         //细分器细分数#define SPR               (FSPR*MICRO_STEP)    //细分后一圈所需脉冲数
#define PULSE_RATIO       (float)(SPR/ENCODER_TOTAL_RESOLUTION)//步进电机单圈脉冲数与编码器单圈脉冲的比值#define SAMPLING_PERIOD   50                   //PID采样频率，单位Hz
#define MOVE_CTRL         0.1f                   //启用速度环控制量#define TARGET_DISP       20                   //步进电机运动时的目标圈数，单位：转#define TARGET_SPEED_MAX  800                 // 目标速度的最大值
typedef struct {  unsigned char stepper_dir : 1;               //步进电机方向  unsigned char stepper_running : 1;           //步进电机运行状态  unsigned char MSD_ENA : 1;                   //驱动器使能状态}__SYS_STATUS;

bsp_stepper_ctrl.c-增量式PID算法实现-增量式PID

/**   * @brief  增量式PID算法实现   * @param  val：当前实际值   * @note   无   * @retval 通过PID计算后的输出   */ float PID_realize(_pid *pid, float temp_val){   /*传入实际值*/   pid->actual_val = temp_val;   /*计算目标值与实际值的误差*/   pid->err=pid->target_val-pid->actual_val;
   /*PID算法实现*/   float increment_val = pid->Kp*(pid->err - pid->err_next) + pid->Ki*pid->err + pid->Kd*(pid->err - 2 * pid->err_next + pid->err_last);   /*传递误差*/   pid->err_last = pid->err_next;   pid->err_next = pid->err;   /*返回增量值*/   return increment_val; }

bsp_stepper_ctrl.c-步进电机位置速度双闭环控制

 /**   * @brief  步进电机位置速度双闭环控制   * @retval 无   * @note   基本定时器中断内调用   */ void Stepper_Ctrl(void){   /* 编码器相关变量 */   static __IO float last_count = 0;   __IO float capture_count = 0;   __IO float capture_per_unit = 0;   /* 经过pid计算后的期望值 */   static __IO float speed_cont_val = 0.0f;   static __IO float move_cont_val = 0.0f;   static int cont_val = 0;
   /* 当电机运动时才启动pid计算 */   if((sys_status.MSD_ENA == 1) && (sys_status.stepper_running == 1))   {     /* 计算编码器脉冲数 */     capture_count = (int)__HAL_TIM_GET_COUNTER(&TIM_EncoderHandle) + (encoder_overflow_count * ENCODER_TIM_PERIOD);     /* 计算速度环的传入值 */     capture_per_unit = capture_count - last_count;     last_count = capture_count;
     /* 编码器脉冲累计值作为实际值传入位置环pid控制器 */     move_cont_val += PID_realize_move(&move_pid, (float)capture_count);// 进行 PID 计算     /* 判断运动方向 */     move_cont_val > 0 ? (MOTOR_DIR(CW)) : (MOTOR_DIR(CCW));     /* 判断是否启用速度环 */     if (fabsf(move_cont_val) >= MOVE_CTRL)     {       /* 传递位置环计算值，便于计算*/       cont_val = move_cont_val;
       /* 目标速度上限处理 */       if (cont_val > TARGET_SPEED_MAX)       {         cont_val = TARGET_SPEED_MAX;       }       else if (cont_val < -TARGET_SPEED_MAX)       {         cont_val = -TARGET_SPEED_MAX;       }
 #if defined(PID_ASSISTANT_EN)       int32_t temp = cont_val;       set_computer_value(SEED_TARGET_CMD, CURVES_CH2, &temp, 1);     // 给通道 2 发送目标值 #endif       /* 设定速度的目标值 */       set_pid_target(&speed_pid, cont_val);       /* 单位时间内的编码器脉冲数作为实际值传入速度环pid控制器 */       speed_cont_val += PID_realize_speed(&speed_pid, (float)capture_per_unit);// 进行 PID 计算       /* 由于OC_Pulse_num为uint16_t变量，取速度环输出值的绝对值进行后续计算*/       cont_val = fabsf(speed_cont_val);       /* 计算比较计数器的值 */       OC_Pulse_num = ((uint16_t)(TIM_STEP_FREQ / (cont_val * PULSE_RATIO * SAMPLING_PERIOD))) >> 1;     }     else     {       /* 计算比较计数器的值 */       OC_Pulse_num = ((uint16_t)(TIM_STEP_FREQ / ((float)move_cont_val * PULSE_RATIO))) >> 1;     } #if PID_ASSISTANT_EN     int Temp_ch2 = capture_per_unit;    // 上位机需要整数参数，转换一下     int Temp_ch1 = capture_count;     set_computer_value(SEED_FACT_CMD, CURVES_CH2, &Temp_ch2, 1);  // 给通道 1 发送实际值     // 给通道 2 发送实际值     set_computer_value(SEED_FACT_CMD, CURVES_CH1, &Temp_ch1, 1);     // 给通道 1 发送实际值
 #else     printf("实际值：%d，目标值：%.0f\r\n", capture_per_unit, pid.target_val);// 打印实际值和目标值 #endif   }   else   {     /*停机状态所有参数清零*/     last_count = 0;     speed_cont_val = 0;     move_cont_val = 0;     speed_pid.actual_val = 0;     speed_pid.err = 0;     speed_pid.err_last = 0;     speed_pid.err_next = 0;     move_pid.actual_val = 0;     move_pid.err = 0;     move_pid.err_last = 0;     move_pid.err_next = 0;   } }

main

 /**   * @brief  主函数   * @param  无   * @retval 无   */ int main(void){   /* 初始化系统时钟为168MHz */   SystemClock_Config();   /*初始化USART 配置模式为 115200 8-N-1，中断接收*/   DEBUG_USART_Config();   printf("欢迎使用野火 电机开发板 步进电机位置速度双环控制 例程\r\n");   printf("按下按键3启动和停止电机\r\n");   /* 初始化时间戳 */   HAL_InitTick(5);   /*按键中断初始化*/   Key_GPIO_Config();   /*led初始化*/   LED_GPIO_Config();   /* 初始化基本定时器定时，20ms产生一次中断 */   TIMx_Configuration();   /* 编码器接口初始化 */   Encoder_Init();   /*步进电机初始化*/   stepper_Init();   /* 上电默认停止电机 */   Set_Stepper_Stop();   /* PID算法参数初始化 */   PID_param_init(); //  MOTOR_DIR(CW);
   /* 目标位置转换为编码器的脉冲数作为pid目标值 */   move_pid.target_val = TARGET_DISP * ENCODER_TOTAL_RESOLUTION;   int32_t Temp = TARGET_DISP * ENCODER_TOTAL_RESOLUTION; #if PID_ASSISTANT_EN   set_computer_value(SEED_STOP_CMD, CURVES_CH1, NULL, 0);    // 同步上位机的启动按钮状态   set_computer_value(SEED_TARGET_CMD, CURVES_CH1, &Temp, 1);// 给通道 1 发送目标值 #endif
   while(1)   {     /* 扫描KEY1，启动电机 */     if( Key_Scan(KEY1_GPIO_PORT,KEY1_PIN) == KEY_ON  )     {     #if PID_ASSISTANT_EN       Set_Stepper_Start();       set_computer_value(SEED_START_CMD, CURVES_CH1, NULL, 0);// 同步上位机的启动按钮状态     #else       Set_Stepper_Start();     #endif     }     /* 扫描KEY2，停止电机 */     if( Key_Scan(KEY2_GPIO_PORT,KEY2_PIN) == KEY_ON  )     {     #if PID_ASSISTANT_EN       Set_Stepper_Stop();       set_computer_value(SEED_STOP_CMD, CURVES_CH1, NULL, 0);// 同步上位机的启动按钮状态     #else       Set_Stepper_Stop();     #endif     }     /* 扫描KEY3，增大目标位置*/     if( Key_Scan(KEY3_GPIO_PORT,KEY3_PIN) == KEY_ON  )     {       /* 目标位置增加48000，对应电机位置增加20圈 */       move_pid.target_val += 48000;
     #if PID_ASSISTANT_EN       int temp = move_pid.target_val;       set_computer_value(SEED_TARGET_CMD, CURVES_CH1, &temp, 1);// 给通道 1 发送目标值     #endif     }     /* 扫描KEY4，减小目标位置 */     if( Key_Scan(KEY4_GPIO_PORT,KEY4_PIN) == KEY_ON  )     {       /* 目标位置减小48000，对应电机位置减少20圈 */       move_pid.target_val -= 48000;
     #if PID_ASSISTANT_EN       int temp = move_pid.target_val;       set_computer_value(SEED_TARGET_CMD, CURVES_CH1, &temp, 1);// 给通道 1 发送目标值     #endif     }   } }

补充知识点

模拟量数字化

实际数字化应用中，PID 系统中的积分项和微分项需要进行离散化处理。

类似的典型应用有数字示波器。对于数字示波器来说它无法直接量化模拟信号，替代的办法就是持续周期性采样，然后将得到的一系列采样点显示出来，当采样速率越高，显示的图像越真实，这就是数学中极限的与微分的思想。

香农（Shannon） 采样定律

•定理内容

香农取样定理是针对有限带宽函数的，为了不失真地恢复模拟信号，采样频率应该不小于模拟信号频谱中最高频率的2倍。