Apollo 10.0
自动驾驶开放平台
apollo::control::PIDICController类 参考

A proportional-integral-derivative controller for speed and steering with integral-clamping-anti-windup 更多...

#include <pid_IC_controller.h>

类 apollo::control::PIDICController 继承关系图:
apollo::control::PIDICController 的协作图:

Public 成员函数

virtual double Control (const double error, const double dt)
 compute control value based on the error, with integral-clamping-anti-windup
 
virtual int OutputSaturationStatus ()
 
- Public 成员函数 继承自 apollo::control::PIDController
void Init (const PidConf &pid_conf)
 initialize pid controller
 
void SetPID (const PidConf &pid_conf)
 set pid controller coefficients for the proportional, integral, and derivative
 
void Reset ()
 reset variables for pid controller
 
void Reset_integral ()
 reset integral for pid controller
 
virtual ~PIDController ()=default
 
int IntegratorSaturationStatus () const
 get saturation status
 
bool IntegratorHold () const
 get status that if integrator is hold
 
void SetIntegratorHold (bool hold)
 set whether to hold integrator component at its current value.
 

额外继承的成员函数

- Protected 属性 继承自 apollo::control::PIDController
double kp_ = 0.0
 
double ki_ = 0.0
 
double kd_ = 0.0
 
double kaw_ = 0.0
 
double previous_error_ = 0.0
 
double previous_output_ = 0.0
 
double integral_ = 0.0
 
double integrator_saturation_high_ = 0.0
 
double integrator_saturation_low_ = 0.0
 
bool first_hit_ = false
 
bool integrator_enabled_ = false
 
bool integrator_hold_ = false
 
int integrator_saturation_status_ = 0
 
double output_saturation_high_ = 0.0
 
double output_saturation_low_ = 0.0
 
int output_saturation_status_ = 0
 

详细描述

A proportional-integral-derivative controller for speed and steering with integral-clamping-anti-windup

在文件 pid_IC_controller.h40 行定义.

成员函数说明

◆ Control()

double apollo::control::PIDICController::Control ( const double  error,
const double  dt 
)
virtual

compute control value based on the error, with integral-clamping-anti-windup

参数
errorerror value, the difference between a desired value and a measured value
dtsampling time interval
返回
control value based on PID terms

重载 apollo::control::PIDController .

在文件 pid_IC_controller.cc28 行定义.

28 {
29 if (dt <= 0) {
30 AWARN << "dt <= 0, will use the last output";
31 return previous_output_;
32 }
33 double diff = 0;
34 double output = 0;
35
36 if (first_hit_) {
37 first_hit_ = false;
38 } else {
39 diff = (error - previous_error_) / dt;
40 }
41 // integral clamping
43 integral_ = 0;
44 } else {
45 double u = error * kp_ + integral_ + error * dt * ki_ + diff * kd_;
46 if (((error * u) > 0) &&
48 } else {
49 // Only update integral then
50 integral_ += error * dt * ki_;
51 }
52 }
53
54 previous_error_ = error;
55 output = error * kp_ + integral_ + diff * kd_;
56
57 if (output >= output_saturation_high_) {
59 } else if (output <= output_saturation_low_) {
61 } else {
63 }
64
65 output = common::math::Clamp(error * kp_ + integral_ + diff * kd_,
67 output_saturation_low_); // Ki already applied
68 previous_output_ = output;
69 return output;
70}
#define AWARN
Definition log.h:43
T Clamp(const T value, T bound1, T bound2)
Clamp a value between two bounds.
Definition math_utils.h:155

◆ OutputSaturationStatus()

int apollo::control::PIDICController::OutputSaturationStatus ( )
virtual

在文件 pid_IC_controller.cc72 行定义.

72 {
74}

该类的文档由以下文件生成: