Autonomous Driving using Predictive Control Model

I am currently enrolled in Udacity’s self driving car nanodegree and this post contains my journey through finishing the last project of Term 2. Overall I would describe Term 2 as heavily focussed on Control theory and made me appreciate all the progress that has been done in Control theory, robotics and application of real world scenarios in these fields.

Goal of this project is to drive a car autonomously around the track in a simulator using a Predictive Control Model.

The Model used in this project is a simple Kinematic Model consisting of Vehicle State and Actuators. Vehicle State refers to the state of the vehicle and can be represented as a vector (px, py, psi, v, cte, epsi). px = vehicle’s x coordinate, py = vehicle’s y coordinate , psi = vehicle’s orientation or heading direction, v = vehicles velocity, cte = cross track error (distance of vehicle from expected trajectory) and epsi = error in orientation of vehicle compared to reference trajectory.

Actuators are set of controls used to navigate the vehicle. For simplicity this project only uses 2 actuators which are represented as vector (delta, a). delta stands for next steering angle that vehicle should apply, in real world a vehicle cannot take very sharp turns due to vehicles turn radius, this has been accounted for in the model constraints and vehicle cannot turn more than +/-18 degree (+/- 0.33 radians) in this simulated environment. a referes to acceleration or throttle that should be applied to vehicle . It can have both positive values for speeding and negative values for breaking.

The Kinematic equations used for updating vehicle state are as follows

xt+1​​=xt​​+vt​​∗cos(ψt​​)∗dt

yt+1​​=yt​​+vt​​∗sin(ψt​​)∗dt

ψt+1​​=ψt​​+​Lf​​​​vt​​​​∗δdt

vt+1​​=vt​​+at​​∗dt

ctet+1​​=f(xt​​)−yt​​+(vt​​∗sin(t​​)∗dt)

t+1​​=t​​+​Lf​​​​vt​​​​∗δt​​∗dt

This model predicts vehciles state after every 100ms which is represented by dt in the kinematic equations above. 100 ms was chosen as taking a value higher than this was giving inaccurate results as the environment was changing too fast for the predictive controller to give accurate results. Changing to a value lower than 100ms was also of not much use as we were unnecessarily making computations for 2 very similar states.

Also the model only predicts next 10 states which is represented by variable N in project code. I tried higher values of N such as 20 but this burdened the ipopt solver used in this project to not produce optimal results as I had constrained the ipopt solver to compute in less than 0.05 seconds.

This N multiplied by dt (0.1 *10) gives 1 second, so our predictive controller only predicts the set of states for next 1 second . This seems about correct for a highly volatile environment for a vehicle driving on a high speed in a high traffic environment.

This project also factors in real world latency that can occur while applying actuator inputs. To simulate this the project’s main thread sleeps for 100ms before sending the actuations to simulator. To account for this while returning the actuations to simulator I use 2 set of actuations — the real actuations for next step and the next predicted actuation after dt which is 0.1 second(100 ms) in my case. Sending the sum of these 2 actuations makes the model proactively apply the next actuation and hence handles the 100ms latency.

The final simulation of this model is shown below

https://youtu.be/ERkIpLQWWK0