From f3dfeb9ace8d56ec526808443d050535a4ab1eb2 Mon Sep 17 00:00:00 2001 From: Astik-2002 <41.astiksrivastava@gmail.com> Date: Tue, 22 Aug 2023 18:28:29 +0530 Subject: [PATCH] modified: libraries/AC_CustomControl/AC_CustomControl.cpp modified: libraries/AC_CustomControl/AC_CustomControl.h new file: libraries/AC_CustomControl/AC_CustomControl_LQR.cpp new file: libraries/AC_CustomControl/AC_CustomControl_LQR.h AC_CustomControl: add LQR controller --- .../AC_CustomControl/AC_CustomControl.cpp | 10 +- libraries/AC_CustomControl/AC_CustomControl.h | 3 +- .../AC_CustomControl/AC_CustomControl_LQR.cpp | 114 ++++++++++++++++++ .../AC_CustomControl/AC_CustomControl_LQR.h | 34 ++++++ libraries/AC_CustomControl/README.md | 10 +- 5 files changed, 166 insertions(+), 5 deletions(-) create mode 100644 libraries/AC_CustomControl/AC_CustomControl_LQR.cpp create mode 100644 libraries/AC_CustomControl/AC_CustomControl_LQR.h diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index 903d854c127a92..c655257313d60f 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -7,6 +7,7 @@ #include "AC_CustomControl_Backend.h" // #include "AC_CustomControl_Empty.h" #include "AC_CustomControl_PID.h" +#include "AC_CustomControl_LQR.h" #include #include @@ -15,7 +16,7 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = { // @Param: _TYPE // @DisplayName: Custom control type // @Description: Custom control type to be used - // @Values: 0:None, 1:Empty, 2:PID + // @Values: 0:None, 1:Empty, 2:PID, 3:LQR-integral // @RebootRequired: True // @User: Advanced AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE), @@ -33,6 +34,9 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = { // parameters for PID controller AP_SUBGROUPVARPTR(_backend, "2_", 7, AC_CustomControl, _backend_var_info[1]), + // parameters for LQR controller + AP_SUBGROUPVARPTR(_backend, "3_", 8, AC_CustomControl, _backend_var_info[2]), + AP_GROUPEND }; @@ -62,6 +66,10 @@ void AC_CustomControl::init(void) _backend = new AC_CustomControl_PID(*this, _ahrs, _att_control, _motors, _dt); _backend_var_info[get_type()] = AC_CustomControl_PID::var_info; break; + case CustomControlType::CONT_LQR: + _backend = new AC_CustomControl_LQR(*this, _ahrs, _att_control, _motors, _dt); + _backend_var_info[get_type()] = AC_CustomControl_LQR::var_info; + break; default: return; } diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index 9a12a347067da2..5c17f463efbf51 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -14,7 +14,7 @@ #include #ifndef CUSTOMCONTROL_MAX_TYPES -#define CUSTOMCONTROL_MAX_TYPES 2 +#define CUSTOMCONTROL_MAX_TYPES 3 #endif class AC_CustomControl_Backend; @@ -49,6 +49,7 @@ class AC_CustomControl { CONT_NONE = 0, CONT_EMPTY = 1, CONT_PID = 2, + CONT_LQR = 3, }; // controller that should be used enum class CustomControlOption { diff --git a/libraries/AC_CustomControl/AC_CustomControl_LQR.cpp b/libraries/AC_CustomControl/AC_CustomControl_LQR.cpp new file mode 100644 index 00000000000000..dce1b357bdfdc9 --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_LQR.cpp @@ -0,0 +1,114 @@ +#include "AC_CustomControl_LQR.h" + +#if CUSTOMCONTROL_LQR_ENABLED + +#include + +// table of user settable parameters +const AP_Param::GroupInfo AC_CustomControl_LQR::var_info[] = { + // @Param: PARAM1 + // @DisplayName: LQR param1 + // @Description: Dumy parameter for LQR custom controller backend + // @User: Advanced + AP_GROUPINFO("PARAM1", 1, AC_CustomControl_LQR, param1, 0.0f), + + // @Param: PARAM2 + // @DisplayName: LQR param2 + // @Description: Dumy parameter for LQR custom controller backend + // @User: Advanced + AP_GROUPINFO("PARAM2", 2, AC_CustomControl_LQR, param2, 0.0f), + + // @Param: PARAM3 + // @DisplayName: LQR param3 + // @Description: Dumy parameter for LQR custom controller backend + // @User: Advanced + AP_GROUPINFO("PARAM3", 3, AC_CustomControl_LQR, param3, 0.0f), + + AP_GROUPEND +}; + +// initialize in the constructor +AC_CustomControl_LQR::AC_CustomControl_LQR(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : + AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt) +{ + _dt = dt; + AP_Param::setup_object_defaults(this, var_info); +} + +// update controller +// return roll, pitch, yaw controller output +Vector3f AC_CustomControl_LQR::update(void) +{ + // reset controller based on spool state + switch (_motors->get_spool_state()) { + case AP_Motors::SpoolState::SHUT_DOWN: + case AP_Motors::SpoolState::GROUND_IDLE: + // We are still at the ground. Reset custom controller to avoid + // build up, ex: integrator + reset(); + break; + + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: + case AP_Motors::SpoolState::SPOOLING_UP: + case AP_Motors::SpoolState::SPOOLING_DOWN: + // we are off the ground + break; + } + + // arducopter main attitude controller already runned + // we don't need to do anything else + + //gcs().send_text(MAV_SEVERITY_INFO, "LQR custom controller working"); + + Quaternion attitude_body, attitude_target; + _ahrs->get_quat_body_to_ned(attitude_body); + + + attitude_target = _att_control->get_attitude_target_quat(); + + + Quaternion attitude_error_quat = attitude_target.inverse() * attitude_body; + + +// axis-angle representation of quaternion is used. The resulting //state-space for attitude control is [v1 v2 v3 p q r] + Vector3f attitude_error_angle; + attitude_error_quat.to_axis_angle(attitude_error_angle); + + + Vector3f gyro_latest = _ahrs->get_gyro_latest(); + + +// k_gain LQR matrix is defined as [kij], where i:[T1 T2 T3](torques in // three axis) and j:[v1 v2 v3 p q r](roll pitch yaw roll-rate //pitch-rate yaw-rate) +// k_gain = [k11 k12 k13 k14 k15 k16; +// k21 k22 k23 k24 k25 k26; +// k31 k32 k33 k34 k35 k36;] +// [T1 T2 T3] = -k_gain*[v1 v2 v3 p q r] + _integralX += 0.03*attitude_error_angle.x*_dt; + _integralY += 0.08*attitude_error_angle.y*_dt; + _integralZ += 0.005*attitude_error_angle.z*_dt; + constrain_float(_integralX, -_kimax_LQR, _kimax_LQR); + constrain_float(_integralY, -_kimax_LQR, _kimax_LQR); + constrain_float(_integralZ, -_kimax_LQR, _kimax_LQR); + + Vector3f motor_out; + + motor_out.x = -_integralX - 0.375*attitude_error_angle.x - 0.09*gyro_latest.x; + + motor_out.y = -_integralY - 0.375*attitude_error_angle.y - 0.125*gyro_latest.y; + + motor_out.z = -_integralZ - 0.40*attitude_error_angle.z - 0.09*gyro_latest.z; + + return motor_out; + +} + +// reset controller to avoid build up on the ground +// or to provide bumpless transfer from arducopter main controller +void AC_CustomControl_LQR::reset(void) +{ + _integralX = 0; + _integralY = 0; + _integralZ = 0; +} + +#endif diff --git a/libraries/AC_CustomControl/AC_CustomControl_LQR.h b/libraries/AC_CustomControl/AC_CustomControl_LQR.h new file mode 100644 index 00000000000000..6a44c1e83473c0 --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_LQR.h @@ -0,0 +1,34 @@ +#pragma once + +#include "AC_CustomControl_Backend.h" + +#ifndef CUSTOMCONTROL_LQR_ENABLED + #define CUSTOMCONTROL_LQR_ENABLED AP_CUSTOMCONTROL_ENABLED +#endif + +#if CUSTOMCONTROL_LQR_ENABLED + +class AC_CustomControl_LQR : public AC_CustomControl_Backend { +public: + AC_CustomControl_LQR(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt); + + + Vector3f update(void) override; + void reset(void) override; + // user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +protected: + // declare parameters here + + float _dt; + + float _integralX, _integralY, _integralZ; + float _kimax_LQR = 0.1; + + AP_Float param1; + AP_Float param2; + AP_Float param3; +}; + +#endif diff --git a/libraries/AC_CustomControl/README.md b/libraries/AC_CustomControl/README.md index 2d2d4a892d1350..00ce6eb9548edd 100644 --- a/libraries/AC_CustomControl/README.md +++ b/libraries/AC_CustomControl/README.md @@ -243,9 +243,9 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl &frontend, AP_AHRS_V _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), - _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, dt), - _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, dt), - _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt) + _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt), + _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt), + _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt) { AP_Param::setup_object_defaults(this, var_info); } @@ -256,3 +256,7 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl &frontend, AP_AHRS_V 12. Add reset functionality inside `reset` function of `AC_CustomControl_XYZ.cpp` file. It is user's responsibility to add proper controller resetting functionality. This highly depends on the controller and it should not be copy-pasted from another backend without testing it in SITL. 13. You can access target attitude and target rate values using `atti_control->get_attitude_target_quat()` and `atti_control->rate_bf_targets()` functions, respectively. You can also access latest gyro measurement using `_ahrs->get_gyro_latest()` function. Take a look at other backends + +## Integral action Linear Quadratic Regulator controller + +1. This controller serves as an example on using this library for deploying your own attitude controllers. The LQR controller implemented here essentially works like an attitude PID controller, wherein the PID gains for each axis have been tuned by solving the Algebraic Riccati Equation (ARE) for the attitude state-space of a quadcopter. Details about quadcopter dynamics are documented here: https://ardupilot.org/copter/docs/systemid-mode-operation.html#identification-of-a-multicopter. For further details behind the mathematics and implementation of this attitude controller, refer to this publication: https://www.researchgate.net/publication/279448184_Quadrotor_Quaternion_Control and this article: https://www.mathworks.com/help/control/ref/ss.lqi.html