沈阳网站关键词优化,网站建设便宜公司,做一个天猫店铺要多少钱,wordpress微博图床文章目录 Apollo MPC控制器的设计架构误差模型和离散化预测模型推导目标函数和约束设计优化求解优化OSQP求解器参考文献 Apollo MPC控制器的设计架构 误差模型和离散化 状态变量和控制变量 1、Apollo MPC控制器中状态变量主要有如下6个 matrix_state_ Matrix::Zero(basic_stat… 文章目录 Apollo MPC控制器的设计架构误差模型和离散化预测模型推导目标函数和约束设计优化求解优化OSQP求解器参考文献 Apollo MPC控制器的设计架构 误差模型和离散化 状态变量和控制变量 1、Apollo MPC控制器中状态变量主要有如下6个 matrix_state_ Matrix::Zero(basic_state_size_, 1);// State matrix update;matrix_state_(0, 0) debug-lateral_error();matrix_state_(1, 0) debug-lateral_error_rate();matrix_state_(2, 0) debug-heading_error();matrix_state_(3, 0) debug-heading_error_rate();matrix_state_(4, 0) debug-station_error();matrix_state_(5, 0) debug-speed_error(); 状态变量计算 控制变量有两个 其中 为前轮转角为加速度补偿 Eigen::MatrixXd control_matrix(controls_, 1);control_matrix 0, 0;在代码中控制量的计算和输出 // 解mpc输出一个vectorcontrol内有10个control_matrix
SolveLinearMPC(matrix_ad_, matrix_bd_, matrix_cd_, matrix_q_updated_,matrix_r_updated_, lower_bound, upper_bound, matrix_state_, reference,mpc_eps_, mpc_max_iteration_, control)
// 已知mpc仅取第一组解为当前时刻最优控制解即control[0]
// steer_single_direction_max_degree_ --- the maximum turn of steer
// control中第一个矩阵中的第一个值用于计算方向盘转角
double steer_angle_feedback control[0](0, 0) * 180 / M_PI *steer_transmission_ratio_ /steer_single_direction_max_degree_ * 100;double steer_angle steer_angle_feedback steer_angle_feedforwardterm_updated_ steer_angle_ff_compensation steer_angle_feedback_augment;// control中第一个矩阵中的第二个值control[0](1, 0)用于计算加速度 加速度补偿 期望加速度 坡度加速度补偿
debug-set_acceleration_cmd_closeloop(control[0](1, 0));
double acceleration_cmd control[0](1, 0) debug-acceleration_reference() control_conf_.enable_slope_offset() * debug-slope_offset_compensation();动力学误差模型 代码实现 // 初始化矩阵
Status MPCController::Init(const ControlConf *control_conf) {...// Matrix init operations.matrix_a_ Matrix::Zero(basic_state_size_, basic_state_size_);matrix_ad_ Matrix::Zero(basic_state_size_, basic_state_size_);...// TODO(QiL): expand the model to accomendate more combined states.matrix_a_coeff_ Matrix::Zero(basic_state_size_, basic_state_size_);...matrix_b_ Matrix::Zero(basic_state_size_, controls_);matrix_bd_ Matrix::Zero(basic_state_size_, controls_);...matrix_bd_ matrix_b_ * ts_;matrix_c_ Matrix::Zero(basic_state_size_, 1);...matrix_cd_ Matrix::Zero(basic_state_size_, 1);...}// 更新矩阵
void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {const double v VehicleStateProvider::instance()-linear_velocity();matrix_a_(1, 1) matrix_a_coeff_(1, 1) / v;matrix_a_(1, 3) matrix_a_coeff_(1, 3) / v;matrix_a_(3, 1) matrix_a_coeff_(3, 1) / v;matrix_a_(3, 3) matrix_a_coeff_(3, 3) / v;Matrix matrix_i Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());matrix_ad_ (matrix_i ts_ * 0.5 * matrix_a_) *(matrix_i - ts_ * 0.5 * matrix_a_).inverse();matrix_c_(1, 0) (lr_ * cr_ - lf_ * cf_) / mass_ / v - v;matrix_c_(3, 0) -(lf_ * lf_ * cf_ lr_ * lr_ * cr_) / iz_ / v;matrix_cd_ matrix_c_ * debug-heading_error_rate() * ts_;
}// 求解MPC
// discrete linear predictive control solver, with control format
// x(i 1) A * x(i) B * u (i) C
bool SolveLinearMPC(const Matrix matrix_a, const Matrix matrix_b,const Matrix matrix_c, const Matrix matrix_q,const Matrix matrix_r, const Matrix matrix_lower,const Matrix matrix_upper,const Matrix matrix_initial_state,const std::vectorMatrix reference, const double eps,const int max_iter, std::vectorMatrix *control) apollo使用的是双线性变换离散法的方法 matrix_ad_ (matrix_i ts_ * 0.5 * matrix_a_) *(matrix_i - ts_ * 0.5 * matrix_a_).inverse();matrix_bd_ matrix_b_ * ts_;matrix_cd_ matrix_c_ * debug-heading_error_rate() * ts_; 补充 Apollo采用的中点欧拉法和向前欧拉法的结合 预测模型推导 为什么不用上述推导出的离散化的状态空间方程来推导我们的预测模型以此来设计MPC控制器 主要考虑在设计目标函数的时候想将我们的控制增量设计到我们的目标函数中去而非直接使用控制量以控制增量为代价能有效改善控制的平滑性。 此处我们需要注意的是 控制步长为Nc,因此当预测步长为kNc1时此时预测状态y(kNc1)的最后一项也只写到kNc-1。因为到kNc时刻及以后的控制量有两种处理策略第一种就是控制步长Nc之后的预测控制量保持不变第二种就是Nc之后的预测控制量全部置零。我们此处达到控制步长之后的控制量全置零最终推导出的系统的预测模型Y, 其可以根据系统当前的状态量以及施加一个未来一段时间Nc序列的控制增量我们就可以知道系统未来(Np步)的表现是什么样子的系统输出即y(k1)~y(kNp) 目标函数和约束设计 设计目标 希望我们的路径跟踪误差在预测时域内尽可能趋于0也即希望目标函数的第一项的几个状态量尽可能趋于0这样我们的车辆才能更好的跟随规划路径其次我们希望目标函数的第二项控制增量的代价越小越好也即希望前后两帧的控制量变化越小越好这样控制的效果越平滑对应前轮转角和加速度变化越平滑目标函数最后一项引入松弛因子(在程序中是一个具体的数值)其主要作用就是改善优化问题的可行解提高求解效率 在MPC控制规律中将控制序列中的第一个参数作为控制量输入给被控系统。 对于车辆控制来说就是找到一组合适的控制量如, 其中 为前轮转角为刹车/油门系数使车辆沿参考轨迹行驶且误差最小、控制最平稳、舒适度最高等。因此在设计目标函数的时候可以考虑以下几点 1、设计代价函数时候一般设计为二次型的样式为的是避免在预测时域内误差忽正忽负导致误差相互抵消 2、考虑的代价主要有如下 横向误差指实际轨迹点与参考轨迹点间的距离航向误差车辆当前航向与参考轨迹点航向的偏差速度误差车辆当前速度与规划轨期望车速的偏差刹车/油门调节量目的是为了保证刹车/油门变化的平稳性 3、约束条件: 最大前轮转角最大前轮转角变化量最大刹车/油门调节量最大方向盘转角最大车速最大加速度等 此处关于控制增量的约束比如说我们的前轮转角打30°需要2s,则1s需要打15°然而我们的控制周期T0.05s则控制周期我们需要下发的转角最大为0.75°也即我们下发转角的最大速率不超过0.75°/T。 关于软约束也即代价函数使其值越小越好。 优化求解 滚动优化求解的目的是为了求最优控制解是一种在线优化它每一时刻都会针对当前误差重新计算控制量通过使某一或某些性能指标达到最优来实现控制作用。因此滚动优化可能不会得到全局最优解但是却能对每一时刻的状态进行最及时的响应达到局部最优。 第一步目标函数如何转化为标准二次型 注意 目标函数化简后的每一项其实都是一个具体的数第三项表示具体数的转置等于其自身。因此合并第二项和第三项二者实质相等。其次化成二次型后G属于已知项且Q也是已知项故二次型的最后一项其实不影响最终的优化求解最小值问题因此放在最后一项且求解过程可以忽略。 第二步约束的转化 将作为此时的控制量输入给系统直到下一个控制时刻系统根据新的状态信息预测下一时段内的输出然后通过优化得到一组新的控制序列。如此反复直至完成整个控制过程。 OSQP求解器 带参构造函数MpcOsqp 求解 输入矩阵Ad,Bd,Q,R,初始状态阵X0,控制量上下边界状态量上下边界参考状态(0矩阵)最大迭代次数mpc_max_iteration_预测时域周期数horizon_求解精度mpc_eps_用输入参数去初始化类对象的数据成员 MpcOsqp::MpcOsqp(const Eigen::MatrixXd matrix_a,const Eigen::MatrixXd matrix_b,const Eigen::MatrixXd matrix_q,const Eigen::MatrixXd matrix_r,const Eigen::MatrixXd matrix_initial_x,const Eigen::MatrixXd matrix_u_lower,const Eigen::MatrixXd matrix_u_upper,const Eigen::MatrixXd matrix_x_lower,const Eigen::MatrixXd matrix_x_upper,const Eigen::MatrixXd matrix_x_ref, const int max_iter,const int horizon, const double eps_abs): matrix_a_(matrix_a),matrix_b_(matrix_b),matrix_q_(matrix_q),matrix_r_(matrix_r),matrix_initial_x_(matrix_initial_x),matrix_u_lower_(matrix_u_lower),matrix_u_upper_(matrix_u_upper),matrix_x_lower_(matrix_x_lower),matrix_x_upper_(matrix_x_upper),matrix_x_ref_(matrix_x_ref),max_iteration_(max_iter),horizon_(horizon),eps_abs_(eps_abs) {state_dim_ matrix_b.rows();control_dim_ matrix_b.cols();ADEBUG state_dim state_dim_;ADEBUG control_dim_ control_dim_;num_param_ state_dim_ * (horizon_ 1) control_dim_ * horizon_;
} OSQP求解结果输出 std::vectordouble control_cmd(controls_, 0);apollo::common::math::MpcOsqp mpc_osqp(matrix_ad_, matrix_bd_, matrix_q_updated_, matrix_r_updated_,matrix_state_, lower_bound, upper_bound, lower_state_bound,upper_state_bound, reference_state, mpc_max_iteration_, horizon_,mpc_eps_);
//取控制序列的第一个作为输出的控制量if (!mpc_osqp.Solve(control_cmd)) {AERROR MPC OSQP solver failed;} else {ADEBUG MPC OSQP problem solved! ;control[0](0, 0) control_cmd.at(0);control[0](1, 0) control_cmd.at(1);}//第一个元素前轮反馈转角steer_angle_feedback Wheel2SteerPct(control[0](0, 0));//第二个元素加速度补偿acc_feedback control[0](1, 0); 参考文献 本文针对运动学MPC控制器进行回顾总结详细可以参考下述文献。 1、【基础】自动驾驶控制算法第五讲 连续方程的离散化与离散LQR原理_哔哩哔哩_bilibili 2、基于MPC轨迹跟踪理论篇上_哔哩哔哩_bilibili 3、Apollo代码学习(六)—模型预测控制(MPC)_apollo mpc-CSDN博客
