# Multi-Purpose MPC

1. [Introduction](#introduction)
2. [Implementation Details](#implementation-details)
   1. [Map](#map)
   2. [Reference Path](#reference-path)
   3. [Spatial Bicycle Model](#spatial-bicycle-model)
   4. [Model Predictive Controller (MPC)](#model-predictive-controller)
3. [How-To](#how-to)
4. [Limitations and Outlook](#limitations-and-outlook)

## Introduction

In this repository you find an implementation of a multi-purpose Model Predictive Controller. The controller was implemented as a contribution to the [Automatic Control Project Course (EL2425)](https://www.kth.se/student/kurser/kurs/EL2425) at KTH Royal Institute of Technology, Stockholm. 
The developed algorithm was tested on a 1:10 RC car provided by [KTH Smart Mobility Lab](https://www.kth.se/dcs/research/control-of-transport/smart-mobility-lab/smart-mobility-lab-1.441539). The test scenarios comprised the following three tasks:

1. Reference Path Tracking
2. Time-Optimal Driving
3. Obstacle Avoidance

The controller is implemented in a way that enables its application to all three tasks by merely tuning the weight matrices of the underlying optimization problem. The illustration below shows the obstacle avoidance task in simulation.

The rest of this readme is structured as follows. In [Section 2](##Components) we will present an overview of the entire system and discuss all fundamental components of the implementation in detail. In [Section 3](##How-To) we will provide guidelines for using the implementation in simulation and practice. [Section 4](##Limitations) will be dedicated to analyzing limitations of the current version of the controller and outline potential extensions of the implementation.

## Implementation Details

The multi-purpose control framework presented in this repository consists of four main components. These components are intended to make the implementation as modular as possible, facilitating extensions to certain components without having to alter the overall structure of the framework. An illustration of the components and their interaction is displayed below.

<p align="center"> 
<img src="MPC_Framework.png">
</p>

### Map 

The map class is a handler for the Occupancy Grid Map of the environment. The map is represented as a binary array classifying each cell as either free or occupied. Moreover, the Map class can act as a wrapper around a potential obstacle detection algorithm. By incorporating e.g. LiDAR measurements and updating the Occupancy Grid Map accordingly, new information about the drivable area can be passed to the reference path object.

### Reference Path

The reference path class is where most of the computations are performed. Ultimately, this is where all the available information is aggregated and processed before passing it to the Model Predictive Controller. In our simulation as well as the real-world test scenario, the entire reference path is known in advance. Consequently, the object contains a list of waypoints along the path at a specified resolution. Each waypoint contains information about its location and orientation within the world coordinate frame as well as the local curvature of the reference path. Furthermore, a speed profile can be computed that associated a reference velocity with each waypoint based on a maximum velocity of the car and the curvature of the path.
In order to be able to account for obstacles in the environment, each waypoint has an additional attribute which contains information about the width of the drivable area on both sides of the center-line. This information is computed dynamically from the information provided by the map class. Consequently, the reference path object contains all necessary information to track the reference path while constraints imposed by obstacles in the environment.

### Spatial Bicycle Model

To model the dynamics of the car, we employ a simple kinematic bicycle model. We transform the model into the spatial domain in order to be able to formulate the path tracking problem more intuitively by taking as state variables the deviation from the provided center-line in terms of location as well as orientation. Besides the more intuitivate formulation of the problem, the reformulation of the model allows for including time as a state variable and perform time-optimal driving. 
The implementation of the spatial bicycle model is based on [Stability Conditions for Linear Time-Varying Model Predictive Control in Autonomous
Driving](http://urn.kb.se/resolve?urn=urn:nbn:se:kth:diva-220576) by Lima, Mårtensson and Wahlberg as well as [Towards Time-Optimal Race Car Driving Using Nonlinear MPC in Real-Time](https://www.researchgate.net/profile/Robin_Verschueren/publication/269860931_Towards_Time-Optimal_Race_Car_Driving_Using_Nonlinear_MPC_in_Real-Time/links/56ab66e108aeadd1bdce436b/Towards-Time-Optimal-Race-Car-Driving-Using-Nonlinear-MPC-in-Real-Time.pdf?origin=publication_detail) by Vershuren, De Bruyne, Zanon and Frash. For more details, please consult the original publications.
In our implementation, we use the non-linear spatial bicycle model for the simulation of the car. For the computation of the control signals, we resort to a Linear-Time-Variant formulation of the model based on the reference path.

### Model Predictive Controller

The Model Predictive Controller is based on a Linear Time-Varying model of the car dynamics. The control signal is recomputed at a specified frequency. Based on the current position of the car, the kinematic bicycle model is linearized around the waypoints on the path. The horizon of the optimization problem corresponds to the number of predicted waypoint and can be transformed into a prediction distance using the uniform distance between the waypoints. 
The weight matrices of the cost function to be minimized allow for influencing the expected behavior of the car. Reference path tracking can be performed using large weights on the deviation from the center-line. Time-optimal driving, on the other hand, can be achieved by penalizing the time the car needs to arrive at the last waypoint of the horizon. This objective corresponds to maximizing the velocity of the car along the reference path, instructing the car to cut corners whenever possible. Finally, obstacle avoidance is performed by shifting the reference value away from the center-line towards the center of the detected drivable area. Thus, the car deviates from the original reference path in order to avoid obstacles with a maximum margin.

## How-To

### Simulation

### Real-World Testing

## Limitations and Outlook