Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add function to adjust LQR controller gain for pure time delay (#2878)

Browse Source 
There were three options for where to put this function:

1. A free function in LinearQuadraticRegulator.h. Returning a K matrix
   means the user can't use the LinearQuadraticRegulator in a loop
   anymore.
2. A default argument added to ctors in LinearQuadraticRegulator for a
   time delay (default of 0). This has the smallest API footprint from
   the user perspective, but it bloats the already substantial
   constructor overload set of LinearQuadraticRegulator.
3. A member function in LinearQuadraticRegulator that modifies the
   internal K. This would still have to take in a LinearSystem or (A, B)
   pair because the ctor doesn't store it. Storing it internally feels
   like paying for what we don't use most of the time.

I went with option 3.

I verified the tests's expected values in Python with
scipy.linalg.fractional_matrix_power().

Closes #2877.
This commit is contained in:
Tyler Veness
2020-11-20 15:28:00 -08:00
committed by GitHub
parent 2816b06c05
commit c8ea1b6c38
7 changed files with 154 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -11,11 +11,16 @@
#include "Eigen/Core"
#include "Eigen/src/Cholesky/LLT.h"
#include "Eigen/src/Eigenvalues/ComplexSchur.h"
#include "Eigen/src/LU/Determinant.h"
#include "Eigen/src/LU/InverseImpl.h"
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
#include "unsupported/Eigen/src/MatrixFunctions/MatrixPower.h"
#include "unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h"
namespace frc {
namespace detail {
@@ -172,6 +177,31 @@ class LinearQuadraticRegulatorImpl {
return Calculate(x);
}
/**
* Adjusts LQR controller gain to compensate for a pure time delay in the
* input.
*
* Linear-Quadratic regulator controller gains tend to be aggressive. If
* sensor measurements are time-delayed too long, the LQR may be unstable.
* However, if we know the amount of delay, we can compute the control based
* on where the system will be after the time delay.
*
* See https://file.tavsys.net/control/controls-engineering-in-frc.pdf
* appendix C.4 for a derivation.
*
* @param plant The plant being controlled.
* @param dt Discretization timestep.
* @param inputDelay Input time delay.
*/
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
units::second_t dt, units::second_t inputDelay) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
}
private:
// Current reference
Eigen::Matrix<double, States, 1> m_r;
@@ -192,7 +222,7 @@ class LinearQuadraticRegulator
/**
* Constructs a controller with the given coefficients and plant.
*
* @param system The plant being controlled.
* @param plant The plant being controlled.
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.