|
@@ -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
|