基于MPC算法控制车辆的运动轨迹

需要用到的一些库。

biyezuopinvip

2808人浏览 · 2022-05-12 00:29:50

biyezuopinvip · 2022-05-12 00:29:50 发布

资源下载地址：https://download.csdn.net/download/sheziqiong/85836018
资源下载地址：https://download.csdn.net/download/sheziqiong/85836018

MPC-Control

在这里插入图片描述

 correct result

在这里插入图片描述

implements Model Predictive Control that handles a 100 millisecond latency.

set　N = 20，dt = 0.05，so T=100ms

size_t N = 20;
double dt = 0.05; // 50ms

Take the next value of the predicted result, mpc.latency=1

double steer_value = result[2][mpc.latency];
double throttle_value = result[3][mpc.latency];

simulator lantency

this_thread::sleep_for(chrono::milliseconds(100));

简介

运动学模型：忽略了轮胎的力，重力和质量的影响。
动力学模型：考虑了轮胎的力，纵向和横向受到的力，重力，惯性，质量，以及车子的结构等。

状态

和追踪车辆状态一样，当前车辆模型预测包括位置x，y，角度ψ，速度v

在这里插入图片描述

汽车的运动学模型

为了使得我们实际的车的状态和汽车预测状态之间的误差最小，我们需要控制车辆驱动。

包括方向盘和加速/刹车输入。

在这里插入图片描述

根据上一时刻的汽车状态，预测下一时刻的状态，公式

注意：Lf表示汽车的头部到汽车重心的距离，距离越大，转向角越小，反之亦然。比如，大货车的转向角度要比小轿车的转向角度小，在转动方向盘相同角度的情况下

在这里插入图片描述

实现运动学模型

#include <math.h>
#include <iostream>
#include "Dense"

double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

const double Lf = 2;

Eigen::VectorXd globalKinematic(Eigen::VectorXd state,
                                Eigen::VectorXd actuators, double dt) {
  Eigen::VectorXd next_state(state.size());
  
  // the next state from inputs
  next_state[0] = state[0] + state[3]*cos(state[2])*dt;
  next_state[1] = state[1] + state[3]*sin(state[2])*dt;
  next_state[2] = state[2] + state[3]/Lf*actuators[0]*dt;
  next_state[3] = state[3] + actuators[1]*dt;
  
  return next_state;
}

int main() {
  // [x, y, psi, v]
  Eigen::VectorXd state(4);
  Eigen::VectorXd actuators(2);
  state << 0, 0, deg2rad(45), 1;
  actuators << deg2rad(5), 1;
  Eigen::VectorXd next_state = globalKinematic(state, actuators, 0.3);
  std::cout << next_state << std::endl;
}

无人驾驶的整体过程

感知 => 定位 => 路径规划 => 车辆控制

在这里插入图片描述

使用多项式拟合规划路径

一般三次多项式就能满足要求

#include <iostream>
#include "Dense"

using namespace Eigen;

// 传入方程的系数和X值求方程的输出y
double polyeval(Eigen::VectorXd coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); i++) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
// 这个函数的作用是传入未知数x，y和方程的最高次数order，根据未知数建立矩阵方程，使用Qr分解解得位置参
// 数，其实也就是用点去拟合曲线
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;
}

int main() {
  Eigen::VectorXd xvals(6);
  Eigen::VectorXd yvals(6);
  xvals << 9.261977, -2.06803, -19.6663, -36.868, -51.6263, -66.3482;
  yvals << 5.17, -2.25, -15.306, -29.46, -42.85, -57.6116;
  // 拟合出的曲线的系数
  Eigen::VectorXd fit_curve = polyfit(xvals, yvals, 3);
  // 测试点
  for (double x = 0; x <= 20; x += 1.0) {
    std::cout << polyeval(fit_curve, x) << std::endl; 
  }
}

Cross Track Error（CTE），距离误差

通过前一个状态的误差，估计后一状态的误差。

yt表示当前所在的位置，f(xt)表示预测路径所在的位置

注意：这个地方使用的是eψt，表示和真实轨迹运动方向的差距

ctet+1 = ctet + vt∗sin(eψt)∗dt

ctet = yt − f(xt)

合并：

ctet+1 = yt − f(xt) + (vt∗sin(eψt)∗dt)
Orientation Error，方向误差

注意：我们并不知道ψdest的规划路径的角度值，可以使用求斜率，再求arctan的方式求得，arctan(f′(xt))。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-nhholm6X-1652167530752)(imgs/8.jpg)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-w8gIOcOC-1652167530753)(imgs/9.jpg)]

合并：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ysxe3sDg-1652167530753)(imgs/10.jpg)]
预测轨迹和参考轨迹之间的误差

把错误的变化包括到状态中，那么新的状态向量为 [x,y,ψ,v,cte,eψ]，向量一共六维。

注意到增加了cte和eψ，表示和到预测路径距离的错误和预测路径方向的错误。

MPC(模型预测控制)

模型预测把路径追踪任务变成了一个优化问题，即寻找最小成本的轨迹，这个地方的成本就是指在满足指定限制条件的基础上，使得路径和参考路径的距离值和角度值最贴近。

总体步骤：

获取当前状态和参考路径
根据当前状态和参考路径，预测轨迹，包括N个控制点，每个点控制的时间段dt，总时间T=N*dt。
控制驱动器输入，第一个控制点的数据。
第一个控制点完成之后，从第一步再递归进行。

为什么不进行一次性预测完成，执行每个控制点之后走完，再预测？

因为现实世界会有各种误差，导致实际情况并不是和预测轨迹一致，所以预测一次走一个控制点。所以MPC也叫“滚动时域控制”。

计算误差

为了减小误差，我们需要调整驱动器输入，控制车辆实际的预测路线和参考路线的距离和方向误差尽可能的小。

计算每一个时间点的误差累计：
```
double cost = 0;
for (int t = 0; t < N; t++) {
    cost += pow(cte[t], 2);
    cost += pow(epsi[t], 2);
}
```
但是如上的代码可能有两个问题：
1. 车辆可能在其中的某个位置停止。
2. 车辆的行驶线路可能是很不平滑的。
解决车辆可能停止的办法：
1. 把速度加入成本函数。
2. 把当前位置距离终点的距离加入成本函数。
解决行驶线路不平滑的问题：把车辆控制参数加入成本函数,两次控制的变化不能太大。
预测线路区间T，由两部分组成N和dt，其中，N表示预测线路有多少个预测点，dt表示每一个预测点的执行的时间。例如：假设一共有20个预测点N=20，每个预测点的时间dt=0.5，则T为20x0.5=10秒。

注意：在下面的图中状态点有7个但是驱动控制向量只有6个，可以这样理解，驱动控制的结果是下一个状态点，最后一个状态点没有下一个状态点，所以不需要驱动输入。

在这里插入图片描述

计算流程
PID和MPC的优缺点比较

PID：有延迟影响，PID基于当前的状态计算出驱动指令，向驱动器发送命令到驱动器执行完成，这之间有一个延迟的过程，差不多100ms，所以等到执行命令的时候状态已经变化，会出现不稳定的情况。

MPC：由于MPC有预测模型，通过当前状态能预测出下一状态，比如：在获取到预测的路线之后，取第二个点作为驱动输入。

总结：MPC控制比PID控制在处理延迟方面更加的高效。
需要用到的一些库

Ipopt：可以通过使得最小化成本函数，获得最优驱动输入[δ1,a1,…,δN−1,aN−1]，由于需要加上驱动限制Ipopt 需要使用雅可比(jacobians)和黑塞(haishens)矩阵，他不会直接计算，所以使用到了CppAD。

CppAD：用于求自动微分，调用数学函数的时候需要使用CppAD::作为前缀，比如：CppAD::pow(x, 2);，使用CppAD的返回类型，比如：CppAD<double>代替double。

代码片段

// 所有预测的状态值存储到一个向量里边，所以需要确定每种状态在向量中的开始位置
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;
	FG_eval(Eigen::VectorXd coeffs) {
		this->coeffs = coeffs;
	}

	typedef CPPAD_TESTVECTOR(AD<double>) ADvector;

	// 该函数的目的是定义约束，fg向量包含的是总损失和约束，vars向量包含的是状态值和驱动器的输入
	void operator()(ADvector& fg, const ADvector& vars) {
		// 任何cost都会加到fg[0]
		fg[0] = 0;
		// 使车辆轨迹和参考路径的误差最小，且使车辆的速度尽量接近参考速度
		for (int t = 0; t < N; t++) {
			fg[0] += CppAD::pow(vars[cte_start + t], 2);
			fg[0] += CppAD::pow(vars[epsi_start + t], 2);
			fg[0] += CppAD::pow(vars[v_start + t] - ref_v, 2);
		}
		// 使车辆行驶更平稳，尽量减少每一次驱动器的输入大小
		// 注意：驱动器的输入是N-1个.最后一个点没有驱动器输入
		for (int t = 0; t < N - 1; t++) {
			fg[0] += CppAD::pow(vars[delta_start + t], 2);
			fg[0] += CppAD::pow(vars[a_start + t], 2);
		}
		// 为了使车辆运动更平滑，尽量减少相邻两次驱动器输入的差距
		// 注意：这个地方是N-2个
		for (int t = 0; t < N - 2; t++) {
			fg[0] += 600*CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
			fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
		}
		for (int t = 0; t < N - 2; t++) {
			fg[0] += 600*CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
			fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 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 = 1; t < N; t++) {
			// t+1时刻的状态
			AD<double> x1 = vars[x_start + t];
			AD<double> y1 = vars[y_start + t];
			AD<double> psi1 = vars[psi_start + t];
			AD<double> v1 = vars[v_start + t];
			AD<double> cte1 = vars[cte_start + t];
			AD<double> epsi1 = vars[epsi_start + t];

			// t时刻的状态
			AD<double> x0 = vars[x_start + t - 1];
			AD<double> y0 = vars[y_start + t - 1];
			AD<double> psi0 = vars[psi_start + t - 1];
			AD<double> v0 = vars[v_start + t - 1];
			AD<double> cte0 = vars[cte_start + t - 1];
			AD<double> epsi0 = vars[epsi_start + t - 1];
			// t时刻的驱动器输入
			AD<double> delta0 = vars[delta_start + t - 1];
			AD<double> a0 = vars[a_start + t - 1];

			// t时刻参考路径的距离和角度值
			AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2]*x0*x0 + coeffs[3]*x0*x0*x0;
			AD<double> psides0 = CppAD::atan(coeffs[1]+2*coeffs[2]*x0 + 3 * coeffs[3]*x0*x0);

			// 根据如上的状态值计算约束条件
			fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
			fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
			fg[1 + psi_start + t] = psi1 - (psi0 + v0 * delta0 / Lf * dt);
			fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
			fg[1 + cte_start + t] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
			fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) + v0 * delta0 / Lf * dt);
		}
	}
};

MPC

确定每一个变量的初始值和取值范围，没有初始值的默认都设为0

vector<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.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】加速度的范围-1—1
	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;
	}
	for (int i = a_start; i < n_vars; i++) {
		vars_lowerbound[i] = -1.0;
		vars_upperbound[i] = 1.0;
	}
	// 前一次计算的驱动器输入作为约束
	vars_lowerbound[a_start] = a_prev;
	vars_upperbound[a_start] = a_prev;
    vars_lowerbound[delta_start] = delta_prev;
    vars_upperbound[delta_start] = delta_prev;

	// 设置约束条件的的最大和最小值，除了初始状态其他约束都为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);

	// 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);

	// 返回值
	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> X;
	vector<double> Y;
	vector<double> Delta;
	vector<double> A;
	for (auto i = 0; i < N - 1; i++) {
		X.push_back(solution.x[x_start + i]);
		Y.push_back(solution.x[y_start + i]);
		Delta.push_back(solution.x[delta_start + i]);
		A.push_back(solution.x[a_start + i]);
	}
	vector<vector<double>> result;
	result.push_back(X);
	result.push_back(Y);
	result.push_back(Delta);
	result.push_back(A);
	return result;
}

坐标转换
把世界坐标转换为汽车坐标，以当前汽车的位置作为源点
1. 计算出相对位置，即参考轨迹坐标减去车辆坐标，ptsx[i] - x
2. 坐标旋转
这时车辆当前位置的坐标x，y和方向都为0，由于相对位置没有改变，所以结果不变，但是计算相对简单。
关于怎么旋转画了个图理解：

在这里插入图片描述

Eigen::MatrixXd transformGlobal2Vehicle(double x, double y, double psi,
		const vector<double> & ptsx, const vector<double> & ptsy) {
	assert(ptsx.size() == ptsy.size());
	unsigned len = ptsx.size();
	auto waypoints = Eigen::MatrixXd(2, len);
	for (auto i = 0; i < len; ++i) {
		waypoints(0, i) = cos(psi) * (ptsx[i] - x) + sin(psi) * (ptsy[i] - y);
		waypoints(1, i) = -sin(psi) * (ptsx[i] - x) + cos(psi) * (ptsy[i] - y);
	}
	return waypoints;
}

误差值

由于转换到当前车辆位置为原点的坐标了，所以误差值也很好计算了

距离误差：只计算源点到曲线的y距离

方向误差：计算曲线的角度值

// 计算CTE误差
double evaluateCte(Eigen::VectorXd coeffs) {
    return polyeval(coeffs, 0);
}

// 计算Epsi误差
double evaluateEpsi(Eigen::VectorXd coeffs) {
    return -atan(coeffs[1]);
}

处理返回的结果值,main()

注意：取到的驱动器输入应该取延迟时间之后的输入

比如：延迟100ms，相邻点的时间间隔是50ms，则取第三个输入，没有延迟取第一个，延迟50ms取第二个，延迟100ms所以取第三个。

// json对象数据
vector<double> ptsx = j[1]["ptsx"];
vector<double> ptsy = j[1]["ptsy"];
double px = j[1]["x"];
double py = j[1]["y"];
double psi = j[1]["psi"];
double v = j[1]["speed"];

// 转换坐标到当前车辆为源点的坐标
Eigen::MatrixXd waypoints = transformGlobal2Vehicle(px, py, psi, ptsx, ptsy);
Eigen::VectorXd Ptsx = waypoints.row(0);
Eigen::VectorXd Ptsy = waypoints.row(1);

// 获取状态值
auto coeffs = polyfit(Ptsx, Ptsy, 3);
double cte = evaluateCte(coeffs);
double epsi = evaluateEpsi(coeffs);
Eigen::VectorXd state(6);
state << 0, 0, 0, v, cte, epsi;

// 计算方向和油门，范围都在[-1, 1]之间
// 这个地方根据延迟的时间去判断选取哪一个点的驱动器输入
vector<vector<double>> result = mpc.Solve(state, coeffs);
double steer_value = result[2][mpc.latency];
double throttle_value = result[3][mpc.latency];

// 设置当前输入作为下下一时刻的驱动器初始值
mpc.delta_prev = steer_value;
mpc.a_prev = throttle_value;

// 注意：需要把角度范围转换到-1到1
json msgJson;
msgJson["steering_angle"] = -steer_value/0.436332;
msgJson["throttle"] = throttle_value;

// 展示MPC预测的路径
vector<double> mpc_x_vals=result[0];
vector<double> mpc_y_vals=result[1];
msgJson["mpc_x"] = mpc_x_vals;
msgJson["mpc_y"] = mpc_y_vals;

// 展示参考路径
vector<double> next_x_vals;
vector<double> next_y_vals;
for (unsigned i=0; i < ptsx.size(); ++i) {
    next_x_vals.push_back(Ptsx(i));
    next_y_vals.push_back(Ptsy(i));
}
msgJson["next_x"] = next_x_vals;
msgJson["next_y"] = next_y_vals;

设置延迟时间100ms

this_thread::sleep_for(chrono::milliseconds(100));

在模拟器中的运行结果

在这里插入图片描述

需要的运行环境
- cmake >= 3.5
- All OSes: click here for installation instructions
- make >= 4.1(mac, linux), 3.81(Windows)
  - Linux: make is installed by default on most Linux distros
  - Mac: install Xcode command line tools to get make
  - Windows: Click here for installation instructions
- gcc/g++ >= 5.4
  - Linux: gcc / g++ is installed by default on most Linux distros
  - Mac: same deal as make - [install Xcode command line tools]((https://developer.apple.com/xcode/features/)
  - Windows: recommend using MinGW
- uWebSockets
  - Run either install-mac.sh or install-ubuntu.sh.
  - If you install from source, checkout to commit
```
e94b6e1
```
    , i.e.
```
git clone https://github.com/uWebSockets/uWebSockets
cd uWebSockets
git checkout e94b6e1
```
    Some function signatures have changed in v0.14.x. See
    
    this PR
    
    for more details.
- Ipopt and CppAD: Please refer to this document for installation instructions.
- Eigen. This is already part of the repo so you shouldn’t have to worry about it.
- Simulator. You can download these from the releases tab.
- Not a dependency but read the DATA.md for a description of the data sent back from the simulator.
项目运行
1. Clone this repo.
2. Make a build directory: mkdir build && cd build
3. Compile: cmake .. && make
4. Run it: ./mpc.

资源下载地址：https://download.csdn.net/download/sheziqiong/85836018
资源下载地址：https://download.csdn.net/download/sheziqiong/85836018

魔乐社区

魔乐社区（Modelers.cn) 是一个中立、公益的人工智能社区，提供人工智能工具、模型、数据的托管、展示与应用协同服务，为人工智能开发及爱好者搭建开放的学习交流平台。社区通过理事会方式运作，由全产业链共同建设、共同运营、共同享有，推动国产AI生态繁荣发展。

更多推荐

全家桶集齐！Qwen3.5四款小模型上线魔乐社区，附昇腾全套实践教程

魔乐社区

Pont - 搭建前后端之桥：高效、灵活的接口管理工具

Pont 是一款强大的数据服务层解决方案，它能够帮助开发者快速搭建前后端之间的桥梁，实现接口的高效管理和代码自动生成。无论是新手还是有经验的开发者，都能通过 Pont 轻松处理接口文档、生成类型安全的 API 代码，从而显著提升开发效率。[![Pont 工具标志](https://raw.gitcode.com/gh_mirrors/po/pont/raw/3f1b7d4bbba3fd2dda

魔乐社区

如何快速上手 hvac：HashiCorp Vault Python 客户端零基础入门指南

**hvac** 是 HashiCorp Vault 的 Python 3.X 客户端库，专为开发者提供简单高效的 Vault 交互方式。无论你是需要管理密钥、配置身份验证，还是实现安全的秘密数据存储，hvac 都能帮助你轻松搞定 Vault 的各项操作。本文将带你零基础快速入门，从安装到基础操作，让你在几分钟内即可上手使用这个强大的工具。[![hvac 客户端 Logo](https://r