主动驾驶控制与规划——Project 4: MPC车辆控制
零、使命介绍补全carla-ros-bridge/src/ros-bridge/carla_shenlan_projects/carla_shenlan_mpc_controller/src/mpc_controller.cpp中动力学模型和代价函数构造部分，实现车辆MPC控制。
一、情况设置
使用如下命令安装CppAD
sudo apt install cppad
ipopt的安装参考Linux | Ubuntu 20.04安装ipopt和cppAD | 安装全流程+报错解决，使用方法二源码安装即可。
CppAD库的使用可以参考CppAD优化问题求解
二、算法原理
本次使用的线性化模型与上一篇博客主动驾驶控制与规划——Project 3: LQR车辆横向控制的相同。使用欧拉前向法离散化，有如下的等式束缚：
x ( k + 1 ) = x ( k ) + v x ( k ) cos ψ ( k ) Δ t − v y ( k ) sin ψ ( k ) Δ t y ( k + 1 ) = y ( k ) + v y ( k ) sin ψ ( k ) Δ t + v y ( k ) cos ψ ( k ) Δ t ψ ( k + 1 ) = ψ ( k ) + r ( k ) Δ t v x ( k + 1 ) = v x ( k ) + a x ( k ) Δ t a y ( k ) = − c f + c r m v x ( k ) v y ( k ) + ( l r c r − l f c f m v x ( k ) − v x ( k ) ) r ( k ) v y ( k + 1 ) = v y ( k ) + a y ( k ) Δ t α ( k ) = l r c r − l f c f I z v x ( k ) v y ( k ) − l f 2 c f + l r 2 c r I z v x ( k ) r ( k ) r ( k + 1 ) = r ( k ) + α ( k ) Δ t \begin{aligned} x(k+1) &= x(k) + v_x(k)\cos \psi(k) \Delta t - v_y(k) \sin \psi(k)\Delta t\\ y(k+1) &= y(k) + v_y(k)\sin\psi(k)\Delta t + v_y(k)\cos\psi(k)\Delta t \\ \psi(k+1) &= \psi(k)+r(k)\Delta t \\ v_x(k+1) &= v_x(k) + a_x(k)\Delta t \\ a_y(k) &= -\frac{c_f + c_r}{mv_x(k)} v_y(k) + \left(\frac{l_r c_r - l_f c_f}{m v_x(k)} - v_x(k)\right) r(k) \\ v_y(k+1) &= v_y(k) + a_y(k) \Delta t \\ \alpha(k) &= \frac{l_r c_r - l_f c_f}{I_z v_x(k)} v_y(k) - \frac{l_f^2c_f + l_r^2c_r}{I_z v_x(k)} r(k) \\ r(k+1)&= r(k) + \alpha(k)\Delta t\\ \end{aligned} x(k+1)y(k+1)ψ(k+1)vx(k+1)ay(k)vy(k+1)α(k)r(k+1)=x(k)+vx(k)cosψ(k)Δt−vy(k)sinψ(k)Δt=y(k)+vy(k)sinψ(k)Δt+vy(k)cosψ(k)Δt=ψ(k)+r(k)Δt=vx(k)+ax(k)Δt=−mvx(k)cf+crvy(k)+(mvx(k)lrcr−lfcf−vx(k))r(k)=vy(k)+ay(k)Δt=Izvx(k)lrcr−lfcfvy(k)−Izvx(k)lf2cf+lr2crr(k)=r(k)+α(k)Δt
其中 x ( k ) x(k) x(k)是 k k k时刻车辆在世界系下的 x x x坐标； y ( k ) y(k) y(k)是 k k k时刻车辆在世界系下的 y y y坐标； v x v_x vx是车辆纵向速率； v y v_y vy是车辆横向速率； ψ \psi ψ是偏航角； r r r是偏航角速率； a y a_y ay是车辆横向加速率； α \alpha α是偏航角加速率； Δ t \Delta t Δt是猜测的时间步长。其他车辆参数寄义可以参考上一期博客主动驾驶控制与规划——Project 3: LQR车辆横向控制。
除上述等式束缚以外，为了使求解速率加快，增长如下的束缚条件令
思量三个方面的代价函数：
三、代码实现
构造函数，初始化状态、目标状态、权重等。
FG_eval::FG_eval(const Eigen::VectorXd &state, VectorXd coeffs, const double &target_v, const int &cte_weight, const int &epsi_weight, const int &v_weight, const int &steer_actuator_cost_weight, const int &acc_actuator_cost_weight, const int &change_steer_cost_weight,
const int &change_accel_cost_weight, const int &mpc_prediction_horizon_length, const int &mpc_control_horizon_length, const double &mpc_control_step_length, const double &kinamatic_para_Lf, const double &a_lateral, const double &old_steer_value, const double &old_throttle_value,
const double &steer_ratio) {
this->steer_ratio = steer_ratio;
this->coeffs = coeffs;
this->ref_v = target_v;
this->cte_weight = cte_weight;
this->epsi_weight = epsi_weight;
this->v_weight = v_weight;
this->steer_actuator_cost_weight_fg = steer_actuator_cost_weight;
this->acc_actuator_cost_weight_fg = acc_actuator_cost_weight;
this->change_steer_cost_weight = change_steer_cost_weight;
this->change_accel_cost_weight = change_accel_cost_weight;
this->Np = mpc_prediction_horizon_length;
this->Nc = mpc_control_horizon_length;
this->dt = mpc_control_step_length;
this->Lf = kinamatic_para_Lf;
this->a_lateral = a_lateral;
/*Indexes on the 1-D vector for readability， These indexes are used for "vars"*/
this->x_start = 0;
this->y_start = x_start + Np;
this->psi_start = y_start + Np;
this->v_longitudinal_start = psi_start + Np;
this->v_lateral_start = v_longitudinal_start + Np;
this->yaw_rate_start = v_lateral_start + Np;
this->cte_start = yaw_rate_start + Np;
this->epsi_start = cte_start + Np;
this->front_wheel_angle_start = epsi_start + Np;
this->longitudinal_acceleration_start = front_wheel_angle_start + Nc;
//控制时域长度为25的时候，控制增量一共有24个,第一个时刻点控制量与控制增量相同
// this->front_wheel_angle_increment_start = epsi_start + Np;
// this->longitudinal_acceleration_increment_start = front_wheel_angle_increment_start + Nc - 1;
this->old_steer_value = old_steer_value;
this->old_throttle_value = old_throttle_value;
/* Explicitly gather state values for readability */
this->x = state;
this->y = state;
this->psi = state;
this->v_longitudinal = state;
this->v_lateral = state;
this->yaw_rate = state;
this->cte = state;
this->epsi = state;
}
FG_eval::operator()，定义MPC的目标函数和束缚条件
void FG_eval::operator()(ADvector &fg, ADvector &vars) {
// 部分代码已经给出，请同学们补全
fg = 0; // 0 is the index at which Ipopt expects fg to store the cost value,
/* TODO: Objective term 1: Keep close to reference values.*/
for (size_t t = 0; t < Np; t++) {
fg += cte_weight * CppAD::pow(vars - ref_cte, 2);
fg += epsi_weight * CppAD::pow(vars - ref_epsi, 2);
// AD<double> speed = CppAD::sqrt(vars * vars + vars * vars);
// std::cout << "ref_v: " << ref_v << std::endl;
fg += v_weight * CppAD::pow(vars - ref_v, 2);
}
/* TODO: Objective term 2: Avoid to actuate as much as possible, minimize the use of actuators.*/
for (size_t t = 0; t < Nc; t++) {
fg += steer_actuator_cost_weight_fg * CppAD::pow(vars, 2);
fg += acc_actuator_cost_weight_fg * CppAD::pow(vars, 2);
}
/* TODO: Objective term 3: Enforce actuators smoothness in change, minimize the value gap between sequential actuation.*/
for (size_t t = 0; t < Nc - 1; t++) {
fg += change_steer_cost_weight * CppAD::pow(vars - vars, 2);
fg += change_accel_cost_weight * CppAD::pow(vars - vars, 2);
}
/* Initial constraints, initialize the model to the initial state*/
fg = vars;
fg = vars;
fg = vars;
fg = vars;
fg = vars;
fg = vars;
fg = vars;
fg = vars;
for (size_t t = 1; t < Np; t++) // 预测模型
{
// Values at time (t),第一次进入该循环的时候,x_0...传入的是给MPC的系统状态的真实值,作为MPC求解的初始条件.
AD<double> x_0 = vars;
AD<double> y_0 = vars;
AD<double> psi_0 = vars;
AD<double> v_longitudinal_0 = vars + 0.00001;
AD<double> v_lateral_0 = vars;
AD<double> yaw_rate_0 = vars;
AD<double> cte_0 = vars;
AD<double> epsi_0 = vars;
// Values at time (t+1)
AD<double> x_1 = vars;
AD<double> y_1 = vars;
AD<double> psi_1 = vars;
AD<double> v_longitudinal_1 = vars + 0.00001;
AD<double> v_lateral_1 = vars;
AD<double> yaw_rate_1 = vars;
AD<double> cte_1 = vars;
AD<double> epsi_1 = vars;
// Only consider the actuation at time t.
AD<double> front_wheel_angle_0;
AD<double> longitudinal_acceleration_0;
front_wheel_angle_0 = vars;
longitudinal_acceleration_0 = vars;
// reference path
AD<double> f_0 = coeffs + coeffs * x_0 + coeffs * CppAD::pow(x_0, 2) + coeffs * CppAD::pow(x_0, 3) + coeffs * CppAD::pow(x_0, 4) + coeffs * CppAD::pow(x_0, 5); // + coeffs * CppAD::pow(x_0, 6);
// reference psi: can be calculated as the tangential angle of the polynomial f evaluated at x_0
AD<double> psi_des_0 = CppAD::atan(1 * coeffs + 2 * coeffs * x_0 + 3 * coeffs * CppAD::pow(x_0, 2) + 4 * coeffs * CppAD::pow(x_0, 3) + 5 * coeffs * CppAD::pow(x_0, 4)); // + 6 * coeffs * CppAD::pow(x_0, 5));
// TODO： 补全车辆动力学模型，作为MPC的等式约束条件，车辆动力学模型需要注意参数的正方向的定义
/*The idea here is to constraint this value to be 0.*/
/* 全局坐标系 */
fg = x_1 - (x_0 + (v_longitudinal_0 * CppAD::cos(psi_0) - v_lateral_0 * CppAD::sin(psi_0)) * dt);
fg = y_1 - (y_0 + (v_longitudinal_0 * CppAD::sin(psi_0) + v_lateral_0 * CppAD::cos(psi_0)) * dt);
/* 航向角变化 */
fg = psi_1 - (psi_0 + yaw_rate_0 * dt);
/* 车辆纵向速度 */
fg = v_longitudinal_1 - (v_longitudinal_0 + longitudinal_acceleration_0 * dt);
/* 车辆侧向速度 */
// 注意这里front_wheel_angle_0符号和第四章推导中定义的delta方向相反
AD<double> a_lateral = -2 * (Cf + Cr) / (m * v_longitudinal_0) * v_lateral_0 + (2 * (lr * Cr - lf * Cf) / (m * v_longitudinal_0) - v_longitudinal_0) * yaw_rate_0 - 2 * Cf / m * front_wheel_angle_0;
fg = v_lateral_1 - (v_lateral_0 + a_lateral * dt);
/* 车辆横摆角速度 */
AD<double> yaw_acceleration = 2 * (lr * Cr - lf * Cf) / (I * v_longitudinal_0) * v_lateral_0 - 2 * (lf * lf * Cf + lr * lr * Cr) / (I * v_longitudinal_0) * psi_0 - 2 * lf * Cf / I * front_wheel_angle_0;
fg = yaw_rate_1 - (yaw_rate_0 + yaw_acceleration * dt);
/* 横向位置跟踪误差 */
fg = cte_1 - (f_0 - y_0 + v_longitudinal_0 * CppAD::tan(epsi_0) * dt);
/* 航向跟踪误差 */
fg = epsi_1 - (psi_0 - psi_des_0 - v_longitudinal_0 * (front_wheel_angle_0 / 1) / Lf * dt);
}
}
mpc_controller::Solve求解MPC优化问题，返回最优控制量以及猜测的状态轨迹。
std::vector<double> mpc_controller::Solve(const Eigen::VectorXd &state, const Eigen::VectorXd &coeffs, const double &target_v, const int &cte_weight, const int &epsi_weight, const int &v_weight, const int &steer_actuator_cost_weight, const int &acc_actuator_cost_weight,
const int &change_steer_cost_weight, const int &change_accel_cost_weight,
// const int &mpc_prediction_horizon_length,
const int &mpc_control_horizon_length, const double &mpc_control_step_length, const double &kinamatic_para_Lf, const double &a_lateral, const double &old_steer_value, const double &old_throttle_value, const double &steer_ratio) {
this->Nc = mpc_control_horizon_length;
this->Np = Nc + 1;
this->x_start = 0;
this->y_start = x_start + Np;
this->psi_start = y_start + Np;
this->v_longitudinal_start = psi_start + Np;
this->v_lateral_start = v_longitudinal_start + Np;
this->yaw_rate_start = v_lateral_start + Np;
this->cte_start = yaw_rate_start + Np;
this->epsi_start = cte_start + Np;
this->front_wheel_angle_start = epsi_start + Np;
this->longitudinal_acceleration_start = front_wheel_angle_start + Nc; //控制时域长度为25的时候，控制量一共有24个。
// this->front_wheel_angle_increment_start = epsi_start + Np;
// this->longitudinal_acceleration_increment_start = front_wheel_angle_increment_start + Nc - 1;
this->a_lateral = a_lateral;
bool ok = true;
typedef CPPAD_TESTVECTOR(double) Dvector;
/* Explicitly gather state values for readability */
double x = state;
double y = state;
double psi = state;
double v_longitudinal = state;
double v_lateral = state;
double yaw_rate = state;
double cte = state;
double epsi = state;
/* Set the number of model variables (includes both states and inputs). */
/* 系统状态量 + 系统控制增量 */
size_t n_vars = Np * 8 + Nc * 2;
/* Set the number of contraints */
size_t n_constraints = Np * 8;
/* Initialize all of independent variables to zero. */
Dvector vars(n_vars);
for (size_t i = 0; i < n_vars; i++) {
vars = 0.0;
}
// Set the initial variable values**初始化变量及变量上下限**
vars = x;
vars = y;
vars = psi;
vars = v_longitudinal;
vars = v_lateral;
vars = yaw_rate;
vars = cte;
vars = epsi;
Dvector vars_lower_bounds(n_vars);
Dvector vars_upper_bounds(n_vars);
/* Set limits for non-actuators (avoid numerical issues during optimization). */
for (size_t i = 0; i < front_wheel_angle_start; i++) {
vars_lower_bounds = -std::numeric_limits<float>::max();
vars_upper_bounds = +std::numeric_limits<float>::max();
}
/* Set upper and lower constraints for steering. */
double max_degree = 24;
double max_radians = max_degree * M_PI / 180;
for (size_t i = front_wheel_angle_start; i < longitudinal_acceleration_start; i++) {
vars_lower_bounds = -max_radians;
vars_upper_bounds = +max_radians;
}
/* Set uppper and lower constraints for acceleration. */
double max_acceleration_value = 1.0;
for (size_t i = longitudinal_acceleration_start; i < n_vars; i++) {
vars_lower_bounds = -max_acceleration_value;
vars_upper_bounds = +max_acceleration_value;
}
/* Initialize to zero lower and upper limits for the constraints*/
Dvector constraints_lower_bounds(n_constraints);
Dvector constraints_upper_bounds(n_constraints);
for (size_t i = 0; i < n_constraints; i++) {
constraints_lower_bounds = 0;
constraints_upper_bounds = 0;
}
/* Force the solver to start from current state in optimization space. */
/* 约束条件的形式为 [x(0); x(1) = f(x(0)); ... x(N) = f(x(N-1)); , 每一个约束的第一项其实就是系统当前状态，所以直接约束该量的第一项。
不能直接给状态 vars 的第一项赋值来限制求解起点，因为状态和控制量理论上都是自由的，所以这里通过设置约束条件第一项的方式来指定优化求解起点，从而加速求解。*/
constraints_lower_bounds = x;
constraints_lower_bounds = y;
constraints_lower_bounds = psi;
constraints_lower_bounds = v_longitudinal;
constraints_lower_bounds = v_lateral;
constraints_lower_bounds = yaw_rate;
constraints_lower_bounds = cte;
constraints_lower_bounds = epsi;
constraints_upper_bounds = x;
constraints_upper_bounds = y;
constraints_upper_bounds = psi;
constraints_upper_bounds = v_longitudinal;
constraints_upper_bounds = v_lateral;
constraints_upper_bounds = yaw_rate;
constraints_upper_bounds = cte;
constraints_upper_bounds = epsi;
/* Object that computes objective and constraints. */
FG_eval fg_eval(state, coeffs, target_v, cte_weight, epsi_weight, v_weight, steer_actuator_cost_weight, acc_actuator_cost_weight, change_steer_cost_weight, change_accel_cost_weight, Np, Nc, mpc_control_step_length, kinamatic_para_Lf, a_lateral, old_steer_value, old_throttle_value, steer_ratio);
/* 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_level0\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 += "Sparsetrue forward\n";
options += "Sparsetrue 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"; // 单位：s
/* Place to return solution. */
CppAD::ipopt::solve_result<Dvector> solution;
/* Solve the problem. */
// cout << "Solve the problem......" << endl;
CppAD::ipopt::solve<Dvector, FG_eval>(options, vars, vars_lower_bounds, vars_upper_bounds, constraints_lower_bounds, constraints_upper_bounds, fg_eval, solution);
/* Check some of the solution values. */
ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
/* Cost! */
// auto cost = solution.obj_value;
// std::cout << "Cost: " << cost << std::endl;
/* Return the first actuator values. The variables can be accessed with 'solution.x'. */
std::vector<double> result;
result.clear();
// cout << solution.x << endl;
// cout << front_wheel_angle_start << endl;
result.push_back(solution.x);
result.push_back(solution.x);
/* Add 'future' solutions (where MPC is going). */
for (size_t i = 0; i < Np - 1; i++) {
result.push_back(solution.x);
result.push_back(solution.x);
}
// std::cout << "end now" << std::endl;
return result;
}
代价的权重：
# MPC 目标函数里面可以调节的权重
mpc_cte_weight_int:600 # ok
mpc_epsi_weight_int: 1000 # ok
mpc_v_weight_int: 200 # ok real 100
# 控制量幅值代价权重
mpc_steer_actuator_cost_weight_int: 400 # ok
mpc_acc_actuator_cost_weight_int: 100 # ok
# 前后两次控制量变化的代价权重
mpc_change_steer_cost_weight_int: 200 # ok
mpc_change_accel_cost_weight_int: 200 # ok
四、效果展示
实验过程中发如今相同的权重下，给定不同的求解时间，会对MPC的性能产生较大的影响：
options += "Numeric max_cpu_time 0.5\n"; // 单位：s
求解时间为0.5s的效果，计时发现求解一次的时间在50ms左右，但是偶然会到200-300ms，跟踪效果良好，无振荡。
https://i-blog.csdnimg.cn/direct/9997df2cf9e34853bd29a45f56707645.png
求解时间为0.2s的效果，求解时间在50ms左右，最大求解时间被限制在200ms左右。
https://i-blog.csdnimg.cn/direct/ecd2a8b579094b5eb8283a9f38f21778.png
跟踪直线轨迹时出现了轻微振荡。
https://i-blog.csdnimg.cn/direct/531b7e5f3d1548c39d0212c581dd63cf.png
求解时间为0.05s的效果，最大求解时间被限制在50ms-70ms之间。
https://i-blog.csdnimg.cn/direct/c939725dbbda4ad29e92c56c2e3edecc.png
跟踪直线轨迹时出现显着振荡：
https://i-blog.csdnimg.cn/direct/7b516b960cbf4fa7aaab31e78089353f.png
实际工程实现中，需要权衡求解精度与实时性，应当通过实验和工程束缚确定最大求解时间等参数。
主动驾驶控制与规划——Project 4: MPC车辆控制
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。
页:
[1]