#pragma once #include #include #include #include "mpc/arc_spline.hpp" #include #include "mpc/iosqp.hpp" #include namespace mpc_car { static constexpr int n = 4; // 四种状态：x y phi v static constexpr int m = 2; // 两个输入： a delta typedef Eigen::Matrix MatrixA; typedef Eigen::Matrix 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 predictState_; // 预测的状态（根据QP求解后得到的控制输入，计算出来的预测状态） std::vector predictInput_; // QP 求解后的决策变量（控制输入） std::deque 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 P_; // (2 * N_, 2 * N_) 最终代价函数二次项的权重矩阵 Eigen::SparseMatrix q_; // (2 * N_, 1) 最终代价函数一次项的权重矩阵 Eigen::SparseMatrix A_; // (4 * N_, 2 * N_) 最终这是什么矩阵？ Eigen::SparseMatrix l_; // (4 * N_, 1) 最终约束下边界矩阵 Eigen::SparseMatrix 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 Cx_; // (1 * N_, 4 * N_) 状态约束的权重系数矩阵？ Eigen::SparseMatrix lx_; // (1 * N_, 1) 状态约束的下边界矩阵 Eigen::SparseMatrix ux_; // (1 * N_, 1) 状态约束的上边界矩阵 /* * 系统控制输入约束 * / u0 \ * | u1 | * lu_ <= Cu_ | u2 | <= uu_ * | ... | * \ uN-1 / * */ // a delta vs constrains // 控制约束？ Eigen::SparseMatrix Cu_; // (3 * N_, 2 * N_) 状态约束的权重系数矩阵？ Eigen::SparseMatrix lu_; // (3 * N_, 1) 控制输入约束下边界矩阵 Eigen::SparseMatrix uu_; // (3 * N_, 1) 控制输入约束上边界矩阵 Eigen::SparseMatrix 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 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 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 BB_sparse = BB.sparseView(); Eigen::SparseMatrix AA_sparse = AA.sparseView(); Eigen::SparseMatrix gg_sparse = gg.sparseView(); Eigen::SparseMatrix 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 Cx = Cx_ * BB_sparse; Eigen::SparseMatrix lx = lx_ - Cx_ * AA_sparse * x0_sparse - Cx_ * gg_sparse; Eigen::SparseMatrix ux = ux_ - Cx_ * AA_sparse * x0_sparse - Cx_ * gg_sparse; /* * / Cx \ / lx \ / ux \ * A_ = \ Cu_ /, l_ = \ lu_ /, u_ = \ uu_ / * */ // 将转换后的状态约束和控制约束合并起来 Eigen::SparseMatrix 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 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(sol.data(), m, N_); Eigen::VectorXd solState = BB * sol + AA * x0 + gg; Eigen::MatrixXd predictMat = Eigen::Map(solState.data(), n, N_); for (int i = 0; i < N_; ++i) { predictInput_[i] = solMat.col(i); predictState_[i] = predictMat.col(i); } return predictInput_.front(); } }; }