Tutorial de Controlador de Estado-Espacio
Nota
Antes de seguir este tutorial, se recomienda que los lectores hayan leído Introducción al Control del Estado-Espacio.
La meta de este tutorial es proveer «de principio a fin» instrucciones para implementar el controlador de estado-espacio para un volante. Siguiendo este tutorial, los lectores podrán aprender a hacer:
Create an accurate state-space model of a flywheel using system identification or CAD software.
Implementar un Filtro Kalman para filtrar las mediciones de velocidad del encoder sin retraso.
Implementar un controlador de retroalimentación LQR que, cuando lo combinamos con uno de feedforward basado en modelo, generará inputs 1 de voltaje para manejar el volante a una reference.
Este tutorial tiene el propósito de ser alcanzable para los equipos sin experiencia en programación o buen manejo de ella. Mientras que la librería de WPILib ofrece cierta flexibilidad en el modo que las características del control de estado-espacio son implementadas, siguen de cerca la implementación descrita en este tutorial debe dar a los equipos una estructura base que pueda ser reusada para sistemas variados de estado-espacio.
The full example is available in the state-space flywheel (Java/C++/Python) and state-space flywheel system identification (Java/C++/Python) example projects.
¿Por qué usar Control de Estado-Espacio?
Porque el control de estado-espacio se enfoca en crear un modelo preciso de nuestro sistema, podemos predecir de manera certera como el modelo responderá para controlar entradas. Esto nos permite simular nuestros mecanismos sin acceso al robot físico, como bien escoger fácilmente las ganancias que sabemos que trabajarán bien. Teniendo un modelo también nos permite crear filtros sin retrasos, como los Filtros Kalman, para filtrar óptimamente las lecturas del sensor.
Modelando Nuestro Flywheel
Recuerde que el sistema continuo de estado-espacio son modelados usando el siguiente sistema de ecuaciones:
Where x-dot is the rate of change of the system’s state, \(\mathbf{x}\) is the system’s current state, \(\mathbf{u}\) is the input to the system, and \(\mathbf{y}\) is the system’s output.
Let’s use this system of equations to model our flywheel in two different ways. We’ll first model it using system identification using the SysId toolsuite, and then model it based on the motor and flywheel’s moment of inertia.
El primer paso para construir nuestro sistema estado-espacio es elegir nuestros estados del sistema. Podemos elegir cualquiera que queramos como estado – podemos elegir estados completamente diferentes si queremos – pero ayudará si elegimos estados que son importantes. Podemos incluir estados ocultos en nuestro estado (como la velocidad del elevador si solo podemos medir su posición) y dejar al Filtro Kalman estimar sus valores. Recuerda que los estados que elegimos serán conducidos hacia sus respectivas referencias por el controlador de retroalimentación (normalmente el Regulador Lineal Cuadrático ya que es el óptimo).
Para nuestro volante, solo nos importa un estado: su velocidad. Mientras que podamos elegir también modelar su aceleración, incluir este estado no es necesario para nuestro sistema.
A continuación, identificamos los inputs a nuestro sistema. Las entradas se pueden considerar como cosas que podemos poner «en» nuestro sistema para cambiar su estado. En el caso del flywheel (y muchos otros mecanismos de articulación simple en FRC®), tenemos solo una entrada: voltaje aplicado al motor. Al elegir el voltaje como nuestra entrada (sobre algo como el ciclo de trabajo del motor), podemos compensar la caída del voltaje de la batería a medida que aumenta la carga de la batería.
Un sistema de tiempo continuo de estado-espacio escribe x-dot, o la velocidad instantánea de cambio del sistemaestado del sistema, proporcional al estado actual y las entradas. Porque nuestro estado es velocidad angular, \(\mathbf{\dot{x}}\) va a ser la aceleración angular del volante.
Después, modelaremos nuestro volante como un sistema de tiempo continuo de estado-espacio. LinearSystem
de WPILib convertirá este tiempo discreto internamente. Repase notación de estado-espacio para más sistemas de tiempo continuo y discreto.
Modelando con Identificación de Sistema
To rewrite this in state-space notation using system identification, we recall from the flywheel state-space notation example, where we rewrote the following equation in terms of \(\mathbf{a}\).
Donde \(\mathbf{v}\) es la velocidad del volante, \(\mathbf{a}\) y \(\mathbf{\dot{v}}\) son la aceleración del volante, y \(V\) es el voltaje. Reescribiendo esto como la convención estándar de \(\mathbf{x}\) para el vector del estado y \(\mathbf{u}\) para el vector de entrada, encontramos:
La segunda parte de la notación de estado-espacio relata que el estado actual del sistema y entradas a la salida. En caso de un volante, nuestro vector de salida \(\mathbf{y}\) (o cosas que pueda medir un sensor) es la velocidad de nuestro volante, que también resulta ser un elemento de nuestro vector de estado \(\mathbf{x}\). Por lo tanto, nuestra matriz de salida es \(\mathbf{C} = \begin{bmatrix}1 \end{bmatrix}\), y nuestra matriz de sistema de alimentación es \(\mathbf{D} = \begin{bmatrix}0 \end{bmatrix}\). Escribiendo esto en notación de estado-espacio con tiempo continuo produce lo siguiente.
La clase LinearSystem
contiene métodos para crear fácilmente sistemas de estado-espacio identificados usando system identification. Este ejemplo muestra un modelo de volante con un kV de 0.023 y un kA de 0.001:
32 // Volts per (radian per second)
33 private static final double kFlywheelKv = 0.023;
34
35 // Volts per (radian per second squared)
36 private static final double kFlywheelKa = 0.001;
37
38 // The plant holds a state-space model of our flywheel. This system has the following properties:
39 //
40 // States: [velocity], in radians per second.
41 // Inputs (what we can "put in"): [voltage], in volts.
42 // Outputs (what we can measure): [velocity], in radians per second.
43 //
44 // The Kv and Ka constants are found using the FRC Characterization toolsuite.
45 private final LinearSystem<N1, N1, N1> m_flywheelPlant =
46 LinearSystemId.identifyVelocitySystem(kFlywheelKv, kFlywheelKa);
17#include <frc/system/plant/LinearSystemId.h>
30 // Volts per (radian per second)
31 static constexpr auto kFlywheelKv = 0.023_V / 1_rad_per_s;
32
33 // Volts per (radian per second squared)
34 static constexpr auto kFlywheelKa = 0.001_V / 1_rad_per_s_sq;
35
36 // The plant holds a state-space model of our flywheel. This system has the
37 // following properties:
38 //
39 // States: [velocity], in radians per second.
40 // Inputs (what we can "put in"): [voltage], in volts.
41 // Outputs (what we can measure): [velocity], in radians per second.
42 //
43 // The Kv and Ka constants are found using the FRC Characterization toolsuite.
44 frc::LinearSystem<1, 1, 1> m_flywheelPlant =
45 frc::LinearSystemId::IdentifyVelocitySystem<units::radian>(kFlywheelKv,
46 kFlywheelKa);
23# Volts per (radian per second)
24kFlywheelKv = 0.023
25
26# Volts per (radian per second squared)
27kFlywheelKa = 0.001
37 # The plant holds a state-space model of our flywheel. This system has the following properties:
38 #
39 # States: [velocity], in radians per second.
40 # Inputs (what we can "put in"): [voltage], in volts.
41 # Outputs (what we can measure): [velocity], in radians per second.
42 #
43 # The Kv and Ka constants are found using the FRC Characterization toolsuite.
44 self.flywheelPlant = (
45 wpimath.system.plant.LinearSystemId.identifyVelocitySystemRadians(
46 kFlywheelKv, kFlywheelKa
47 )
48 )
Modelando Usando el Volante con Momento de Inercia y Engranaje
A flywheel can also be modeled without access to a physical robot, using information about the motors, gearing and flywheel’s moment of inertia. A full derivation of this model is presented in Section 12.3 of Controls Engineering in FRC.
The LinearSystem
class contains methods to easily create a model of a flywheel from the flywheel’s motors, gearing and moment of inertia. The moment of inertia can be calculated using CAD software or using physics. The examples used here are detailed in the flywheel example project (Java/C++/Python).
Nota
Para las clases de estado-espacio de WIPLib, los engranes son escritos como salida en vez de entrada – es esto, si el volante gira más lento que los motores, este número debe ser mayor que uno.
Nota
La clase LinearSystem de C++ usa la Libreria de Unidades de C++ para prevenir mezla de unidades y afirmar dimensionalmente.
33 private static final double kFlywheelMomentOfInertia = 0.00032; // kg * m^2
34
35 // Reduction between motors and encoder, as output over input. If the flywheel spins slower than
36 // the motors, this number should be greater than one.
37 private static final double kFlywheelGearing = 1.0;
38
39 // The plant holds a state-space model of our flywheel. This system has the following properties:
40 //
41 // States: [velocity], in radians per second.
42 // Inputs (what we can "put in"): [voltage], in volts.
43 // Outputs (what we can measure): [velocity], in radians per second.
44 private final LinearSystem<N1, N1, N1> m_flywheelPlant =
45 LinearSystemId.createFlywheelSystem(
46 DCMotor.getNEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);
17#include <frc/system/plant/LinearSystemId.h>
31 static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
32 0.00032_kg_sq_m;
33
34 // Reduction between motors and encoder, as output over input. If the flywheel
35 // spins slower than the motors, this number should be greater than one.
36 static constexpr double kFlywheelGearing = 1.0;
37
38 // The plant holds a state-space model of our flywheel. This system has the
39 // following properties:
40 //
41 // States: [velocity], in radians per second.
42 // Inputs (what we can "put in"): [voltage], in volts.
43 // Outputs (what we can measure): [velocity], in radians per second.
44 frc::LinearSystem<1, 1, 1> m_flywheelPlant =
45 frc::LinearSystemId::FlywheelSystem(
46 frc::DCMotor::NEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);
21kFlywheelMomentOfInertia = 0.00032 # kg/m^2
22
23# Reduction between motors and encoder, as output over input. If the flywheel spins slower than
24# the motors, this number should be greater than one.
25kFlywheelGearing = 1
37 # The plant holds a state-space model of our flywheel. This system has the following properties:
38 #
39 # States: [velocity], in radians per second.
40 # Inputs (what we can "put in"): [voltage], in volts.
41 # Outputs (what we can measure): [velocity], in radians per second.
42 self.flywheelPlant = wpimath.system.plant.LinearSystemId.flywheelSystem(
43 wpimath.system.plant.DCMotor.NEO(2),
44 kFlywheelMomentOfInertia,
45 kFlywheelGearing,
46 )
Filtros Kalman: Observando el Estado del Flywheel
Los filtros Kalman son usados para filtrar nuestras mediciones de velocidad usando nuestro modelo de estado-espacio para generar un estado estimado \(\mathbf{\hat{x}}\). Como nuestro modelo de volante es lineal, podemos usar un filtro Kalman para estimar la velocidad del volante. EL filtro Kalman de WPILib toma un LinearSystem
(que encontramos arriba), a lo largo con derivaciones estándar de mediciones de modelo y sensores. Podemos ajustar que tan «fluido» es nuestra estimación de estado es por ajustar estas cargas. Las desviaciones estándar de estado más grandes harán que el filtro «desconfíe» de nuestra estimación estatal y favorezca más las nuevas mediciones, mientras que las desviaciones estándar de las mediciones más grandes harán lo contrario.
En el caso de un volante comenzamos con una desviación estándar de estado de 3 rad/s y una desviación estándar de medición de 0.01 rad/s. Estos valores son a elección del usuario – estos pesos produjeron un filtro que era tolerante a algo de ruido pero cuya estimación de estado reaccionó rápidamente a perturbaciones externas para un volante de inercia – y deben ajustarse para crear un filtro que se comporte bien para su volante específico. Graficar estados, medidas, entradas, referencias y salidas a lo largo del tiempo es una excelente forma visual de ajustar los filtros de Kalman.
La gráfica de arriba muestra 2 diferentes ajustes del filtro Kalman, así como filtro IIR con un polo y un Filtro de Mediana. Estos datos fueron recolectados con un lanzador arriba de ~5 segundos, y cuatro bolas pasaron a través del lanzador (como se ve en las cuatro caídas de velocidad). Si bien no existen reglas estrictas para elegir un buen estado y medir las desviaciones estándar, en general deben ajustarse para confiar en el modelo lo suficiente como para rechazar el ruido y reaccionar rápidamente a las perturbaciones externas.
Dado que el controlador de retroalimentación calcula el error utilizando el :término:`x-hat` estimado por el filtro Kalman, el controlador reaccionará a las perturbaciones sólo con la rapidez con la que cambie la estimación de estado del filtro. En el gráfico anterior, el gráfico superior de la izquierda (con una desviación estándar del estado de 3,0 y una desviación estándar de la medición de 0,2) produjo un filtro que reaccionó rápidamente a las perturbaciones mientras rechazaba el ruido, mientras que el gráfico superior de la derecha muestra un filtro que apenas se vio afectado por las caídas de velocidad.
48 // The observer fuses our encoder data and voltage inputs to reject noise.
49 private final KalmanFilter<N1, N1, N1> m_observer =
50 new KalmanFilter<>(
51 Nat.N1(),
52 Nat.N1(),
53 m_flywheelPlant,
54 VecBuilder.fill(3.0), // How accurate we think our model is
55 VecBuilder.fill(0.01), // How accurate we think our encoder
56 // data is
57 0.020);
13#include <frc/estimator/KalmanFilter.h>
48 // The observer fuses our encoder data and voltage inputs to reject noise.
49 frc::KalmanFilter<1, 1, 1> m_observer{
50 m_flywheelPlant,
51 {3.0}, // How accurate we think our model is
52 {0.01}, // How accurate we think our encoder data is
53 20_ms};
48 # The observer fuses our encoder data and voltage inputs to reject noise.
49 self.observer = wpimath.estimator.KalmanFilter_1_1_1(
50 self.flywheelPlant,
51 [3], # How accurate we think our model is
52 [0.01], # How accurate we think our encoder data is
53 0.020,
54 )
Porque los filtros Kalman usan nuestros modelos de estado-espacio en el Predecir paso, es importante que nuestro modelo es tan preciso como es posible. Una manera de verificar esto es registrar el voltaje de entrada del volante y la velocidad en el tiempo, y vuelve a reproducir esta información llamando solo predict
en el filtro Kalman. Después, las ganancias de kV y kA (o el momento de inercia y otras constantes) pueden ser justadas hasta que el modelo se acerque a igualar los datos registrados.
Reguladores Lineales Cuadráticos y Feedforward de Inversión de Planta
El Regulador Lineal Cuadrático encuentra un controlador de retroalimentación para manejar nuestro sistema de volante a su referencia. Porque nuestro volante solo tiene un estado, la ley de control tomada por nuestro LQR va a estar en la forma \(\mathbf{u = K (r - x)}\) donde \(\mathbf{K}\) es una matriz de 1x1; en otras palabras, la ley de control tomada por el LQR es simplemente un controlador proporcional, o un controlador PID con solo ganancia P. Esta ganancia es elegida por nuestro LQR en función de la excursión de estado y los esfuerzos de control que la pasamos. Más de la afinación de controladores LQR puede encontrarse en ejemplo de aplicación de LQR.
Mucho como SimpleMotorFeedforward
puede ser usado para generar la entrada de voltaje del feedfoward dadas las constantes kS, kV, kA, la clase Feedforward de Inversión de Planta genera entradas de voltaje de feedforward dado un sistema de estado-espacio. Los comandos de voltaje generados por la clase LinearSystemLoop
son la suma de las entradas de feedfoward y de retroaliemntación.
59 // A LQR uses feedback to create voltage commands.
60 private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
61 new LinearQuadraticRegulator<>(
62 m_flywheelPlant,
63 VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease
64 // this to more heavily penalize state excursion, or make the controller behave more
65 // aggressively.
66 VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
67 // heavily penalize control effort, or make the controller less aggressive. 12 is a good
68 // starting point because that is the (approximate) maximum voltage of a battery.
69 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
70 // lower if using notifiers.
11#include <frc/controller/LinearQuadraticRegulator.h>
55 // A LQR uses feedback to create voltage commands.
56 frc::LinearQuadraticRegulator<1, 1> m_controller{
57 m_flywheelPlant,
58 // qelms. Velocity error tolerance, in radians per second. Decrease this
59 // to more heavily penalize state excursion, or make the controller behave
60 // more aggressively.
61 {8.0},
62 // relms. Control effort (voltage) tolerance. Decrease this to more
63 // heavily penalize control effort, or make the controller less
64 // aggressive. 12 is a good starting point because that is the
65 // (approximate) maximum voltage of a battery.
66 {12.0},
67 // Nominal time between loops. 20ms for TimedRobot, but can be lower if
68 // using notifiers.
69 20_ms};
70
71 // The state-space loop combines a controller, observer, feedforward and plant
72 // for easy control.
73 frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller,
74 m_observer, 12_V, 20_ms};
56 # A LQR uses feedback to create voltage commands.
57 self.controller = wpimath.controller.LinearQuadraticRegulator_1_1(
58 self.flywheelPlant,
59 [8], # qelms. Velocity error tolerance, in radians per second. Decrease
60 # this to more heavily penalize state excursion, or make the controller behave more
61 # aggressively.
62 [12], # relms. Control effort (voltage) tolerance. Decrease this to more
63 # heavily penalize control effort, or make the controller less aggressive. 12 is a good
64 # starting point because that is the (approximate) maximum voltage of a battery.
65 0.020, # Nominal time between loops. 0.020 for TimedRobot, but can be lower if using notifiers.
66 )
Trayendo todo Junto: LinearSystemLoop
LinearSystemLoop combina nuestro sistema, controlador, y observador que creamos antes. El constructor que se muestra también crea una instancia PlantInversionFeedforward
.
72 // The state-space loop combines a controller, observer, feedforward and plant for easy control.
73 private final LinearSystemLoop<N1, N1, N1> m_loop =
74 new LinearSystemLoop<>(m_flywheelPlant, m_controller, m_observer, 12.0, 0.020);
15#include <frc/system/LinearSystemLoop.h>
71 // The state-space loop combines a controller, observer, feedforward and plant
72 // for easy control.
73 frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller,
74 m_observer, 12_V, 20_ms};
68 # The state-space loop combines a controller, observer, feedforward and plant for easy control.
69 self.loop = wpimath.system.LinearSystemLoop_1_1_1(
70 self.flywheelPlant, self.controller, self.observer, 12.0, 0.020
71 )
Una vez que tenemos nuestro LinearSystemLoop
, lo único que queda por hacer es ejecutarlo. Para hacer eso, actualizaremos periódicamente nuestro filtro Kalman con nuestras nuevas medidas de velocidad del codificador y le aplicaremos nuevos comandos de voltaje. Para hacer eso, primero establecemos el referencia, luego corregir
con la velocidad actual del volante, predecir
el filtro de Kalman en el siguiente paso de tiempo y aplicar las entradas generadas usando getU
.
94 @Override
95 public void teleopPeriodic() {
96 // Sets the target speed of our flywheel. This is similar to setting the setpoint of a
97 // PID controller.
98 if (m_joystick.getTriggerPressed()) {
99 // We just pressed the trigger, so let's set our next reference
100 m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
101 } else if (m_joystick.getTriggerReleased()) {
102 // We just released the trigger, so let's spin down
103 m_loop.setNextR(VecBuilder.fill(0.0));
104 }
105
106 // Correct our Kalman filter's state vector estimate with encoder data.
107 m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
108
109 // Update our LQR to generate new voltage commands and use the voltages to predict the next
110 // state with out Kalman filter.
111 m_loop.predict(0.020);
112
113 // Send the new calculated voltage to the motors.
114 // voltage = duty cycle * battery voltage, so
115 // duty cycle = voltage / battery voltage
116 double nextVoltage = m_loop.getU(0);
117 m_motor.setVoltage(nextVoltage);
118 }
119}
5#include <numbers>
6
7#include <frc/DriverStation.h>
8#include <frc/Encoder.h>
9#include <frc/TimedRobot.h>
10#include <frc/XboxController.h>
11#include <frc/controller/LinearQuadraticRegulator.h>
12#include <frc/drive/DifferentialDrive.h>
13#include <frc/estimator/KalmanFilter.h>
14#include <frc/motorcontrol/PWMSparkMax.h>
15#include <frc/system/LinearSystemLoop.h>
16#include <frc/system/plant/DCMotor.h>
17#include <frc/system/plant/LinearSystemId.h>
92 void TeleopPeriodic() override {
93 // Sets the target speed of our flywheel. This is similar to setting the
94 // setpoint of a PID controller.
95 if (m_joystick.GetRightBumperButton()) {
96 // We pressed the bumper, so let's set our next reference
97 m_loop.SetNextR(frc::Vectord<1>{kSpinup.value()});
98 } else {
99 // We released the bumper, so let's spin down
100 m_loop.SetNextR(frc::Vectord<1>{0.0});
101 }
102
103 // Correct our Kalman filter's state vector estimate with encoder data.
104 m_loop.Correct(frc::Vectord<1>{m_encoder.GetRate()});
105
106 // Update our LQR to generate new voltage commands and use the voltages to
107 // predict the next state with out Kalman filter.
108 m_loop.Predict(20_ms);
109
110 // Send the new calculated voltage to the motors.
111 // voltage = duty cycle * battery voltage, so
112 // duty cycle = voltage / battery voltage
113 m_motor.SetVoltage(units::volt_t{m_loop.U(0)});
114 }
87 def teleopPeriodic(self) -> None:
88 # Sets the target speed of our flywheel. This is similar to setting the setpoint of a
89 # PID controller.
90 if self.joystick.getTriggerPressed():
91 # We just pressed the trigger, so let's set our next reference
92 self.loop.setNextR([kSpinUpRadPerSec])
93
94 elif self.joystick.getTriggerReleased():
95 # We just released the trigger, so let's spin down
96 self.loop.setNextR([0.0])
97
98 # Correct our Kalman filter's state vector estimate with encoder data.
99 self.loop.correct([self.encoder.getRate()])
100
101 # Update our LQR to generate new voltage commands and use the voltages to predict the next
102 # state with out Kalman filter.
103 self.loop.predict(0.020)
104
105 # Send the new calculated voltage to the motors.
106 # voltage = duty cycle * battery voltage, so
107 # duty cycle = voltage / battery voltage
108 nextVoltage = self.loop.U()
109 self.motor.setVoltage(nextVoltage)
Angle Wrap with LQR
Mechanisms with a continuous angle can have that angle wrapped by calling the code below instead of lqr.Calculate(x, r)
.
var error = lqr.getR().minus(x);
error.set(0, 0, MathUtil.angleModulus(error.get(0, 0)));
var u = lqr.getK().times(error);
Eigen::Vector<double, 2> error = lqr.R() - x;
error(0) = frc::AngleModulus(units::radian_t{error(0)}).value();
Eigen::Vector<double, 2> u = lqr.K() * error;
error = lqr.R() - x
error[0] = wpimath.angleModulus(error[0])
u = lqr.K() * error