From bd97c2b77b2da02a0a00007194b1c72282679408 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Thu, 28 May 2026 23:19:48 -0500 Subject: [PATCH] feat(adrc): Add Active Disturbance Rejection Control (ADRC) component --- .github/workflows/build.yml | 2 + .github/workflows/upload_components.yml | 1 + components/adrc/CMakeLists.txt | 4 + components/adrc/README.md | 21 + components/adrc/example/CMakeLists.txt | 21 + components/adrc/example/README.md | 24 + components/adrc/example/main/CMakeLists.txt | 2 + components/adrc/example/main/adrc_example.cpp | 173 ++++ components/adrc/idf_component.yml | 20 + components/adrc/include/adrc.hpp | 789 ++++++++++++++++++ components/adrc/src/adrc.cpp | 1 + doc/Doxyfile | 2 + doc/en/adrc.rst | 147 ++++ doc/en/adrc_example.md | 2 + doc/en/index.rst | 1 + 15 files changed, 1210 insertions(+) create mode 100755 components/adrc/CMakeLists.txt create mode 100755 components/adrc/README.md create mode 100755 components/adrc/example/CMakeLists.txt create mode 100755 components/adrc/example/README.md create mode 100755 components/adrc/example/main/CMakeLists.txt create mode 100755 components/adrc/example/main/adrc_example.cpp create mode 100755 components/adrc/idf_component.yml create mode 100644 components/adrc/include/adrc.hpp create mode 100755 components/adrc/src/adrc.cpp create mode 100755 doc/en/adrc.rst create mode 100755 doc/en/adrc_example.md diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 2399acfd4..19b55bd5c 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -21,6 +21,8 @@ jobs: test: - path: 'components/adc/example' target: esp32 + - path: 'components/adrc/example' + target: esp32 - path: 'components/ads1x15/example' target: esp32 - path: 'components/ads7138/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index d9b1d8b6f..05a481f7c 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -28,6 +28,7 @@ jobs: # components: | components/adc + components/adrc components/ads1x15 components/ads7138 components/adxl345 diff --git a/components/adrc/CMakeLists.txt b/components/adrc/CMakeLists.txt new file mode 100755 index 000000000..41697b32d --- /dev/null +++ b/components/adrc/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES base_component) diff --git a/components/adrc/README.md b/components/adrc/README.md new file mode 100755 index 000000000..737270e18 --- /dev/null +++ b/components/adrc/README.md @@ -0,0 +1,21 @@ +# ADRC (Active Disturbance Rejection Control) Component + +[![Badge](https://components.espressif.com/components/espp/adrc/badge.svg)](https://components.espressif.com/components/espp/adrc) + +The `adrc` component provides reusable active disturbance rejection control +implementations for ESPP applications. + +## Features + +- Linear first-order ADRC +- Linear second-order ADRC +- Han-style nonlinear first-order ADRC +- Han-style nonlinear second-order ADRC +- Han tracking differentiator utility for smoothing references and estimating + reference rate +- Thread-safe configuration and state updates + +## Example + +The [example](./example) shows how to use the ADRC classes against simulated +first-order and second-order plants with injected disturbances. diff --git a/components/adrc/example/CMakeLists.txt b/components/adrc/example/CMakeLists.txt new file mode 100755 index 000000000..6a48bcb6c --- /dev/null +++ b/components/adrc/example/CMakeLists.txt @@ -0,0 +1,21 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py adrc" + CACHE STRING + "List of components to include" + ) + +project(adrc_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/adrc/example/README.md b/components/adrc/example/README.md new file mode 100755 index 000000000..f9c9551c7 --- /dev/null +++ b/components/adrc/example/README.md @@ -0,0 +1,24 @@ +# ADRC Example + +This example shows how to use the `espp::LinearAdrcFirstOrder`, +`espp::LinearAdrcSecondOrder`, `espp::HanAdrcFirstOrder`, and +`espp::HanAdrcSecondOrder` classes against simulated plants with injected +disturbances. + +## How to use example + +### Build and Flash + +Build the project and flash it to the target, then run monitor tool to view +serial output: + +``` +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +(To exit the serial monitor, type ``Ctrl-]``.) + +See the Getting Started Guide for full steps to configure and use ESP-IDF to +build projects. diff --git a/components/adrc/example/main/CMakeLists.txt b/components/adrc/example/main/CMakeLists.txt new file mode 100755 index 000000000..a941e22ba --- /dev/null +++ b/components/adrc/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/adrc/example/main/adrc_example.cpp b/components/adrc/example/main/adrc_example.cpp new file mode 100755 index 000000000..de7b1a0f0 --- /dev/null +++ b/components/adrc/example/main/adrc_example.cpp @@ -0,0 +1,173 @@ +#include + +#include "adrc.hpp" + +using namespace std::chrono_literals; + +namespace { +struct FirstOrderPlant { + float pole{-1.2f}; + float gain{1.0f}; + float disturbance{0.0f}; + float y{0.0f}; + + float update(float input, float dt) { + y += dt * (pole * y + gain * input + disturbance); + return y; + } +}; + +struct SecondOrderPlant { + float natural_frequency{3.5f}; + float damping_ratio{0.45f}; + float gain{1.0f}; + float disturbance{0.0f}; + float x1{0.0f}; + float x2{0.0f}; + + float update(float input, float dt) { + auto acceleration = -2.0f * damping_ratio * natural_frequency * x2 - + natural_frequency * natural_frequency * x1 + gain * input + disturbance; + x1 += dt * x2; + x2 += dt * acceleration; + return x1; + } +}; +} // namespace + +extern "C" void app_main(void) { + constexpr float dt = 0.001f; + constexpr int num_steps = 5000; + constexpr int print_interval = 100; + fmt::print("ADRC example\n"); + + { + fmt::print("Linear first-order ADRC example\n"); + //! [adrc linear first order example] + espp::LinearAdrcFirstOrder controller({ + .b0 = 1.0f, + .controller_bandwidth = 8.0f, + .observer_bandwidth = 24.0f, + .output_min = -4.0f, + .output_max = 4.0f, + }); + FirstOrderPlant plant; + for (int i = 0; i < num_steps; ++i) { + auto time = i * dt; + auto reference = time >= 0.25f ? 1.0f : 0.0f; + plant.disturbance = time >= 2.5f ? -0.8f : 0.0f; + auto control = controller.update(reference, plant.y, dt); + auto output = plant.update(control, dt); + if (i % print_interval == 0 || i == num_steps - 1) { + auto state = controller.get_state(); + fmt::print("t={:0.2f}s ref={:0.2f} y={:0.3f} u={:0.3f} z2={:0.3f}\n", time, reference, + output, state.output, state.z2); + } + } + //! [adrc linear first order example] + } + + { + fmt::print("Linear second-order ADRC example\n"); + //! [adrc linear second order example] + espp::LinearAdrcSecondOrder controller({ + .b0 = 1.0f, + .controller_bandwidth = 10.0f, + .observer_bandwidth = 36.0f, + .output_min = -8.0f, + .output_max = 8.0f, + }); + SecondOrderPlant plant; + for (int i = 0; i < num_steps; ++i) { + auto time = i * dt; + auto reference = time >= 0.25f ? 1.0f : 0.0f; + plant.disturbance = time >= 2.5f ? 1.2f : 0.0f; + auto control = controller.update(reference, plant.x1, dt); + auto output = plant.update(control, dt); + if (i % print_interval == 0 || i == num_steps - 1) { + auto state = controller.get_state(); + fmt::print("t={:0.2f}s ref={:0.2f} y={:0.3f} u={:0.3f} z3={:0.3f}\n", time, reference, + output, state.output, state.z3); + } + } + //! [adrc linear second order example] + } + + { + fmt::print("Han first-order ADRC example\n"); + //! [adrc han first order example] + espp::HanAdrcFirstOrder controller({ + .b0 = 1.0f, + .controller_gain = 7.0f, + .observer_bandwidth = 22.0f, + .observer_alpha = 0.5f, + .controller_alpha = 0.8f, + .fal_delta = 0.01f, + .use_tracking_differentiator = true, + .tracking_config = + { + .tracking_bandwidth = 45.0f, + .filter_factor = 5.0f, + }, + .output_min = -4.0f, + .output_max = 4.0f, + }); + FirstOrderPlant plant; + for (int i = 0; i < num_steps; ++i) { + auto time = i * dt; + auto reference = time >= 0.25f ? 1.0f : 0.0f; + plant.disturbance = time >= 2.5f ? -0.8f : 0.0f; + auto control = controller.update(reference, plant.y, dt); + auto output = plant.update(control, dt); + if (i % print_interval == 0 || i == num_steps - 1) { + auto state = controller.get_state(); + fmt::print("t={:0.2f}s ref={:0.2f} td={:0.3f} y={:0.3f} u={:0.3f}\n", time, reference, + state.td_reference, output, state.output); + } + } + //! [adrc han first order example] + } + + { + fmt::print("Han second-order ADRC example\n"); + //! [adrc han second order example] + espp::HanAdrcSecondOrder controller({ + .b0 = 1.0f, + .position_gain = 30.0f, + .rate_gain = 6.0f, + .observer_bandwidth = 32.0f, + .observer_alpha1 = 0.5f, + .observer_alpha2 = 0.25f, + .controller_alpha1 = 0.8f, + .controller_alpha2 = 1.5f, + .fal_delta = 0.01f, + .use_tracking_differentiator = true, + .tracking_config = + { + .tracking_bandwidth = 60.0f, + .filter_factor = 5.0f, + }, + .output_min = -8.0f, + .output_max = 8.0f, + }); + SecondOrderPlant plant; + for (int i = 0; i < num_steps; ++i) { + auto time = i * dt; + auto reference = time >= 0.25f ? 1.0f : 0.0f; + plant.disturbance = time >= 2.5f ? 1.2f : 0.0f; + auto control = controller.update(reference, plant.x1, dt); + auto output = plant.update(control, dt); + if (i % print_interval == 0 || i == num_steps - 1) { + auto state = controller.get_state(); + fmt::print("t={:0.2f}s ref={:0.2f} td={:0.3f} y={:0.3f} u={:0.3f} z3={:0.3f}\n", time, + reference, state.td_reference, output, state.output, state.z3); + } + } + //! [adrc han second order example] + } + + fmt::print("ADRC example complete!\n"); + while (true) { + std::this_thread::sleep_for(1s); + } +} diff --git a/components/adrc/idf_component.yml b/components/adrc/idf_component.yml new file mode 100755 index 000000000..f307a7368 --- /dev/null +++ b/components/adrc/idf_component.yml @@ -0,0 +1,20 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Active disturbance rejection control (ADRC) component for ESP-IDF" +url: "https://github.com/esp-cpp/espp/tree/main/components/adrc" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/adrc.html" +examples: + - path: example +tags: + - cpp + - Component + - Control + - ADRC + - Math +dependencies: + idf: + version: ">=5.0" + espp/base_component: ">=1.0" diff --git a/components/adrc/include/adrc.hpp b/components/adrc/include/adrc.hpp new file mode 100644 index 000000000..229f280d3 --- /dev/null +++ b/components/adrc/include/adrc.hpp @@ -0,0 +1,789 @@ +#pragma once + +#include +#include +#include + +#include "base_component.hpp" + +/** + * @file adrc.hpp + * @brief Active disturbance rejection control utilities and controllers. + * + * This header provides a Han tracking differentiator plus first-order and + * second-order ADRC controllers in both linear and Han-style nonlinear forms. + * + * ADRC combines three ideas: + * - a reference shaper / tracking differentiator, + * - an extended state observer (ESO), and + * - a feedback law that cancels the estimated disturbance. + * + * In practice, the observer lumps together plant uncertainty, friction, load + * torque changes, unmodeled dynamics, and external disturbances into an extra + * state (`z2` or `z3`). The controller then subtracts that estimate from the + * commanded control effort. + * + * Typical robotics usage: + * - use a first-order ADRC around speed, current, or other approximately + * first-order actuator dynamics; + * - use a second-order ADRC around position, heading, or other loops where the + * commanded input primarily affects acceleration; + * - use the Han nonlinear variants when you want gentler small-signal behavior + * and stronger response on large errors than the linear forms provide. + * + * @note `dt` is expressed in seconds and should be reasonably consistent from + * update to update. + * @note `b0` is the estimated plant input gain. Its sign should be correct and + * its magnitude should be in the right ballpark, but ADRC does not require a + * perfect plant model. + */ +namespace espp { +namespace detail { +inline constexpr float adrc_epsilon = 1e-6f; + +inline float safe_nonzero(float value) { + if (std::fabs(value) >= adrc_epsilon) { + return value; + } + return value < 0.0f ? -adrc_epsilon : adrc_epsilon; +} + +inline float sign(float value) { return (0.0f < value) - (value < 0.0f); } + +inline float fal(float error, float alpha, float delta) { + auto safe_delta = std::max(delta, adrc_epsilon); + auto abs_error = std::fabs(error); + if (abs_error <= safe_delta) { + return error / std::pow(safe_delta, 1.0f - alpha); + } + return std::copysign(std::pow(abs_error, alpha), error); +} + +inline float fsg(float value, float delta) { + return 0.5f * (sign(value + delta) - sign(value - delta)); +} + +inline float fst(float x1, float x2, float r, float h) { + auto safe_h = std::max(h, adrc_epsilon); + auto safe_r = std::max(r, adrc_epsilon); + auto d = safe_r * safe_h * safe_h; + auto a0 = safe_h * x2; + auto y = x1 + a0; + auto a1 = std::sqrt(d * (d + 8.0f * std::fabs(y))); + auto a2 = a0 + sign(y) * (a1 - d) * 0.5f; + auto sy = fsg(y, d); + auto a = (a0 + y - a2) * sy + a2; + auto sa = fsg(a, d); + return -safe_r * ((a / d - sign(a)) * sa + sign(a)); +} +} // namespace detail + +/** + * @brief Han tracking differentiator for smoothing a reference trajectory and + * estimating its rate. + * + * The tracking differentiator (TD) is commonly used at the front of an ADRC + * loop to turn a stepped reference into a smooth internal command. That reduces + * jerk, limits observer shock, and provides an internally consistent reference + * derivative for second-order loops. + * + * For robotics motor control, the TD is useful when a planner or user interface + * produces abrupt setpoint changes but the plant should accelerate smoothly. In + * a position loop, for example, the TD can convert a position step into a + * smoother position and velocity command pair. + * + * @par Tuning + * `tracking_bandwidth` controls how aggressively the TD chases the target. + * Larger values follow the reference more tightly but can reintroduce abrupt + * motion. `filter_factor` scales the internal `h0` term and can be increased to + * further soften the commanded trajectory. + * + * \section adrc_ex1 Linear First-Order ADRC Example + * \snippet adrc_example.cpp adrc linear first order example + * \section adrc_ex2 Linear Second-Order ADRC Example + * \snippet adrc_example.cpp adrc linear second order example + * \section adrc_ex3 Han First-Order ADRC Example + * \snippet adrc_example.cpp adrc han first order example + * \section adrc_ex4 Han Second-Order ADRC Example + * \snippet adrc_example.cpp adrc han second order example + */ +class HanTrackingDifferentiator : public BaseComponent { +public: + /// Configuration for the Han tracking differentiator. + struct Config { + float tracking_bandwidth; ///< Tracking aggressiveness parameter, commonly noted `r`. + float filter_factor{5.0f}; ///< Multiplier applied to `dt` for the Han `h0` term. + espp::Logger::Verbosity log_level{ + espp::Logger::Verbosity::WARN}; ///< Verbosity for the internal logger. + }; + + /// State of the tracking differentiator. + struct State { + float position{0.0f}; ///< Smoothed reference value. + float rate{0.0f}; ///< Estimated first derivative of the smoothed reference. + }; + + /// Construct the tracking differentiator. + /// @param config Tracking differentiator configuration. + explicit HanTrackingDifferentiator(const Config &config) + : BaseComponent("HanTrackingDifferentiator", config.log_level) + , config_(config) {} + + /// Change the tracking differentiator configuration. + /// @param config New configuration. + /// @param reset_state If true, reset the differentiator state after applying the config. + void set_config(const Config &config, bool reset_state = true) { + std::lock_guard lk(mutex_); + config_ = config; + if (reset_state) { + clear(); + } + } + + /// Reset the differentiator state. + /// @param value Initial position value after reset. + void clear(float value = 0.0f) { + std::lock_guard lk(mutex_); + state_.position = value; + state_.rate = 0.0f; + } + + /// Advance the tracking differentiator. + /// @param target Target reference value to follow. + /// @param dt Timestep in seconds. + /// @return Updated tracking differentiator state. + State update(float target, float dt) { + std::lock_guard lk(mutex_); + if (dt <= 0.0f) { + return state_; + } + auto h0 = std::max(config_.filter_factor * dt, dt); + auto accel = detail::fst(state_.position - target, state_.rate, config_.tracking_bandwidth, h0); + state_.position += dt * state_.rate; + state_.rate += dt * accel; + return state_; + } + + /// Advance the tracking differentiator. + /// @param target Target reference value to follow. + /// @param dt Timestep in seconds. + /// @return Updated tracking differentiator state. + State operator()(float target, float dt) { return update(target, dt); } + + /// Get the most recently computed state. + /// @return Current differentiator state. + State get_state() const { return state_; } + + /// Get the active differentiator configuration. + /// @return Current configuration. + const Config &get_config() const { return config_; } + +protected: + Config config_; + State state_; + mutable std::recursive_mutex mutex_; +}; + +/** + * @brief Linear first-order active disturbance rejection controller. + * + * This controller targets plants that can be approximated as a first-order + * system in the controlled variable, such as a velocity loop whose command acts + * roughly like torque or acceleration after inner current regulation. + * + * The linear ESO estimates: + * - `z1`: the controlled output, and + * - `z2`: the lumped disturbance and modeling error. + * + * The resulting control law is simple to tune with bandwidth parameters and is + * often a good first ADRC choice when migrating from PI or PID speed control. + * + * @par Typical robotics use + * Use this class for wheel speed control, conveyor speed control, or other + * actuator loops where the measured state is dominated by a single pole. In a + * cascaded servo, this is commonly the outer speed loop above a faster current + * controller. + * + * @par Tuning + * Start with a conservative `controller_bandwidth`, then choose + * `observer_bandwidth` several times higher so the ESO settles faster than the + * controller. `b0` should match the sign of the actuator path and roughly scale + * command to output-rate change. If the controller saturates immediately, + * reduce the bandwidth or improve the `b0` estimate before increasing gains. + */ +class LinearAdrcFirstOrder : public BaseComponent { +public: + /// Configuration for the first-order linear ADRC controller. + struct Config { + float b0; ///< Estimated control gain of the plant. + float controller_bandwidth; ///< Closed-loop bandwidth used for the state error feedback gain. + float observer_bandwidth; ///< Observer bandwidth used for the linear extended state observer. + float output_min; ///< Minimum output command. + float output_max; ///< Maximum output command. + espp::Logger::Verbosity log_level{ + espp::Logger::Verbosity::WARN}; ///< Verbosity for the internal logger. + }; + + /// Observer and controller state. + struct State { + float measurement{0.0f}; ///< Most recent measured output. + float reference{0.0f}; ///< Most recent reference input. + float z1{0.0f}; ///< Estimated plant output state. + float z2{0.0f}; ///< Estimated total disturbance state. + float output{0.0f}; ///< Most recent controller output. + }; + + /// Construct the first-order linear ADRC controller. + /// @param config Controller configuration. + explicit LinearAdrcFirstOrder(const Config &config) + : BaseComponent("LinearAdrcFirstOrder", config.log_level) + , config_(config) {} + + /// Change the controller configuration. + /// @param config New controller configuration. + /// @param reset_state If true, reset the observer and control state after applying the config. + void set_config(const Config &config, bool reset_state = true) { + std::lock_guard lk(mutex_); + config_ = config; + if (reset_state) { + clear(); + } + } + + /// Reset the observer and controller state. + /// @param measurement Initial measured output for the observer state. + void clear(float measurement = 0.0f) { + std::lock_guard lk(mutex_); + state_.measurement = measurement; + state_.reference = measurement; + state_.z1 = measurement; + state_.z2 = 0.0f; + state_.output = 0.0f; + } + + /// Update the controller. + /// @param reference Desired output reference. + /// @param measurement Measured output. + /// @param dt Timestep in seconds. + /// @return Control output. + float update(float reference, float measurement, float dt) { + std::lock_guard lk(mutex_); + if (dt <= 0.0f) { + return state_.output; + } + + auto wo = std::max(config_.observer_bandwidth, detail::adrc_epsilon); + auto beta1 = 2.0f * wo; + auto beta2 = wo * wo; + auto b0 = detail::safe_nonzero(config_.b0); + auto error = state_.z1 - measurement; + + state_.measurement = measurement; + state_.reference = reference; + state_.z1 += dt * (state_.z2 - beta1 * error + config_.b0 * state_.output); + state_.z2 += dt * (-beta2 * error); + + auto k1 = std::max(config_.controller_bandwidth, detail::adrc_epsilon); + auto u0 = k1 * (reference - state_.z1); + state_.output = std::clamp((u0 - state_.z2) / b0, config_.output_min, config_.output_max); + return state_.output; + } + + /// Update the controller. + /// @param reference Desired output reference. + /// @param measurement Measured output. + /// @param dt Timestep in seconds. + /// @return Control output. + float operator()(float reference, float measurement, float dt) { + return update(reference, measurement, dt); + } + + /// Get the current controller state. + /// @return Current state. + State get_state() const { return state_; } + + /// Get the active configuration. + /// @return Current configuration. + const Config &get_config() const { return config_; } + +protected: + Config config_; + State state_; + mutable std::recursive_mutex mutex_; +}; + +/** + * @brief Linear second-order active disturbance rejection controller. + * + * This controller targets plants whose commanded input primarily affects the + * second derivative of the controlled variable. Typical examples include motor + * position loops, gimbal angle loops, and heading loops where the command acts + * like torque or angular acceleration. + * + * The linear ESO estimates: + * - `z1`: position, + * - `z2`: rate, and + * - `z3`: the lumped disturbance and unmodeled dynamics. + * + * @par Typical robotics use + * Use this class when your feedback variable is position but your actuator + * fundamentally produces acceleration. In a mobile robot or arm joint, this is + * usually the position controller that sits above a faster velocity or torque + * loop. + * + * @par Tuning + * `controller_bandwidth` shapes the closed-loop stiffness and damping. The + * observer is usually tuned faster than the controller so `z3` converges before + * it dominates the motion. If a trajectory generator already provides a + * velocity feedforward term, pass it through the four-argument `update()` + * overload as `reference_rate`. + */ +class LinearAdrcSecondOrder : public BaseComponent { +public: + /// Configuration for the second-order linear ADRC controller. + struct Config { + float b0; ///< Estimated control gain of the plant. + float controller_bandwidth; ///< Closed-loop bandwidth used to derive the feedback gains. + float observer_bandwidth; ///< Observer bandwidth used for the linear extended state observer. + float output_min; ///< Minimum output command. + float output_max; ///< Maximum output command. + espp::Logger::Verbosity log_level{ + espp::Logger::Verbosity::WARN}; ///< Verbosity for the internal logger. + }; + + /// Observer and controller state. + struct State { + float measurement{0.0f}; ///< Most recent measured position. + float reference{0.0f}; ///< Most recent reference position. + float reference_rate{0.0f}; ///< Most recent reference rate. + float z1{0.0f}; ///< Estimated plant position state. + float z2{0.0f}; ///< Estimated plant rate state. + float z3{0.0f}; ///< Estimated total disturbance state. + float output{0.0f}; ///< Most recent controller output. + }; + + /// Construct the second-order linear ADRC controller. + /// @param config Controller configuration. + explicit LinearAdrcSecondOrder(const Config &config) + : BaseComponent("LinearAdrcSecondOrder", config.log_level) + , config_(config) {} + + /// Change the controller configuration. + /// @param config New controller configuration. + /// @param reset_state If true, reset the observer and control state after applying the config. + void set_config(const Config &config, bool reset_state = true) { + std::lock_guard lk(mutex_); + config_ = config; + if (reset_state) { + clear(); + } + } + + /// Reset the observer and controller state. + /// @param measurement Initial measured position for the observer state. + /// @param measurement_rate Initial measured rate estimate for the observer state. + void clear(float measurement = 0.0f, float measurement_rate = 0.0f) { + std::lock_guard lk(mutex_); + state_.measurement = measurement; + state_.reference = measurement; + state_.reference_rate = 0.0f; + state_.z1 = measurement; + state_.z2 = measurement_rate; + state_.z3 = 0.0f; + state_.output = 0.0f; + } + + /// Update the controller using an explicit reference rate. + /// @param reference Desired position reference. + /// @param reference_rate Desired reference rate. + /// @param measurement Measured position. + /// @param dt Timestep in seconds. + /// @return Control output. + float update(float reference, float reference_rate, float measurement, float dt) { + std::lock_guard lk(mutex_); + if (dt <= 0.0f) { + return state_.output; + } + + auto wo = std::max(config_.observer_bandwidth, detail::adrc_epsilon); + auto beta1 = 3.0f * wo; + auto beta2 = 3.0f * wo * wo; + auto beta3 = wo * wo * wo; + auto b0 = detail::safe_nonzero(config_.b0); + auto error = state_.z1 - measurement; + + state_.measurement = measurement; + state_.reference = reference; + state_.reference_rate = reference_rate; + state_.z1 += dt * (state_.z2 - beta1 * error); + state_.z2 += dt * (state_.z3 - beta2 * error + config_.b0 * state_.output); + state_.z3 += dt * (-beta3 * error); + + auto wc = std::max(config_.controller_bandwidth, detail::adrc_epsilon); + auto kp = wc * wc; + auto kd = 2.0f * wc; + auto u0 = kp * (reference - state_.z1) + kd * (reference_rate - state_.z2); + state_.output = std::clamp((u0 - state_.z3) / b0, config_.output_min, config_.output_max); + return state_.output; + } + + /// Update the controller assuming a zero reference-rate command. + /// @param reference Desired position reference. + /// @param measurement Measured position. + /// @param dt Timestep in seconds. + /// @return Control output. + float update(float reference, float measurement, float dt) { + return update(reference, 0.0f, measurement, dt); + } + + /// Update the controller assuming a zero reference-rate command. + /// @param reference Desired position reference. + /// @param measurement Measured position. + /// @param dt Timestep in seconds. + /// @return Control output. + float operator()(float reference, float measurement, float dt) { + return update(reference, measurement, dt); + } + + /// Get the current controller state. + /// @return Current state. + State get_state() const { return state_; } + + /// Get the active configuration. + /// @return Current configuration. + const Config &get_config() const { return config_; } + +protected: + Config config_; + State state_; + mutable std::recursive_mutex mutex_; +}; + +/** + * @brief Han-style nonlinear first-order active disturbance rejection + * controller. + * + * This variant replaces the linear error terms with Han's nonlinear `fal()` + * functions and can optionally prefilter the reference with a tracking + * differentiator. The nonlinear feedback is often attractive when you want high + * authority on large errors without making the loop excessively sharp around + * the operating point. + * + * @par Typical robotics use + * Use this class for speed loops that must reject large step disturbances but + * remain well-behaved near zero speed, for example wheel or propeller speed + * control with friction, stiction, or battery-voltage variation. + * + * @par Tuning + * `controller_gain` is the main feedback gain. `observer_bandwidth` sets the + * speed of the nonlinear ESO. `controller_alpha`, `observer_alpha`, and + * `fal_delta` determine how strongly the loop transitions between linear + * small-error behavior and nonlinear large-error behavior. Increase + * `fal_delta` if the loop is too twitchy around the setpoint, especially with + * noisy sensors. + */ +class HanAdrcFirstOrder : public BaseComponent { +public: + /// Configuration for the first-order nonlinear ADRC controller. + struct Config { + float b0; ///< Estimated control gain of the plant. + float controller_gain; ///< Nonlinear state error feedback gain. + float observer_bandwidth; ///< Observer bandwidth used to derive nonlinear ESO gains. + float observer_alpha{0.5f}; ///< `fal()` exponent used by the observer disturbance state. + float controller_alpha{0.8f}; ///< `fal()` exponent used by the nonlinear feedback term. + float fal_delta{0.01f}; ///< Small-signal linear region width used by `fal()`. + bool use_tracking_differentiator{true}; ///< Enable reference smoothing through the Han TD. + HanTrackingDifferentiator::Config tracking_config{ + .tracking_bandwidth = 40.0f, + .filter_factor = 5.0f}; ///< Tracking differentiator configuration. + float output_min; ///< Minimum output command. + float output_max; ///< Maximum output command. + espp::Logger::Verbosity log_level{ + espp::Logger::Verbosity::WARN}; ///< Verbosity for the internal logger. + }; + + /// Observer and controller state. + struct State { + float measurement{0.0f}; ///< Most recent measured output. + float reference{0.0f}; ///< Most recent reference input. + float td_reference{0.0f}; ///< Smoothed reference from the tracking differentiator. + float z1{0.0f}; ///< Estimated plant output state. + float z2{0.0f}; ///< Estimated total disturbance state. + float output{0.0f}; ///< Most recent controller output. + }; + + /// Construct the first-order nonlinear ADRC controller. + /// @param config Controller configuration. + explicit HanAdrcFirstOrder(const Config &config) + : BaseComponent("HanAdrcFirstOrder", config.log_level) + , config_(config) + , tracking_differentiator_(config.tracking_config) {} + + /// Change the controller configuration. + /// @param config New controller configuration. + /// @param reset_state If true, reset the observer and control state after applying the config. + void set_config(const Config &config, bool reset_state = true) { + std::lock_guard lk(mutex_); + config_ = config; + tracking_differentiator_.set_config(config.tracking_config, reset_state); + if (reset_state) { + clear(); + } + } + + /// Reset the observer, controller, and tracking differentiator state. + /// @param measurement Initial measured output for the observer state. + void clear(float measurement = 0.0f) { + std::lock_guard lk(mutex_); + state_.measurement = measurement; + state_.reference = measurement; + state_.td_reference = measurement; + state_.z1 = measurement; + state_.z2 = 0.0f; + state_.output = 0.0f; + tracking_differentiator_.clear(measurement); + } + + /// Update the controller. + /// @param reference Desired output reference. + /// @param measurement Measured output. + /// @param dt Timestep in seconds. + /// @return Control output. + float update(float reference, float measurement, float dt) { + std::lock_guard lk(mutex_); + if (dt <= 0.0f) { + return state_.output; + } + + auto td_state = config_.use_tracking_differentiator + ? tracking_differentiator_.update(reference, dt) + : HanTrackingDifferentiator::State{.position = reference, .rate = 0.0f}; + auto wo = std::max(config_.observer_bandwidth, detail::adrc_epsilon); + auto beta1 = 2.0f * wo; + auto beta2 = wo * wo; + auto b0 = detail::safe_nonzero(config_.b0); + auto error = state_.z1 - measurement; + + state_.measurement = measurement; + state_.reference = reference; + state_.td_reference = td_state.position; + state_.z1 += dt * (state_.z2 - beta1 * error + config_.b0 * state_.output); + state_.z2 += dt * (-beta2 * detail::fal(error, config_.observer_alpha, config_.fal_delta)); + + auto feedback = + config_.controller_gain * + detail::fal(td_state.position - state_.z1, config_.controller_alpha, config_.fal_delta); + state_.output = std::clamp((feedback - state_.z2) / b0, config_.output_min, config_.output_max); + return state_.output; + } + + /// Update the controller. + /// @param reference Desired output reference. + /// @param measurement Measured output. + /// @param dt Timestep in seconds. + /// @return Control output. + float operator()(float reference, float measurement, float dt) { + return update(reference, measurement, dt); + } + + /// Get the current controller state. + /// @return Current state. + State get_state() const { return state_; } + + /// Get the active configuration. + /// @return Current configuration. + const Config &get_config() const { return config_; } + + /// Get the current tracking differentiator state. + /// @return Tracking differentiator state. + HanTrackingDifferentiator::State get_tracking_state() const { + return tracking_differentiator_.get_state(); + } + +protected: + Config config_; + State state_; + HanTrackingDifferentiator tracking_differentiator_; + mutable std::recursive_mutex mutex_; +}; + +/** + * @brief Han-style nonlinear second-order active disturbance rejection + * controller. + * + * This controller combines a second-order ESO with nonlinear state error + * feedback and an optional Han tracking differentiator. It is the most flexible + * controller in this component and is useful when a position-like loop must + * reject significant disturbances while still handling large setpoint changes + * gracefully. + * + * @par Typical robotics use + * Use this class for position control of motor-driven joints, pan/tilt systems, + * steering axes, or mobile-robot heading loops when load variation and + * unmodeled friction are too large for simple PD or PID tuning to stay robust. + * + * @par Tuning + * `position_gain` and `rate_gain` play roles similar to proportional and + * derivative action in the nonlinear feedback law. `observer_bandwidth` should + * usually be faster than the desired closed-loop motion, but if encoder noise + * is significant you may need to back it off and rely more on the tracking + * differentiator to keep the reference smooth. `controller_alpha1` and + * `controller_alpha2` change how strongly large position and rate errors are + * amplified relative to small errors. + */ +class HanAdrcSecondOrder : public BaseComponent { +public: + /// Configuration for the second-order nonlinear ADRC controller. + struct Config { + float b0; ///< Estimated control gain of the plant. + float position_gain; ///< Nonlinear feedback gain for position error. + float rate_gain; ///< Nonlinear feedback gain for rate error. + float observer_bandwidth; ///< Observer bandwidth used to derive nonlinear ESO gains. + float observer_alpha1{0.5f}; ///< `fal()` exponent used for the ESO middle state. + float observer_alpha2{0.25f}; ///< `fal()` exponent used for the ESO disturbance state. + float controller_alpha1{0.8f}; ///< `fal()` exponent used for the position error. + float controller_alpha2{1.5f}; ///< `fal()` exponent used for the rate error. + float fal_delta{0.01f}; ///< Small-signal linear region width used by `fal()`. + bool use_tracking_differentiator{true}; ///< Enable reference smoothing through the Han TD. + HanTrackingDifferentiator::Config tracking_config{ + .tracking_bandwidth = 60.0f, + .filter_factor = 5.0f}; ///< Tracking differentiator configuration. + float output_min; ///< Minimum output command. + float output_max; ///< Maximum output command. + espp::Logger::Verbosity log_level{ + espp::Logger::Verbosity::WARN}; ///< Verbosity for the internal logger. + }; + + /// Observer and controller state. + struct State { + float measurement{0.0f}; ///< Most recent measured position. + float reference{0.0f}; ///< Most recent reference position. + float td_reference{0.0f}; ///< Smoothed reference position. + float td_reference_rate{0.0f}; ///< Smoothed reference rate. + float z1{0.0f}; ///< Estimated plant position state. + float z2{0.0f}; ///< Estimated plant rate state. + float z3{0.0f}; ///< Estimated total disturbance state. + float output{0.0f}; ///< Most recent controller output. + }; + + /// Construct the second-order nonlinear ADRC controller. + /// @param config Controller configuration. + explicit HanAdrcSecondOrder(const Config &config) + : BaseComponent("HanAdrcSecondOrder", config.log_level) + , config_(config) + , tracking_differentiator_(config.tracking_config) {} + + /// Change the controller configuration. + /// @param config New controller configuration. + /// @param reset_state If true, reset the observer and control state after applying the config. + void set_config(const Config &config, bool reset_state = true) { + std::lock_guard lk(mutex_); + config_ = config; + tracking_differentiator_.set_config(config.tracking_config, reset_state); + if (reset_state) { + clear(); + } + } + + /// Reset the observer, controller, and tracking differentiator state. + /// @param measurement Initial measured position for the observer state. + /// @param measurement_rate Initial measured rate estimate for the observer state. + void clear(float measurement = 0.0f, float measurement_rate = 0.0f) { + std::lock_guard lk(mutex_); + state_.measurement = measurement; + state_.reference = measurement; + state_.td_reference = measurement; + state_.td_reference_rate = 0.0f; + state_.z1 = measurement; + state_.z2 = measurement_rate; + state_.z3 = 0.0f; + state_.output = 0.0f; + tracking_differentiator_.clear(measurement); + } + + /// Update the controller using an externally supplied reference rate when the + /// tracking differentiator is disabled. + /// @param reference Desired position reference. + /// @param reference_rate Desired reference rate. + /// @param measurement Measured position. + /// @param dt Timestep in seconds. + /// @return Control output. + float update(float reference, float reference_rate, float measurement, float dt) { + std::lock_guard lk(mutex_); + if (dt <= 0.0f) { + return state_.output; + } + + auto td_state = config_.use_tracking_differentiator + ? tracking_differentiator_.update(reference, dt) + : HanTrackingDifferentiator::State{ + .position = reference, + .rate = reference_rate, + }; + auto wo = std::max(config_.observer_bandwidth, detail::adrc_epsilon); + auto beta1 = 3.0f * wo; + auto beta2 = 3.0f * wo * wo; + auto beta3 = wo * wo * wo; + auto b0 = detail::safe_nonzero(config_.b0); + auto error = state_.z1 - measurement; + + state_.measurement = measurement; + state_.reference = reference; + state_.td_reference = td_state.position; + state_.td_reference_rate = td_state.rate; + state_.z1 += dt * (state_.z2 - beta1 * error); + state_.z2 += + dt * (state_.z3 - beta2 * detail::fal(error, config_.observer_alpha1, config_.fal_delta) + + config_.b0 * state_.output); + state_.z3 += dt * (-beta3 * detail::fal(error, config_.observer_alpha2, config_.fal_delta)); + + auto e1 = td_state.position - state_.z1; + auto e2 = td_state.rate - state_.z2; + auto u0 = + config_.position_gain * detail::fal(e1, config_.controller_alpha1, config_.fal_delta) + + config_.rate_gain * detail::fal(e2, config_.controller_alpha2, config_.fal_delta); + state_.output = std::clamp((u0 - state_.z3) / b0, config_.output_min, config_.output_max); + return state_.output; + } + + /// Update the controller assuming a zero reference-rate command when the + /// tracking differentiator is disabled. + /// @param reference Desired position reference. + /// @param measurement Measured position. + /// @param dt Timestep in seconds. + /// @return Control output. + float update(float reference, float measurement, float dt) { + return update(reference, 0.0f, measurement, dt); + } + + /// Update the controller assuming a zero reference-rate command when the + /// tracking differentiator is disabled. + /// @param reference Desired position reference. + /// @param measurement Measured position. + /// @param dt Timestep in seconds. + /// @return Control output. + float operator()(float reference, float measurement, float dt) { + return update(reference, measurement, dt); + } + + /// Get the current controller state. + /// @return Current state. + State get_state() const { return state_; } + + /// Get the active configuration. + /// @return Current configuration. + const Config &get_config() const { return config_; } + + /// Get the current tracking differentiator state. + /// @return Tracking differentiator state. + HanTrackingDifferentiator::State get_tracking_state() const { + return tracking_differentiator_.get_state(); + } + +protected: + Config config_; + State state_; + HanTrackingDifferentiator tracking_differentiator_; + mutable std::recursive_mutex mutex_; +}; +} // namespace espp diff --git a/components/adrc/src/adrc.cpp b/components/adrc/src/adrc.cpp new file mode 100755 index 000000000..27580c54b --- /dev/null +++ b/components/adrc/src/adrc.cpp @@ -0,0 +1 @@ +#include "adrc.hpp" diff --git a/doc/Doxyfile b/doc/Doxyfile index 367a91594..987e263d6 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -69,6 +69,7 @@ MARKDOWN_SUPPORT = NO # what's here and what's not EXAMPLE_PATH = \ $(PROJECT_PATH)/components/adc/example/main/adc_example.cpp \ + $(PROJECT_PATH)/components/adrc/example/main/adrc_example.cpp \ $(PROJECT_PATH)/components/ads1x15/example/main/ads1x15_example.cpp \ $(PROJECT_PATH)/components/ads7138/example/main/ads7138_example.cpp \ $(PROJECT_PATH)/components/adxl345/example/main/adxl345_example.cpp \ @@ -176,6 +177,7 @@ INPUT = \ $(PROJECT_PATH)/components/adc/include/adc_types.hpp \ $(PROJECT_PATH)/components/adc/include/oneshot_adc.hpp \ $(PROJECT_PATH)/components/adc/include/continuous_adc.hpp \ + $(PROJECT_PATH)/components/adrc/include/adrc.hpp \ $(PROJECT_PATH)/components/ads1x15/include/ads1x15.hpp \ $(PROJECT_PATH)/components/ads7138/include/ads7138.hpp \ $(PROJECT_PATH)/components/adxl345/include/adxl345.hpp \ diff --git a/doc/en/adrc.rst b/doc/en/adrc.rst new file mode 100755 index 000000000..8576ecd32 --- /dev/null +++ b/doc/en/adrc.rst @@ -0,0 +1,147 @@ +ADRC APIs +********* + +ADRC +---- + +The `adrc` component provides active disturbance rejection controllers for +first-order and second-order plants, including both linear ADRC and Han-style +nonlinear ADRC variants. + +At a high level, ADRC treats plant uncertainty and external disturbances as an +extra state that can be estimated online by an extended state observer (ESO). +Instead of relying on a very accurate plant model, you provide an approximate +input gain ``b0`` and let the observer estimate the remaining dynamics. That +usually makes ADRC attractive for embedded robotics systems where friction, +battery sag, payload changes, cable drag, and contact disturbances are present +but hard to model precisely. + +This component includes: + +- ``HanTrackingDifferentiator`` for setpoint shaping and reference-rate + estimation +- ``LinearAdrcFirstOrder`` for approximately first-order plants +- ``LinearAdrcSecondOrder`` for approximately second-order plants +- ``HanAdrcFirstOrder`` for nonlinear first-order ADRC +- ``HanAdrcSecondOrder`` for nonlinear second-order ADRC + +Choosing a controller +--------------------- + ++--------------------------+----------------------------------------------+---------------------------------------------+ +| Controller | Best fit | Typical robotics example | ++==========================+==============================================+=============================================+ +| Linear first-order ADRC | Output behaves roughly like a one-pole loop | Motor velocity loop above an inner current | +| | | or torque loop | ++--------------------------+----------------------------------------------+---------------------------------------------+ +| Linear second-order ADRC | Input primarily affects acceleration | Servo position loop, gimbal angle loop, | +| | | heading loop | ++--------------------------+----------------------------------------------+---------------------------------------------+ +| Han first-order ADRC | First-order loop needing nonlinear shaping | Velocity loop with stiction, load steps, | +| | or better large-error behavior | or wide operating range | ++--------------------------+----------------------------------------------+---------------------------------------------+ +| Han second-order ADRC | Second-order loop with large disturbances | Motor-driven joint position control or | +| | and aggressive setpoint changes | heading control on variable terrain | ++--------------------------+----------------------------------------------+---------------------------------------------+ + +For many motor-control applications, a good starting point is: + +1. use a faster inner current or torque loop if one already exists, +2. place first-order ADRC around motor speed, or second-order ADRC around + position, +3. move to the Han nonlinear variants only if the linear controllers do not + give the disturbance rejection or large-error behavior you want. + +Robotics motor-control usage +---------------------------- + +In a practical robotics stack, ADRC is usually most effective in the outer +mechanical loops rather than as a replacement for every low-level regulator. +For example: + +- a BLDC or DC motor drive might keep its inner current loop and use ADRC for + wheel speed, +- a servo joint might use second-order ADRC on position with the motor driver + handling the faster electrical dynamics, +- a mobile robot heading controller can use second-order ADRC to compensate for + uneven traction, slope changes, or payload variation. + +The observer states are often useful for debugging and tuning: + +- ``z2`` in the first-order controllers is the estimated lumped disturbance, +- ``z3`` in the second-order controllers is the estimated lumped disturbance, +- large persistent disturbance estimates usually indicate load torque, friction, + bias, or a poor ``b0`` estimate. + +Those estimated disturbance states are especially valuable in robotics, because +they can explain why a loop feels "mysteriously" different under battery sag, +ground contact, or changing payload. + +Tuning for motors and actuators +------------------------------- + +ADRC is often tuned by bandwidth rather than by directly shaping multiple +independent gains. A practical workflow for motor-control applications is: + +1. **Pick the structure first.** Use first-order ADRC for speed-like loops and + second-order ADRC for position-like loops. +2. **Estimate ``b0``.** The sign must be correct. The magnitude only needs to + be approximate, but if it is very wrong the controller will feel badly + scaled and may saturate too early. +3. **Start with conservative bandwidth.** Increase the controller bandwidth + until the loop is responsive but not noisy or oscillatory. +4. **Make the observer faster than the controller.** A faster ESO helps reject + disturbances early, but too much observer bandwidth will amplify encoder or + velocity-estimate noise. +5. **Use the tracking differentiator to soften steps.** This is especially + helpful for position commands that would otherwise demand unrealistic + acceleration from the motor. +6. **Watch saturation.** If the command rails against ``output_min`` / + ``output_max``, back off the bandwidth or revisit ``b0`` and trajectory + shaping before increasing gains further. + +Some practical heuristics: + +- for the linear variants, choose ``observer_bandwidth`` several times larger + than ``controller_bandwidth`` and increase gradually; +- if encoder noise or quantization dominates, reduce observer bandwidth before + blaming the control law; +- if large setpoint steps cause overshoot or chatter, enable the tracking + differentiator or lower its aggressiveness; +- for the Han nonlinear variants, increase ``fal_delta`` if the loop is too + sharp around zero error, and adjust the ``alpha`` parameters only after the + main gain and bandwidth choices are in the right range. + +.. note:: + + The most important ADRC tuning parameter is usually not a fancy nonlinear + exponent, but the combination of loop rate, actuator saturation, sensor + quality, and a reasonable ``b0`` estimate. If the sample period is too slow + or the actuator is saturating constantly, ADRC will not rescue the loop. + +References +---------- + +Useful starting points for ADRC background and tuning include: + +- Jingqing Han, *From PID to Active Disturbance Rejection Control*, IEEE + Transactions on Industrial Electronics, 2009. +- Zhiqiang Gao, *Active Disturbance Rejection Control: A Paradigm Shift in + Feedback Control System Design*, Proceedings of the American Control + Conference, 2006. +- Zhiqiang Gao, *Scaling and Bandwidth-Parameterization Based Controller + Tuning*, Proceedings of the American Control Conference, 2002. +- Gernot Herbst, *Practical Active Disturbance Rejection Control: Tuning + Methods and Guidelines for Continuous-Time Systems*, IFAC-PapersOnLine, 2015. + +Code examples for the ADRC API are provided in the ``components/adrc/example`` +folder. + +.. toctree:: + + adrc_example + +API Reference +------------- + +.. include-build-file:: inc/adrc.inc diff --git a/doc/en/adrc_example.md b/doc/en/adrc_example.md new file mode 100755 index 000000000..1dfa8c685 --- /dev/null +++ b/doc/en/adrc_example.md @@ -0,0 +1,2 @@ +```{include} ../../components/adrc/example/README.md +``` diff --git a/doc/en/index.rst b/doc/en/index.rst index f3c6416ad..6922cddbf 100755 --- a/doc/en/index.rst +++ b/doc/en/index.rst @@ -11,6 +11,7 @@ This is the documentation for esp-idf c++ components, ESPP (`espp