目录
介绍及公式
pid算法用于简单的控制非常方便，它常用于底层的控制。
因此我们常常把pid控制做成固定的库，方便调用。
库文件?
说明：这里的库程序，是基于esp32开发板写的，其他开发板类似，主要区别就是获取当前时间戳不一样。其余的算法是一样的。如果你想移植到其他平台，只需要修改获取时间戳的函数即可。时间戳单位是微秒。
位置式
pid.h
#define PID_H
#include "time_utils.h"
#include "foc_utils.h"
/**
* PID controller class
*/
class PIDController
{
public:
/**
*
* @param P - Proportional gain
* @param I - Integral gain
* @param D - Derivative gain
* @param ramp - Maximum speed of change of the output value
* @param limit - Maximum output value
*/
PIDController(float P, float I, float D, float ramp, float limit);
~PIDController() = default;
float operator() (float error);
float P; //!< Proportional gain
float I; //!< Integral gain
float D; //!< Derivative gain
float output_ramp; //!< Maximum speed of change of the output value
float limit; //!< Maximum output value
protected:
float integral_prev; //!< last integral component value
float error_prev; //!< last tracking error value
unsigned long timestamp_prev; //!< Last execution timestamp
float output_prev; //!< last pid output value
};
pid.cpp
#include "pid.h"
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
: P(P)
, I(I)
, D(D)
, output_ramp(ramp) // output derivative limit [volts/second]
, limit(limit) // output supply limit [volts]
, integral_prev(0.0)
, error_prev(0.0)
, output_prev(0.0)
{
timestamp_prev = _micros();
}
// PID controller function
float PIDController::operator() (float error){
// 计算上次调用到现在的时间Ts(秒)
unsigned long timestamp_now = _micros();
float Ts = (timestamp_now - timestamp_prev) * 1e-6;
// 快速修复异常 (micros overflow)，如果溢出或者出现其他异常，则Ts=0.001ms
if(Ts <= 0 || Ts > 0.5) Ts = 1e-3;
// u(s) = (P + I/s + Ds)e(s) //计算输入函数u(s)
// Discrete implementations //分元素计算
// proportional part //比例项
// u_p = P *e(k) //u_p = P * e(k)
float proportional = P * error;
// Tustin transform of the integral part //积分项的双线性变换
// u_ik = u_ik_1 + I*Ts*(ek + ek _1)/2 //u_ik = u_ik_1 + I*Ts*(ek + ek _1)/2
float integral = integral_prev + I*Ts*(error + error_prev)*0.5;
// antiwindup - limit the output
integral = _constrain(integral, -limit, limit);
// Discrete derivation
// u_dk = D(ek - ek_1)/Ts //微分项，除以Ts，是在求微分
float derivative = D*(error - error_prev)/Ts;
// sum all the components
float output = proportional + integral + derivative;
// antiwindup - limit the output variable
output = _constrain(output, -limit, limit);
// if output ramp defined //这里解决超调问题。
if(output_ramp > 0){
// limit the acceleration by ramping the output
float output_rate = (output - output_prev)/Ts;
if (output_rate > output_ramp)
output = output_prev + output_ramp*Ts;
else if (output_rate < -output_ramp)
output = output_prev - output_ramp*Ts;
}
// saving for the next pass
integral_prev = integral;
output_prev = output;
error_prev = error;
timestamp_prev = timestamp_now;
return output;
}
增量式
pid.h
#define PID_H
#include "time_utils.h"
#include "foc_utils.h"
/**
* PID controller class
*/
class PIDController
{
public:
/**
*
* @param P - Proportional gain
* @param I - Integral gain
* @param D - Derivative gain
* @param ramp - Maximum speed of change of the output value
* @param limit - Maximum output value
*/
PIDController(float P, float I, float D, float ramp, float limit);
~PIDController() = default;
float operator() (float error);
float P; //!< 比例增益
float I; //!< 积分增益
float D; //!< 微分增益
float output_ramp; //!< 输出值最大变化率
float limit; //!< 输出限制(最大输出绝对值)
protected:
float error_prev; //!< k-1时刻误差
float error_prev1; //!< k-2时刻误差
unsigned long timestamp_prev; //!< 上次执行计算时的时间戳
float output_prev; //!< k-1时刻输出
float output; //
};pid.cpp
#include "pid.h"
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
: P(P)
, I(I)
, D(D)
, output_ramp(ramp) // output derivative limit [volts/second]
, limit(limit) // 输出限制(常用pwm分辨率)
, error_prev(0.0)
, output_prev(0.0)
, output(0.0)
{
timestamp_prev = _micros();
}
// PID controller function
float PIDController::operator() (float error){
// 计算上次调用到现在的时间Ts(秒)
unsigned long timestamp_now = _micros();
float Ts = (timestamp_now - timestamp_prev) * 1e-6;
// 快速修复异常 (micros overflow)，如果溢出或者出现其他异常，则Ts=0.001ms
if(Ts <= 0 || Ts > 0.5) Ts = 1e-3;
// 计算输出函数u(s)
// u(k) = u(k-1) + Kp * (e(k)-e(k-1)) + Ki * Ts * (e(k)+e(k-1))/2 + Kd * (e(k)-2*e(k-1)+e(k-2))/Ts
// 分别计算
// 比例项: u_p = P * (e(k)-e(k-1))
float proportional = P * (error - error_prev);
// 积分项: u_ik = I * Ts * (e(k)+e(k-1))/2
float integral = I * Ts * (error + error_prev)*0.5;
// 微分项: u_dk = D * (e(k)-2*e(k-1)+e(k-1))/Ts
float derivative = D * (error - 2 * error_prev + error_prev1)/Ts;
// 各部分加和
output += proportional + integral + derivative;
// antiwindup - limit the output variable
output = _constrain(output, -limit, limit);
// if output ramp defined //这里解决超调问题。
if(output_ramp > 0){
// limit the acceleration by ramping the output
float output_rate = (output - output_prev)/Ts;
if (output_rate > output_ramp)
output = output_prev + output_ramp*Ts;
else if (output_rate < -output_ramp)
output = output_prev - output_ramp*Ts;
}
// 存储数据以便下次调用
output_prev = output;
error_prev1 = error_prev;
error_prev = error;
timestamp_prev = timestamp_now;
return output;
}用法
程序用起来比较简单。
1.把库文件放进自己的工程
2.在主程序中包含pid库文件
3.新建pid控制器并初始化pid参数
PIDController pidv{2.8, 8.0, 0.01, 50000.0, 255.0}; //单纯速度环4.计算error
自己计算一下error，比如我期望电机转速为5r/s,现在通过编码器检测到电机实际速度为3r/s。则error=5-3=2r/s.
5.调用pid控制器计算输出
通过下面一行代码，把误差放进去，pid控制器就会自己计算出来输出。
output = pidv(error);6.将输出施加到系统
比如我是pid调速的，输入的是有效电压（通过调节pwm占空比调节），输出的是速度。
那么我们拿到pid控制器输出的数值，作为系统的输入，也就是pwm占空比施加到系统即可。
