# PID Controller Design for Steering and Throttle Control of a Self-Driving Car

This project develops a PID controller for Udacity’s self-driving car simulator.  The project is implemented in C++ and communicates with the simulator using a local WebSocket. The simulator sends the cross track error, velocity and orientation information to the controller which sends back the steering and throttle values to control the simulated car along the reference trajectory.

### Controlling a Self-Driving Car

A self-driving car (SDC) can be seen as a collection of various different subsystems connected in the form a pipeline to ultimately fulfill the task of autonomous locomotion. The main subsystems of this pipeline are computer vision, sensor fusion, localization, path planning, and control. Computer vision together with the sensor fusion (LIDAR, Radar) provides the SDC with a sense of its environment by gathering data of its surrounding. Localization uses this sensory data along with the information about the map to derive the probable location of the SDC on the map. The path planning component utilizes the vehicle location to compute an optimal trajectory to drive the SDC towards its destination.

At this stage, based on the reference trajectory generated by the path planning subsystem,  the SDC controller controls the throttle (and braking) and steering of the car to make the car to move along the reference trajectory.  There are many ways to implement the control system for the SDC. These implementations can range from AI based (using deep learning), non-AI based (PID Controller, Model Predictive Control) or a combination of multiple controllers. The most basic controller used in such a scenario is PID controller. Now let’s see how we can implement a PID controller for an SDC.

### PID Controller for Steering a Self-Driving Car

Let’s say the path planning subsystem generates a certain reference trajectory as shown in the figure and instead of moving along this reference trajectory, the SDC is moving along some other trajectory at a velocity $v$. The cross track error ($CTE$) denotes the SDC’s current deviation from the reference trajectory. Now the role of the PID controller is to somehow reduce $CTE$ to enable the SDC to move along the reference trajectory. This is done by controlling the steering of the SDC with PID controller.

At any instant $t$, PID controller takes cross track error $CTE(t)$ as its input and returns the steering angle ($\alpha(t) = u(t)$) as its output. The $CTE(t)$ is the difference between the reference trajectory ($r(t)$) and SDC’s current trajectory $y(t)$ i.e. $CTE(t) = r(t) - y(t)$. The PID controller output can be calculated as:

$u(t) = \alpha(t) = - K_P CTE(t) - K_I\int_{0}^{t}CTE(t)dt - K_D\frac{d(CTE(t))}{dt}$

The component $K_P CTE(t)$ provides proportional feedback, $K_I\int_{0}^{t}CTE(t)dt$  provides integral feedback and $K_D\frac{d(CTE(t))}{dt}$ differential feedback to adjust the steering angle ($\alpha(t)$. Here $K_P$$K_I$ and $K_D$ are called proportional, integral and differential gains.

### Role of P, I and D feedback components

1. P Component: This component adjusts the steering angle based on the current cross track error ($CTE(t)$). By doing so the SDC steers towards the reference trajectory resulting in reduced ($CTE$). But it does not consider the inertia of the SDC, which ultimately leads to overshooting.
2. D Component: This component adjusts the steering angle based on the time-differential of the $CTE$. Thus as the $CTE$ reduces D component acts in opposite direction to the P component resulting in reduced overshooting.
3. I Component: This component acts by adjusting the steering angle based on the sum (or integral) of $CTE$ accumulated so far. This is useful in a situation where due to drift (due to controller bias), the PD control results in undershooting. In that situation I component results in a faster response, thus reducing the $CTE$ value to 0 quickly.

In discrete case, the controller function can be formulated as:
$\alpha[t] = - K_P CTE[t] - K_I(\sum_{\tau = 0}^{t} CTE[\tau])\Delta T - K_D\frac{(CTE[t]-CTE[t-1])}{\Delta T}$

### Results of Steering Control

Clearly, the PID controller is able to control the steering angle of the SDC at up to 44 MPH speed. But the motion is a bit jerky due to the presence of some oscillations. This is due to the fact that PID controller is a purely reactive control. The controller gains were tuned using Twiddle algorithm. The final gain values taken were $K_P = 0.1$, $K_D = 2.8$ and $K_I = 0.001$.

### Throttle Control using PID Controller

So far the implementation controls the steering angle of the SDC. We can also control the throttle (Thr) of the SDC using a PID controller as follows:

$Thr[t] = Thr_{max} - K_{P_2} CTE[t] - K_{I_2}(\sum_{\tau = 0}^{t} CTE[\tau])\Delta T - K_{D_2}\frac{(CTE[t]-CTE[t-1])}{\Delta T}$

The gain values for this controller was taken as $K_{P_2} = 0.45$, $K_{D_2} = 0.5$ and $K_{I_2} = 0$. Clearly, the SDC is able to reach maximum speed around 60 MPH, but due to reactive nature of the PID controller, the amount of oscillations have further increased. This problem of oscillating motion can be solved by predictive controllers like Model Predictive Control. You can see the working of the model predictive control from here.

Check my Github page for complete implementation.