This is a brief description of two control projects. You won’t find the fancy math like Laplace transforms or higher order differential equations that is usually involved with control theory and taught at university. But this is more fun. For a more theoretical understanding you might have a look at this (German).

# PID steering angle controller

As part of Udacity’s second term I worked on two control projects. The first control project was the implementation of a simple PID controller in C++ within the simulator.

The control response of the controller is calculated in the following way:

Source: Wikipedia PID controller

where `e(t)` is the time dependent error, in this case the cross track error. `K_p`, `K_i` and `K_d` are the respective parameters for proportional, integral and derivative part of the controller. In the discrete time domain the integral becomes a simple sum at each timestep.These parameters were chosen manually. An end2end deep learning approach for learning a steering angle predictor can be found in my other project here.

# Model predictive control for throttle and steering angle

The second control project covered the more advanced topic of Model Predictive Control (MPC). Instead of only reacting to detected deviations from the reference variable, MPC uses models (like kinematic equations) to predict the system’s response to its actuation control. This planning is not limited to the immediate system’s response. The time horizon to account for is set by the parameter `N`.

Source: Wikipedia on MPC Martin Behrendt, License: CC BY-SA 3.0

This leads to a set of equations which formulate an optimization problem, where the decisions (here the control response) influences the next state. The optimization problem consists of equations for the relationships between the different time steps as well as constraints, like a maximum steering angle. The model equations and constraints for the project are shown below.

Like any optimization problem we need to formulate an objective or cost function which in this case needs to be minimized. A general example for a MPC objective is shown below.

Source: Wikipedia on MPC

## Implementation

The cost function I used is shown in the code below. To allow the optimizer to use automatic differentiation, variables and operations are wrapped with the CppAD library and then passed to the Ipopt optimizer, which are both dependencies.

``````
fg[0] = 0;
for (size_t t = 0; t < N; ++t) {
fg[0] += 1000 * CppAD::pow(vars[cte_start + t], 2);
fg[0] += 20000 * CppAD::pow(vars[epsi_start + t], 2);
// penalize deviations from ref speed
fg[0] += CppAD::pow(vars[v_start + t] - ref_v, 2);
}

// Minimize the use of actuators.
for (size_t t = 0; t < N - 1; ++t) {
fg[0] += 50000 * CppAD::pow(vars[delta_start + t], 2);
fg[0] += CppAD::pow(vars[a_start + t], 2);
}

// Minimize the value gap between sequential actuations.
for (size_t t = 0; t < N - 2; ++t) {
fg[0] += 80000 * CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
}

``````

The model equations for each timestep `t` are implemented like this:

``````
// following the 3rd degree polynomial
AD<double> f0 = coeffs[0] + coeffs[1] * x0 + CppAD::pow(x0, 2) * coeffs[2] + CppAD::pow(x0, 3) * coeffs[3];
// psi desired is calculated as arctan(f'(x_t))
// actuation
AD<double> a0 = vars[a_start + t - 1];
AD<double> delta0 = vars[delta_start + t - 1];

// formulating state variable equations so that they can be used by the solver
fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
fg[1 + psi_start + t] = psi1 - (psi0 + v0 / Lf * delta0 * dt);
fg[1 + v_start + t] = v1 - (v0 + a0 * dt);

// error equations
fg[1 + cte_start + t] = cte1 - (CppAD::cos(psi0) * f0 - CppAD::cos(psi0) * y0 + v0 * CppAD::sin(epsi0) * dt);
fg[1 + epsi_start + t] = epsi1 - (psi0 - psides0 + v0 / Lf * delta0 * dt);
``````

Although the MPC considers a future timehorizon of length `N` timesteps and calculates a series of actuation controls for the entire horizon, we only use the first control outputs. This way the MPC is able to react fast enough on environment changes. Then after the period `dt` (another parameter) the MPC starts again with the updated state of timestep `t + dt`. This allows the MPC to plan better / smoother trajectories by considering the road ahead.

### Latency

Latency is the delay until the control outputs are actually physically implemented. There are (at least) two ways of how to deal with latency. The first way I tried, was to set `dt = latency` and then formulate my state equations in dependency of the actuations at
`t - dt`, which take effect in `t + latency`. This leads to the correct equations with the actual actuations at time `t`.

Another way is to use the kinematic equations and predict the state at time `t` for `t + latency` and pretend it is the current state. This way the MPC anticipates the state when his actuations finally transfer into action. Both methods work fine, however they require very different parameterization of the objective function and possibly `N` and `dt`.

Tags:

Categories: