Apollo MPC横纵向耦合控制学习笔记
先上参考链接
【运动控制】Apollo6.0的mpc_controller解析
Apollo MPC OSQP Solver
详细的车辆横向动力学模型推导参考我另一篇博客
Apollo control模块横向控制原理及核心代码逐行解析
因为和上述链接里LQR控制的代码及框架类似,因此在此仅代码不赘述,主要介绍原理
MPC横纵向控制原理
一.mpc_controller框架
代码参见apollo/modules/control/controller/mpc_controller.cc
1.1 注册控制器
\modules\control\conf\control_conf.pb.txt里active_controllers: MPC_CONTROLLER
active_controllers: MPC_CONTROLLER
在这里激活不同的控制器。
LQR+PID 激活 LON_CONTROLLER + LAT_CONTROLLER
MPC控制只激活 MPC_CONTROLLER,横纵向耦合控制
1.2 InitializeFilters
初始化3个lpf低通滤波器
digital_filter_:二阶数字滤波器,用于对方向盘角度控制指令滤波
lateral_error_filter_:均值低通滤波器,对横向误差CTE进行滤波,滤波窗口大小mean_filter_window_size在control_conf.pb.txt种被定义为10个周期
heading_error_filter_:均值低通滤波器,对航向误差进行滤波,滤波窗口大小mean_filter_window_size同lateral_error_filter_
1.3 Init
1.3.1 LoadControlConf
LoadControlConf:加载控制配置文件中的参数到MPCLatController类的数据成员中
1.3.2 初始化车辆状态方程矩阵
车辆横向状态方程
X'=AX+Bu+B1*(φdes)'
初始化
A矩阵中常项,含速度项的后续继续处理;
B矩阵全常项,并 x ts进行离散化;
B1初始化为4x1的0矩阵;
X状态矩阵初始化为4x1的0矩阵;
MPC中的Q矩阵初始化4x4的矩阵,从配置文件读取;
MPC中的R矩阵初始化为1x1的单位矩阵,从配置文件读取;
1.3.3 调用InitializeFilters初始化3个低通滤波器
1.3.4 LoadMPCGainScheduler
从控制配置加载增益调度表
总共有4个增益调度表
lat_err_interpolation_:加载控制配置文件中的横向误差随车速的增益表到MPCController类数据成员,车速越大,Q矩阵中横向误差的权重系数就越小
heading_err_interpolation_:加载控制配置文件中的航向误差随车速的增益表到MPCController类数据成员,车速越大,Q矩阵中航向误差的权重系数就越小,低速偏重于横向误差,高速偏重于航向误差。
feedforwardterm_interpolation_:前馈项的增益表
steer_weight_gain_scheduler:控制量的增益表,R矩阵中控制量的惩罚系数 x 这个根据车速插值出来的ratio,车速越高,控制量的惩罚系数越大,ratio越大
1.4 ComputeControlCommand
UpdateState
函数作用:
1.计算横向控制所需的各个误差项,量放入debug指针;
2.更新车辆的状态矩阵matrix_state_,直接从上一步debug指针中取出横向/航项误差/误差率
UpdateMatrix
主要是更新车辆状态方程里的矩阵A,B1中与速度相关的项
matrix_a_
matrix_c_
FeedforwardUpdate
steer_angle_feedforwardterm_
主要是更新车辆状态方程里的前馈项里包含速度相关项
利用gain_scheduler根据当前车速更新
横向误差惩罚系数,航向误差惩罚系数,前馈控制量,MPC控制量惩罚系数
定义各种过程矩阵及滚动时域的矩阵序列
调用MpcOsqp优化求解器
输入各种参数及约束,调用\modules\common\math\mpc_osqp.cc
调用MpcOsqp类的成员函数solve求解,求解的结果放入control控制序列里,取出第一个时刻的控制量下发。
这个控制量先进行转速限幅,滤波,再百分比限幅,最后下发。
1.5 UpdateState
函数作用:
1.调用ComputeLateralErrors计算横向控制所需的各个误差项,量放入debug指针;
2.更新车辆的状态矩阵matrix_state_,直接从上一步debug指针中取出横向/航项误差/误差率
1.6 UpdateMatrix
主要是更新车辆状态方程里的矩阵A,B1中与速度相关的项
matrix_a_
matrix_c_
1.7 FeedforwardUpdate
steer_angle_feedforwardterm_
主要是更新车辆状态方程里的前馈项里包含速度相关项
1.8 ComputeLateralErrors
计算横向误差相关量放入debug指针
二. mpc_osqp求解器
OSQP二次规划优化求解器,求解MPC问题,代码位于apollo/modules/common/math/mpc_osqp.cc里,定义了MpcOsqp类,主要实现了OSQP优化求解MPC问题的功能。
2.1 框架
2.2 带参构造函数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_;
}
state_dim_是状态矩阵的维度,等于矩阵Bd的行数
control_dim_是控制矩阵的维度,等于矩阵Bd的列数
num_param_是OSQP求解器待求解的变量X包含的参数个数
2.3 CalculateKernel
计算二次规划标准形式中的矩阵P
void MpcOsqp::CalculateKernel(std::vector<c_float> *P_data,std::vector<c_int> *P_indices,std::vector<c_int> *P_indptr) {// col1:(row,val),...; col2:(row,val),....; ...std::vector<std::vector<std::pair<c_int, c_float>>> columns;columns.resize(num_param_);size_t value_index = 0;// state and terminal statefor (size_t i = 0; i <= horizon_; ++i) {for (size_t j = 0; j < state_dim_; ++j) {// (row, val)columns[i * state_dim_ + j].emplace_back(i * state_dim_ + j,matrix_q_(j, j));++value_index;}}// controlconst size_t state_total_dim = state_dim_ * (horizon_ + 1);for (size_t i = 0; i < horizon_; ++i) {for (size_t j = 0; j < control_dim_; ++j) {// (row, val)columns[i * control_dim_ + j + state_total_dim].emplace_back(state_total_dim + i * control_dim_ + j, matrix_r_(j, j));++value_index;}}CHECK_EQ(value_index, num_param_);int ind_p = 0;for (size_t i = 0; i < num_param_; ++i) {// TODO(SHU) Check thisP_indptr->emplace_back(ind_p);for (const auto &row_data_pair : columns[i]) {P_data->emplace_back(row_data_pair.second); // valP_indices->emplace_back(row_data_pair.first); // row++ind_p;}}P_indptr->emplace_back(ind_p);
}
这个函数没有完全看明白,不过前两个for循环很好理解,就是将Q,R矩阵塞到QP问题的海森矩阵P里
2.4 CalculateGradient
计算二次规划标准形式中的矩阵q
// reference is always zero
void MpcOsqp::CalculateGradient() {// populate the gradient vectorgradient_ = Eigen::VectorXd::Zero(state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);for (size_t i = 0; i < horizon_ + 1; i++) {gradient_.block(i * state_dim_, 0, state_dim_, 1) =-1.0 * matrix_q_ * matrix_x_ref_;}ADEBUG << "Gradient_mat";ADEBUG << gradient_;
}
不过这里呢,因为Apollo输入的参考状态矩阵matrix_x_ref始终是0其实这个q也始终是0矩阵吧
2.5 CalculateEqualityConstraint
计算二次规划约束中的矩阵Ac
void MpcOsqp::CalculateEqualityConstraint(std::vector<c_float> *A_data,std::vector<c_int> *A_indices,std::vector<c_int> *A_indptr) {static constexpr double kEpsilon = 1e-6;// block matrixEigen::MatrixXd matrix_constraint = Eigen::MatrixXd::Zero(state_dim_ * (horizon_ + 1) + state_dim_ * (horizon_ + 1) +control_dim_ * horizon_,state_dim_ * (horizon_ + 1) + control_dim_ * horizon_);Eigen::MatrixXd state_identity_mat = Eigen::MatrixXd::Identity(state_dim_ * (horizon_ + 1), state_dim_ * (horizon_ + 1));ADEBUG << "state_identity_mat" << state_identity_mat;matrix_constraint.block(0, 0, state_dim_ * (horizon_ + 1),state_dim_ * (horizon_ + 1)) =-1 * state_identity_mat;ADEBUG << "matrix_constraint";ADEBUG << matrix_constraint;Eigen::MatrixXd control_identity_mat =Eigen::MatrixXd::Identity(control_dim_, control_dim_);for (size_t i = 0; i < horizon_; i++) {matrix_constraint.block((i + 1) * state_dim_, i * state_dim_, state_dim_,state_dim_) = matrix_a_;}ADEBUG << "matrix_constraint with A";ADEBUG << matrix_constraint;for (size_t i = 0; i < horizon_; i++) {matrix_constraint.block((i + 1) * state_dim_,i * control_dim_ + (horizon_ + 1) * state_dim_,state_dim_, control_dim_) = matrix_b_;}ADEBUG << "matrix_constraint with B";ADEBUG << matrix_constraint;Eigen::MatrixXd all_identity_mat =Eigen::MatrixXd::Identity(num_param_, num_param_);matrix_constraint.block(state_dim_ * (horizon_ + 1), 0, num_param_,num_param_) = all_identity_mat;ADEBUG << "matrix_constraint with I";ADEBUG << matrix_constraint;std::vector<std::vector<std::pair<c_int, c_float>>> columns;columns.resize(num_param_ + 1);int value_index = 0;// state and terminal statefor (size_t i = 0; i < num_param_; ++i) { // colfor (size_t j = 0; j < num_param_ + state_dim_ * (horizon_ + 1);++j) // rowif (std::fabs(matrix_constraint(j, i)) > kEpsilon) {// (row, val)columns[i].emplace_back(j, matrix_constraint(j, i));++value_index;}}ADEBUG << "value_index";ADEBUG << value_index;int ind_A = 0;for (size_t i = 0; i < num_param_; ++i) {A_indptr->emplace_back(ind_A);for (const auto &row_data_pair : columns[i]) {A_data->emplace_back(row_data_pair.second); // valueA_indices->emplace_back(row_data_pair.first); // row++ind_A;}}A_indptr->emplace_back(ind_A);
}
按照分块矩阵往Ac里塞A,B,I
Eigen库Eigen::MatrixXd定义矩阵分块操作函数.block()的用法
2.7 CalculateConstraintVectors
计算二次规划约束中的上边界,下边界矩阵l,u
void MpcOsqp::CalculateConstraintVectors() {// evaluate the lower and the upper inequality vectorsEigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);for (size_t i = 0; i < horizon_; i++) {lowerInequality.block(control_dim_ * i + state_dim_ * (horizon_ + 1), 0,control_dim_, 1) = matrix_u_lower_;upperInequality.block(control_dim_ * i + state_dim_ * (horizon_ + 1), 0,control_dim_, 1) = matrix_u_upper_;}ADEBUG << " matrix_u_lower_";for (size_t i = 0; i < horizon_ + 1; i++) {lowerInequality.block(state_dim_ * i, 0, state_dim_, 1) = matrix_x_lower_;upperInequality.block(state_dim_ * i, 0, state_dim_, 1) = matrix_x_upper_;}ADEBUG << " matrix_x_lower_";// evaluate the lower and the upper equality vectorsEigen::VectorXd lowerEquality =Eigen::MatrixXd::Zero(state_dim_ * (horizon_ + 1), 1);Eigen::VectorXd upperEquality;lowerEquality.block(0, 0, state_dim_, 1) = -1 * matrix_initial_x_;upperEquality = lowerEquality;lowerEquality = lowerEquality;ADEBUG << " matrix_initial_x_";// merge inequality and equality vectorslowerBound_ = Eigen::MatrixXd::Zero(2 * state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);lowerBound_ << lowerEquality, lowerInequality;ADEBUG << " lowerBound_ ";upperBound_ = Eigen::MatrixXd::Zero(2 * state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);upperBound_ << upperEquality, upperInequality;ADEBUG << " upperBound_";
}
2.8 Solve
根据输入的矩阵求解二次规划,求解结果取出控制序列放入cmd中,然后cmd中第一个控制量就可以用于控制了。
bool MpcOsqp::Solve(std::vector<double> *control_cmd)
Apollo MPC横纵向耦合控制学习笔记相关推荐
- 自动驾驶横纵向耦合控制-复现Apollo横纵向控制
自动驾驶横纵向耦合控制-复现Apollo横纵向控制 基于动力学误差模型,使用mpc算法,一个控制器同时控制横向和纵向,实现横纵向耦合控制 matlab与simulink联合仿真,纵向控制已经做好油门刹 ...
- 小狐狸横版游戏开发学习笔记(上)
小狐狸横版游戏开发学习笔记(上) 目录 小狐狸横版游戏开发学习笔记(上) 1.关于如何创建Tilemap 2.关于地图格子之间出现间隙的问题 3.如何设置自己想要的控制按键 4.如何解决玩家移动过程中 ...
- 运动学习与控制-学习笔记(三)——运动控制理论
运动学习与控制-学习笔记(三)--运动控制理论 一.理解控制理论以及两个重要概念 1.Motor Control Theory 2.两个重要概念:Coordination,Degree of Free ...
- 电机控制学习笔记——PMSM数学模型
电机控制学习笔记--PMSM数学模型 0 引言 1 电压方程和磁链方程 2 转矩方程和运动方程 参考文献 0 引言 要实现对永磁同步电机的精准控制,首先需要对永磁同步电机进行建模,获得其电压方程. ...
- 滑模控制学习笔记(六)
滑模控制学习笔记(六) 等效滑模控制 等效滑模控制器设计 等效控制设计 滑模控制设计 仿真实例 等效滑模控制 滑模控制率可由等效控制uequ_{eq}ueq和切换鲁棒控制uswu_{sw}usw ...
- 海气耦合模态--学习笔记
海气耦合模态–学习笔记 热带海气耦合 Bjerknes正反馈 机制: 1.赤道流的存在使得海盆西侧(西太平洋海域)温度高于东侧,造成温跃层结构的东西差异 2.西侧的热源加热驱动大气环流深对流,加强了纬 ...
- 三相永磁同步电机的矢量控制学习笔记一
三相永磁同步电机的矢量控制学习笔记1 一 内容 三相永磁同步电机CPMSM)是一个强搞合.复杂的非线性系统,为了能够更好地 设计先进的 PMSM 矢量控制算法,建立合适的数学模型就显得尤为重要.本次学 ...
- 滑模控制学习笔记(三)
滑模控制学习笔记(三) 基于趋近律的滑模控制 几种典型的趋近律 等速趋近律 指数趋近律 幂次趋近律 一般趋近律 基于趋近律的控制器设计 仿真实例 状态空间模型建立 滑模控制器模型建立 仿真结果 基于趋 ...
- 滑模控制学习笔记(四)
滑模控制学习笔记(四) 基于上界的滑模控制 系统描述 控制器设计 仿真实例 基于准滑动模态的滑模控制 仿真实例 基于上界的滑模控制 系统描述 考虑二阶非线性系统如下:θ¨=f(θ,θ˙)+g(θ, ...
最新文章
- E. Turn Off The TV Educational Codeforces Round 29
- 开课吧Java教程:如何用listFiles()方法
- adb命令从手机端复制数据库文件到PC
- 高中化学实验室建设及常规设备配置
- “计算机组成原理”视频资料(加上全集链接啦)
- 牛客网Python笔试技巧、单行多行输入方法以及代码调试技巧
- 也许黎曼猜想是错误的
- android游戏手柄怎么用,王者荣耀怎么用手柄玩?手柄游戏详细教程
- 使用Pytorch识别字符验证码
- (实战)决策树例子+画图
- TweenMax特效
- ESXI下安装OpenWrt/LEDE软路由教程(附超全功能固件镜像下载)
- 7.camera驱动06-自己实现v4l2驱动-虚拟摄像头
- 微信小程序-电影app程序遇到得问题
- C++Primer笔记——拷贝控制
- 搭建传奇游戏,都需要准备什么东西。
- vue 自定义指令 directives
- Spark分布式计算框架之SparkStreaming+kafka
- oracle9i oci driver,Oracle连接 Oracle 的OCI Driver 和 Thin Driver的区别
- 客似云来——习题精解
热门文章
- 汽车安全攻击篇:智能网联系统的短板,如何防护汽车的安全
- 【无限互联】iOS开发之瀑布流布局实现(UICollectionView拓展Layout)
- SwiftUI 设置TextEditor字体颜色和背景颜色
- Word控件Spire.Doc 【Table】教程(7): 如何在C#中用表格替换word文档中的文本
- 国家强制性灯具安全标准GB7000.1-2015
- 服务器拒绝了您发送离线文件的请求,电脑中QQ提示服务器拒绝了您发送离线文件如何解决...
- 【项目经验】Vscode添加vue前端注释模板
- 牛顿科特斯型matlab,工程与科学数值方法的MATLAB实现(第4版)[PDF][119.29MB]
- 18- Adaboost梯度提升树 (集成算法) (算法)
- unity5.x中的关节和布料