当前位置：首页 > news >正文

MPC源码解读及路径跟踪demo

news 来源：原创 2024/9/23 1:36:56

一、前言

上篇文章对MPC原理进行了推导，这篇文章对网上开源代码进行解读及实现路径跟踪demo。

分享一段心灵鸡汤：
最近在读的一本书《杀死一只知更鸟》，里面有这样一段话引起了我的共鸣：”我想让你见识一下什么是真正的勇敢，而不是错误地认为一个人手里拿把枪就是勇敢，勇敢就是，在你还没有开始的时候就知道自己注定会输，但依然义无反顾的去做，并且不管发生什么都坚持到底“，这段话讲诉的勇敢是带有语境，不是说一股脑的执拗就是勇敢。这是一本中学生推荐书目，是一本关于勇气与正义的成长教科书，但也非常适合成年人去阅读。

二、MPC 跟踪效果视频

MPC_TRACKING

注释：视频中的原始路径没有经过任何平滑处理（考虑车辆运动学模型、速度、加速度等限制），所以存在跟踪误差，属于正常现象。

三、MPC 源码解读

需要注意：理论推导和代码实现存在较大的差异性
1、算法实现中是将预测过程得到的中间状态量+控制量全部放在一个大矩阵中进行计算求解的；
2、理论推导中控制量使用的是速度和方向盘角，代码实现中使用的是加速度和方向盘角（更符合实际情况）；
3、理论推到中使用的是误差模型，代码实现中使用的是路径相对位置（y轴偏差和朝向偏差），所以存在坐标系的转换；
4、代码实现中使用 IPOPT求解器，需要进行设置（代码注释中有解释）。

// MPC.HPP文件

/*** @file mpc.hpp* @brief  mpc控制器* @author annotation by author mce* @date 2024-5-25*/
#ifndef MPC_H
#define MPC_H
#define HAVE_CSTDDEF
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#undef HAVE_CSTDDEF
#include <vector>#include "../src/Eigen-3.3/Eigen/Core"// CppAD::vector<double>
typedef CPPAD_TESTVECTOR(double) Dvector;// 预测步长
const int N = 50;
// 控制周期
const double dt = 0.02;
// 轴距
const double Lf = 2.5;
// 最大速度
const double VELOCITY_MAX = 100.0;
// 状态量的数量（ px, py, psi朝向, v速度, cte横向偏差, epsi角度偏差 ）
const int NUMBER_OF_STATES = 6;
// 控制量个数（ steering angle 方向盘角度, acceleration 加速度）
const int NUMBER_OF_ACTUATIONS = 2;
// 未来一段时间状态量总个数 + 未来一段时间控制量总个数
const int NX = N * NUMBER_OF_STATES + (N - 1) * NUMBER_OF_ACTUATIONS;
// 约束个数
const int NG = N * NUMBER_OF_STATES;// 状态量
const int ID_FIRST_px = 0;
const int ID_FIRST_py = ID_FIRST_px + N;
const int ID_FIRST_psi = ID_FIRST_py + N;
const int ID_FIRST_v = ID_FIRST_psi + N;
const int ID_FIRST_cte = ID_FIRST_v + N;
const int ID_FIRST_epsi = ID_FIRST_cte + N;
// 控制量
const int ID_FIRST_delta = ID_FIRST_epsi + N;  // 方向盘角度
const int ID_FIRST_a = ID_FIRST_delta + N - 1; // 加速度
// 状态量权重
const double W_cte = 1500.0;  // y轴偏差 1500
const double W_epsi = 1500.0; // 朝向偏差 1500
const double W_v = 1.0;       // 速度
// 控制量权重
const double W_delta = 10.0; // 方向盘角度 10
const double W_a = 10.0;     // 加速度  10
// 控制量变化量的变化率
const double W_ddelta = 150.0; // 150 weight cost for high difference between consecutive steering actuations
const double W_da = 15.0;      // 15 weight cost for high difference between consecutive acceleration actuationsclass MPC {
public:double steer;    // 反向盘角度double throttle; // 油门（加速度）// 存放所有的状态和驱动变量Dvector x;// x变量的上下限Dvector x_lowerbound; // lower limit for each corresponding variable in xDvector x_upperbound; // upper limit for each corresponding variable in x//Dvector g_lowerbound; // value constraint for each corresponding constraint expressionDvector g_upperbound; // value constraint for each corresponding constraint expression// 预测状态的X、Ystd::vector<double> future_xs;std::vector<double> future_ys;MPC();virtual ~MPC();// MPC 求解器void solve(Eigen::VectorXd state, Eigen::VectorXd K);
};
#endif /* MPC_H */

// MPC.CPP文件

#include "../include/MPC.hpp"
using CppAD::AD;
using namespace std;class FG_eval {
public:// 传入的轨迹Eigen::VectorXd K;FG_eval(Eigen::VectorXd Kin) : K(Kin) {}typedef CPPAD_TESTVECTOR(AD<double>) ADvector;// fg 包含代价和所有约束的向量// x 包含所有状态量和控制量void operator()(ADvector &fg, const ADvector &x) {//*********************************************************//* COST DEFINED HERE 状态量+控制量 代价//*********************************************************fg[0] = 0.0;for (int i = 0; i < N; ++i) {// y轴偏差const auto cte = x[ID_FIRST_cte + i];// 朝向偏差const auto epsi = x[ID_FIRST_epsi + i];// 速度const auto v = x[ID_FIRST_v + i] - VELOCITY_MAX;fg[0] += (W_cte * cte * cte + W_epsi * epsi * epsi + W_v * v * v);}for (int i = 0; i < N - 1; ++i) {// 方向盘角度const auto delta = x[ID_FIRST_delta + i];// 加速度const auto a = x[ID_FIRST_a + i];fg[0] += (W_delta * delta * delta + W_a * a * a);}for (int i = 0; i < N - 2; ++i) {// 方向盘角度变化速度const auto ddelta = x[ID_FIRST_delta + i + 1] - x[ID_FIRST_delta + i];// 加速度变化速度const auto da = x[ID_FIRST_a + i + 1] - x[ID_FIRST_a + i];fg[0] += (W_ddelta * ddelta * ddelta + W_da * da * da);}//*********************************************************//* CONSTRAINTS DEFINED HERE  约束//*********************************************************// given state does not vary 是不是延时？fg[ID_FIRST_px + 1] = x[ID_FIRST_px];fg[ID_FIRST_py + 1] = x[ID_FIRST_py];fg[ID_FIRST_psi + 1] = x[ID_FIRST_psi];fg[ID_FIRST_v + 1] = x[ID_FIRST_v];fg[ID_FIRST_cte + 1] = x[ID_FIRST_cte];fg[ID_FIRST_epsi + 1] = x[ID_FIRST_epsi];// constraints based on our kinematic modelfor (int i = 0; i < N - 1; ++i) {// where the current state variables of interest are stored// stored for readabilityconst int ID_CURRENT_px = ID_FIRST_px + i;const int ID_CURRENT_py = ID_FIRST_py + i;const int ID_CURRENT_psi = ID_FIRST_psi + i;const int ID_CURRENT_v = ID_FIRST_v + i;const int ID_CURRENT_cte = ID_FIRST_cte + i;const int ID_CURRENT_epsi = ID_FIRST_epsi + i;const int ID_CURRENT_delta = ID_FIRST_delta + i;const int ID_CURRENT_a = ID_FIRST_a + i;// current state and actuationsconst auto px0 = x[ID_CURRENT_px];const auto py0 = x[ID_CURRENT_py];const auto psi0 = x[ID_CURRENT_psi];const auto v0 = x[ID_CURRENT_v];const auto cte0 = x[ID_CURRENT_cte];const auto epsi0 = x[ID_CURRENT_epsi];const auto delta0 = x[ID_CURRENT_delta];const auto a0 = x[ID_CURRENT_a];// next stateconst auto px1 = x[ID_CURRENT_px + 1];const auto py1 = x[ID_CURRENT_py + 1];const auto psi1 = x[ID_CURRENT_psi + 1];const auto v1 = x[ID_CURRENT_v + 1];const auto cte1 = x[ID_CURRENT_cte + 1];const auto epsi1 = x[ID_CURRENT_epsi + 1];// desired py and psiconst auto py_desired = K[3] * px0 * px0 * px0 + K[2] * px0 * px0 + K[1] * px0 + K[0];const auto psi_desired = CppAD::atan(3.0 * K[3] * px0 * px0 + 2.0 * K[2] * px0 + K[1]);// 推导下一时刻车辆的状态const auto px1_f = px0 + v0 * CppAD::cos(psi0) * dt;const auto py1_f = py0 + v0 * CppAD::sin(psi0) * dt;const auto psi1_f = psi0 + v0 * tan(delta0) / Lf * dt;const auto v1_f = v0 + a0 * dt;const auto cte1_f = py_desired - py0 + v0 * CppAD::sin(epsi0) * dt;const auto epsi1_f = psi0 - psi_desired + v0 * tan(delta0) / Lf * dt;// store the constraint expression of two consecutive statesfg[ID_CURRENT_px + 2] = px1 - px1_f;fg[ID_CURRENT_py + 2] = py1 - py1_f;fg[ID_CURRENT_psi + 2] = psi1 - psi1_f;fg[ID_CURRENT_v + 2] = v1 - v1_f;fg[ID_CURRENT_cte + 2] = cte1 - cte1_f;fg[ID_CURRENT_epsi + 2] = epsi1 - epsi1_f;}}
};MPC::MPC() {//**************************************************************//* SET INITIAL VALUES OF VARIABLES  初始化变量//**************************************************************this->x.resize(NX);// all states except the ID_FIRST are set to zero the aformentioned states will be initialized when solve() is calledfor (int i = 0; i < NX; ++i) {this->x[i] = 0.0;}//**************************************************************//* SET UPPER AND LOWER LIMITS OF VARIABLES 设置变量的约束条件//**************************************************************this->x_lowerbound.resize(NX);this->x_upperbound.resize(NX);// all other values large values the computer can handlefor (int i = 0; i < ID_FIRST_delta; ++i) {this->x_lowerbound[i] = -1.0e10;this->x_upperbound[i] = 1.0e10;}// all actuation inputs (steering, acceleration) should have values between [-1, 1]// 方向盘转向角约束 大概是40°左右for (int i = ID_FIRST_delta; i < ID_FIRST_a; ++i) {this->x_lowerbound[i] = -0.70;this->x_upperbound[i] = 0.70;}// 加速度约束 -3~5for (int i = ID_FIRST_a; i < NX; ++i) {this->x_lowerbound[i] = -3.0;this->x_upperbound[i] = 5.0;}//**************************************************************//* SET UPPER AND LOWER LIMITS OF CONSTRAINTS 设置约束//**************************************************************this->g_lowerbound.resize(NG);this->g_upperbound.resize(NG);// the first constraint for each state veriable// refer to the initial state conditions// this will be initialized when solve() is called// the succeeding constraints refer to the relationship// between succeeding states based on our kinematic model of the systemfor (int i = 0; i < NG; ++i) {this->g_lowerbound[i] = 0.0;this->g_upperbound[i] = 0.0;}
}MPC::~MPC() {}void MPC::solve(Eigen::VectorXd state, Eigen::VectorXd K) {// 机器人初始状态const double px = state[0];   // xconst double py = state[1];   // yconst double psi = state[2];  // 朝向角const double v = state[3];    // 速度const double cte = state[4];  // y轴偏差（横向偏差）const double epsi = state[5]; // 朝向角偏差（角度偏差）this->x[ID_FIRST_px] = px;this->x[ID_FIRST_py] = py;this->x[ID_FIRST_psi] = psi;this->x[ID_FIRST_v] = v;this->x[ID_FIRST_cte] = cte;this->x[ID_FIRST_epsi] = epsi;this->g_lowerbound[ID_FIRST_px] = px;this->g_lowerbound[ID_FIRST_py] = py;this->g_lowerbound[ID_FIRST_psi] = psi;this->g_lowerbound[ID_FIRST_v] = v;this->g_lowerbound[ID_FIRST_cte] = cte;this->g_lowerbound[ID_FIRST_epsi] = epsi;this->g_upperbound[ID_FIRST_px] = px;this->g_upperbound[ID_FIRST_py] = py;this->g_upperbound[ID_FIRST_psi] = psi;this->g_upperbound[ID_FIRST_v] = v;this->g_upperbound[ID_FIRST_cte] = cte;this->g_upperbound[ID_FIRST_epsi] = epsi;//**************************************************************//* SOLVE//**************************************************************// IPOPT 优化求解器std::string options;// Uncomment this if you'd like more print information 打印开关options += "Integer print_level  0\n";// NOTE: Setting sparse to true allows the solver to take advantage of sparse routines, this makes the computation MUCH FASTER.// 将稀疏设置为 "true "可让求解器利用稀疏例程，这将大大加快计算速度options += "Sparse  true        forward\n";options += "Sparse  true        reverse\n";// NOTE: Currently the solver has a maximum time limit of 0.5 seconds.// 容许求解器最大的计算时间(实际上应该设置为一个控制周期时间)options += "Numeric max_cpu_time          0.5\n";// 优化求解结果CppAD::ipopt::solve_result<Dvector> solution;// 包含代价函数和约束条件FG_eval fg_eval(K);// solve the problemCppAD::ipopt::solve<Dvector, FG_eval>(options, x, x_lowerbound, x_upperbound, g_lowerbound, g_upperbound, fg_eval, solution);// 判断优化器是否计算成功auto cost = solution.obj_value;bool ok = solution.status == CppAD::ipopt::solve_result<Dvector>::success;if (ok) {// std::cout << "OK! Cost:" << cost << std::endl;} else {std::cout << "SOMETHING IS WRONG!" << cost << std::endl;}//**************************************************************//* STORE RELEVANT INFORMATION FROM SOLUTION//**************************************************************this->steer = solution.x[ID_FIRST_delta];this->throttle = solution.x[ID_FIRST_a];this->future_xs = {};this->future_ys = {};for (int i = 0; i < N; ++i) {const double px = solution.x[ID_FIRST_px + i];const double py = solution.x[ID_FIRST_py + i];// cout << "x: " << px << ", y:" << py << endl;this->future_xs.emplace_back(px);this->future_ys.emplace_back(py);}
}

// MAIN.CPP文件

#include <math.h>
#include <iostream>
#include <thread>
#include <vector>
#include "../include/MPC.hpp"
#include "Eigen-3.3/Eigen/Core"
#include "Eigen-3.3/Eigen/QR"
using namespace std;// 车辆模型机器人状态
typedef struct state {state(){};state(float _x, float _y, float _theta, float _v, float _delta, float _a) : x(_x), y(_y), theta(_theta), v(_v), delta(_delta), a(_a){};// ======= 状态量 ======float x = 0;float y = 0;float theta = 0;// ======= 控制量 ======float v = 0;     // 速度float delta = 0; // 转向角float a = 0;
} State;// Fit a polynomial.  多项式拟合
Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {assert(xvals.size() == yvals.size());assert(order >= 1 && order <= xvals.size() - 1);Eigen::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;
}// 获取矩阵K
Eigen::VectorXd getK(state state, std::vector<double> points_xs, std::vector<double> points_ys) {const double px = state.x;const double py = state.y;const double psi = state.theta;   // 车头朝向角度const double v = state.v;         // 速度const double delta = state.delta; // 转向角const double a = state.a;const int NUMBER_OF_WAYPOINTS = points_xs.size();Eigen::VectorXd waypoints_xs(NUMBER_OF_WAYPOINTS);Eigen::VectorXd waypoints_ys(NUMBER_OF_WAYPOINTS);// 变换成以车辆中心为坐标原点的路径for (int i = 0; i < NUMBER_OF_WAYPOINTS; ++i) {const double dx = points_xs[i] - px;const double dy = points_ys[i] - py;waypoints_xs[i] = dx * cos(-psi) - dy * sin(-psi);waypoints_ys[i] = dy * cos(-psi) + dx * sin(-psi);// cout << "x:" << waypoints_xs[i] << " y:" << waypoints_ys[i] << endl;}const int ORDER = 3;auto K = polyfit(waypoints_xs, waypoints_ys, ORDER);return K;
}// 获取状态矩阵
Eigen::VectorXd getCarState(State state, Eigen::VectorXd K) {const double cte = K[0];         // y轴偏差const double epsi = -atan(K[1]); // 朝向偏差const double current_px = 0.0 + state.v * dt;const double current_py = 0.0;const double current_psi = 0.0 + state.v * tan(state.delta) / Lf * dt;const double current_v = state.v + state.a * dt;const double current_cte = cte + state.v * sin(epsi) * dt;const double current_epsi = epsi + state.v * tan(state.delta) / Lf * dt;Eigen::VectorXd state_eigen(NUMBER_OF_STATES);state_eigen << current_px, current_py, current_psi, current_v, current_cte, current_epsi;return state_eigen;
}int main() {// 路径std::vector<double> points_xs = {1, 5, 5, 15};std::vector<double> points_ys = {1, 5, 10, 15};// mpc求解器MPC mpc;// 当前车辆状态()state state(1.f, 1.f, M_PI * 45 / 180, 0, -M_PI * 5 / 180, 0);auto K = getK(state, points_xs, points_ys);Eigen::VectorXd car_state = getCarState(state, K);mpc.solve(car_state, K);cout << "加速度：" << mpc.throttle << "m/s^2,  转向角：" << mpc.steer * 180 / M_PI << endl;
}