writeup.md

Implementation of MPC controller

Motion model and optimization problem

In this section we describe a kinematic model and optimization problem that assume that there is no latency when applying activators. In the next section we describe an extension of the model and optimization problem that takes into account the latency.

We used a global kinematic model with 6-dimensional state vector

s_t = [x_t, y_t, ψ_t, v_t, cte_t, eψ_t]

where

• x_t and y_t are car coordinates at time t
• ψ_t is a direction of velocity at time t
• v_t is a velocity at time t
• cte_t is a cross-track error at time t, defined as the distance between the desired position (x, f(x)) and the actual position (x_t, y_t) of the car. In our code we approximated cte_t by f(x_t) - y_t. The value of f(x) is obtained by fitting a polynomial to the input waypoints, as described here.
• eψ_t is an error of the velocity direction at time t, defined as ψ_t - arctan(f'(x_t)).

The state vector s_t+1 at time t+1 is predicted from the state vector at time t using the following equations:

x_t+1 = x_t + v_t cos(ψ_t) dt
y_t+1 = y_t + v_t sin(ψ_t) dt
ψ_t+1 = ψ_t - v_t δ_t dt / L_f
v_t+1 = v_t + a_t dt
cte_t+1 = cte_t + v_t sin(eψ_t) dt = f(x_t) - y_t + v_t sin(eψ_t) dt
eψ_t+1 = eψ_t - v_t δ_t dt / L_f = ψ_t - arctan(f'(x_t)) - v_t δ_t dt / L_f

where

• δ_t and a_t are change of direction and acceleration applied at time t
• dt is the difference between time t+1 and t
• L_f is the distance between the center of mass of the vehicle and it's front axle. We used L_f=2.67 .

The values of δ_t and a_t are found by solving an optimization problem. The objective function of this optimization problem has 4 components:

• error component, ∑_{t=0,1,...,N-2} ( C_cte•(cte_t)² + C_eψ•(eψ_t)² + C_v•(v_t - v_target)²), that ensures that the car has low cross-track and direction errors and also drives with the velocity that is as close to the desired velocity v_target as possible. The value of v_target is a constant positive parameter.

• actuators component, ∑_{t=0,1,...,N-2} (C_δ•(δ_t)²+C_a•(a_t)²), that ensures that the driving is not wobbly and the velocity is as close to constant as possible.

• smoothness component, ∑_{t=0,1,...,N-3} (C_{delta_diff}•(δ_t+1-δ_t)²+C_{a_diff}•(a_t+1-a_t)²), that ensures that the car drives smoothly and doesn't have abrupt changes of speed and direction.

• slowdown component, ∑_{t=0,1,...,N-2} C_slowdown•((cte_t•a_t)²+(eψ_t•a_t)²), that reduces acceleration when the car is in dangerous zone and has a large cross-track error and/or large error in the direction of velocity.

In all these components the parameters C_cte, C_eψ, C_v, C_δ, C_a, C_{delta_diff}, C_{a_diff}, C_slowdown have non-negative values. The parameter N is the number of look-ahead steps and is described in detail in the last section.

The final optimization problem is

min _{δ0,...,δN-2,a0,...,aN-2} ∑_{t=0,1,...,N-2} ( C_cte•(cte_t)² + C_eψ•(eψ_t)² + C_v•(v_t - v_target)²) + ∑_{t=0,1,...,N-2} (C_δ•(δ_t)²+C_a•(a_t)²) +
                              ∑_{t=0,1,...,N-3} (C_{delta_diff}•(δ_t+1-δ_t)²+C_{a_diff}•(a_t+1-a_t)²) + ∑_{t=0,1,...,N-2} C_slowdown•((cte_t•a_t)²+(eψ_t•a_t)²)

such that

∀ t=0,...,N-2, -1 ≤ a_t ≤ 1
∀ t=0,...,N-2, -0.436332 ≤ δ_t ≤ 0.436332
∀ t=0,...,N-2, x_t+1 = x_t + v_t cos(ψ_t) dt
∀ t=0,...,N-2, y_t+1 = y_t + v_t sin(ψ_t) dt
∀ t=0,...,N-2, ψ_t+1 = ψ_t - v_t δ_t dt / L_f
∀ t=0,...,N-2, v_t+1 = v_t + a_t dt
∀ t=0,...,N-2, cte_t+1 = f(x_t) - y_t + v_t sin(eψ_t) dt
∀ t=0,...,N-2, eψ_t+1 = ψ_t - arctan(f'(x_t)) - v_t δ_t dt / L_f

Notice that the last six constraints are the global kinematic model described above. These constraints describe a trajectory of the car, from the known state s₀ = [x₀, y₀, ψ₀, v₀, cte₀, eψ₀] of the car immediately before solving optimization problem, to the future states s₁,...,s_N-1.

We use the values of δ₀ and a₀, found by solving the above optimization problem, to accelerate the car and change its current direction.

Model Predictive Control with Latency

In our car there is a latency of 100 milliseconds when we decide to apply the actuators δ₀ and a₀ to accelerate the car and change its current direction. In this section we describe an extension of the optimization problem to deal with this latency.

To accomodate the latency, we decompose the time interval [t,t+1] into two parts, [t,t'] and [t',t+1]. The time between t and t' is the latency interval. The time between t' and t+1 is the new value of dt. When applying the new actuators δ₀ and a₀ at time t, the car continues to drive at time [t,t'] with the old values of actuators. The new actuators are only applied at time t'.

Since each time iterval [t,t+1] is split into two parts, we replace N with 2N-1 lookahead steps. The time between each even step and its successive odd step is the latency interval. The new actuators are found at even steps, but applied at the odd steps and last for two time steps. Hence we define dt_i to be dt when i is odd and latency interval when i is even. The modified optimization problem is

min _{δ0,...,δN-2,a0,...,aN-2} ∑_{t=0,1,...,2N-1} ( C_cte•(cte_t)² + C_eψ•(eψ_t)² + C_v•(v_t - v_target)²) + ∑_{t=0,1,...,N-2} (C_δ•(δ_t)²+C_a•(a_t)²) +
                              ∑_{t=0,1,...,N-3} (C_{delta_diff}•(δ_t+1-δ_t)²+C_{a_diff}•(a_t+1-a_t)²) + ∑_{t=0,1,...,N-2} C_slowdown•((cte_t•a_t)²+(eψ_t•a_t)²)

such that

∀ t=0,...,N-2, -1 ≤ a_t ≤ 1
∀ t=0,...,N-2, -0.436332 ≤ δ_t ≤ 0.436332
∀ t=0,...,2N-1, x_t+1 = x_t + v_t cos(ψ_t) dt_i
∀ t=0,...,2N-1, y_t+1 = y_t + v_t sin(ψ_t) dt_i
∀ t=1,...,2N-1, ψ_t+1 = ψ_t - v_t δ_⌊t/2⌋-1 dt_i / L_f
∀ t=1,...,2N-1, v_t+1 = v_t + a_⌊t/2⌋-1 dt_i
∀ t=0,...,2N-1, cte_t+1 = f(x_t) - y_t + v_t sin(eψ_t) dt_i
∀ t=1,...,2N-1, eψ_t+1 = ψ_t - arctan(f'(x_t)) - v_t δ_⌊t/2⌋-1 dt_i / L_f

Notice that the fifth, sixth and eighth constraints now start from t=1 instead of t=0. That's because, due to the latency, at time t=0 these constraints use the current δ_curr and a_curr direction and acceleration of the car, that are found when solving optimization problem at previous iteration. Hence the optimization problem has three additional constraints:

ψ₁ = ψ₀ - v₀ δ_curr dt / L_f
v₁ = v₀ + a_curr dt
eψ₁ = ψ₀ - arctan(f'(x₀)) - v₀ δ_curr dt / L_f

Similarly to the optimization problem in the previous section, the state of the car s₀ = [x₀, y₀, ψ₀, v₀, cte₀, eψ₀] immediately before solving the optimization problem is known.

After tuning parameters, we set N = 5, dt = 0.11, C_cte = 200, C_eψ = 200, C_v = 5, C_delta = 100, C_a = 100, C_{delta_diff} = 200, C_{a_diff} = 10, C_slowdown = 500 and v_target=100. In the last section we discuss further the influence of N and dt on car's driving.

Polynomial Fitting and MPC preprocessing

At each iteration we receive from the simulator the car's current state [x_car, y_car, ψ_car, v_car] in a global coordinate system. We performed a number of preprocessing steps before solving optimization problem:

• Coordinate transformation. Since the visualization of waypoints and predicted trajectory is done in car coordinate system, we decided to use this coordinate system in our optimization process. The origin of car coordinate system is at car location, x axis is aligned with the direction of velocity and y axis points to the left. Since simulator sends us waypoints in a global coordinate system, we transformed waypoints to car coordinate system. Let (x,y) be waypoint global coordinates. We denote by (x',y') the same waypoint with coordinates in car's coordinate system. We used the following equations to transform waypoints to car coordinate system:

  x' = (x-x_car)•cos(-ψ) - (y-y_car)•sin(-ψ)
  y' = (x-x_car)•sin(-ψ) + (y-y_car)•cos(-ψ)

• Polynomial fitting. We fitted a second degree polynomial to waypoints in car's coordinate system. By decreasing polynomial degree from 3 to 2 we reduced processing time at each iteration, while still obtaining a smooth curve that goes through waypoints. We discuss a connection between processing time and the accuracy of car driving in the next section.

• Initialization of state vector. After fitting polynomial, we initialize car's state vector [x₀, y₀, ψ₀, v₀, cte₀, eψ₀] in its own coordinate system. By the definition of car coordinate system, x₀=y₀=ψ₀=0. The value of v₀=v_car remains the same in global and car coordinate systems.

  Let f(x) be a polynomial fitted at the previous step. The value of cte₀ should be the distance from (0,0) to the closest point of the polynomial. The computation of this distance is not straightforward, will increase the processing time, which in turn will affect car's driving. Instead of computing cross-track error exactly, we approximated cte₀ by f(0).

  The desired direction at point x=0 is arctan(f'(0)). Since the direction of the car in car coordinate system is 0, eψ₀ = 0 - arctan(f'(0)) = -arctan(f'(0)).

Timestamp length and elapsed duration (N & dt)

In this section we describe our reasoning for choosing the values of N and dt.

The parameter N defines the number of predictions when building car's trajectory and solving optimization problem. The optimization time is proportional to N. A small value of N (e.g. 3,4) would reduce processing time, but will make projected trajectory noisy and less reliable. A large value of N (e.g. 10) will result in an accurate prediction of car trajectory, but will also increase the processing time. We observed emprically that the large processing time causes car to react slowly to sharp turns, which in turn might put the car on ledges or off-track.

The parameter dt defines the length of prediction interval. When dt is small (e.g. less than 0.1) the car drives wobbly. Also when dt is large (e.g. 0.2) the car reacts slowly to sharp turns, which in turn might put the car on ledges or off-track.

Based on these considerations, after several trials we choose N=5 and dt=0.11.