流程

输入
输入
比例调节
比例调节
积分调节
积分调节
微分调节
微分调节
执行机构
执行机构
测量元件
测量元件
+
+
-
-
输出
输出
Viewer does not support full SVG 1.1

代码

限制速度加速度的PID控制

 1
 2#include <algorithm>
 3#include <cmath>
 4#include <chrono>
 5#include <ros/time.h>
 6#include "Configure.hpp"
 7
 8// todo 自动计算dt
 9
10class PIDController
11{
12public:
13    void init(double kpIn, double kiIn, double kdIn, double maxVIn, double maxAIn);
14    double calculate(double setpoint, double current, double currentV);
15
16private:
17    bool initialized = false;
18    double kp;
19    double ki;
20    double kd;
21    double maxV;
22    double maxA;
23    double prevError;
24    double integral;
25    double prevTime;
26};
27
28#define RATE 10.0
29
30void PIDController::init(double kpIn, double kiIn, double kdIn, double maxVIn, double maxAIn)
31{
32    kp = kpIn;
33    ki = kiIn;
34    kd = kdIn;
35    maxV = maxVIn;
36    maxA = maxAIn;
37    prevError = 0;
38    integral = 0;
39    initialized = true;
40    prevTime = ros::Time::now().toSec() - 1.0 / RATE;
41}
42
43double PIDController::calculate(double setpoint, double current, double currentV)
44{
45    if (!initialized) {
46        std::abort();
47    }
48    double dt = ros::Time::now().toSec() - prevTime;
49    prevTime = ros::Time::now().toSec();
50
51    double error = setpoint - current;
52    integral += error * dt;
53    double derivative = (error - prevError) / dt;
54    prevError = error;
55    double output = kp * error + ki * integral + kd * derivative;
56    // 限制最大速度
57    if (std::abs(output) > maxV) {
58        output = std::copysign(maxV, output);
59    }
60    // 限制最大加速度
61    else if (std::abs(output - currentV) / dt > maxA) {
62        output = currentV + std::copysign(maxA * dt, output - currentV);
63    }
64    return output;
65}