Kinematic models are simplifications of dynamic models that ignore tire forces, gravity, and mass.
Dynamic models aim to embody the actual vehicle dynamics as closely as possible.
Not all dynamic models are created equal! Some may consider more of these factors than others.
A state is represented by )
. Actuators are represented by steering wheel 
and throttle/brake 
. Simple physics. The interesting thing is that a car is not a point but has a finite size. So the actual rotation angle satisfies: 
.
#include <Eigen/dense>
double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }
const double Lf = 2;
using namespace Eigen;
// NOTE: state is [x, y, psi, v]
// NOTE: actuators is [delta, a]
VectorXd globalKinematic(VectorXd state,VectorXd actuators, double dt) {
VectorXd next_state(state.size());
double x = state[0];
double y = state[1];
double psi = state[2];
double v = state[3];
next_state[0] = x + v * cos (psi) * dt;
next_state[1] = y + v * sin (psi) * dt;
next_state[2] = psi + v / Lf * actuators[0] * dt;
next_state[3] = v + actuators[1] * dt;
return next_state;
}
polyfit
#include <math.h>
#include <iostream>
#include <Eigen/dense>
using namespace Eigen;
// Evaluate a polynomial.
double polyeval(VectorXd coeffs, double x) {
double result = 0.0;
for (int i = 0; i < coeffs.size(); i++) {
result += coeffs[i] * pow(x, i);
}
return result;
}
// Fit a polynomial.
VectorXd polyfit(VectorXd xvals, VectorXd yvals,int order) {
assert(xvals.size() == yvals.size());
assert(order >= 1 && order <= xvals.size() - 1);
MatrixXd A(xvals.size(), order + 1);
for (int i = 0; i < xvals.size(); i++) {
A(i, 0) = 1.0;
}
for (int j = 0; j < xvals.size(); j++) {
for (int i = 0; i < order; i++) {
A(j, i + 1) = A(j, i) * xvals(j);
}
}
auto Q = A.householderQr();
auto result = Q.solve(yvals);
return result;
}
int main() {
VectorXd xvals(6),yvals(6);
xvals << 9.261977, -2.06803, -19.6663, -36.868, -51.6263, -66.3482;
yvals << 5.17, -2.25, -15.306, -29.46, -42.85, -57.6116;
VectorXd coeffs = polyfit(xvals,yvals,3);
for (double x = 0; x <= 20; x += 1.0) {
std::cout << polyeval(coeffs,x) << std::endl;
}
}
Model predictive control
cost function:
- cte: cross track error
- epsi: error of heading orientation
- v - speed_limit
- change rate of input
Model Predictive Control (MPC) uses an optimizer to find the control inputs that minimize the cost function.We actually only execute the very first set of control inputs. This brings the vehicle to a new state and then we repeat the process.
MPC algorithm:
- Define the length of the trajectory, N, and duration of each timestep, dt.
- Define vehicle dynamics and actuator limitations along with other constraints.
- Define the cost function.
Loop:
- We pass the current state as the initial state to the model predictive controller.
- We call the optimization solver. Given the initial state, the solver will return the vector of control inputs that minimizes the cost function. The solver we’ll use is called Ipopt.
- We apply the first control input to the vehicle.
In a real car, there will a latency (~100 ms) from the actuation command to the system. So the advantage of MPC over PID is that MPC try to compute a control input based on a future error, which will lead to more stability.
Quiz
brew install ipopt --with-openblas
Warning: homebrew/science/ipopt dependency gcc was built with a different C++ standard library (libstdc++ from clang). This may cause problems at runtime.
🍺 /usr/local/Cellar/ipopt/3.12.5_2: 66 files, 6.4MB,
brew install cppad
🍺 /usr/local/Cellar/cppad/20150000_3: 1,483 files, 11.8MB
core code:
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include <Eigen/QR>
auto coeffs = polyfit(ptsx, ptsy, 1); // linear fit
double cte = polyeval(coeffs, x) - y;
double epsi = psi - atan(coeffs[1]);
Eigen::VectorXd state(6);
state << x, y, psi, v, cte, epsi; // initialize state
auto vars = mpc.Solve(state, coeffs);
state << vars[0], vars[1], vars[2], vars[3], vars[4], vars[5]; // update state
mpc.Solve has to be specifically modified to incorporate constraints and target information.size_t N = 25 ; // number of timesteps
double dt = 0.05 ; // timestep
size_t n_vars = N*8 - 2; // number of independent variables
size_t n_constraints = N * 6; // Number of constraints
Dvector vars(n_vars), vars_lowerbound(n_vars), vars_upperbound(n_vars), constraints_lowerbound(n_constraints), constraints_upperbound(n_constraints);
// ... initial values for these vectors
FG_eval fg_eval(coeffs); // constructor
CppAD::ipopt::solve_result<Dvector> solution; // get space
// solve the problem
CppAD::ipopt::solve<Dvector, FG_eval>(options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound, constraints_upperbound, fg_eval, solution);
Note that the cost function is defined in
FG_eval class method operator