From d3cb806627693c8f24902a36613fa58bae858fd9 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 29 Sep 2021 11:18:50 +0200 Subject: [PATCH] pid_design: add model-based PID tuning law based on GMVC --- src/lib/CMakeLists.txt | 1 + src/lib/pid_design/CMakeLists.txt | 38 +++++++++ src/lib/pid_design/pid_design.hpp | 105 +++++++++++++++++++++++++ src/lib/pid_design/pid_design_test.cpp | 58 ++++++++++++++ 4 files changed, 202 insertions(+) create mode 100644 src/lib/pid_design/CMakeLists.txt create mode 100644 src/lib/pid_design/pid_design.hpp create mode 100644 src/lib/pid_design/pid_design_test.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 3c0c928fe6..38bf63ee1c 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -58,6 +58,7 @@ add_subdirectory(motion_planning) add_subdirectory(output_limit) add_subdirectory(perf) add_subdirectory(pid) +add_subdirectory(pid_design) add_subdirectory(pwm) add_subdirectory(rc) add_subdirectory(sensor_calibration) diff --git a/src/lib/pid_design/CMakeLists.txt b/src/lib/pid_design/CMakeLists.txt new file mode 100644 index 0000000000..434e9928ff --- /dev/null +++ b/src/lib/pid_design/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(PidDesign + pid_design.hpp +) + +px4_add_unit_gtest(SRC pid_design_test.cpp) diff --git a/src/lib/pid_design/pid_design.hpp b/src/lib/pid_design/pid_design.hpp new file mode 100644 index 0000000000..b1c7e9d042 --- /dev/null +++ b/src/lib/pid_design/pid_design.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pid_design.hpp + * @brief Helper functions to design PID controllers using identified models + * + * @author Mathieu Bresciani + */ + +#pragma once + +#include +#include + +namespace pid_design +{ + +/* +* Compute a set of PID gains using General Minimum Variance Control law design +* +* @param num: the coefficients of the numerator of the discrete-time plant transfer function +* @param den: the coefficients of the denominator of the discrete-time plant transfer function +* @param dt: the sampling time in seconds +* @param sigma: the desired closed-loop rise time in seconds +* @param delta: the damping index (between 0 and 2). 0 is critical damping, 1 is Butterworth +* @param lbda: the "detuning" coefficients. This affects the gain of the controller kc only (increase to detune the controller) +* +* @return The PID gains in standard form: u = kc*[1 + ki*dt + kd/dt]*e +* kc: the controller gain +* ki: the integral gain (= 1/Ti) +* kd: the derivative gain (= Td) +* +* Reference: +* T.Yamatoto, K.Fujii and M.Kaneda, Design and implementation of a self-tuning pid controller, 1998 +*/ + +inline matrix::Vector3f computePidGmvc(const matrix::Vector3f &num, const matrix::Vector3f &den, float dt, + float sigma = 0.1f, float delta = 1.f, float lbda = 0.5f) +{ + sigma = math::constrain(sigma, 0.01f, 1.f); + delta = math::constrain(delta, 0.f, 1.f); + lbda = math::constrain(lbda, 0.f, 10.f); + + const float a1 = den(1); + const float a2 = den(2); + const float b0 = num(0); + const float b1 = num(1); + const float b2 = num(2); + + // Solve GMVC law (see derivation in pid_synthesis_symbolic.py) + const float rho = dt / sigma; + const float mu = 0.25f * (1.f - delta) + 0.51f * delta; // mu is in the interval [0.25 0.51] + const float p1 = -2.f * expf(-rho / (2.f * mu)) * cosf(sqrtf(4.f * mu - 1.f) * rho / (2.f * mu)); + const float p2 = exp(-rho / mu); + const float e1 = -a1 + p1 + 1.f; + const float f0 = -a1 * e1 + a1 - a2 + e1 + p2; + const float f1 = a1 * e1 - a2 * e1 + a2; + const float f2 = a2 * e1; + + // Translate to PID gains + const float nu = lbda + (e1 + 1.f) * (b0 + b1 + b2); + + if (fabsf(nu) < FLT_EPSILON) { + return matrix::Vector3f(); + } + + const float kc = -(f1 + 2.f * f2) / nu; + float ki = -(f0 + f1 + f2) / (dt * (f1 + 2.f * f2)); + ki /= 5.f; // This is not part of the original implementation but is required to produce reasonable gains + const float kd = -dt * f2 / (f1 + 2.f * f2); + + return matrix::Vector3f(kc, ki, kd); +} +} // namespace pid_design diff --git a/src/lib/pid_design/pid_design_test.cpp b/src/lib/pid_design/pid_design_test.cpp new file mode 100644 index 0000000000..c9b9339cae --- /dev/null +++ b/src/lib/pid_design/pid_design_test.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2020-2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Test code for pid_design functions + * Run this test only using make tests TESTFILTER=pid_design + */ + +#include +#include + +#include "pid_design.hpp" + +using namespace pid_design; +using namespace matrix; + +TEST(PidDesignTest, testImpl) +{ + const Vector3f num(.0098f, -0.162f, .147f); + const Vector3f den(1.f, -1.814f, .822f); + const float dt = 0.004f; + const Vector3f kid = computePidGmvc(num, den, dt, 0.1f, 1.f, 0.5f); + + // Compare with values generated from the python implementation + EXPECT_NEAR(kid(0), 0.129f, .001f); + EXPECT_NEAR(kid(1), 11.911f / 5.f, .001f); + EXPECT_NEAR(kid(2), 0.0463f, .0001f); +}