ApolloMPCOSQPSolver
Apollo MPC OSQP Solver
Apollo MPC算法之前使⽤qpOASES Solver, 现在替换为OSQP,  ⼆次规划问题的求解速度和怎么构造优化问题有⼀定的联系, 要想提升整体的求解速度和求解成功率, 应该尽量少的使⽤等式约束。
使⽤qpOASES Solver构造⼆次规划问题的形式与OSQP Solver构造⼆次规划问题的形式不相同，关于qpOASES Solver是如何构造⼆次规划问题的可以参考：
关于Apollo使⽤的基于道路进⾏线性化的车辆动⼒学公式以及离散化⽅式请参考：现在假设在采样时刻  我们已经得到如下车辆Error Dynamics 线性系统状态⽅程
同时， 我们状态量和输⼊量的上下限, , , 已经规定好， 和为时刻的状态惩罚矩阵和输⼊惩罚矩阵。,
和分别为时刻 Linearized Error Dynamics的系统矩阵， 输⼊矩阵和⼲扰矩阵。
在时刻，将这些量带⼊初始化OSQP Solver：
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_;}
参数对应表格：
参数
代码变量名称matrix_a
k x =k +1A (k )x +k B (k )u +k C (k )
x min x max u min u max Q (k )R (k )k A k B k C k k k A k
matrix_b matrix_q matrix_r matrix_initial_x matrix_x_upper matrix_x_lower matrix_u_upper matrix_u_lower
参数
代码变量名称优化⽬标函数和约束
Apollo MPC使⽤如下优化⽬标函数，
其中为prediction horizon。
OSQP ⼆次规划标准形式
OSQP优化问题的标准形式如下：
上述⼆次规划问题只包括不等式约束不包括等式约束，对于等式约束，应该对其进⾏如下变换，将其化为不等式约束：
对于Apollo模型预测控制问题，等式约束来⾃于系统的状态⽅程。 ⽬前开源出的代码中， OSQP等式约束中是忽略了这⼀项的， 只⽤了约束。
针对于OSQP求解器的该种接⼝形式，需要对MPC优化问题进⾏重新构造，从⽽适配接⼝，也就是说要求出对应于上节提到优化问题的Hessian Matrix , Gradient Vector , Equality Constraint Matrix 和Equality Constraint Vectors ，。OSQP Solver中的矩阵数据采取CSC格式，参考博客如下：
所以针对下⾯的矩阵P , 若使⽤需要⽤五个关键变量去描述这个矩阵
B k Q k R k x k
x max x min u max u min
u =0
∗(x −x ,u k k min k =0∑N k x )Q (x −r T
k x )+r u Ru k =0∑N −1
k T k
x =k +1Ax +k Bu k x ≤min x ≤k x ma x u ≤min x ≤k u ma x
x =0x ˉ
N    x Px +x
min
21
T q x
T subject  to  l ≤A x ≤c u
A x =eq b eq b ≤eq A x ≤eq b eq
x =k +1A (k )x +k B (k )u +k C (k )C k x =k A x +k k B u k k P q A c l u
1. 矩阵⾏数 numRows: 类型 int 数值 3
2. 矩阵列数 numCols：类型 int 数值 3
3. colPtrs: the index corresponding to the start of a new column: 类型 Array {int} 数值 (0, 2, 3, 6), 这⼀个array的长度为矩阵
列数加1, 第⼀个元素⼀直为0, 第⼆个元素是第⼀列⾮零元素，以此类推.
4. rowIndices: the row index of the entry: 类型 Array{int} 数值(0, 2, 1, 0, 1, 2)
5. values non-zero matrix entries in column major 类型 Array {double} Array(1.0,2.0,3.0,4.0,5.0,
6.0)
为了防⽌⼤家混淆变量，我们重新梳理⼀下变量和的含义, 上⼀节中提到的车辆Linearized Error Dynamics中的包括横向和纵向误差变量⼀共六个状态变量, 为当前采样时刻。
输⼊包括⽅向盘转⾓和纵向加速度OSQP solver中为⼆次规划问题的决策变量，由当前时刻的状态变量，当前时刻的输⼊，预测的状态变量和输⼊构成。
其中为预测步长。Hessian矩阵形式如下：
P =⎣⎡102030456
⎦⎤x (k )k x (k )=⎣⎢⎢⎢⎢⎢⎢⎡e (k )lat (k )e
˙lat e (k )heading (k )e ˙heading e (k )station e (k )
speed ⎦
⎥⎥⎥⎥⎥⎥⎤
u k δa
u (k )=[δ(k )
a (k )]
x x =⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡x (k )x (k +1)⋮x (k +N )u (k )u (k +1)⋮u (k +N −1)
⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤N P P =diag(Q ,Q ,...,Q ,R ,...,R )
void  MpcOsqp ::CalculateKernel (std ::vector <c_float > *P_data ,                              std ::vector <c_i
nt > *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_);  int  value_index = 0;  // state and terminal state
for  (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 ;    }  }  // control
const  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 this    P_indptr ->emplace_back (ind_p );
for  (const  auto  &row_data_pair : columns [i ]) {
P_data ->emplace_back (row_data_pair .second );    // val      P_indices ->emplace_back (row_data_pair .first );  // row      ++ind_p ;    }  }
identity matrix是什么意思P_indptr ->emplace_back (ind_p );}
Gradient Vector 构造形式⼊下：
q =
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡−Qx r −Qx r ⋮−Qx r 0⋮0⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
// reference is always zero
void  MpcOsqp ::CalculateGradient () {  // populate the gradient vector  gradient_ = 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_;}
Equality Constraint Matrix构造如下， 包括上下两个部分，上部分对应等式约束，下部分对应不等式约束， 每个部分⼜分为左右两部分， 左部分对应于决策变量中的状态量部分，右部分对应决策变量中输⼊量部分。
// equality constraints x(k+1) = A*x(k)
void  MpcOsqp ::CalculateEqualityConstraint (std ::vector <c_float > *A_data ,                                          std ::vector <c_int > *A_indices ,                                          std ::vector <c_int > *A_indptr ) {  constexpr  double  kEpsilon = 1e-6;  // block matrix
Eigen ::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_);
x (k )u (k )A =
c ⎣
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡A 0⋮0I 00⋮000⋮0−I A ⋮00I 0⋮000⋮0
0−I ⋮000I ⋮000⋮0
⋯⋯⋱⋯⋯⋯⋯⋱⋯⋯⋯⋱⋯
00⋮−I 000⋮I 00⋮0
B 0⋮0000⋮0I 0⋮0
0B ⋮0000⋮00I ⋮0
⋯⋯⋱⋯⋯⋯⋯⋱⋯⋯⋯⋱⋯
00⋮B 000⋮000⋮I ⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。