自动驾驶-MPC控制器
发布时间
阅读量：
阅读量
在上一节中讨论了使用PID控制器来控制车辆的情况。其优点在于实现过程较为简便，并且运行效率较高。然而它无法应对存在延时系统的挑战。本章所介绍的MPC（modle predictive control）控制器则非常有效地解决了这一问题。
MPC控制器与PID控制器在结构上具有相似性。两者均接受车辆下一时刻的运动规划数据以及当前状态信息作为输入，并分别生成航向角和加速度等输出信号来控制方向盘、油门和刹车装置。然而，在执行机制上存在显著差异：PID控制器基于当前状态与目标轨迹之间的偏差进行实时调整以跟踪目标轨迹；而MPC控制器则通过将未来时间段划分为多个离散时刻节点，并对每个节点处的车辆状态进行预测规划，在优化控制指令以使系统尽可能接近预期轨迹。
MPC控制器的一般流程如下：
利用定位模块与地图信息确定当前行驶路线后,将其分割为一系列坐标点序列,并采用三阶多项式进行拟合;
基于当前位置信息及初始阶段拟合得到的多项式系数参数,推算出未来时间段内车辆的动力学行为特征;
通过比较实时状态与预测值之间的偏差来优化转向角与加速度设置;
如此往复循环操作。
其中第三步是MPC控制器的核心。
在操作过程中需要注意的一点是，在调整航向角和加速度的同时要考虑乘客的感受。因此需要确保加速度和航向角的变化尽可能平稳，并在下文中详细介绍每一步的操作原理并附上相应的代码实现。
拟合多项式
理解多项式的拟合过程并不困难。它相当于将高速公路的路线形状用一个三阶多项式来表示，并最终计算得到的各阶系数值。
// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
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;
}预测未来时间的行驶状态
基于上一步计算得到的多项式系数值，则可以推导出未来一段时间内车辆的位置坐标。注意，在实际应用中所使用的地图提供的路线信息采用的是全局坐标系；然而，在控制和调整车辆运动状态时应当以自身为中心参考系；因此需要执行相应的坐标系转换操作。
// Evaluate a polynomial.
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;
}
Eigen::VectorXd roadside_x(ptsx.size());
Eigen::VectorXd roadside_y(ptsx.size());
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"];
double temp_x;
double temp_y;
// transform Descartes coordinates car space
for (size_t i = 0; i < ptsx.size(); i++) {
temp_x = ptsx[i] - px;
temp_y = ptsy[i] - py;
roadside_y[i] = (temp_y * cos(psi) - temp_x * sin(psi));
roadside_x[i] = (temp_y * sin(psi) + temp_x * cos(psi));
}
auto coeffs = polyfit(roadside_x, roadside_y, 3);多项式能够预测车辆在未来特定时间段上的精确坐标。然而，在实现这些定位的过程中，则是控制器的核心职责。
输出航向角和加速度
MPC控制器的输出计算过程可被视为一个工程最优化问题，在此过程中需要设定目标函数并施加约束条件，在满足所有约束条件的情况下寻求使目标函数值最小的各控制变量最优解。其包含位置偏差CTE、航向偏差EPsi以及速度偏差等变量，在确保乘客乘坐舒适性考虑下，限制加速度及转向角速率的变化幅度也不宜过高。如图所示为系统的动态模型架构图及优化框架示意图，在此框架下利用动态模型预测未来N个时间段的状态信息，并在此基础上设定目标函数及约束条件即可将系统转化为一个工程优化问题求解任务。最终求解得到的是ψ和a两个最优控制量值即通过调节方向盘和加速/制动力度来实现对车辆按规划轨迹的有效控制
本章调用库函数 cppad::ipopt::solve（见附录A.1）对工程最优化问题进行求解，在该实现中，默认参数设置可满足大多数常见问题需求；其中目标函数值由变量 f[0] 代表，在后续计算过程中将通过初始猜测值逐步确定；其余索引 f[1]至f[6N+1] 则对应于一系列约束条件方程组的构建与求解过程
class FG_eval {
public:
// Fitted polynomial coefficients
Eigen::VectorXd coeffs;
FG_eval(Eigen::VectorXd coeffs) { this->coeffs = coeffs; }
typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
void operator()(ADvector& fg, const ADvector& vars) {
// TODO: implement MPC
// `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
// NOTE: You'll probably go back and forth between this function and
// the Solver function below.
fg[0] = 0;
// Cost function
// TODO: Define the cost related the reference state and
// any anything you think may be beneficial.
// The part of the cost based on the reference state.
for (int t = 0; t < N; t++) {
fg[0] += 10*CppAD::pow(vars[cte_start + t], 2);
fg[0] += 100*CppAD::pow(vars[epsi_start + t], 2);
fg[0] += CppAD::pow(vars[v_start + t] - ref_v, 2);
}
// Minimize the use of actuators.
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);
//fg[0] += 150*CppAD::pow(vars[delta_start + t] * vars[v_start+t],2);
}
// Minimize the value gap between sequential actuations.
for (int t = 0; t < N - 2; t++) {
fg[0] += 10000*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[1 + px_start] = vars[px_start];
fg[1 + py_start] = vars[py_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];
for(int t = 1; t < N ; t++){
// psi, v, delta at time t
AD<double> psi0 = vars[psi_start + t - 1];
AD<double> v0 = vars[v_start + t - 1];
AD<double> delta0 = vars[delta_start + t - 1];
// psi at time t+1
AD<double> psi1 = vars[psi_start + t];
// how psi changes
fg[1 + psi_start + t] = psi1 - (psi0 - v0 * delta0 / Lf * dt);
}
for (int t = 1; t < N; t++) {
// The state at time t+1 .
AD<double> x1 = vars[px_start + t];
AD<double> y1 = vars[py_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];
// The state at time t.
AD<double> x0 = vars[px_start + t - 1];
AD<double> y0 = vars[py_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];
// Only consider the actuation at time t.
AD<double> delta0 = vars[delta_start + t - 1];
AD<double> a0 = vars[a_start + t - 1];
if (t>1) {
a0 = vars[a_start + t - 2];
delta0 = vars[delta_start + t - 2];
}
AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2]* CppAD::pow(x0,2)+coeffs[3] * CppAD::pow(x0,3);
AD<double> psides0 = CppAD::atan(coeffs[1]+ 2 * coeffs[2] * x0 + 3 * coeffs [3] * CppAD::pow(x0,2));
// Here's `x` to get you started.
// The idea here is to constraint this value to be 0.
//
// Recall the equations for the model:
// x_[t] = x[t-1] + v[t-1] * cos(psi[t-1]) * dt
// y_[t] = y[t-1] + v[t-1] * sin(psi[t-1]) * dt
// psi_[t] = psi[t-1] + v[t-1] / Lf * delta[t-1] * dt
// v_[t] = v[t-1] + a[t-1] * dt
// cte[t] = f(x[t-1]) - y[t-1] + v[t-1] * sin(epsi[t-1]) * dt
// epsi[t] = psi[t] - psides[t-1] + v[t-1] * delta[t-1] / Lf * dt
fg[1 + px_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
fg[1 + py_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);
}
}
};
CppAD::vector<double> MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) {
bool ok = true;
size_t i;
typedef CPPAD_TESTVECTOR(double) Dvector;
// Set the number of model variables (includes both states and inputs).
// For example: If the state is a 4 element vector, the actuators is a 2
// element vector and there are 10 timesteps. The number of variables is:
//
// 4 * 10 + 2
size_t n_vars = 6 * N + (N - 1) * 2;
// Set the number of constraints
size_t n_constraints = 6 * N ;
// Initial value of the independent variables.
// SHOULD BE 0 besides initial state.
Dvector vars(n_vars);
for (i = 0; i < n_vars; i++) {
vars[i] = 0;
}
vars[px_start] = state[0];
vars[py_start] = state[1];
vars[psi_start] = state[2];
vars[v_start] = state[3];
vars[cte_start] = state[4];
vars[epsi_start] = state[5];
Dvector vars_lowerbound(n_vars);
Dvector vars_upperbound(n_vars);
// Set lower and upper limits for variables.
for (i = 0; i < delta_start; i++) {
vars_lowerbound[i] = -1.0e10;
vars_upperbound[i] = 1.0e10;
}
for (i = delta_start; i < a_start; i++) {
vars_lowerbound[i] = -0.436;
vars_upperbound[i] = 0.436;
}
for (i = a_start; i < n_vars; i++) {
vars_lowerbound[i] = -1.0;
vars_upperbound[i] = 1.0;
}
Dvector constraints_lowerbound(n_constraints);
Dvector constraints_upperbound(n_constraints);
for (i = 0; i < n_constraints; i++) {
constraints_lowerbound[i] = 0;
constraints_upperbound[i] = 0;
}
constraints_lowerbound[px_start] = state[0];
constraints_upperbound[px_start] = state[0];
constraints_lowerbound[py_start] = state[1];
constraints_upperbound[py_start] = state[1];
constraints_lowerbound[psi_start] = state[2];
constraints_upperbound[psi_start] = state[2];
constraints_lowerbound[v_start] = state[3];
constraints_upperbound[v_start] = state[3];
constraints_lowerbound[cte_start] = state[4];
constraints_upperbound[cte_start] = state[4];
constraints_lowerbound[epsi_start] = state[5];
constraints_upperbound[epsi_start] = state[5];
// Lower and upper limits for the constraints
// Should be 0 besides initial state.
// object that computes objective and constraints
FG_eval fg_eval(coeffs);
//
// NOTE: You don't have to worry about these options
//
// options for IPOPT solver
std::string options;
// Uncomment this if you'd like more print information
options += "Integer print_level 0\n";
// NOTE: Setting sparse to true allows the solver to take advantage
// of sparse routines, this makes the computation MUCH FASTER. If you
// can uncomment 1 of these and see if it makes a difference or not but
// if you uncomment both the computation time should go up in orders of
// magnitude.
options += "Sparse true forward\n";
options += "Sparse true reverse\n";
// NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
// Change this as you see fit.
options += "Numeric max_cpu_time 0.5\n";
// place to return solution
CppAD::ipopt::solve_result<Dvector> solution;
#ifdef DEBUG
// solve the proble
std::cout<<"vars "<<vars<<std::endl;
std::cout<<"vars_low"<<vars_lowerbound<<std::endl;
std::cout<<"vars_upper"<<vars_upperbound<<std::endl;
std::cout<<"cons_lower"<<constraints_lowerbound<<std::endl;
std::cout<<"cons_upp"<<constraints_upperbound<<std::endl;
#endif
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
ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
// Cost
double cost = solution.obj_value;
std::cout << "Cost " << cost << std::endl;
return solution.x;
}完整代码请戳这里
全部评论 (0)
还没有任何评论哟~