Nonlinear MPC Deployment to Speedgoat Hardware for Real-Time Testing | Understanding Model Predictive Control, Part 9

Previously we designed a Nonlinear MPC Controller for a lane following application, and assessed the performance of the controller through desktop simulations. We demonstrated the use of the Nonlinear MPC block that implements MPC using the fmincon algorithm from the Optimization Toolbox. We also showed you how you can easily update the model to implement MPC using Embotech's FORCES PRO nonlinear solver, that is optimized for real-time MPC applications.

In this video we will show you how to generate code from the Nonlinear MPC algorithm for real time testing. Starting with this model, I will first show you how to generate code from the controller. And then Timo from Speedgoat will discuss code generation and deployment of the FORCES PRO based MPC algorithm to Speedgoat hardware for real-time testing.

In this model I will generate code from the Lane Following Decision Logic and Controller, which contains these blocks you see here, including the non-linear MPC Controller and the state estimator. To generate code from the subsystem we right click and select C/C++ Code, then Build This Subsystem. Once the code is generated, we get this report.

This includes the traceability report, where we can view the code associated with the Nonlinear MPC block or any other blocks. Next, Timo will show us how he sets up the FORCES PRO based MPC applications for code generation, how he generates code, and finally deploy on Speedgoat real-time target machines.

Thank you, Melda. I'm going to use your MPC lane following example with the FORCES PRO solver introduced in the previous video, which is optimized for real-time execution. I'll show how to run this application on Speedgoat real-time target hardware, using automotive I/O interfaces for data transfer. But first, let me share some motivation on why we want to run our model in a real-time environment.

Testing your controller in real-time allows you to assess its embedded performance. You can verify I/O communication, and run safety critical tests in a safe environment. You also can connect it to other hardware components through I/O interfaces. This is vital for Rapid Controller Prototyping-- using the real car-- or Hardware-In-The-Loop testing of your electronic control units.

Let's start with the same Simulink model we used in the previous video, which includes the Simulation 3D Scenario, the Lane Following Decision Logic and Controller, and the Vehicle Dynamics. But this time we don't run the entire model on a single system. Instead, we partition the model to run on multiple machines, and establish communication using automotive I/O buses.

This is closer to a real world testing scenario-- where your car incorporates many different components which exchange data over automotive passes-- such as the Controller Area Network, or CAN for short, or Ethernet. We can test and verify this I/O communication, and also take into account the latency due to transfer delays by separating the controller from the plant model in this way.

The Simulation 3D Scenario subsystem creates the 3D environment and front facing camera image in Unreal Engine. This part remains on the host computer, and communicates with the Speedgoat real-time target machines via Ethernet using the User Datagram Protocol, or UDP.

The Lane Following Decision Logic and Controller subsystem runs on the baseline real-time target machine, and the Vehicle Dynamics simulation runs on a second machine of the same type. The two are connected via the automotive CAN bus. To achieve this partitioning to three separate systems, we need to create three separate Simulink models and set up the communication interfaces.

Let's start with the Simulation 3D Scenario. The inputs and outputs need to be sent and received over Ethernet via the host target link. So we use Simulink blocks to pack the lane data into a UDP packet, and send it to the target machine running the MPC Controller. Similarly on the incoming side, we unpack the incoming UDP data packets to get the vehicle posts for visualization.

For the host model we also activate simulation pacing, to make sure the model runs as close to real-time as possible. And this already completes the first of our three models. Let's continue with the controller subsystem, which we will deploy to a real-time target machine.

The first inputs here are the Lane Detections arriving from the host computer via UDP, which we unpack. The controller also needs the current longitudinal velocity of the vehicle. We will use the CAN protocol for communication between the controller and the vehicle.

For this we use a CAN read and an unpack block, where I have manually configured the CAN message containing the velocity signal. If you have a database defining your CAN buses you can also import a DBC file here, and simply select the respective signals.

With the setup block we can configure the channels and board rates of the module. The acceleration and steering commands need to be sent out to the Vehicle Dynamics model, so this will also use the automotive CAN bus. We pack the data, and then send them using the CAN write block.

The FORCES PRO Nonlinear MPC block still uses the same data structure we created in the previous video for the control algorithm. To be able to generate code for our real-time target machines, we just need to add one parameter to our initialization script. We need to set the ForcesTargetPlatform to Speedgoat-QNX. For a detailed explanation of the other steps to set up your MPC Controller please refer to the previous video, Part Eight, of the MPC video series.

Lastly, we need to set our code generation target for the model. For Speedgoat target machines, you can simply click the Simulink real-time shortcut from the Apps tool strip. This adjusts a few settings in the model configuration parameters. Namely, setting the solver type to Fixed-step, and setting the code generation target to Simulink real-time.

And this completes the model of our MPC Controller. The last part which is left now is the Vehicle Dynamics model. It's pretty much the same process, so let's just take a brief look. I've added interfaces to receive the acceleration and steering angle signals from the controller, over the CAN bus.

The vehicle dynamics model uses this to simulate the movement of the vehicle. And afterwards the relevant vehicle state information is sent out to the controller over the CAN bus, and to the host computer for visualization over UDP. And finally-- since this model also runs on a real-time target machine-- we set the model configuration parameters accordingly.

We are now ready to build and deploy the model as a real-time application, which we can do using the respective buttons on the Simulink tool strip. Once the build process completes, we can inspect the code in the code generation report if we want.

We connect to the Speedgoat real-time target machine with just one click, and then we can load the application using the Deploy to Target button.

For our other Simulink real-time model with the MPC Controller, I also perform the code generation and deploy the real-time application to the second target machine. The two machines are connected with CAN cables to enable the data exchange. The 3D Scenario Simulation runs on the host computer in regular desktop simulation.

To start both real-time systems when we run the host model, I'm going to use callback functions of the host model. In the start function I'm adding two commands to also start both real-time target machines. Similarly, we stop the target machines when we stop execution of the host model.

I have also added an interface to visualize the steering angle on a Logitech Force Feedback racing wheel, which we can control using Simulink blocks. Now we are ready to run the model, as both real-time target machines have a loaded application ready to start.

We start the host model using the Run button, and our callback function starts the real-time applications as well. You can see a live capture of the steering wheel, and the main Unreal Engine window opens automatically. We can also see the front facing camera image and the results of our World's Eye Transform and the Lane Detection algorithm.

Now don't be alarmed. With our initial parameters, the controls are actually not performing well in the real-time setup. The additional communication latency leads to unstable behavior, but this is exactly why we run such tests. So let's stop the model and see if we can fix the issue.

To improve the stability of the controls, I went back to the control parameters and reduced the ControlHorizon from 2 to 1. Let's skip the process of rebuilding the models, and see right away how the updated controller performs in real-time.

We can see that the steering is much more stable now, and the car stays well in between the lane markings while cornering. The data exchange between all three systems is working correctly. We can also check the task execution time in the Simulink real-time explorer to assess the computational load of our applications.

The TET Monitor shows that the MPC Controller causes roughly 25% CPU load, while the load from the Vehicle Dynamics model is barely even visible. For detailed data analysis we can, of course, log data from each system, same as we would during desktop simulation.

If you select signals for logging, they show up in the simulation data inspector grouped by each platform. We can analyze, for example, how well our controller is tracking the commanded velocity, the lateral deviation from the Lane Center, and the relative yaw angle of the vehicle.

Log signals can be streamed live while the model is running, or logged to the internal SSD of the target machine for later analysis. Now let's recap what we have seen in this video. After adding just one line of code to the definition of the MPC Controller, we can use the same Simulink model to generate code for real-time execution on Speedgoat target machines.

We have seen how to partition a model to run on multiple machines, and how to easily set up I/O communication with Simulink driver blocks. We saw how to build, deploy, and run models on Speedgoat machines directly from Simulink. And last but not least you can easily log data from your real-time simulation, and directly use the powerful tools from MATLAB and Simulink for data analysis.