Steering Control for self-driving car

Doing cool things with data

A typical self driving car starts with the perception system, which estimates the state of the surrounding environment, including landmarks and vehicles and pedestrians.

Perception of local environment

The localization block compares what it has learned to a map to figure out where the vehicle is.

Localizing a self driving car

Once the vehicle’s location is known, the path planning block charts a trajectory to the destination.

Path planning trajectory

Finally, the control loop decides the steering and throttle/brake at every time step to guide the vehicle on this trajectory. In this blog I will talk about how control loop decides steering and throttle values

For my project 10 in the Udacity Self Driving car nano degree, I implemented predictive control to drive the car around in the simulator. See pic below. The simulator provides position and velocity of the car at each time step back to the user and in turn the user provides the steering and acceleration to the simulator to apply to the car. Big thanks to Udacity for creating this simulator and for providing me the expertise to implement model predictive control

Self Driving Car in Udacity Simulator

I have shared the link to my GitHub with the full code in C++.

Model Predictive Control

State: State of the vehicle at any point is described by 4 vectors — x position, y position, velocity (v) and the orientation (psi).

Actuators or control inputs are the steering angle (delta) and the throttle value (a). Braking can be expressed as negative throttle. So throttle can have values between -1 an +1. Steering angle is usually set to be between -30 deg to +30 deg.

The Kinematic motion model can be used to predict new state from previous state after steering and throttle are applied.

x_[t+1] = x[t] + v[t] * cos(psi[t]) * dt
y_[t+1] = y[t] + v[t] * sin(psi[t]) * dt
psi_[t+1] = psi[t] + v[t] / Lf * delta[t] * dt
v_[t+1] = v[t] + a[t] * dt

Here Lf is the distance between the front of the vehicle and its center of gravity. The larger the vehicle, the slower the turn rate

To estimate the ideal steering angle and throttle, we estimate the error of our new state from our ideal state — actual trajectory we want to follow and the velocity and orientation we want to maintain and use a Ipopt Solver to minimize this error. This helps select the steering and throttle that minimizes the error with the desired trajectory.

So the key to good motion control is to define this error well. I set error to be based on the following

1. Distance from the reference — see equation below. We sum up the error due to difference from reference x, y — defined as the cross track error (CTE), difference from desired orientation defined as the orientation error (epsi) and difference from our reference velocity. N is the no. of time steps we are forecasting into the future.

double cost = 0;
for (int t=0; t < N; t++) {
    cost += pow(epsi[t], 2); //Orientation error
    cost += pow(cte[t], 2); //Cross track error
    cost += pow(v[t]- ref_v, 2);//Delta from ref velocity
}

2. Error in proportion to actuators

cost += pow(delta[t], 2); //Steering angle
cost += pow(a[t], 2);//Throttle 

3. Error in proportion to change in actuators — This additional term in the cost function captures the difference between the next actuator state and the current one. This ensures smooth changes in actuator values

for (int t = 0; t < N-1; t++) {
  cost += pow(delta[t+1] - delta[t], 2) //Change in steering
  cost += pow(a[t+1] - a[t], 2)  //Change in throttle
}

The above terms can be multiplied by weights and these weights increase the importance of that term in the overall cost equation. For example if we multiply pow(delta[t+1] - delta[t], 2) multiply by 500 that will ensure very smooth changes in steering and will prevent jerky motion in the simulator

The C++ Ipopt solver was used to minimize the error and calculate the optimal values of steering and throttle.

Two other fairly important parameters were no. of time steps in to the future (N) we want to predict steering and throttle for. And time step length (dt).

• dt— Is the time gap between different time steps. I found dt to be a very important parameter for the overall performance in the simulator. If dt was too low, it led to the car oscillating to and fro around the center of the track. This happens probably because the actuator input is received very quickly and the vehicle is constantly responding. Also if dt is smaller than the latency (0.1 s) then the new actuator signal is received before the previous one has been executed. This leads to jerky motion of the car. On the other hand if dt is too large, the car covers too much distance before the actuator is received and while this leads to smooth performance along the straight portions of the track, it leads to car going off the road on the curves.
• N — Is the number of timesteps the model predicts ahead. As N increases, the model predicts further ahead. The further ahead that the model predicts, the more inaccurate those predictions will be. Also if N is large, then a lot more computations need to happen which can lead to inaccurate results from the solver or solver unable to provide the solution in real time. I found that N around 10–15 steps was the best

Once all the model is set and all the parameters are defined,

1. We pass the current state as the initial state to the model predictive controller.

2. We call the optimization solver. Given the initial state, the solver will return the vector of actuators that minimizes the cost function.

3. We apply the steering and throttle to the vehicle. and Back to step 1.

Driving the car around the simulator by predicting the correct steering and throttle at each step was not an easy task but an awesome learning experience. Big thanks to Udacity for creating this course!

Other writings: https://medium.com/@priya.dwivedi/

PS: I live in Toronto and I am looking to switch career into deep learning. If you like my post and can connect me to anyone, I will be grateful :). My email is priya.toronto3@gmail.com

References:

Udacity Self Driving Cars Nano Degree — I thank Udacity and Sebastian Thrun for giving me the chance to be part of their new Self Driving Car program. It has been a very interesting journey. Most of the code used by me was suggested in classroom lectures. The images and the videos references here were also shared in the lectures

Pictures: https://arxiv.org/pdf/1612.02649.pdf