Nonlinear MPC Design with Model Predictive Control Toolbox and FORCESPRO | Understanding Model Predictive Control, Part 8

Understanding Model Predictive Control, Part 8: Nonlinear MPC Design with Model Predictive Control Toolbox and FORCESPRO

In this video, we’ll design a nonlinear MPC controller for a lane following application. For control design, we’ll first show you how to use the nonlinear MPC block which solves a nonlinear optimization problem using the “fmincon” algorithm from Optimization Toolbox. We’ll then demonstrate how to use Embotech’s Forces PRO nonlinear solver with Model Predictive Control Toolbox. This capability requires a publicly available plugin and an Embotech FORCESPRO license.

As we discussed in part-4 video, if we have a highly nonlinear plant that cannot be approximated by a linear model, or we have nonlinear constraints and a cost function, then a linear MPC controller won’t be sufficient to control the plant. Instead, we can use a nonlinear MPC controller which works similar to traditional MPC in the sense that it uses a prediction model and solves an optimization problem to compute the control actions. The key difference to traditional MPC is that nonlinear MPC lets us use a nonlinear prediction model and have nonlinear constraints and a non-quadratic cost function. Note that we also need to use a nonlinear estimator for estimating system states.

We’ll now take a look at this lane following example which consists of these three subsystems and currently lacks the controller under the “Lane Following Decision Logic and Controller” subsystem. Before we start with control design, let’s discuss what these modules are implementing. This one models the nonlinear lateral and longitudinal vehicle dynamics using a bicycle model. It takes two inputs, the steering angle and acceleration computed by the controller and outputs the longitudinal velocity along with the vehicle pose containing vehicle’s position and orientation. The other subsystem takes outputs generated by the vehicle dynamics module and implements a traffic scenario with 3D visualization using Unreal engine and a front facing camera sensor that reads frames from the unreal environment. The pre-defined open-loop scenario used in this example contains a single vehicle traveling at a pre-set velocity of 14 m/s along a curvy road. The front view images captured by the camera are used to detect the lane boundaries which becomes an input to the lane following decision logic and controller. This module is responsible for controlling the acceleration and steering angle of the vehicle to keep it along the center of the lane while maintaining its longitudinal velocity at a desired value. This picture depicts the lane following scenario and shows how the system parameters are defined. To keep the vehicle along the lane center, the controller needs to bring the lateral deviation and relative yaw angle close to zero. These two values are computed by these modules. This one takes the lane boundary information and estimates the lane center which is then used by the next module to compute the lateral deviation and relative yaw angle. The “Preview Curvature” subsystem also takes the longitudinal velocity and uses it along with the lane center to detect the road curvature sequence over the prediction horizon of the MPC controller which we’ll discuss shortly.  

Now that we know how the different modules of this lane following application work, we’ll move on to the controller design. To control the nonlinear vehicle dynamics, we will use the nonlinear MPC block from Model Predictive Control Toolbox. To use this block, we need to create a nonlinear MPC object and specify it in the block dialog. We will now go over this script which shows us how to create the nonlinear MPC controller and define its parameters. The first section specifies the parameters for the vehicle, scenario and sensor. The second section starts by setting the longitudinal velocities. The initial velocity is 10 m/s and the target velocity we need to keep the vehicle at is 14 m/s.  As we discussed previously, the nonlinear MPC controller uses a nonlinear prediction model to predict the future plant behavior. Here, we’re using the “nlmpc” command to create the nonlinear MPC controller with a nonlinear prediction model that has seven states, three outputs, two manipulated variables, one measured disturbance and one unmeasured disturbance. After creating the controller, we need to specify the state and output functions for the nonlinear prediction model. In this picture, we see the state and output equations that are defined in the highlighted functions. These are the states. The MVs are the acceleration and the steering angle. The measured disturbance is modeled as the product of the road curvature and the longitudinal velocity. The prediction model also includes an unmeasured disturbance modeled by white noise which gets added to the relative yaw angle output as seen in the output equation. The other outputs are the longitudinal velocity and lateral deviation that we’ve discussed previously.

The state and output equations that we’ve just seen are defined in the state and output functions, respectively. The highlighted code shows how these functions are specified for the prediction model. The best practice is to also provide an analytical Jacobian for the state and output functions which helps with increasing the computational efficiency. If analytical Jacobians are not available, the nonlinear MPC controller computes them numerically.

In the next step, we set the design parameters for our nonlinear MPC controller. The controller sample time is set to 0.1 s, the prediction and control horizons are set to 30 and 2, respectively. Here, we define the constraints for the steering angle and acceleration. In this example we have linear constraints but note that you can also specify nonlinear constraints for your nonlinear MPC problem. In this section, we set the scale factors for the outputs, manipulated variables and measured disturbance. And finally, we specify the weights in the MPC cost function. In this example, we are trying to control three output values with only two manipulated variables leading to a non-square system. Therefore, we’re prioritizing the reference tracking of two of the outputs, the longitudinal velocity and lateral deviation by setting their associated weights to non-zero values. We also set the weights for the MVs and MV rates. The selected weights for MV rates are to ensure a smooth ride by letting the acceleration and steering angle change in small increments. The zero weights for the MVs indicate that they don’t need to track any reference values. If MVs had targets, we would select their weights to be much smaller than OV weights to prevent conflicting goals between OV and MV reference tracking. Once we’re done with the controller design, we can validate the controller functions at an arbitrary operating point. This is to test the prediction model functions for any potential problems.

Next, we run this script and then go back to our model to specify the nonlinear MPC controller in the controller block dialog and connect its inputs and outputs. The nonlinear MPC controller requires an estimate of the system states for which we’re using an Extended Kalman Filter. This block uses the same state and output functions as the nonlinear MPC controller. The next input is the reference values that we want to control the outputs at. We want to keep the longitudinal velocity at the previously defined target value and drive lateral deviation and relative yaw angle to zero. The nonlinear MPC block also requires the MVs from the previous control interval. We’re feeding them back to the controller with a unit delay. We also input the current MVs to the Extended Kalman Filter block as they’re required for state estimation calculations. The MVs computed by the controller are the steering angle and acceleration which we connect as seen here.  Finally, we’ll input the measured disturbance which we’ve previously defined as the product of the longitudinal velocity and the road curvature.

Now that we’re done with the control design, we will simulate this system and see how well the nonlinear MPC controller handles the constraints and performs reference tracking. This model is configured to log the signals of interest like the outputs and MVs which we’ll later view using the data inspector. Once the simulation starts, it generates multiple visualizations using images captured by the front facing camera. In this video, we see how the vehicle stays within its lane. The other videos show us how the lane detection works. This is the raw segmentation, and the other videos show the detected left and right lane boundaries in red and green, respectively. We’ll now switch to the data inspector to look at the logged signals during simulation. The plots show the measured outputs and how they compare to their reference values. The vehicle starts at 10 m/s and quickly converges to the target velocity which it tracks successfully in the rest of the simulation. The lateral deviation and relative yaw angle are kept close to zero. This implies that the vehicle is traveling along the lane center which we already confirmed with the 3D vehicle simulations. We can also view how acceleration and steering angle computed by the nonlinear MPC controller change to keep the outputs at their desired values. The plots also display the constraints that we specified for the MVs. We see that both MVs stay within the specified lower and upper bounds. In this timeframe, the acceleration hits the upper bound while the controller tries to speed up the vehicle from 10 to 14 m/s. But throughout the simulation, the acceleration stays within the specified upper and lower bounds. Similarly, the steering angle meets the specified constraints. In the next step, you can generate C code from your controller and deploy it for real-time control which we’ll discuss in more detail in a later video. Next, we’ll hear from Thiva on how to use the Embotech FORCESPRO solver for the same nonlinear MPC problem.

Thanks a lot Melda for showing us how to formulate the nonlinear MPC problem with the Model Predictive Control Toolbox. Now, we are going to design a FORCESPRO solver for this application.

We, MathWorks and Embotech have jointly designed a plugin so that there is a straight-forward workflow to combine MPC Toolbox and the Embotech FORCESPRO solver. The MathWorks’ Model Predictive Control Toolbox allows easy design and validation of the nonlinear MPC algorithm. Additionally, with the workflow that we will present now, we will obtain a tailored FORCESPRO solver for the specific NMPC application and target platform. FORCESPRO is developed for embedded MPC problems, where computation time, reliability, and memory efficiency are important. After generating the solver, we can use it again within the MathWorks environment, particularly in MATLAB, Simulink or the Model Predictive Control Toolbox. If you want to use this workflow, you will need an Embotech FORCESPRO license.
As a first step, let us revisit the lane following example. We are going to use the same MATLAB code for defining the Nonlinear MPC object as we have seen before. There is just one difference: We do not specify the Jacobian functions. For the plugin workflow we will use an automatic differentiation tool called CASADI. This tool will directly calculate the Jacobian functions so that we do not need to provide them. The remaining parts stay unchanged. So, this includes defining parameters like the horizon, the cost function, and the model. After defining the NMPC object, we can set up the FORCESPRO-solver for this optimization problem. We construct the option struct and label it. This allows us to set the options for the solver. First, we set an initial guess for warm starting of the solver. We can also choose the optimization algorithm that is used within the solver. We will take the interior point option here. Alternatively, there is also the option of using the sequential quadratic programing algorithm.  After setting all options, we can use the command “nlmpcToForces”. The command generates a tailored solver. The first input to the command defines the NMPC problem and the second input contains the options struct with all the settings of the solver. As an output of the function we receive the struct “coredata” and “onlinedata”, which are the structs that we need for running the MPC solver. Now, we are ready, so that we can run the command “nlmpcToForces”. After the solver generation is finished, we will find all the necessary files like the compiled library and the mex versions in our folder.

To run the generated solver within MATLAB, we use the command “nlmpcmoveForces”. The input “coredata” contains information about the constant part of the MPC solver, for instance the prediction horizon. Additionally, we input “x” which are the current system states and “lastMV” which are the last actuated values. With “onlinedata” we input runtime parameters such as the reference values and the initial guess for the solution. Now, we can run the command “nlmpcmoveForces”. After the calculation is ready, we receive as output the optimal actuated values. We can also have a look at the info structure. It gives us for example an exitflag, which is 1 here, meaning that the problem was successfully solved until convergence. The info struct also shows us the overall computation time. The optimization problem of the lane following example is solved in only 4.8 msec which demonstrates the computational efficiency of FORCESPRO.

Next, we will integrate the FORCESPRO-based NMPC controller into the lane following Simulink model. We will use the Simulink model that we have already seen before. There we will integrate the FORCESPRO Simulink block. This block is included with the FORCESPRO client installation. In the option, we need to set “coredata” which was the struct that was created while generating the solver. We also specify that we have measured disturbances. We see that other options are available as well such as outputting the optimal cost, the optimal control sequence, or the status of the nonlinear program. For integrating the FORCESPRO solver into our Simulink lane following model, we just replace the NMPC controller that is based on “fmincon” with the one that is based on FORCESPRO and that is it already.

Now, we can run the closed-loop control simulation. We can validate the control behavior by looking at the graphical simulation. The vehicle stays properly in the lane. Additionally, we can have a look at the data inspector. The plots show that the various reference values can be tracked with very high performance. In fact, beside small numerical differences, the same closed-loop behavior as with “fmincon” is achieved. At the same time, FORCESPRO has very small computation times and is very memory efficient.

Alright, so this was an overview on the entire workflow for integrating the FORCESPRO solver into your nonlinear MPC project with the Model Predictive Control Toolbox. With the workflow that we have developed, there is an easy process available to achieve this. Now, I will hand over to Melda again.

In this video, we’ve showed you how to design a nonlinear MPC controller for a lane following application using the Model Predictive Control Toolbox. We’ve also discussed how to design a Forces PRO solver for the same application. For more information on model predictive control, please check out this page and the links given in the video description.