状态空间控制器演练

备注

建议您在阅读本教程之前阅读:ref:docs/software/advanced-controls/state-space/state-space-intro:Introduction to state-space control

本教程的目的是提供有关为飞轮实现状态空间控制器的“端到端”指令。遵循本教程,读者将学习如何:

  1. Create an accurate state-space model of a flywheel using system identification or CAD software.

  2. 实现卡尔曼滤波器以无延迟地过滤编码器速度测量值。

  3. 实现一个 LQR <docs/software/advanced-controls/state-space/state-space-intro:The Linear-Quadratic Regulator>`反馈控制器,该反馈控制器与基于模型的前馈结合时将产生电压 :term:`inputs 1,来驱动飞轮到:term:reference

本教程适用于没有大量编程专业知识的团队。虽然WPILib库在实现其状态空间控制功能的方式上提供了显着的灵活性,但是紧跟本教程中概述的实现方式,应该为团队提供可以在各种状态空间系统中重用的基本结构。

完整的示例在状态空间飞轮(Java <https://github.com/wpilibsuite/allwpilib/blob/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java> __ /C ++)和状态空间飞轮系统标识(Java Java <https://github.com/wpilibsuite/allwpilib/blob/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java>`__ /C ++)中可用。

为什么要使用状态空间控制?

因为状态空间控制关注于创建系统的精确模型,所以我们可以准确地预测:term:model 将如何响应控制 inputs 1。这允许我们在不接触物理机器人的情况下模拟我们的机制,以及轻松地选择我们知道将有效的 gains 2。拥有模型还可以使我们创建无滞后的滤波器,例如卡尔曼滤波器,以最佳地过滤传感器读数。

飞轮建模

Recall 连续状态空间系统是使用以下方程组建模的:

\[\begin{split}\dot{\mathbf{x}} &= \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} \\ \mathbf{y} &= \mathbf{C}\mathbf{x} + \mathbf{D}\mathbf{u}\end{split}\]

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.

建立状态空间系统的第一步是选择系统的状态。我们可以选择任何想要的状态——如果需要,我们可以选择完全不相关的状态——但是选择重要的状态是有帮助的。我们可以在我们的状态中包含 hidden states 1 (比如电梯速度,如果我们只能测量它的位置),并让卡尔曼滤波器估计它们的值。记住,我们选择的状态将被反馈控制器(通常是:ref:Linear-Quadratic Regulator <docs/software/advanced-controls/state-space/state-space-intro:The Linear-Quadratic Regulator>,因为它是最优的) 驱动到它们各自的 references 2

对于飞轮,我们只关心一种状态:它的速度。尽管我们可以选择对其加速进行建模,但是对于我们的系统而言,不需要包含此状态。

接下来,我们识别系统的 inputs 1。可以将输入视为可以放入系统以更改其状态的事物。对于飞轮(以及FRC | reg |中的许多其他单联机构),我们只有一个输入:施加到电机的电压。通过选择电压作为我们的输入(而不是像电机占空比这样的东西),我们可以补偿电池电压下降,电池负载增加。

连续时间状态空间系统将 x-dot`或:term:`system's状态的瞬时变化率,与:term:stateinputs 1 成正比。因为我们的状态是角速度,\(\mathbf{\dot{x}}\) 将是飞轮的角加速度。

接下来,我们将把飞轮建模为一个连续时间状态空间系统。WPILib的线性系统将在内部将其转换为离散时间。回顾更多关于连续时间和离散时间系统的:ref:state-space notation <docs/software/advanced-controls/state-space/state-space-intro:What is State-Space Notation?>

使用系统识别建模

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}\).

\[\begin{split}V = kV \cdot \mathbf{v} + kA \cdot \mathbf{a}\\ \mathbf{a} = \mathbf{\dot{v}} = \begin{bmatrix}\frac{-kV}{kA}\end{bmatrix} v + \begin{bmatrix}\frac{1}{kA}\end{bmatrix} V\end{split}\]

其中:math:mathbf{v} 为飞轮速度,\(\mathbf{a}`和:math:\)mathbf{dot{v}}` 为飞轮加速度, \(V\) 为电压。用 \(\mathbf{x}\) 的标准约定重写此状态向量,用:math:`mathbf{u}`的标准约定重写输入向量,我们发现:

\[\mathbf{\dot{x}} = \begin{bmatrix}\frac{-kV}{kA} \end{bmatrix} \mathbf{x} + \begin{bmatrix}\frac{1}{kA} \end{bmatrix} \mathbf{u}\]

状态空间符号的第二部分将系统的当前 state`和:term:`inputs 1output`联系起来。对于飞轮,我们的输出向量:math:mathbf{y}`(或者传感器可以测量的东西)是飞轮的速度,它也是 state`向量:math:mathbf{x}`的一个元素,因此,我们的输出矩阵是 \(\mathbf{C} = \begin{bmatrix}1 \end{bmatrix}\),系统馈通矩阵是:math:mathbf{D} = begin{bmatrix}0 end{bmatrix}。用连续时间状态空间符号写出这个结果。

\[\begin{split}\mathbf{\dot{x}} &= \begin{bmatrix}\frac{-kV}{kA} \end{bmatrix} \mathbf{x} + \begin{bmatrix}\frac{1}{kA} \end{bmatrix} \mathbf{u} \\ \mathbf{y} &= \begin{bmatrix}1\end{bmatrix} \mathbf{x} + \begin{bmatrix}0\end{bmatrix} \mathbf{u}\end{split}\]

“线性系统”类包含容易创建使用 :term:`system identification`的状态空间系统的方法。这个例子展示了一个kV为0.023 kA为0.001的飞轮模型:

33  // Volts per (radian per second)
34  private static final double kFlywheelKv = 0.023;
35
36  // Volts per (radian per second squared)
37  private static final double kFlywheelKa = 0.001;
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  //
45  // The Kv and Ka constants are found using the FRC Characterization toolsuite.
46  private final LinearSystem<N1, N1, N1> m_flywheelPlant =
47      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);

利用飞轮的惯性矩和齿轮传动进行建模

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++).

备注

对于WPILib的状态空间类,齿轮传动被写为输入的输出-也就是说,如果飞轮的旋转速度慢于电动机的旋转速度,则该数字应大于1。

备注

线性系统类使用 :ref:`the C++ Units Library <docs/software/basic-programming/cpp-units:The C++ Units Library>`来防止单元混淆和断言维数。

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#include <frc/system/plant/LinearSystemId.h>
32  static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
33      0.00032_kg_sq_m;
34
35  // Reduction between motors and encoder, as output over input. If the flywheel
36  // spins slower than the motors, this number should be greater than one.
37  static constexpr double kFlywheelGearing = 1.0;
38
39  // The plant holds a state-space model of our flywheel. This system has the
40  // following properties:
41  //
42  // States: [velocity], in radians per second.
43  // Inputs (what we can "put in"): [voltage], in volts.
44  // Outputs (what we can measure): [velocity], in radians per second.
45  frc::LinearSystem<1, 1, 1> m_flywheelPlant =
46      frc::LinearSystemId::FlywheelSystem(
47          frc::DCMotor::NEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);

卡尔曼滤波器:观察飞轮状态

卡尔曼滤波器用于过滤我们的速度测量使用我们的状态空间模型,以产生一个状态估计:math:mathbf{hat{x}}。由于我们的飞轮模型是线性的,我们可以使用卡尔曼滤波估计飞轮的速度。WPILib的卡尔曼滤波器采用线性系统(我们在上面发现的),以及模型和传感器测量值的标准差。我们可以通过调整这些权重来调整我们的状态估计的“平滑度”。较大的状态标准差会导致过滤器“不信任”我们的状态估计值,更倾向于新的测量值,而较大的测量标准差则相反。

对于飞轮,我们的状态标准偏差为3 rad/s,测量标准偏差为0.01 rad/s。这些值由用户来选择——这些权重产生了一个过滤器,它可以容忍一些噪音,但是它的状态估计可以快速地对*a*飞轮的外部干扰做出反应——应该进行调整,以创建一个适合特定飞轮的过滤器。将状态、测量值、输入、引用和输出随时间变化的图形化是调整卡尔曼滤波器的一种很好的可视化方法。

The effect of a Kalman, median, and IIR filters on some flywheel data.

上图显示了两个不同调谐的卡尔曼滤波器,以及一个:ref:single-pole IIR filter <docs/software/advanced-controls/filters/linear-filter:Linear Filters> 和 一个:ref:docs/software/advanced-controls/filters/median-filter:Median Filter。用射手在约5秒钟内收集了此数据,并且有四个球穿过射手(从四个速度下降中可以看出)。尽管没有关于选择良好状态和测量标准偏差的硬性规则,但通常应该对其进行调整以使模型具有足够的信任度,从而在对外部干扰做出快速反应的同时抑制噪声。

由于反馈控制器使用卡尔曼滤波器估计的 x-hat 来计算误差,因此控制器只会对滤波器的状态估计值变化迅速做出反应。在上图中,橙色曲线(状态标准偏差为3.0,测量标准偏差为0.2)产生了一个过滤器,该过滤器对干扰快速反应,同时抑制了噪声,而品红色过滤器几乎不受速度下降的影响。

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};

由于卡尔曼滤波器在 :ref:`docs/software/advanced-controls/state-space/state-space-observers:Predict step`步骤中使用我们的状态空间模型,因此我们的模型尽可能准确是很重要的。验证这一点的一种方法是记录飞轮随时间变化的输入电压和速度,然后通过仅调用Kalman滤波器上的预测来重放此数据。然后,可以调节kV和kA增益(或惯性矩和其他常数),直到模型与记录的数据紧密匹配为止。

线性二次调节器和设备反转前馈

线性二次调节器 找到了一个反馈控制器来将我们的飞轮 system 驱动到:term:reference。因为我们的飞轮只有一个状态,所以我们的LQR选择的控制律将采用:math:mathbf{u = K (r - x)} 的形式,其中:math:mathbf{K}`是一个1x1矩阵;换句话说,由LQR选择的控制律只是比例控制器,或仅具有P增益的PID控制器。该增益由我们的LQR根据我们通过它的状态偏移和控制努力来选择。更多有关LQR控制器调优的信息可在 :ref:`LQR application example <docs/software/advanced-controls/state-space/state-space-intro:LQR: example application>.中找到。

在给定kS,kV和kA常数的情况下,类似于SimpleMotorFeedforward可以用来生成:ref:feedforward <docs/software/advanced-controls/state-space/state-space-intro:Visualizing Feedforward> 电压输入,而在状态空间系统下,Plant Inversion前馈类可以生成前馈电压输入。由LinearSystemLoop类生成的电压命令是前馈和反馈输入的总和。

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>
54  // A LQR uses feedback to create voltage commands.
55  frc::LinearQuadraticRegulator<1, 1> m_controller{
56      m_flywheelPlant,
57      // qelms. Velocity error tolerance, in radians per second. Decrease this
58      // to more heavily penalize state excursion, or make the controller behave
59      // more aggressively.
60      {8.0},
61      // relms. Control effort (voltage) tolerance. Decrease this to more
62      // heavily penalize control effort, or make the controller less
63      // aggressive. 12 is a good starting point because that is the
64      // (approximate) maximum voltage of a battery.
65      {12.0},
66      // Nominal time between loops. 20ms for TimedRobot, but can be lower if
67      // using notifiers.
68      20_ms};
69
70  // The state-space loop combines a controller, observer, feedforward and plant
71  // for easy control.
72  frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller,
73                                        m_observer, 12_V, 20_ms};

整合在一起:LinearSystemLoop

LinearSystemLoop结合了我们先前创建的系统,控制器和观察器。所示的构造函数还将实例化``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};

一旦我们有了``LinearSystemLoop’’,剩下的唯一要做的就是实际运行它。为此,我们将使用新的编码器速度测量值定期更新我们的卡尔曼滤波器,并对其应用新的电压命令。为此,我们首先设置 reference,然后使用当前飞轮速度进行“校正”,将卡尔曼滤波器“预测”到下一个时间步,然后应用使用“ getU”生成的输入`。

 95  @Override
 96  public void teleopPeriodic() {
 97    // Sets the target speed of our flywheel. This is similar to setting the setpoint of a
 98    // PID controller.
 99    if (m_joystick.getTriggerPressed()) {
100      // We just pressed the trigger, so let's set our next reference
101      m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
102    } else if (m_joystick.getTriggerReleased()) {
103      // We just released the trigger, so let's spin down
104      m_loop.setNextR(VecBuilder.fill(0.0));
105    }
106
107    // Correct our Kalman filter's state vector estimate with encoder data.
108    m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
109
110    // Update our LQR to generate new voltage commands and use the voltages to predict the next
111    // state with out Kalman filter.
112    m_loop.predict(0.020);
113
114    // Send the new calculated voltage to the motors.
115    // voltage = duty cycle * battery voltage, so
116    // duty cycle = voltage / battery voltage
117    double nextVoltage = m_loop.getU(0);
118    m_motor.setVoltage(nextVoltage);
119  }
120}
 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.GetRightBumper()) {
 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  }

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;