删除MPC相关代码与配置

src/common/CMakeLists.txt

@@ -207,8 +207,6 @@ include_directories(
 # catkin_add_nosetests(test)
 add_executable(commonmain
     time.h
-    mpc.h
-    mpc.cpp
     util.h
     util.cpp
     file.h
@@ -230,6 +228,5 @@ target_link_libraries(commonmain
     gflags
     pthread
     protobuf
-    ipopt
     sqlite3)
 

src/common/mpc.cpp

@@ -0,0 +1,236 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
+#include "mpc.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+
+
+using CppAD::AD;
+
+namespace robot {
+namespace common {
+
+
+// 设置预测状态点的个数和两个相邻状态点之间的时间间隔
+size_t N = 10;
+double dt = 0.05; // 50ms
+
+// 设置汽车头到车辆重心之间的距离
+const double Lf = 2.67;
+
+// 参考速度，为了避免车辆在行驶中停止
+//double ref_v = 65;
+double ref_cte = 0;
+double ref_epsi = 0;
+
+// solver使用的是一个向量存储所有的状态值，所以需要确定每种状态在向量中的开始位置
+size_t x_start = 0;
+size_t y_start = x_start + N;
+size_t psi_start = y_start + N;
+size_t v_start = psi_start + N;
+size_t cte_start = v_start + N;
+size_t epsi_start = cte_start + N;
+size_t delta_start = epsi_start + N;
+size_t a_start = delta_start + N -1;
+
+class FG_eval {
+public:
+
+    // 参考路径方程的系数
+    Eigen::VectorXd coeffs;
+    double ref_v = 0;
+    FG_eval(Eigen::VectorXd coeffs, double speed) {
+        this->coeffs = coeffs;
+        ref_v = speed;
+    }
+
+    typedef CPPAD_TESTVECTOR(AD<double>)ADvector;
+
+    // 该函数的目的是定义约束，fg向量包含的是总损失和约束，vars向量包含的是状态值和驱动器的输入
+     void operator()(ADvector& fg, const ADvector& vars) {
+        // 任何cost都会加到fg[0]
+        fg[0] = 0;
+        // 使车辆轨迹和参考路径的误差最小，且使车辆的速度尽量接近参考速度
+        for (int i = 0; i < N; i++) {
+             fg[0] += 2000*CppAD::pow(vars[cte_start + i] - ref_cte, 2);
+             fg[0] += 2000*CppAD::pow(vars[epsi_start + i] - ref_epsi, 2);
+             fg[0] += CppAD::pow(vars[v_start + i] - ref_v, 2);
+           }
+        // 使车辆行驶更平稳，尽量减少每一次驱动器的输入大小
+        // 注意：驱动器的输入是N-1个.最后一个点没有驱动器输入
+        for (int i = 0; i < N - 1; i++) {
+          fg[0] += 5*CppAD::pow(vars[delta_start + i], 2);
+          fg[0] += 5*CppAD::pow(vars[a_start + i], 2);
+        }
+        // 为了使车辆运动更平滑，尽量减少相邻两次驱动器输入的差距
+        // 注意：这个地方是N-2个
+        for (int i = 0; i < N - 2; i++) {
+          fg[0] += 200*CppAD::pow(vars[delta_start + i + 1] - vars[delta_start + i], 2);
+          fg[0] += 10*CppAD::pow(vars[a_start + i + 1] - vars[a_start + i], 2);
+        }
+
+        // 设置fg的初始值为状态的初始值，这个地方为初始条件约束
+        fg[1 + x_start] = vars[x_start];
+        fg[1 + y_start] = vars[y_start];
+        fg[1 + psi_start] = vars[psi_start];
+        fg[1 + v_start] = vars[v_start];
+        fg[1 + cte_start] = vars[cte_start];
+        fg[1 + epsi_start] = vars[epsi_start];
+
+        // 因为t=0初始条件约束已经有了，计算其他约束条件
+        for (int t = 0; t < N -1; t++) {
+            // t+1时刻的状态
+            AD<double> x1 = vars[x_start + t + 1];
+            AD<double> y1 = vars[y_start + t + 1];
+            AD<double> psi1 = vars[psi_start + t + 1];
+            AD<double> v1 = vars[v_start + t + 1];
+            AD<double> cte1 = vars[cte_start + t + 1];
+            AD<double> epsi1 = vars[epsi_start + t + 1];
+
+            // t时刻的状态
+            AD<double> x0 = vars[x_start + t ];
+            AD<double> y0 = vars[y_start + t ];
+            AD<double> psi0 = vars[psi_start + t ];
+            AD<double> v0 = vars[v_start + t ];
+            AD<double> cte0 = vars[cte_start + t ];
+            AD<double> epsi0 = vars[epsi_start + t ];
+            // t时刻的驱动器输入
+            AD<double> delta0 = vars[delta_start + t ];
+            AD<double> a0 = vars[a_start + t ];
+
+            // t时刻参考路径的距离和角度值
+            AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * x0 * x0 + coeffs[3] * x0 * x0 * x0;
+            AD<double> psides0 = CppAD::atan(3*coeffs[3]* x0* x0 + 2*coeffs[2]* x0 + coeffs[1]);
+
+            // 根据如上的状态值计算约束条件
+            fg[2 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
+            fg[2 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
+            fg[2 + psi_start + t] = psi1 - (psi0 - v0 * delta0 / Lf * dt);
+            fg[2 + v_start + t] = v1 - (v0 + a0 * dt);
+            fg[2 + cte_start + t] =
+                cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
+            fg[2 + epsi_start + t] =
+                epsi1 - ((psi0 - psides0) - v0 * delta0 / Lf * dt);
+        }
+    }
+};
+
+// MPC class definition
+MPC::MPC() {
+}
+MPC::~MPC() {
+}
+
+vector<double> MPC::Solve(Eigen::VectorXd x0, Eigen::VectorXd coeffs) {
+  size_t i;
+  typedef CPPAD_TESTVECTOR(double) Dvector;
+
+  double x = x0[0];
+  double y = x0[1];
+  double psi = x0[2];
+  double v = x0[3];
+  double cte = x0[4];
+  double epsi = x0[5];
+
+  // 独立状态的个数，注意：驱动器的输入（N - 1）* 2
+  size_t n_vars = N * 6 + (N - 1) * 2;
+  // 约束条件的个数
+  size_t n_constraints = N * 6;
+
+    // 除了初始值，初始化每一个状态为0
+  Dvector vars(n_vars);
+   for (int i = 0; i < n_vars; i++) {
+     vars[i] = 0;
+   }
+   vars[x_start] = x;
+   vars[y_start] = y;
+   vars[psi_start] = psi;
+   vars[v_start] = v;
+   vars[cte_start] = cte;
+   vars[epsi_start] = epsi;
+
+    // 设置每一个状态变量的最大和最小值
+    // 【1】设置非驱动输入的最大和最小值
+    // 【2】设置方向盘转动角度范围-25—25度
+    // 【3】加速度的范围-—100%
+   Dvector vars_lowerbound(n_vars);
+   Dvector vars_upperbound(n_vars);
+   for (int i = 0; i < delta_start; i++) {
+       vars_lowerbound[i] = -1.0e19;
+       vars_upperbound[i] = 1.0e19;
+     }
+   for (int i = delta_start; i < a_start; i++) {
+       vars_lowerbound[i] = -0.436332;
+       vars_upperbound[i] = 0.436332;
+//     vars_lowerbound[i] = (delta_prev < -0.32) ? -0.436332 : delta_prev - 0.1;
+//     vars_upperbound[i] = (delta_prev > 0.32) ? 0.436332 : delta_prev + 0.1;
+   }
+   for (int i = a_start; i < n_vars; i++) {
+       vars_lowerbound[i] = -0.5; // -0.36
+       vars_upperbound[i] = 0.5;
+//     vars_lowerbound[i] = (a_prev < -0.31) ? -0.36 : a_prev - 0.05;; // -0.36
+//     vars_upperbound[i] = (a_prev > 0.31) ? 0.36 : a_prev + 0.05;;  // 0.36
+   }
+   // 设置约束条件的的最大和最小值，除了初始状态其他约束都为0
+   Dvector constraints_lowerbound(n_constraints);
+   Dvector constraints_upperbound(n_constraints);
+   for (int i = 0; i < n_constraints; i++) {
+     constraints_lowerbound[i] = 0;
+     constraints_upperbound[i] = 0;
+   }
+   constraints_lowerbound[x_start] = x;
+   constraints_lowerbound[y_start] = y;
+   constraints_lowerbound[psi_start] = psi;
+   constraints_lowerbound[v_start] = v;
+   constraints_lowerbound[cte_start] = cte;
+   constraints_lowerbound[epsi_start] = epsi;
+
+   constraints_upperbound[x_start] = x;
+   constraints_upperbound[y_start] = y;
+   constraints_upperbound[psi_start] = psi;
+   constraints_upperbound[v_start] = v;
+   constraints_upperbound[cte_start] = cte;
+   constraints_upperbound[epsi_start] = epsi;
+
+    // Object that computes objective and constraints
+    FG_eval fg_eval(coeffs, ref_speed);
+
+    // options
+    std::string options;
+    options += "Integer print_level  0\n";
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+
+    // 计算这个问题
+    CppAD::ipopt::solve_result<Dvector> solution;
+    CppAD::ipopt::solve<Dvector, FG_eval>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
+
+    //
+    // Check some of the solution values
+    //
+    bool ok = true;
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+    //auto cost = solution.obj_value;
+    //std::cout << "Cost " << cost << std::endl;
+
+    vector<double> result;
+
+    result.push_back(solution.x[delta_start]);
+    result.push_back(solution.x[a_start]);
+
+    for (int i = 0; i < N-1; i++)
+    {
+       result.push_back(solution.x[x_start + i + 1]);
+       result.push_back(solution.x[y_start + i + 1]);
+    }
+    return result;
+}
+
+}  // namespace common
+}  // namespace robot