agv_ros_ws/my_planner/include/mpc/mpc_solver.h

#pragma once

#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>

#include "mpc/arc_spline.hpp"
#include <deque>
#include "mpc/iosqp.hpp"
#include <algorithm>

namespace mpc_car
{
    static constexpr int n = 4; // 四种状态：x y phi v
    static constexpr int m = 2; // 两个输入： a delta
    typedef Eigen::Matrix<double, n, n> MatrixA;
    typedef Eigen::Matrix<double, n, m> MatrixB;
    typedef Eigen::Vector4d VectorG;
    typedef Eigen::Vector4d VectorX;
    typedef Eigen::Vector2d VectorU;

    class MpcCar
    {
    private:
        ros::NodeHandle nh_;
        arc_spline::ArcSpline s_; // 参考的轨迹

        double wheel_base_; // 车辆轴距
        double dt_;         // 采样时间
        double rho_;        // 原始代价函数中的权重系数
        double rhoN_;       // 原始代价函数中的权重系数
        double v_max_;      // 速度最大值
        double a_max_;      // 加速度最大值
        double delta_max_;  // 最大转向角
        double ddelta_max_; // 最大转向角变化速度
        double delay_;      // 延迟时间
        double desired_v_;  // 规划速度
        double Desired_v_;  // 期望速度
        int N_;             // 向前采样的次数
        int run_times_ = 1;

        // arc_spline::ArcSpline s_; // 参考的轨迹

        osqp::IOSQP qpSolver_;

        std::vector<VectorX> predictState_; // 预测的状态（根据QP求解后得到的控制输入，计算出来的预测状态）
        std::vector<VectorU> predictInput_; // QP 求解后的决策变量（控制输入）
        std::deque<VectorU> historyInput_;  // 历史输入，用来帮忙计算延迟状态的！
        int history_length_;                // 历史输入的长度
        VectorX x0_observe_;                // 观测到的状态

        // x_{k+1} = Ad * x_{k} + Bd * u_k + gd
        MatrixA Ad_;
        MatrixB Bd_;
        VectorG gd_;

        /**
         * osqp interface:     QP求解器要求的形式如下，往下面矩阵填内容的时候必须填成下面形式
         *
         * minimize     0.5 x^T P_ x + q_^T x
         * subject to   l_ <= A_ x <= u_
         *
         * OSQP（Operator Splitting Quadratic Program）是一个用于解决凸二次规划问题的求解器。
         * 它是一个高性能的、开源的数值优化工具，旨在有效地解决大规模稀疏凸二次规划问题。
         **/

        // 最后喂给QP求解器的矩阵
        Eigen::SparseMatrix<double> P_; // (2 * N_, 2 * N_)  最终 代价函数二次项的权重矩阵
        Eigen::SparseMatrix<double> q_; // (2 * N_, 1)       最终 代价函数一次项的权重矩阵
        Eigen::SparseMatrix<double> A_; // (4 * N_, 2 * N_)  最终 这是什么矩阵？
        Eigen::SparseMatrix<double> l_; // (4 * N_, 1)       最终 约束下边界矩阵
        Eigen::SparseMatrix<double> u_; // (4 * N_, 1)       最终 约束上边界矩阵
        /*
          A_ 和 l_、u_ 的行数是相同的，但是列数不同
          然后，最终的代价函数和约束，都转换成以 控制输入 u 为自变量 的式子
          所以，上面的 l_ <= A_ x <= u_  应该是  l_ <= A_ * x <= u_
          其中， x 是各个采样状态的控制输入，尺寸应该是：(2 * N_, 1)   这样一乘，不等式的尺寸就对上了
          即： l_(4 * N_, 1)  <= A_(4 * N_, 2 * N_) * (2 * N_, 1) <= u_(4 * N_, 1)
        */

        /* *  系统状态的约束
         *               /  x1  \
         *               |  x2  |
         *  lx_ <=  Cx_  |  x3  |  <= ux_
         *               | ...  |
         *               \  xN  /
         * */

        // p, v constrains    // 状态约束
        Eigen::SparseMatrix<double> Cx_; // (1 * N_, 4 * N_)  状态约束的权重系数矩阵？
        Eigen::SparseMatrix<double> lx_; // (1 * N_, 1)       状态约束的下边界矩阵
        Eigen::SparseMatrix<double> ux_; // (1 * N_, 1)       状态约束的上边界矩阵

        /* *  系统控制输入约束
         *               /  u0  \
         *               |  u1  |
         *  lu_ <=  Cu_  |  u2  |  <= uu_
         *               | ...  |
         *               \ uN-1 /
         * */

        // a delta vs constrains  // 控制约束？
        Eigen::SparseMatrix<double> Cu_; // (3 * N_, 2 * N_)  状态约束的权重系数矩阵？
        Eigen::SparseMatrix<double> lu_; // (3 * N_, 1)       控制输入约束下边界矩阵
        Eigen::SparseMatrix<double> uu_; // (3 * N_, 1)       控制输入约束上边界矩阵

        Eigen::SparseMatrix<double> Qx_; // (4 * N_, 4 * N_)  原始代价函数的权重系数矩阵

        void linearization(const double &phi, const double &v, const double &delta)
        {
            // x_{k+1} = Ad * x_{k} + Bd * u_k + gd
            // TODO: set values to Ad_, Bd_, gd_
            Ad_ << 0, 0, -v * sin(phi), cos(phi),
                0, 0, v * cos(phi), sin(phi),
                0, 0, 0, tan(delta) / wheel_base_,
                0, 0, 0, 0;

            Bd_ << 0, 0,
                0, 0,
                0, v / (wheel_base_ * pow(cos(delta), 2)),
                1, 0;

            gd_ << v * phi * sin(phi),
                -v * phi * cos(phi),
                -v * delta / (wheel_base_ * pow(cos(delta), 2)),
                0;

            // 离散化处理
            Ad_ = MatrixA::Identity() + dt_ * Ad_;
            Bd_ = dt_ * Bd_;
            gd_ = dt_ * gd_;
            return;
        }

        void calLinPoint(const double &s0, double &phi, double &v, double &delta, arc_spline::ArcSpline &s_)
        {
            Eigen::Vector2d dxy = s_(s0, 1);
            Eigen::Vector2d ddxy = s_(s0, 2);
            double dx = dxy.x();
            double dy = dxy.y();
            double ddx = ddxy.x();
            double ddy = ddxy.y();
            double dphi = (ddy * dx - dy * ddx) / (dx * dx + dy * dy);
            phi = atan2(dy, dx);
            v = desired_v_;
            delta = atan2(wheel_base_ * dphi, 1.0);
        }

        inline VectorX diff(const VectorX &state,
                            const VectorU &input) const
        { // 根据控制输入和当前状态，返回下一步的状态
            VectorX ds;
            double phi = state(2);
            double v = state(3);
            double a = input(0);
            double delta = input(1);
            ds(0) = v * cos(phi);
            ds(1) = v * sin(phi);
            ds(2) = v / wheel_base_ * tan(delta);
            ds(3) = a;
            return ds;
        }

        inline void step(VectorX &state, const VectorU &input, const double dt) const
        {
            // Runge–Kutta
            VectorX k1 = diff(state, input);
            VectorX k2 = diff(state + k1 * dt / 2, input);
            VectorX k3 = diff(state + k2 * dt / 2, input);
            VectorX k4 = diff(state + k3 * dt, input);
            state = state + (k1 + k2 * 2 + k3 * 2 + k4) * dt / 6;
        }
        /*
            计算四个中间变量（k1, k2, k3, k4），它们基于当前和预测的状态使用diff函数来估计状态的变化率。
            k1: 当前状态的变化率。
            k2: 在dt/2时间后，使用k1预测的中间状态的变化率。
            k3: 同样在dt/2时间后，使用k2预测的另一个中间状态的变化率。
            k4: 在dt时间后，使用k3预测的状态的变化率。
            最后，根据这四个变化率的加权平均，更新当前状态。这里使用的加权平均方法确保了更新的准确性和稳定性。
        */

        VectorX compensateDelay(const VectorX &x0)
        {                          // 补偿延迟
            VectorX x0_delay = x0; // 现在观测到的状态
            // TODO: compensate delay
            double dt = 1e-3;
            for (double t = delay_; t > 0; t = t - dt)
            {                                                       // t 是越来越小的，
                int i = std::ceil(t / dt_);                         // 向上取整
                VectorU input = historyInput_[history_length_ - i]; // ！！！ 用来模拟延迟的！！！
                step(x0_delay, input, dt);
            }
            return x0_delay; // 根据历史的输入，和当前观测到的状态，估算出这次计算出来的控制信号对应的状态

            // x(t+tao) 只于当前状态和过去 tao 时间的输入有关，与当前输入无关
            // 当前观测的状态是 x0
            // 延时的时长是 tao
            // 预测下一个状态 x0_delay 就得根据 tao 时刻前到 现在 的输入信号来计算了
        }

    public:
        MpcCar() {}
        void initialize()
        {
            std::cout << "mpc car initialize !!" << std::endl;
            ros::param::get("/trajectory_tracker/planning_params/desired_v_", desired_v_);
            // desired_v_ = 0;
            ros::param::get("/trajectory_tracker/planning_params/wheel_base_", wheel_base_); // 这个变量名该一下，跟Hybird A* 的统一
            ros::param::get("/trajectory_tracker/planning_params/dt_", dt_);
            ros::param::get("/trajectory_tracker/planning_params/rho_", rho_);
            ros::param::get("/trajectory_tracker/planning_params/N_", N_);
            ros::param::get("/trajectory_tracker/planning_params/rhoN_", rhoN_);
            ros::param::get("/trajectory_tracker/planning_params/v_max_", v_max_);
            ros::param::get("/trajectory_tracker/planning_params/a_max_", a_max_);
            ros::param::get("/trajectory_tracker/planning_params/steering_angle_", delta_max_);
            ros::param::get("/trajectory_tracker/planning_params/ddelta_max_", ddelta_max_);
            ros::param::get("/trajectory_tracker/planning_params/delay_", delay_);

            delta_max_ = delta_max_ * 3.141592 / 180; // 转换成弧度制
            ddelta_max_ = ddelta_max_ * 3.141592 / 180;
            history_length_ = std::ceil(delay_ / dt_); // 用来帮忙计算delay mpc的

            // TODO: set initial value of Ad, Bd, gd
            Ad_.setIdentity(); // Ad for instance

            // set size of sparse matrices
            P_.resize(m * N_, m * N_);
            q_.resize(m * N_, 1);
            Qx_.resize(n * N_, n * N_);

            int n_cons = 4; // v a delta ddelta
            A_.resize(n_cons * N_, m * N_);
            l_.resize(n_cons * N_, 1);
            u_.resize(n_cons * N_, 1);
            // v constrains
            Cx_.resize(1 * N_, n * N_);
            lx_.resize(1 * N_, 1);
            ux_.resize(1 * N_, 1);
            // a delta constrains
            Cu_.resize(3 * N_, m * N_); // 稀疏矩阵，默认是全0
            lu_.resize(3 * N_, 1);
            uu_.resize(3 * N_, 1);

            // stage cost   // 设置权重系数
            Qx_.setIdentity(); // 正常的所有状态的权重系数都是 1
            for (int i = 1; i <= N_; ++i)
            {                                           //  四种状态：x y phi v
                Qx_.coeffRef(i * n - 2, i * n - 2) = 1; // 车辆朝向代价权重系数  // 这里的权重系数改成自己写的吧
                Qx_.coeffRef(i * n - 1, i * n - 1) = 1; // 车辆速度代价权重系数
                Qx_.coeffRef(i * n - 1, i * n - 1) = 0.5;
                Qx_.coeffRef(i * n - 1, i * n - 1) = 0.8;
                // 这里将速度的权重设置成0，说明在代价函数中不考虑速度，即不管算出来的速度跟期望速度差多少，都不会增加代价值
                // 在算不同状态的时候，都是通过期望速度来计算的，如果权重不设置成0,那么优化出来的速度会尽量网期望速度上靠
                // 设置成0,感觉效果不是很好，0.5的时候就感觉一般，试试1    1效果还不错喔！  但是不能总调高这个，因为位置跟踪也是很重要的
            }
            // Qx_.coeffRef(N_ * n - 4, N_ * n - 4) = rhoN_;
            // Qx_.coeffRef(N_ * n - 3, N_ * n - 3) = rhoN_;
            // Qx_.coeffRef(N_ * n - 2, N_ * n - 2) = rhoN_ * rho_;

            // set lower and upper boundaries   设置约束边界
            for (int i = 0; i < N_; ++i)
            {
                // TODO: set stage constraints of inputs (a, delta, ddelta)   控制输入的约束
                // -a_max <= a <= a_max for instance:
                Cu_.coeffRef(i * 3 + 0, i * m + 0) = 1; // 跟下面的 Cx_ 同理
                lu_.coeffRef(i * 3 + 0, 0) = -a_max_;
                uu_.coeffRef(i * 3 + 0, 0) = a_max_;

                // Cu_.coeffRef(i * 3 + 1, i * m + 0) = 1;  原来代码的列索引是跟上面的一样的，感觉是有问题的，应该改成下面这行这样，编译运行也是正常能跑的。。
                Cu_.coeffRef(i * 3 + 1, i * m + 1) = 1; // 乘出来的控制输入项是： delta   跟下面的 delta_max 也对应上了
                lu_.coeffRef(i * 3 + 1, 0) = -delta_max_;
                uu_.coeffRef(i * 3 + 1, 0) = delta_max_;

                if (i > 0)
                {
                    Cu_.coeffRef(i * 3 + 2, (i - 1) * m + 1) = -1;
                    // 这个项乘出来是： -delta_{i - 1} + delta_{i}  两个状态间的转向角度变化差异，和下面的 dt * ddelta_max_ 也对应上了
                }

                Cu_.coeffRef(i * 3 + 2, i * m + 1) = 1;
                lu_.coeffRef(i * 3 + 2, 0) = -dt_ * ddelta_max_;
                uu_.coeffRef(i * 3 + 2, 0) = dt_ * ddelta_max_;

                // TODO: set stage constraints of states (v)    // 状态的约束（主要是速度最大最小值的约束）
                // -v_max <= v <= v_max
                Cx_.coeffRef(i, i * n + 3) = 1; // 权重系数矩阵
                // 稀疏矩阵，不给定初值默认元素是0,然后这里给定每行对应状态的第4个元素是1,其他元素都是0,就意味着屏蔽状态的前3个元素
                // 状态向量：x y phi v
                lx_.coeffRef(i, 0) = -v_max_; // 下边界约束
                ux_.coeffRef(i, 0) = v_max_;  // 上边界约束
            }

            // set predict mats size
            predictState_.resize(N_);
            predictInput_.resize(N_);
            for (int i = 0; i < N_; ++i)
            {
                predictInput_[i].setZero();
            }
            for (int i = 0; i < history_length_; ++i)
            {
                historyInput_.emplace_back(0, 0);
            }
            std::cout << "mpc car initialize finish !!" << std::endl;

        }

        Eigen::Vector2d solveQP(const VectorX &x0_observe, const nav_msgs::Path &local_plan, bool teb_run_falg)
        {
            // 速度规划方面，只需要在这里改 desired_v_ 就好了？  看看代价函数
            // std::cout << "MPC runtimes = " << run_times_++ << std::endl;
            // desired_v_ = std::min(desired_v_ + 0.2 * 1/15, 1.0);


            x0_observe_ = x0_observe; // 当前时刻机器人的状态
            historyInput_.pop_front();
            historyInput_.push_back(predictInput_.front());
            // 求解初值，用上一次的求解结果，弹出第一个控制量，并且在最后家一个控制量
            std::vector<double> track_points_x, track_points_y;
            track_points_x.clear();
            track_points_y.clear();
            for (int i = 0; i < local_plan.poses.size(); i++)
            {
                track_points_x.push_back(local_plan.poses[i].pose.position.x);
                track_points_y.push_back(local_plan.poses[i].pose.position.y);
            }

            if (teb_run_falg == true)
                s_.setWayPoints(track_points_x, track_points_y); // 根据轨迹点，求解出对应的：关于 t 的参数方程 x(t)  y(t)， 关于弧长 s 的参数方程 x(s),  y(s)  // 这里可以优化吧，没调用TEB算法的话，不用重新setWayPoints
            lu_.coeffRef(2, 0) = -delta_max_;                    // 第一组控制输入的转向角下界（运行到这里的时候lu_.coeffRef(2, 0) 和 uu_.coeffRef(2, 0) 不是构造函数中给定的值，很奇怪，但不深究了，这里再重新赋一次值
            uu_.coeffRef(2, 0) = delta_max_;

            VectorX x0 = compensateDelay(x0_observe_); // 延迟补偿
            // 他这里的延迟补偿，只是根据当前观测到的状态x0_observe_ 和 设定的延迟时间 delay_ 和 历史控制信号输入  计算出 delay_ 秒前的状态？
            // set BB, AA, gg
            Eigen::MatrixXd BB, AA, gg;
            BB.setZero(n * N_, m * N_);
            AA.setZero(n * N_, n);
            gg.setZero(n * N_, 1);
            double s0 = s_.findS(x0.head(2));
            double phi, v, delta;
            double last_phi = x0(2);
            Eigen::SparseMatrix<double> qx;
            qx.resize(n * N_, 1);

            for (int i = 0; i < N_; ++i)
            {
                calLinPoint(s0, phi, v, delta, s_); // 根据弧长 s0, 计算出对应轨迹点的 车辆朝向 phi  后轮速度 v   转向输入 delta
                if (phi - last_phi > M_PI)
                {
                    phi -= 2 * M_PI;
                }
                else if (phi - last_phi < -M_PI)
                {
                    phi += 2 * M_PI;
                }
                last_phi = phi;
                linearization(phi, v, delta); // 线性化 + 离散化   生成对应的 Ac  Bc  gc

                // calculate big state-space matrices
                /* *                BB                AA
                 * x1    /       B    0  ... 0 \    /   A \
                 * x2    |      AB    B  ... 0 |    |  A2 |
                 * x3  = |    A^2B   AB  ... 0 |u + | ... |x0 + gg
                 * ...   |     ...  ...  ... 0 |    | ... |
                 * xN    \A^(n-1 )B  ...  ... B /    \ A^N /
                 *
                 *     X = BB * U + AA * x0 + gg
                 * */

                if (i == 0)
                {
                    BB.block(0, 0, n, m) = Bd_;
                    AA.block(0, 0, n, n) = Ad_;
                    gg.block(0, 0, n, 1) = gd_;
                }
                else
                {
                    // TODO: set BB AA gg
                    BB.block(n * i, m * i, n, m) = Bd_;
                    for (int j = i - 1; j >= 0; j--)
                    {
                        BB.block(n * i, m * j, n, m) = Ad_ * BB.block(n * (i - 1), m * j, n, m);
                    }
                    AA.block(n * i, 0, n, n) = Ad_ * AA.block(n * (i - 1), 0, n, n);
                    gg.block(n * i, 0, n, 1) = Ad_ * gg.block(n * (i - 1), 0, n, 1) + gd_;
                }

                // TODO: set qx
                Eigen::Vector2d xy = s_(s0); // reference (x_r, y_r)
                // s_(s0)  返回的是在轨迹的s0弧长处的 x 和 y 坐标系

                // cost function should be represented as follows:
                /* *
                 *           /  x1  \T       /  x1  \         /  x1  \
                 *           |  x2  |        |  x2  |         |  x2  |
                 *  J =  0.5 |  x3  |   Qx_  |  x3  | + qx^T  |  x3  | + const.
                 *           | ...  |        | ...  |         | ...  |
                 *           \  xN  /        \  xN  /         \  xN  /
                 *  Qx_ 是关于系统状态的二次项，  qx 是关于系统的线性项
                 * */

                // 原始代价函数的一次项权重矩阵
                qx.coeffRef(n * i, 0) = -Qx_.coeffRef(n * i, n * i) * xy(0);
                qx.coeffRef(n * i + 1, 0) = -Qx_.coeffRef(n * i + 1, n * i + 1) * xy(1);
                qx.coeffRef(n * i + 2, 0) = -Qx_.coeffRef(n * i + 2, n * i + 2) * phi;
                qx.coeffRef(n * i + 3, 0) = 0;

                s0 += desired_v_ * dt_; // 轨迹的弧长索引往前进一步
                s0 = s0 < s_.arcL() ? s0 : s_.arcL();
            }

            Eigen::SparseMatrix<double> BB_sparse = BB.sparseView();
            Eigen::SparseMatrix<double> AA_sparse = AA.sparseView();
            Eigen::SparseMatrix<double> gg_sparse = gg.sparseView();
            Eigen::SparseMatrix<double> x0_sparse = x0.sparseView();

            // state constrants propogate to input constraints using "X = BB * U + AA * x0 + gg"
            /* *
             *               /  x1  \                              /  u0  \
             *               |  x2  |                              |  u1  |
             *  lx_ <=  Cx_  |  x3  |  <= ux_    ==>    lx <=  Cx  |  u2  |  <= ux
             *               | ...  |                              | ...  |
             *               \  xN  /                              \ uN-1 /
             *    ！ ！ ！
             * */

            // 把状态约束转换成控制输入的约束
            Eigen::SparseMatrix<double> Cx = Cx_ * BB_sparse;
            Eigen::SparseMatrix<double> lx = lx_ - Cx_ * AA_sparse * x0_sparse - Cx_ * gg_sparse;
            Eigen::SparseMatrix<double> ux = ux_ - Cx_ * AA_sparse * x0_sparse - Cx_ * gg_sparse;

            /* *      / Cx  \       / lx  \       / ux  \
             *   A_ = \ Cu_ /, l_ = \ lu_ /, u_ = \ uu_ /
             * */

            // 将转换后的状态约束 和 控制约束合并起来
            Eigen::SparseMatrix<double> A_T = A_.transpose();
            A_T.middleCols(0, Cx.rows()) = Cx.transpose();
            A_T.middleCols(Cx.rows(), Cu_.rows()) = Cu_.transpose();
            A_ = A_T.transpose();
            for (int i = 0; i < lx.rows(); ++i)
            {
                l_.coeffRef(i, 0) = lx.coeff(i, 0);
                u_.coeffRef(i, 0) = ux.coeff(i, 0);
            }
            for (int i = 0; i < lu_.rows(); ++i)
            {
                l_.coeffRef(i + lx.rows(), 0) = lu_.coeff(i, 0);
                u_.coeffRef(i + lx.rows(), 0) = uu_.coeff(i, 0);
            }

            Eigen::SparseMatrix<double> BBT_sparse = BB_sparse.transpose();
            P_ = BBT_sparse * Qx_ * BB_sparse;
            q_ = BBT_sparse * Qx_.transpose() * (AA_sparse * x0_sparse + gg_sparse) + BBT_sparse * qx;
            // osqp
            Eigen::VectorXd q_d = q_.toDense(); // toDense() 函数，它们分别被转换为密集向量
            Eigen::VectorXd l_d = l_.toDense();
            Eigen::VectorXd u_d = u_.toDense();

            /**
             * osqp interface:     QP求解器要求的形式如下，往下面矩阵填内容的时候必须填成下面形式
             *
             * minimize     0.5 x^T P_ x + q_^T x
             * subject to   l_ <= A_ x <= u_
             *
             * OSQP（Operator Splitting Quadratic Program）一个用于解决凸二次规划问题的求解器
             * 一个高性能、开源的数值优化工具，旨在有效地解决大规模稀疏凸二次规划问题。
             **/

            qpSolver_.setMats(P_, q_d, A_, l_d, u_d);
            qpSolver_.solve();
            int ret = qpSolver_.getStatus();
            if (ret != 1)
            {
                ROS_ERROR("fail to solve QP!");
                return Eigen::Vector2d::Zero();
            }
            Eigen::VectorXd sol = qpSolver_.getPrimalSol();
            Eigen::MatrixXd solMat = Eigen::Map<const Eigen::MatrixXd>(sol.data(), m, N_);
            Eigen::VectorXd solState = BB * sol + AA * x0 + gg;
            Eigen::MatrixXd predictMat = Eigen::Map<const Eigen::MatrixXd>(solState.data(), n, N_);

            for (int i = 0; i < N_; ++i)
            {
                predictInput_[i] = solMat.col(i);
                predictState_[i] = predictMat.col(i);
            }
            return predictInput_.front();
        }
    };

}