Apollo MPC OSQP Solver

Apollo MPC算法之前使用qpOASES Solver, 现在替换为OSQP, 　二次规划问题的求解速度和怎么构造优化问题有一定的联系, 要想提升整体的求解速度和求解成功率, 应该尽量少的使用等式约束。
OSQP Benchmark
使用qpOASES Solver构造二次规划问题的形式与OSQP Solver构造二次规划问题的形式不相同，关于qpOASES Solver是如何构造二次规划问题的可以参考：
qpOASES QP Formulation

关于Apollo使用的基于道路进行线性化的车辆动力学公式以及离散化方式请参考：
Linearized Lateral Error Dynamics
现在假设在采样时刻 $k$ 我们已经得到如下车辆Error Dynamics 线性系统状态方程
$$x_{k+1}=A(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$时刻 Linearized Error Dynamics的系统矩阵， 输入矩阵和干扰矩阵。

在$k$时刻，将这些量带入初始化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_;
}

参数对应表格：

参数	代码变量名称
$A_k$	matrix_a
$B_k$	matrix_b
$Q_k$	matrix_q
$R_k$	matrix_r
$x_k$	matrix_initial_x
$x_{max}$	matrix_x_upper
$x_{min}$	matrix_x_lower
$u_{max}$	matrix_u_upper
$u_{min}$	matrix_u_lower

优化目标函数和约束

Apollo MPC使用如下优化目标函数，
$$\begin{gathered} u_0^{\ast }=\underset{x_k,u_k}{\min }\sum _{k=0}^N(x_k-x_r{)}^{T}Q(x_k-x_r)+\sum _{k=0}^{N-1}{u}_k^{T}R{u}_k \\ {x}_{k+1}=A{x}_k+B{u}_k \\ {x}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le x_k\le x_{\mathrm{m}\mathrm{a}\mathrm{x}} \\ {u}_{\mathrm{m}\mathrm{i}\mathrm{n}}\le x_k\le u_{\mathrm{m}\mathrm{a}\mathrm{x}} \\ {x}_0=\bar{x} \end{gathered}$$
其中$N$为prediction horizon。

OSQP 二次规划标准形式

OSQP优化问题的标准形式如下：
$$\begin{gathered} \underset{x}{\min }\frac{1}{2}{x}^{T}Px+q^{T}x \\ subjecttol\le A_{c}x\le u \end{gathered}$$
上述二次规划问题只包括不等式约束不包括等式约束，对于等式约束$A_{eq}x=b_{eq}$，应该对其进行如下变换，将其化为不等式约束：
$$b_{eq}\le A_{eq}x\le$$