PID位置式和增量式控制算法（公式+代码）

·公式

（1）位置式：

 （2）增量式：

 

·代码

<.h>

/* USER CODE BEGIN Header */

/* USER CODE END Header */

/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __PID_CTRL_H
#define __PID_CTRL_H

#ifdef __cplusplus
extern "C"
{
#endif

    /* Includes ------------------------------------------------------------------*/

    /* Private includes ----------------------------------------------------------*/
    /* USER CODE BEGIN Includes */
#include "stm32f4xx_hal.h"
    /* USER CODE END Includes */

    /* Exported types ------------------------------------------------------------*/
    /* USER CODE BEGIN ET */

    /**
     * @brief PID_TypeDef
     * @param set_point 目标值
     * @param actualvalue   期望值
     * @param Kp    比例系数P
     * @param Ki    积分系数I
     * @param Kd    微分系数D
     * @param sum_error 偏差累积
     * @param error 第n次偏差
     * @param last_error    第n-1次偏差
     * @param prev_error    第n-2次偏差
     */
    typedef struct
    {
        __IO float set_point;
        __IO float actualvalue;
        __IO float Kp;
        __IO float Ki;
        __IO float Kd;
        __IO float sum_error;
        __IO float error;
        __IO float last_error;
        __IO float prev_error;
    } PID_TypeDef;

    float pid_location_style_ctrl(PID_TypeDef *PID, float input, float integral_limit);
    float pid_incremental_ctrl(PID_TypeDef *PID, float input);

    /* USER CODE END ET */

    /* Exported constants --------------------------------------------------------*/
    /* USER CODE BEGIN EC */

    /* USER CODE END EC */

    /* Exported macro ------------------------------------------------------------*/
    /* USER CODE BEGIN EM */

    /* USER CODE END EM */

    /* Exported functions prototypes ---------------------------------------------*/

    /* USER CODE BEGIN EFP */

    /* USER CODE END EFP */

    /* Private defines -----------------------------------------------------------*/

    /* USER CODE BEGIN Private defines */

    /* USER CODE END Private defines */

#ifdef __cplusplus
}
#endif

#endif /* __PID_CTRL_H */

 

<.c>

#include "pid_ctrl.h"
#include "log.h"

const static char *TAG = "pid_ctrl.c";

/**
 * @brief pid_init
 *
 * @param PID pid struct
 * @param set_point target value
 * @param actualvalue expectations
 * @param Kp
 * @param Ki
 * @param Kd
 */
void pid_init(PID_TypeDef *PID, float set_point, float actualvalue, float Kp, float Ki, float Kd)
{
    PID->set_point = set_point;
    PID->actualvalue = actualvalue;
    PID->Kp = Kp;
    PID->Ki = Ki;
    PID->Kd = Kd;
    MY_LOGI(TAG, "pid init success!\r\n");
}

/**
 * @brief pid_position_ctrl (uk = Kp*e_k + Ki*Σe_j + Kd*(e_k - e_k-1))
 *
 * @param PID
 * @param input
 * @return float
 */
float pid_location_style_ctrl(PID_TypeDef *PID, float input, float integral_limit)
{
    PID->error = PID->set_point - input;
    PID->sum_error += PID->error;
    /* Add input Limiting processing begin */
    if (PID->sum_error >= integral_limit) /* Avoid integral saturation */
    {
        PID->sum_error = integral_limit;
    }
    else if (PID->sum_error <= -integral_limit)
    {
        PID->sum_error = -integral_limit;
    }
    /* Add input Limiting processing end */
    PID->actualvalue = (PID->Kp * PID->error)                        /* Kp*e_k */
                       + (PID->Ki * PID->sum_error)                  /* Ki*Σe_j */
                       + (PID->Kd * (PID->error - PID->last_error)); /* Kd*(e_k - e_k-1) */
    PID->last_error = PID->error;                                    /* update error */

    /* Add output Limiting processing begin */

    /* Add output Limiting processing end */
    return PID->actualvalue;
}

/**
 * @brief pid_incremental_ctrl (uk = Kp*(e_k - e_j) + Ki*e_k + Kd*(e_k - 2e_k-1 +e_k-2))
 *
 * @param PID
 * @param input
 * @return float
 */
float pid_incremental_ctrl(PID_TypeDef *PID, float input)
{
    PID->error = (PID->set_point - input);
    PID->actualvalue += (PID->Kp * (PID->error - PID->last_error))                          /* Kp*(e_k - e_j) */
                        + (PID->Ki * PID->error)                                            /* Ki*e_k */
                        + (PID->Kd * (PID->error - 2 * PID->last_error + PID->prev_error)); /* Kd*(e_k - 2e_k-1 +e_k-2) */
    PID->prev_error = PID->last_error;                                                      /* update n-2 error */
    PID->last_error = PID->error;                                                           /* update n-1 error */

    /* Add output Limiting processing begin */

    /* Add output Limiting processing end */
    return PID->actualvalue;
}

 

[ PS:代码仅供参考，若要使用在实际场景还需自己添加一定的处理。发博客只是记录一下哈 ]