MPC源码解读及路径跟踪demo

MPC源码解读及路径跟踪demo

附赠自动驾驶最全的学习资料和量产经验：链接

三、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 actuations

class MPC {
public:
    double steer;    // 反向盘角度
    double throttle; // 油门（加速度）

    // 存放所有的状态和驱动变量
    Dvector x;
    // x变量的上下限
    Dvector x_lowerbound; // lower limit for each corresponding variable in x
    Dvector x_upperbound; // upper limit for each corresponding variable in x
    //
    Dvector g_lowerbound; // value constraint for each corresponding constraint expression
    Dvector g_upperbound; // value constraint for each corresponding constraint expression

    // 预测状态的X、Y
    std::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 model
        for (int i = 0; i < N - 1; ++i) {
            // where the current state variables of interest are stored
            // stored for readability
            const 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 actuations
            const 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 state
            const 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 psi
            const 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 states
            fg[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 called
    for (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 handle
    for (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~5
    for (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 system

    for (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];   // x
    const double py = state[1];   // y
    const 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 problem
    CppAD::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;
}

代码实现参考了源码： https://github.com/mithi/mpc