引言

在前面的一篇文章PID控制算法简单说明了PID算法的原理。今天我们一起用C++实现一个简单的PID控制算法吧。
目前实现的是离散位置式PID和增量式PID。

pid_controller.hpp文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
#pragma once

namespace control_algorithm {
class PID {
 public:
  typedef struct {
    float k; /* 控制器增益 */
    float p; /* 比例项增益 */
    float i; /* 积分项增益 */
    float d; /* 微分项增益 */
  } pid_param;

  typedef struct {
    float i_limit;   /* 积分项上限 */
    float d_limit;   /* 微分项限制 */
    float out_limit; /* 输出绝对值限制 */
  } pid_limit;


 private:
  typedef struct {
    float err = 0.0f; /* 误差 */
    float fb = 0.0f;  /* 反馈值 */
    float out = 0.0f; /* 输出 */
  } state;

  pid_param pid_param_;           /* 控制器参数 */
  pid_limit pid_limit_;           /* 控制器限制 */
  state last_;         /* 上一次状态 */
  state previous_; /* 上上一次状态 */

  float interget_; /* 积分项 */

  float dt_min_; /* 最小调用间隔 */

  float last_time_; /* 上一次调用时间 */

 public:
  PID(PID::pid_param& param, PID::pid_limit& limit, float sample_freq);
  ~PID();
  void SetPidParam(PID::pid_param& param);
  void SetPidLimit(PID::pid_limit& limit);
  void SetK(float k); 
  float PositionPIDCalculate(float target, float fb,
                          float dt);  // 输入期望、反馈、抽样周期，计算输出
  float PositionPIDCalculate(float err,
                          float current_time);  // 输入误差、当前时间计算输出

  float IncrementalPIDCalculate(float target, float fb, float dt);
  void Reset();
};
}  // namespace control_algorithm

pid_controller.cpp文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
#include "pid_controller.hpp"

#include <algorithm>

using namespace control_algorithm;

// 构造函数，初始化 PID 参数
PID::PID(PID::pid_param& param, PID::pid_limit& limit, float sample_freq)
    : pid_param_(param), pid_limit_(limit) {
  float dt_min = 1.0f / sample_freq;
  this->dt_min_ = dt_min;
  this->Reset();
  this->SetK(1);
}

void PID::Reset() {
  this->last_ = {0.0f};
  this->previous_ = {0.0f};
  this->interget_ = 0.0f;
}

void PID::SetPidParam(PID::pid_param& param) { this->pid_param_ = param; }

void PID::SetPidLimit(PID::pid_limit& limit) { this->pid_limit_ = limit; }
void PID::SetK(float k) { this->pid_param_.k = k; }

float PID::PositionPIDCalculate(float err, float current_time) {
  /* 计算时间间隔 */
  float dt = current_time - this->last_time_;
  if (dt < 0) {
    return this->last_.out;
  } else if (dt == 0) {
    return this->last_.out;
  }
  /* 加入K项的作用 */
  float k_err = this->pid_param_.k * err;
  /* 计算P项输出 */
  float p_out = k_err * this->pid_param_.p;

  float d_out = (k_err - this->last_.err * this->pid_param_.k) /
                std::max(dt, this->dt_min_);
  d_out *= this->pid_param_.d;
  d_out =
      std::clamp(d_out, -this->pid_limit_.d_limit, this->pid_limit_.d_limit);

  /* 计算I项输出 */
  float interget = this->interget_ + (k_err * dt);
  interget =
      std::clamp(interget, -this->pid_limit_.i_limit, this->pid_limit_.i_limit);
  float i_out = interget * this->pid_param_.i;

  this->interget_ = interget;

  /* 计算PID输出 */
  float output = p_out + d_out + i_out;
  output = std::clamp(output, -this->pid_limit_.out_limit,
                      this->pid_limit_.out_limit);

  this->last_time_ = current_time;
  this->last_.err = err;
  this->last_.out = output;

  return last_.out;
}

float PID::PositionPIDCalculate(float target, float fb, float dt) {
  /* 计算误差值 */
  float err = target - fb;
  /* 加入K项的作用 */
  float k_err = this->pid_param_.k * err;
  /* 计算P项输出 */
  float p_out = k_err * this->pid_param_.p;

  /* 计算D项 */
  /* 改进一下，避免target改变造成err的突变 */
  /* 当目标值不变时，误差的微分等于负的反馈的微分 */
  float k_fb = this->pid_param_.k * fb;

  float d = (k_fb - this->last_.fb * this->pid_param_.k) /
            std::max(dt, this->dt_min_);

  this->last_.err = err;
  this->last_.fb = fb;

  /* 计算D输出 */
  float d_out = -(d * this->pid_param_.d);
  d_out =
      std::clamp(d_out, -this->pid_limit_.d_limit, this->pid_limit_.d_limit);

  /* 计算I项输出 */
  float interget = this->interget_ + (k_err * dt);
  interget =
      std::clamp(interget, -this->pid_limit_.i_limit, this->pid_limit_.i_limit);
  float i_out = interget * this->pid_param_.i;

  this->interget_ = interget;

  /* 计算PID输出 */
  float output = p_out + d_out + i_out;
  output = std::clamp(output, -this->pid_limit_.out_limit,
                      this->pid_limit_.out_limit);

  this->last_.out = output;

  return last_.out;
}

float PID::IncrementalPIDCalculate(float target, float fb, float dt) {
  /* 计算误差值 */
  float err = target - fb;

  /* 加入K项的作用 */
  float k_err = this->pid_param_.k * err;
  float k_last_err = this->pid_param_.k * this->last_.err;
  float k_previous_err = this->pid_param_.k * this->previous_.err;
  /* 计算比例增量 */
  float delta_p =
      this->pid_param_.p * (k_err - k_last_err);

  /* 计算积分增量 */
  float delta_i = this->pid_param_.i * err;

  /* 计算微分增量 */
  float delta_d =
      this->pid_param_.d * (k_err - 2 * k_last_err + k_previous_err);

  /* 增量式PID输出 */
  float output = this->last_.out + delta_p + delta_i + delta_d;

  /* 限制输出 */
  output = std::clamp(output, -this->pid_limit_.out_limit,
                      this->pid_limit_.out_limit);

  /* 更新状态 */
  this->previous_.err = this->last_.err;
  this->last_.err = err;
  this->previous_.out = this->last_.out;
  this->last_.out = output;

  return output;
}

main.cpp文件

在main函数中，实例化pid类一个对象，并调用Calculate函数进行计算，即可。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
#include <iostream>

#include "pid_controller.hpp"

int main() {
  // 定义PID参数和限制
  control_algorithm::PID::pid_param params = {1.0f, 0.1f, 0.01f, 0.05f};
  control_algorithm::PID::pid_limit limits = {10.0f, 5.0f, 20.0f};

  // 采样频率
  float sample_freq = 100.0f;  // 每秒100次采样

  // 实例化PID对象
  control_algorithm::PID pid_controller(params, limits, sample_freq);

  // 模拟输入
  float target = 100.0f;      // 目标值
  float feedback = 90.0f;     // 当前反馈值
  float current_time = 0.0f;  // 当前时间（秒）
  float dt = 0.01f;           // 采样时间间隔（秒）

  // 模拟运行PID控制器
  std::cout << "运行PositionPIDCalculate函数:" << std::endl;
  for (int i = 0; i < 10; ++i) {
    current_time += dt;
    float output = pid_controller.PositionPIDCalculate(target, feedback, dt);
    feedback += output * dt;  // 简单模拟系统响应
    std::cout << "时间: " << current_time << "，目标: " << target
              << "，反馈: " << feedback << "，输出: " << output << std::endl;
  }

  std::cout << "\n运行IncrementalPIDCalculate函数:" << std::endl;
  feedback = 90.0f;  // 重置反馈值
  for (int i = 0; i < 10; ++i) {
    current_time += dt;
    float output = pid_controller.IncrementalPIDCalculate(target, feedback, dt);
    feedback += output * dt;  // 简单模拟系统响应
    std::cout << "时间: " << current_time << "，目标: " << target
              << "，反馈: " << feedback << "，输出: " << output << std::endl;
  }

  return 0;
}