What Does it Take to Land a Rocket? Part 2 — The Challenge is Bigger than I Thought
The final code and results can be found here: https://github.com/jamesHo22/VTVLRocket/tree/main/main_functions
The textbook I reference, “Advances in Industrial Control,” can be found here: https://www.springer.com/series/1412
Rocket landing with Unconstrained MPC
Updates Summary / TLDR
Sometimes you will never know how deep the deep end is without jumping in. Since the previous post, I have been swimming in the deep end reading up on different control strategies that can potentially solve this optimization problem. A very important requirement was that the controller has to account for hard input constraints. This means that it should not request infeasible amounts of thrust from the engine or unattainable gimbal rates from the actuators. Because I wanted a controller that can handle these constraints, I ended up learning about model predictive control (I haven’t yet implemented the constrained optimization in this post).
(I have to disclaim that I have not considered the suitability of other control architectures and will be something I dive into in the future. I am just beginning to learn about control theory and don’t yet understand which controllers work best for a given system. If you have suggestions or resources, please let me know.)
To recap from the previous post, model predictive control is a control method that optimizes a control input subject to constraints. In summary, for every time step, the MPC controller looks at the current state of the system, the previous input, the desired output, and calculates the optimal input trajectory for the next n time steps. It then picks the first input and implements that one. It does this at every time step.
I was able to implement a discrete time, unconstrained MPC controller for regulating (getting the state to zero) the inverted rocket and understand the high level math behind it. The limitations with this implementation is that it cannot track a reference trajectory yet and is still unconstrained. I also cannot specify specific penalty weights for the actuation energy or state error (something you can easily do with LQR). I also don’t have a way of quantifying the performance of the system yet, making it difficult to evaluate different controllers.
There are also still a few bugs with it. For instance, when I increase the mass of the rocket and keep all the other parameters the same, it becomes unstable.
Approach Update
I could very easily sink all my time into “perfecting” the MPC controller and never make it past simulation. To keep the project on track, my next steps are to start building hardware to test these controllers on. Something I have learned from working on my formula electric racing team is that you should always validate your models with tests. Without grounding your model with experimental data, the results it produces are only as good as its assumptions, which by definition are not true.
To get ready for this process, I will be building a 3D linear rocket state space model and an LQR controller to stabilize it. I will then begin the mechanical design process. As I work on these subsystems, I continue to build on the MPC controller and learn about other control methods.
The rest of this post will walk through the math and code behind MPC.
MPC Explained Intuitively
Simply put, model predictive control computes an optimal control signal by predicting what will happen in the near future. A real life example of this could be you planning out your work day. Before you start, you need to know how well you work and how the effort you put in affects the state of the task. This is analogous to the system model. You also need to know the current state of your task. This is analogous to the state of the system. You also need to know what resources you can access. These are your system constraints.
You start the day at 10:00 am and look at the current status of your work. Given this information and knowledge of how you work, you plan ahead for the next 5 hours. You then implement the first hour of your plan. At 11:00 am, you check how much work you’ve actually done and plan for the next 5 hours again. While planning, you also keep in mind what resources you have access to so you don’t demand anything you cannot physically do. You keep repeating this process until your tasks are complete.
The key pieces of information are:
- The time window for the planning (prediction horizon)
- The current status of the work (system)
- Constraints on what work you can do (constraints)
This is the essence of model predictive control. Now we will derive and implement the unconstrained version of MPC.
Mathematical Representation and Implementation
Linearizing about fixed points
We start off with our state space system of the rocket in the form of x_dot = Ax + Bu. The first step is to linearize our system about a fixed point. This is done by taking the Jacobian of the A matrix with respect to the state variables and evaluating it at the fixed points.
Example of finding the Jacobian of a matrix with respect to x
Finally, we plug in our fixed points and compute the matrix. Here is the code that does that:
Creating the Augmented Model
Now that we have our linearized dynamical system, we can begin creating a control law for it. We now define the C and D matrices, which are the output and feedthrough matrices respectively. These two matrices are used to calculate what the system’s output is. In our case, we are going to set C to a square identity matrix of size 6*6. This means that the controller can observe the full state of the system. D is a zero matrix of size 6*2. This means that the current output cannot affect the current state. Since we need the current information of the system for prediction and control, we have implicitly assumed that the input cannot affect the output at the same time [Advances in Industrial Control, pg 4, section 1.2.1].
The next step is to convert it from a continuous time system to a discrete time system. This will allow us to create a discrete MPC controller for it and later implement it on a microcontroller. This is done by using this MATLAB command.
Next, we want to write these matrices in a form that tells us the difference in the state vector from one time step to the next. This form will also help us later on when we need to compute the optimal control inputs. The full derivation can be found on pages 4 and 5 in “Advances in Industrial Control (Liuping Wang),” so I will not go over it in detail here. We end up with an augmented state space model.
Here is the MATLAB code that creates the augmented model, which we will use in the design of predictive control:
Predictive Control
Using this model, we can predict what the state will be in the next Np sample steps given the current state and input. The derivation can be found on page 7 and 8 in “Advances in Industrial Control (Liuping Wang).” In summary, x at k + Np where k is the current time step and Np is the prediction horizon…
We compute the output matrix y at k + Np in a similar manner. Again, the derivation can be found in the book on page 8.
If we rewrite these equations into a compact matrix form, we end up with the following…
This allows us to predict the outputs of the system for Np time steps into the future given the current state and a set of inputs, Delta U.
The following code does this:
Optimization
Finally, we can optimize our system to find the best control input. To do this, we start by defining a set point and a cost function. In our case, we set the set point to zero and define our cost function as
Where Rs is the setpoint and R_bar is an identity matrix multiplied by a control input penalty. The larger the penalty, the less energy our controller will try to use to reach the setpoint.
Here, Nc is called the control horizon. It is the number of control steps we want to optimize for. It has to be less than or equal to the number of prediction steps (Np). Once again, I will refer to the textbook for the derivation (pg. 9).
It can be shown that the optimal control signal is…
The final step is to take the first control signal and apply it to our system. This is one iteration of the system. We perform this optimization at every time step, implement the first control signal, recompute the state and output, and repeat. Page 15 in the textbook walks through a SISO implementation of this algorithm. The code I have written adapts it to work with MIMO systems.
The following code snippet simulates the linearized discrete MPC system and controller:
The complete code can be found on my github page.
Results and Limitations
Finally, we can simulate our system! Below is a video of the rocket starting at (50, 100) m and going to (0, 0).
There are still a number of limitations with this implementation. First, the controller only regulates the system (makes the state vector zero). Ideally, we want to be able to track any trajectory. The simulation is also calculated on the discrete linearized system, which is far from the actual nonlinear system. Finally, I am having a few issues with the system becoming unstable when the mass of the rocket increases. I am not sure if this is an issue with my implementation or a matter of tuning the parameters.
I am also still learning about concepts that will allow me to quantitatively evaluate the performance of a controller. This is important for comparing different control laws and assessing the stability of the system.
Conclusion
If you made it this far, congratulations! A lot of the algebra I breezed over is very clearly described by Wang in the “Advances in Industrial Control” book I reference. I heavily recommend reading it if you’re interested in a step by step walk through of the math behind the controller.
Next Steps
- Implement 3D rocket model
- Control it with LQR
- Begin mechanical design of the rocket
