Disclaimer: The simulation environment in this repository is derived from the Car MPC project of Udacity. If you like it feel free to support it in the above link!
The equations of motion are assumed as follows:
x[k+1] = x[k] + v[k] * cos(psi[k]) * dt
y[k+1] = y[k] + v[k] * sin(psi[k]) * dt
v[k+1] = v[k] + a[k] * dt
psi[k+1] = psi[k] + v[k] / L * delta[k] * dt
cte[k+1] = f(x[k]) - y[k] + v[k] * sin(epsi[k]) * dt
epsi[k+1] = psi[k] - psi_des[k] + v[k] / L * delta[k] * dt
where the state is x, y (position), v (velocity), psi (orientation), cte (cross-track error, i.e., the difference between the nominal trajectory and the current vehicle position y), and epsi (orientation error), and the input is a (acceleration) and delta (steering angle).
The input constraints are a \in [-1. 1], corresponding to full brake and full throttle, and delta \in [-25 deg, +25 deg].
The goal here is to use MPC to perform trajectory following while adhering to the constraints. To this end, we design a cost function we seek to minimize over time k:
J = \sum_k (||cte[k]||2 + ||epsi[k]||2 + ||v[k]-v_des||2) + \sum_k (||a[k]||2 + ||delta[k]||2) + \sum_k (||a[k]-a[k-1]||2 + ||delta[k]-delta[k-1]||2)
The first sum asks to follow closely the desire trajectory by minimizing the magnitude of cte, epsi, and v-v_des. The second sum asks to keep the actuation low (efficiency). The third sum asks to not have abrupt changes in accelerating/decelerating or steering (comfort).
The simulator provides waypoints that act as nominal trajectory. These way points have to be transformed to the car coordinate system:
// For waypoint i:
diffx = ptsx[i]-px;
diffy = ptsy[i]-py;
ptsx[i] = diffx * cos(psi) + diffy * sin(psi);
ptsy[i] = diffy * cos(psi) - diffx * sin(psi);
Then we fit a 3rd-degree polynomial to generate the reference trajectory for the MPC.
The DATA.md contains a description of the data sent back from the simulator.
- Udacity's Term 2 Simulator.
- uWebSocketIO, which can be installed using the provided bash scripts by Udacity for either Linux (
install-ubuntu.sh) or Mac (install-mac.sh) systems. - Eigen template library for linear algebra.
- cmake >= 3.5
- make >= 4.1 (Linux, Mac)
- gcc/g++ >= 5.4
- ipopt, an Interior Point OPTimizer.
- cppAD, for Algorithmic Differentiation.
Once the dependencies are installed, the main program is built and run as follows:
mkdir build
cd build
cmake ..
make
./mpc
For more details on how main.cpp uses uWebSocketIO to communicate with the simulator see the original Udacity repository.