# 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 applicability 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](#implementation-details) 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-and-outlook) 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.
本仓库中展示的多功能控制框架由四个主要组件组成。这些组件旨在尽可能模块化实现，方便扩展某些组件而无需修改框架的整体结构。下方展示了各组件及其交互的示意图。
### 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.
Map类是环境占据栅格地图的处理器。地图以二进制数组表示，将每个单元格分类为“空闲”或“被占据”。此外，Map类还可以作为潜在障碍物检测算法的封装器。通过结合例如LiDAR测量并相应地更新占据栅格地图，可以将有关可驾驶区域的新信息传递给Reference Path对象。
### 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 associates 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 respecting constraints imposed by obstacles in the environment.
Reference Path类是执行大部分计算的核心部分。最终，所有可用信息都会在这里聚合和处理，然后传递给模型预测控制器。在我们的仿真和实际测试场景中，整个参考路径都是事先已知的。因此，该对象包含沿路径按指定分辨率排列的一系列路径点。每个路径点都包含其在世界坐标系中的位置和方向信息，以及参考路径的局部曲率。此外，还可以计算速度剖面，根据车辆的最大速度和路径曲率为每个路径点关联一个参考速度。
为了能够考虑环境中的障碍物，每个路径点还具有一个附加属性，用于存储关于中心线两侧可驾驶区域宽度的信息。这些信息由Map类提供的数据动态计算得出。因此，Reference Path对象包含了跟踪参考路径所需的所有信息，同时满足由环境中的障碍物施加的约束。
### 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.
为了建模车辆的动力学，我们采用了简单的运动学自行车模型（Kinematic Bicycle Model）。我们将该模型转化到空间域，以便通过使用位置和方向偏离提供的中心线作为状态变量，更直观地表述路径跟踪问题。除了问题表述更直观外，模型的重新表述还允许将时间作为状态变量，从而实现时间最优驾驶（Time-Optimal Driving）。
### Model Predictive Controller
The Model Predictive Controller is based on a Linear Time-Variant model of the car dynamics. The control signal is recomputed at a specified frequency. Based on the current position of the car, the Spatial 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.
模型预测控制器（Model Predictive Controller, MPC）基于车辆动力学的线性时变模型（Linear Time-Variant Model）。控制信号以指定频率重新计算。根据车辆的当前位置，将空间自行车模型在路径上的路径点附近进行线性化。优化问题的预测范围（horizon）对应于预测路径点的数量，并且可以通过路径点之间的均匀距离转换为预测距离。
代价函数的权重矩阵可以用来影响车辆的期望行为：
参考路径跟踪：通过对偏离中心线的偏差赋予较大的权重，可以实现精确的路径跟踪。
时间最优驾驶：通过惩罚车辆到达预测范围最后一个路径点所需的时间，可以实现时间最优驾驶。这一目标等同于沿参考路径最大化车辆速度，指引车辆尽可能切线转弯。
最后，障碍物规避通过将参考值从中心线移向检测到的可驾驶区域的中心来实现。因此，为了避免障碍物，车辆会以最大安全余量偏离原始参考路径。
## How-To
The implementation is based on a small set of libraries. For the optimization problem of the MPC we chose [OSQP](https://osqp.org) due to its simplicity and efficiency. Below we provide a list of all required libraries.
该实现基于少量的库完成。其中，对于MPC的优化问题，我们选择了 OSQP，因为它简单且高效。以下是所有所需库的列表：
```
osqp
numpy
scipy
skimage
matplotlib
```
### Simulation
To start the simulation, go to ```simulation.py```. This script provides a template for loading a map image, specifying a reference path, setting up the motion model and running the simulation. This script was used to generate the GIF displayed above. All simulation parameters can be changed in this script, allowing to expolore the capabilities of the algorithm in different scenarios. Furthermore, the environment can be modified by adding obstacles and boundaries to the map. The modular structure of the implementation allows for an intuitive setup of the simulation, centered around the two functions ```u = mpc.get_control()``` and ```car.send_control(u)```.
要启动仿真，请打开 simulation.py 文件。此脚本提供了一个模板，用于加载地图图像、指定参考路径、设置运动模型并运行仿真。该脚本用于生成上方显示的GIF。所有仿真参数都可以在该脚本中更改，从而能够在不同场景下探索算法的功能。此外，还可以通过在地图中添加障碍物和边界来修改环境。
实现的模块化结构使仿真的设置更加直观，核心围绕以下两个函数展开：
u = mpc.get_control()：获取控制信号。
car.send_control(u)：将控制信号发送给车辆。
### Real-World Testing
In order to test the controller on a real car, we adapt certain components of the implementation to a ROS framework provided for the communication with the vehicle. Again, the modular structure facilitated a quick adaptation. For example, the pose attribute of the spatial bicycle model subscribes to the topic published to by the localization node. The map object is modified by an obstacle detection algorithm that subscribes to the LiDAR data collected by the car. Furthermore, the Spatial Bicycle Model is modified to include a low-level control interface that sends the computed control signals to the respective actuators. We chose not to include the code for the real-world test in this repository as most of the code is tailored towards the proprietary software of the RC car.
为了在真实车辆上测试控制器，我们将实现的某些组件适配到一个用于与车辆通信的ROS框架中。模块化的结构再次使这种适配变得快捷。例如，空间自行车模型的位姿属性会订阅由定位节点发布的主题；地图对象则通过一个订阅车辆采集的LiDAR数据的障碍物检测算法进行修改。此外，空间自行车模型还进行了调整，加入了低层控制接口，用于将计算的控制信号发送至对应的执行机构。
由于大部分代码针对遥控车的专用软件进行了定制，我们选择不在该仓库中包含真实测试的代码。
## Limitations and Outlook
The test scenarios, in simulation as well as real-world, provide valuable insights into limitations of the controller and illustrate how modifications to the algorithm might improvide performance. The greatest limitation is to be seen in the Kinematic Bicycle Model which doesn't account for a number of physical effects such as torque, traction and tire slip. Moreover, the car is assumed to be velocity-controlled, neglecting the dynamics of the low-level speed controller. We are looking to extend the implementation to include a dynamic bicycle model in the future. We expect a more advanced model to significantly increase the performance on Time-Optimal Driving. Using the current model, optimality corresponds to finding the shortest path. The algorithm is expected to display much more realistic behaviour upon the inclusion of additional physical effects.
A second limitation is related to the task of obstacle avoidance. The avoidance maneuver is entirely based on modifying the constraints of the optimization problem, limiting the allowed deviation from the center-line to the drivable area. This algorithm proves to work very well in simulation. However, uncertainties in the LiDAR measurements which lead to spurious elements in the dynamically updated map data, can lead to inconsitencies in the prediction. This is especially problematic in the case of obstacles that can be avoided on both sides. We are looking to introduce a more robust update of the driveable area constraints that ensures consistency across prediction horizons.
测试场景（包括仿真和现实场景）提供了关于控制器局限性的宝贵见解，并展示了算法改进如何提升性能。最大的局限性在于运动学自行车模型（Kinematic Bicycle Model），因为它未考虑诸如扭矩、牵引力和轮胎打滑等多个物理效应。此外，车辆被假设为速度控制，忽略了底层速度控制器的动力学特性。我们计划未来扩展实现，包括一个动态自行车模型（Dynamic Bicycle Model）。我们预计，更先进的模型将显著提高时间最优驾驶的性能。当前模型下，优化目标等同于找到最短路径，而引入额外的物理效应后，算法将表现出更加真实的行为。
第二个局限性与障碍物规避任务有关。当前的规避操作完全基于修改优化问题的约束，将中心线偏离限制在可驾驶区域内。该算法在仿真中表现良好。然而，LiDAR测量中的不确定性可能导致动态更新的地图数据中出现错误元素，从而引发预测不一致性。这在可以从两侧绕过的障碍物情况下尤为严重。我们计划引入一种更鲁棒的可驾驶区域约束更新方法，以确保预测范围内的一致性。