Welcome back. In the last video, we studied the Stanley controller for lateral path tracking control. In this video, we will explore an advanced applied control strategy, known as Model Predictive Control or MPC, to understand how to incorporate dynamic modeling into controller design. Specifically in this video, we will describe the MPC architecture and the concept of receding horizon control, formulate an MPC optimization problem for both linear and nonlinear models, and apply MPC to joint longitudinal and lateral vehicle control. Let's dive in. First, let's quickly go through the key aspects of Model Predictive Control. MPC refers to the control design approach that numerically solves an optimization problem at each time-step. Because solving an optimization problem at each time step can take time, MPC was originally applied to slow processes such as industrial chemical processing. However, the ever-improving performance of today's computing hardware has made MPC a viable approach even on embedded hardware. More and more automotive applications are turning to MPC as a way to improve performance and expand operating range for a suite of different embedded controllers, from traction control and stability control, to emission reduction, and idle speed control. Longitudinal and lateral control for autonomous vehicles is another extremely suitable application for MPC. Model Predictive Control is often interchangeably referred to as Receding Horizon Control, since the controller generates an actuator signal based on a fixed finite length horizon at each time-step which receives as time moves forward. The key advantages to solving online optimizations as part of the controller are as follows: The formulation of an MPC controller is straightforward requiring the definition of an objective function and relevant constraints that are then optimized using well-established solvers. The states and control signals can be constrained to stay within safe operating bounds and controls can be selected to maximize multiple objectives simultaneously. Both hard constraints and soft penalties can be employed, leading to a rich set of solutions for constrained control problems. As many automotive subsystems have rigid actuator constraints and diverse performance objectives, MPC has emerged as a major tool for vehicle control. The controller can be explicitly applied to the linear or non-linear models of the vehicle and its subsystems, meaning that we can use the same approach even as our models change or improve over time. The trade-off these advantages must be weighed against, is that MPC requires significantly more computational resources than a Static Control Law. It is certainly possible to create optimization formulations that are too expensive to compute at the high update rates required for smooth vehicle control. Careful implementation is needed to avoid overloading processors. Before we start designing MPC controllers, let's take a closer look at the concept of Receding Horizon. Receding Horizon Control solves a fixed size optimization at each time-step, which identifies optimal control inputs to apply from the current time to the end of the horizon based on the objectives constraints and current state of the vehicle. One issue that arises in implementation however, is that because optimization can take some amount of time, the state of the vehicle when starting the optimization, will be different from the state of the vehicle when completing the optimization. As a results, we must use a predicted state in the optimization for the time at which the control input will actually be applied. Let's step through the process and clarify the notation needed. First, we define the receding horizon length t. Then, we set the initial state for the optimization to be the predicted state at the end of the optimization x at time t. Next, we solve the optimization as the vehicle moves from its current state at time t minus one to x of t using the control input identified in the previous optimization. Although we won't exactly arrive at the predicted state at time t due to disturbances, we do expect to be reasonably close if the time interval is short. Finally, we apply the control signal from the first time step of the receding horizon optimization and repeat the process for the next time step. We can visualize the Receding Horizon or MPC Algorithm, using the following block diagram for a control. We have two main blocks, an optimizer block, which is the core MPC component or a Receding Horizon Control component, and the dynamic model. The model takes in the past inputs and state from time t minus one and takes in the output of the optimizer which is the current sequence of inputs U at each time step in the horizon. The model then outputs predicted states at the next time-step, which are compared to the reference trajectory and passed into the optimizer as the future or predicted error. The optimizer also receives updated constraints and the cost function to use, which can be fixed in advanced or varied based on changing operating modes. The optimizer then solves its optimization and the process repeats. Now, let's take a look at the linear MPC design in particular. We use the state space formulation which represents a motion model in discrete form. The future states are linearly related to the current states and the actuator signals. Note that, A and B are the coefficient matrices and are assumed to be time-invariant. MPC seeks to find a control policy U of inputs over a finite horizon. If all the states are to be driven to zero, the objective function or cost function when we minimize, can be defined as follows: with quadratic error on both deviations of the state from zero and on non-zero control inputs. This is similar to the optimization problem of optimal control theory and trades off control performance and input aggressiveness. Note that, the matrices Q and R are called weight matrices and can be selected to achieve a particular type of response. If instead we need to track a reference signals such as a desired trajectory, we modify the formulation to include the error delta x relative to the desired state. This is a famous optimization formulation and has a closed form solution, the Linear Quadratic Regulator or LQR. The closed form solution uses full state feedback, meaning that all states are used in the control response. The LQR solution defines a control gain matrix K, which can be computed from the A and B matrices of the state-space model and the Q and R matrices of the cost function. We've included links in the supplemental materials to this fascinating result of states-pace control theory. In the more general case, the objective function is any differentiable non-linear function of a state and inputs over the receding horizon. The constraints imposed on the optimization can include; non-linear dynamic models of motion, state and input bounds that capture things like maximum steering angles, and any other inequality constraints g are equality constraints h that affect our system. For such a general optimization problem however, no closed form solution exists. So, we must rely on numerical optimization to find a solution. Even the kinematic bicycle model falls into this category. So, almost all MPC controllers for autonomous driving will be solved numerically. Let's now look at the implementation of an MPC controller for trajectory tracking on a self-driving car. MPC will be used in the same feedback structure presented earlier, but we include the conversion from the tire forces to throttle, break, and steering commands as a low level controller inside the loop. The inputs to the MPC block or the reference trajectory, which include the reference path and velocity, as well as the vehicle states at each time step. The outputs of the MPC block are the lateral and longitudinal forces needed to follow the desired trajectory. These forces are then translated into throttle, breaking, and steering commands, as the output of the low-level control. Finally, the actuation signals are applied to the vehicle at each time-step, and a new vehicle state is achieved closing the feedback loop. The MPC optimization will be set up as follows to perform a double lane change maneuver. First, we define a cost for tracking the desired trajectory, which includes deviation from the desired trajectory and minimization of control command magnitude. Next, we define motion constraints on the vehicle, which rely on the lateral and longitudinal models developed in earlier videos. We also impose maximum limits on the tire forces to restrict them to fall within the linear tire region to avoid extreme responses to control our errors. These costs and constraints define the optimization used in our example, which then gets converted into actual vehicle commands by the low-level controller. It is also possible to incorporate the low-level control into the MPC optimization, which would involve including as constraints, the engine map, full vehicle dynamic models, actuator forces, and tire force models. The result is a large optimization problem that may be challenging to solve in real time, but let's have a look at the results. This simulation is done for the double lane change scenario, where the vehicle first accelerates to a steady-state speed of 17 meters per second or 60 kilometers per hour, then maneuvers four meters to the left, and returns four meters to the right immediately thereafter. The following plots show the results of the simulated maneuver with MPC control, with the reference trajectory in blue and the actual vehicle trajectory in red. We can see that the tracking performance with the MPC controller is excellent, lagging slightly, but without overshoot or oscillation. This is perhaps not surprising as the simulation and MPC use the same model and parameters. The output of model predictive controllers, the lateral and longitudinal forces, can be seen to be smoothly varying and well-behaved. Also, the vehicle yaw rate during the double lane change maneuver is plotted, revealing precise tracking throughout the states of a vehicle. MPC shows a lot of promises a control technique for autonomous driving and can be used without modification for a wide range of operating conditions and a large variety of scenarios. This flexibility and convenience comes at the cost of increased computational requirements and relies on the availability of robust optimization solvers to always return feasible solutions in the available time window. Let's summarize what we've discussed in this video. We first explored the definition of Model Predictive Control and the Receding Horizon and constructed the closed loop block diagram for an MPC system. We then define the costs and constraints used in MPC. Finally, we applied MPC to the trajectory tracking problem for self-driving cars. Congratulations, you've made it to the end of the module. In this module, you learned how to define the lateral path tracking problem, applied two geometric path tracking controllers, the pure pursuit and Stanley controllers to the path tracking problem, and defined a Model Predictive Controller for joint, lateral, and longitudinal control. In the final module in this course, you'll get your first hands on look at the Carlos simulator where you'll design lateral and longitudinal controllers to navigate a vehicle around a race track.
