[Control] Add pid controller with integral clamping anti windup

modules/control/common/BUILD

@@ -82,6 +82,22 @@ cc_library(
     ],
 )
 
+cc_library(
+    name = "pid_IC_controller",
+    srcs = [
+        "pid_IC_controller.cc",
+    ],
+    hdrs = [
+        "pid_IC_controller.h",
+    ],
+    deps = [
+        "//modules/common:log",
+        ":pid_controller",
+        "//modules/common/math",
+        "//modules/control/proto:control_proto",
+    ],
+)
+
 cc_library(
     name = "trajectory_analyzer",
     srcs = [
@@ -186,6 +202,21 @@ cc_test(
     ],
 )
 
+cc_test(
+    name = "pid_IC_controller_test",
+    size = "small",
+    srcs = [
+        "pid_IC_controller_test.cc",
+    ],
+    data = ["//modules/control:control_testdata"],
+    deps = [
+        ":pid_IC_controller",
+        "//modules/common/util",
+        "//modules/control/proto:control_proto",
+        "@gtest//:main",
+    ],
+)
+
 cc_test(
     name = "trajectory_analyzer_test",
     size = "small",

modules/control/common/pid_IC_controller.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/control/common/pid_IC_controller.h"
+
+#include <cmath>
+
+#include "modules/common/log.h"
+
+namespace apollo {
+namespace control {
+
+double PIDICController::Control(const double error, const double dt) {
+  if (dt <= 0) {
+    AWARN << "dt <= 0, will use the last output";
+    return previous_output_;
+  }
+  double diff = 0;
+  double output = 0;
+
+  if (first_hit_) {
+    first_hit_ = false;
+  } else {
+    diff = (error - previous_error_) / dt;
+  }
+  // integral clamping
+  if (!integrator_enabled_) {
+    integral_ = 0;
+  } else {
+    double u = error * kp_ + integral_ + error * dt * ki_ + diff * kd_;
+    if (((error * u) > 0) &&
+        ((u > saturation_high_) || (u < saturation_low_))) {
+      // Do not update integral_
+    } else {
+      integral_ += error * dt * ki_;
+    }
+  }
+
+  previous_error_ = error;
+  output = error * kp_ + integral_ + diff * kd_;  // Ki already applied
+  previous_output_ = output;
+  return output;
+}
+
+}  // namespace control
+}  // namespace apollo

modules/control/common/pid_IC_controller.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file pid_controller.h
+ * @brief Defines the PIDICController class.
+ */
+
+#ifndef MODULES_CONTROL_COMMON_PID_INTEGRAL_CLAMPING_CONTROLLER_H_
+#define MODULES_CONTROL_COMMON_PID_INTEGRAL_CLAMPING_CONTROLLER_H_
+
+#include "modules/control/common/pid_controller.h"
+#include "modules/control/proto/pid_conf.pb.h"
+
+/**
+ * @namespace apollo::control
+ * @brief apollo::control
+ */
+namespace apollo {
+namespace control {
+
+/**
+ * @class PIDICController
+ * @brief A proportional–integral–derivative controller for speed and steering
+with integral-clamping-anti-windup
+ */
+class PIDICController : public PIDController {
+ public:
+  /**
+   * @brief compute control value based on the error,
+   with integral-clamping-anti-windup
+   * @param error error value, the difference between
+   * a desired value and a measured value
+   * @param dt sampling time interval
+   * @return control value based on PID terms
+   */
+  virtual double Control(const double error, const double dt);
+
+  /**
+   * @brief get saturation status
+   * @return saturation status
+   */
+  int SaturationStatus() const;
+
+  /**
+   * @brief get status that if integrator is hold
+   * @return if integrator is hold return true
+   */
+  bool integrator_hold() const;
+
+ private:
+};
+
+}  // namespace control
+}  // namespace apollo
+
+#endif  // MODULES_CONTROL_COMMON_PID_INTEGRAL_CLAMPING_CONTROLLER_H_

modules/control/common/pid_IC_controller_test.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/control/common/pid_IC_controller.h"
+
+#include <string>
+
+#include "gtest/gtest.h"
+#include "modules/common/util/file.h"
+#include "modules/control/proto/control_conf.pb.h"
+#include "modules/control/proto/pid_conf.pb.h"
+
+namespace apollo {
+namespace control {
+
+class PidICControllerTest : public ::testing::Test {
+ public:
+  virtual void SetUp() {
+    std::string control_conf_file =
+        "modules/control/testdata/conf/lincoln.pb.txt";
+    CHECK(::apollo::common::util::GetProtoFromFile(control_conf_file,
+                                                   &control_conf_));
+    lon_controller_conf_ = control_conf_.lon_controller_conf();
+  }
+
+ protected:
+  ControlConf control_conf_;
+  LonControllerConf lon_controller_conf_;
+};
+
+TEST_F(PidICControllerTest, StationPidController) {
+  PidConf pid_conf = lon_controller_conf_.station_pid_conf();
+  PIDICController pid_IC_controller;
+  pid_IC_controller.Init(pid_conf);
+  pid_IC_controller.Reset();
+  double dt = 0.01;
+  EXPECT_NEAR(pid_IC_controller.Control(0.0, dt), 0.0, 1e-6);
+  pid_IC_controller.Reset();
+  EXPECT_NEAR(pid_IC_controller.Control(0.1, dt), 0.01, 1e-6);
+  pid_IC_controller.Reset();
+  double control_value = pid_IC_controller.Control(-0.1, dt);
+  EXPECT_NEAR(control_value, -0.01, 1e-6);
+  dt = 0.0;
+  EXPECT_EQ(pid_IC_controller.Control(100, dt), control_value);
+  EXPECT_EQ(pid_IC_controller.IntegratorHold(), false);
+}
+TEST_F(PidICControllerTest, SpeedPidController) {
+  PidConf pid_conf = lon_controller_conf_.low_speed_pid_conf();
+  PIDICController pid_IC_controller;
+  pid_IC_controller.Init(pid_conf);
+  pid_IC_controller.Reset();
+  double dt = 0.01;
+  EXPECT_NEAR(pid_IC_controller.Control(0.0, dt), 0.0, 1e-6);
+  pid_IC_controller.Reset();
+  EXPECT_NEAR(pid_IC_controller.Control(0.1, dt), 0.1505, 1e-6);
+  pid_IC_controller.Reset();
+  EXPECT_NEAR(pid_IC_controller.Control(-0.1, dt), -0.1505, 1e-6);
+  pid_IC_controller.Reset();
+  EXPECT_NEAR(pid_IC_controller.Control(500.0, dt), 750.3, 1e-6);
+  pid_IC_controller.Reset();
+  EXPECT_EQ(pid_IC_controller.SaturationStatus(), 1);
+  pid_IC_controller.Reset();
+  EXPECT_NEAR(pid_IC_controller.Control(-500.0, dt), -750.3, 1e-6);
+  EXPECT_EQ(pid_IC_controller.SaturationStatus(), -1);
+}
+}  // namespace control
+}  // namespace apollo

modules/control/proto/pid_conf.proto

@@ -9,4 +9,5 @@ message PidConf {
   optional double ki = 4;
   optional double kd = 5;
   optional double kaw = 6 [default = 0.0];
+  optional double output_saturation_level = 7;
 }