Apollo Open Space Planner 介绍 2-warm start

[TOC]
本文为 Apollo Open Space Planner 介绍的第二部分 warm start, 即根据上一篇 Hybrid A* 搜索的结果优化出较好的 initial guess 以便优化主体模型时能快速收敛.
本篇介绍基于 IPOPT 求解器的优化过程.
公式推导请参考上 2 篇博文: 1, 2.

优化模型

书接上回, Hybrid A* 搜索得到了 temporal profile and dual variables 2 个 warm starts. 得到的结果为在 $[0:t_s:T]$ 时间上的如下 $k+1$ 个 struct.

struct HybridAStartResult {
  std::vector<double> x;    //MPC x_k位置姿态
  std::vector<double> y;    //MPC x_k位置姿态
  std::vector<double> phi;  //MPC x_k位置姿态
  std::vector<double> v;    //MPC x_k位置姿态
  std::vector<double> a;    //MPC u_k控制输入
  std::vector<double> steer;//MPC u_k控制输入
  std::vector<double> accumulated_s;
};

如论文《TDR-OBCA A Reliable Planner for Autonomous Driving in Free-Space Environment》最后得到的 warm start 模型.

$$ \begin{aligned} \min _{\boldsymbol{\mu}, \boldsymbol{\lambda}, \boldsymbol{d}} \quad & w \sum_{m=1}^{M} \sum_{k=1}^{K} d_{m}(k) \\\\ \text{s.t.} \quad &\left\|A_{m}^{T} \lambda_{m}(k)\right\|_{2} \leq 1, \quad \cdots (1)\\\\ &G^{T} \mu_{m}(k)+R\left(\tilde{x}^{*}(k)\right)^{T} A_{m}^{T} \lambda_{m}(k)=0 \quad \cdots (2)\\\\ &-g^{T} \mu_{m}(k)+\left(A_{m} t\left(\tilde{x}^{*}(k)\right)-b_{m}\right)^{T} \lambda_{m}(k)+d_{m}(k)=0 \quad \cdots (3)\\\\ &\lambda_{m}(k) \succeq 0, \mu_{m}(k) \succeq 0, d_{m}(k)<0, \quad \cdots (4)\\\\ &\text { for } k=1, \ldots K, m=1, \ldots, M \end{aligned} $$

其中 $w$ 为标定系数, Apollo 里默认为 1.0.
这篇博文只介绍上述模型的实现, 为了模型计算的数据准备以及逻辑代码会在后面的博文中介绍.

先介绍各个模型中组件的实现, 然后再在代码进行对照讲解.
注意 IPOPT 中的 $\textbf{d} \geq \textbf{0}$, 即与模型中相反.

几何抽象

首先是自车, 障碍物, freespace 边际这些几何元素的数学表示. 这里需要注意的是这里的障碍物均为静态障碍物.

将自车抽象为一个矩形(自车坐标系下), 障碍物为凸多边形也就是说复杂的障碍物可以表示为多个四边形的拼接(如 O3), freespace 边际表示为直线.

自车(矩形):

$$ G^Te \leq g \Rightarrow \\ \begin{bmatrix} g_0 \\ g_1 \\ g_2 \\ g_3 \end{bmatrix} \begin{bmatrix} y \\ x \end{bmatrix} \leq \begin{bmatrix} \frac{L}{2} \\ \frac{W}{2} \\ \frac{L}{2} \\ \frac{W}{2} \end{bmatrix} \Rightarrow \\ \begin{bmatrix} 1 & 0 \\ 0 & 1 \\ -1 & 0 \\ 0 & -1 \end{bmatrix} \begin{bmatrix} y \\ x \end{bmatrix} \leq \begin{bmatrix} \frac{L}{2} \\ \frac{W}{2} \\ \frac{L}{2} \\ \frac{W}{2} \end{bmatrix} $$

障碍物(拆成四边形)

共 $m$ 个

$$ A_m^TO_m \leq b_m \Rightarrow \\ \begin{bmatrix} a_{m0} \\ a_{m1} \\ a_{m2} \\ a_{m3} \end{bmatrix} \begin{bmatrix} y \\ x \end{bmatrix} \leq \begin{bmatrix} b_{m0} \\ b_{m1} \\ b_{m2} \\ b_{m3} \end{bmatrix} $$

freespace边际(直线)

共 $p$ 个

$$
A_p^TL_p \leq b_p
$$

位姿变换

这里不加推导直接给出.

旋转矩阵:

$$ \begin{bmatrix} y_1 \\ x_1 \end{bmatrix} = R \begin{bmatrix} y_0 \\ x_0 \end{bmatrix} $$ $$ R = \begin{bmatrix} \cos \phi & -\sin \phi \\ \sin \phi & \cos \phi \end{bmatrix} $$

位移矩阵:

$$ \begin{bmatrix} y_1 \\ x_1 \\ 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & f \sin \phi \\ 0 & 1 & f \cos \phi \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} y_0 \\ x_0 \\ 1 \end{bmatrix} $$ $$ t=\begin{bmatrix} 1 & 0 & f \sin \phi \\ 0 & 1 & f \cos \phi \\ 0 & 0 & 1 \end{bmatrix} $$

其中 $\phi$ 为自车 yaw. $f$ 为自车几何中心与动力学中心(后轴中心)的 offset.

数据整理

障碍物数据

可以看到障碍物也可以表示为四条直线因此可以与 freespace 边际统一成直线障碍物进行存储. freespace 边际的直线处理后续在专门讲解的博文中详细展开(包括 Hybrid A* 搜索的结果整理等).

因此直线障碍物数目: $n=4m+p$. 下面把两种形式统一的障碍物叫做广义障碍物.
障碍物存储的层级为时刻里 $k \rightarrow$ 先是直线障碍物 $p$ 个 $ \rightarrow$ 然后是四边形障碍物 $m$ 个.

实际存储结构如下图:

有了上面的抽象模型与障碍物输入数据就可以很容易得到求解器所需的接口.
下面以 IPOPT 接口为例子.

IPOPT C++ 标准接口

问题维度

首先对 $\textbf X$ 仅需考虑 $x,y$ 位置维度(应用于超平面求间距, $\phi$ 因素被整合到了位姿变换矩阵中, $v$ 反映到了 Temporal profile 中), 因此 $\boldsymbol{\lambda}, \boldsymbol{\mu}$ 也只需考虑 $\lambda_x, \lambda_y, \mu_x, \mu_y$.

变量 $\boldsymbol{\lambda}$ 数目: $n \times (k+1)$.

变量 $\boldsymbol{\mu}$ 数目: $4m \times (k+1)$.

变量 $\textbf d$ 数目: $m \times (k+1)$.

变量总数: $(9m+p) \times (k+1)$

约束(1)个数: $m \times (k+1)$

约束(2)个数: $2m \times (k+1)$, 分为对 $x,y$ 的两个约束.

约束(3)个数: $m \times (k+1)$

约束(4)个数: $(9m+p) \times (k+1)$

某一时刻下由对偶变量的性质可以得出, $\boldsymbol{\lambda}$ 对应的是广义障碍物, 维度 $4m+p$. $\boldsymbol{\mu}$ 对应的是四边形障碍物的四条边, 维度 $4m$.

起点

$\boldsymbol{\lambda},\boldsymbol{\mu},\textbf d$ 初始均为 $\textbf 0$.

g(x) 表达式

某个时刻 $i$ 下, 对第 $m$ 个广义障碍物.
约束(1):

$$ g_{1m}: ||A^T \lambda||_2^2 = A_x^T \lambda_x A_x + A_y^T \lambda_y A_y \\ =\sum_{k=1}^{1or4} (a_{y_{imk}} \lambda_{y_{imk}})^2 + \sum_{k=1}^{1or4} (a_{x_{imk}} \lambda_{x_{imk}})^2 $$

其中 $m$ 为障碍物编号, $k$ 为某一个障碍物的边编号.

约束(2), 分成2个约束表达式:
令

$$
tmp_1 = \sum_{k=1}^{1 or 4} a_{y_{imk}} \lambda_{y_{imk}} \
tmp_2 = \sum_{k=1}^{1 or 4} a_{x_{imk}} \lambda_{x_{imk}}
$$

对 $y$ 坐标, 应用自车形状矩阵 $\textbf G$ 与旋转矩阵 $\textbf R$ 可得:

$$ g_{2m}: \mu_{y_{im}} - \mu_{y_{(i+1)m}} + \cos \phi _i * \text{tmp}_1 + \sin \phi _i * \text{tmp}_2 $$

对 $x$ 坐标, 应用自车形状矩阵 $\textbf G$ 与旋转矩阵 $\textbf R$ 可得:

$$ g_{3m}: \mu_{x_{im}} - \mu_{x_{(i+1)m}} - \sin \phi _i * \text{tmp}_1 + \cos \phi _i * \text{tmp}_2 $$

约束(3):
应用自车形状矩阵 $\textbf g$ 与移动矩阵 $\textbf t$ 可得:

$$ g_{4m}: d_m + g^T \mu_{m} + (A_m^T t)^T \lambda_{m} + b_m^T \lambda_{m} = \\ d_m + \sum_{k=1}^{4}g_k \mu_{mk} - (x_i + f \cos \phi _i) * tmp_1 - (y_i + f \sin \phi _i) * tmp_2 + \sum_{k=1}^{0or4}b_{mk} \lambda _{mk} $$

最后一项如果障碍物为直线障碍物则 $k=0$, 否则 $k=4$.

上面的约束以 1 个广义障碍物的约束为一组, 顺序为 $g_{1m},g_{2m},g_{3m},g_{4m}$.
因此合计 $4m+p$ 个.

约束(4):
对于变量 $\boldsymbol{\lambda}, \boldsymbol{\mu}, \textbf d$ 均有:

$$ g_{\lambda _i}: \lambda _i $$ 共$n$个. $$ g_{\mu _i} : \mu _i $$ 共$4m$个. $$ g_{d_i} : d_i $$ 共$m$个.

以上约束是时刻 $i$ 下的, 总数需要再乘以 $k+1$ 总时刻数.

bound 确定

待求变量 $\boldsymbol{\lambda},\boldsymbol{\mu},\textbf d$ 不设限制为 $[+\infty, -\infty]$. 但是在约束中 $\textbf g$ 中限制大于 $\textbf 0$.
即:

$$
\textbf 0 \leq \boldsymbol{\lambda},\boldsymbol{\mu},\textbf d \leq +\infty
$$

梯度矩阵求解

目标函数梯度 $\nabla f(x)$

$$
\nabla f(x) = \textbf w
$$

$\textbf w$ 为 $n \times (k+1)$ 的向量, 每个元素均为 $w$.

约束 Jacobian 矩阵 $\nabla g(x)^T$

Jacobian 矩阵为稀疏矩阵(这里的维度为 $[(13m+2p)(k+1))] \times [(9m+p)(k+1))]$, IPOPT 里采用 Triplet Format.
对其赋值的方法请参考 IPOPT 用法博文
按照上面约束方程的顺序, 即时刻 $i$ 下对广义障碍物 $m$ 的 4 或者 1 个边进行求解其位置坐标以及值.
只要足够小心就能够对齐位置, 这里不求位置坐标(参看源码), 只关心求值.

1. $\textbf g_{1}$ 对 $\partial\boldsymbol{\lambda}$ $$ j_1: 2 \text{tmp}_1 * a_{y_{imk}} + 2 \text{tmp}_2 * a_{x_{imk}} $$ 2. $\textbf g_{2}$ 对 $\partial\boldsymbol{\lambda}$ $$ j_2: \sum_{k=1}^{1or4} \cos \phi _i a_{y_{imk}} + \sum_{k=1}^{1or4} \sin \phi _i a_{x_{imk}} $$ 3. $\textbf g_{2}$ 对 $\partial\boldsymbol{\mu}$ $$ j_3 : \left\{ \begin{aligned} &1 \quad for \quad \mu_{y_{im}}\\ &-1 \quad for \quad \mu_{y_{(i+1)m}} \end{aligned} \right. $$ 4. $\textbf g_{3}$ 对 $\partial\boldsymbol{\lambda}$ $$ j_4: \sum_{k=1}^{1 \text{ or }4} - \sin \phi _i a_{y_{imk}} + \sum_{k=1}^{1 \text{ or }4} \cos \phi _i a_{x_{imk}} $$ 5. $\textbf g_{3}$ 对 $\partial\boldsymbol{\mu}$ $$ j_5 : \left\{ \begin{aligned} &1 \quad \text{ for } \quad \mu_{x_{im}}\\ &-1 \quad \text{ for } \quad \mu_{x_{(i+1)m}} \end{aligned} \right. $$ 6. $\textbf g_{4}$ 对 $\partial\boldsymbol{\lambda}$ $$ j_6: \sum_{k=1}^{1or4}a_{y_{imk}}(x_i + f \cos \phi _i) - \sum_{k=1}^{1or4}a_{x_{imk}}(y_i + f \sin \phi _i) + \sum_{k=1}^{0or4}b_{mk} $$ 7. $\textbf g_{4}$ 对 $\partial\boldsymbol{\mu}$ $$ j_7: \sum_{k=1}^{4}g_k $$ 8. $\textbf g_{4}$ 对 $\partial\textbf d$ $$ j_8:\textbf 1 $$ 9. $\textbf g_{5}$ 对 $\partial\boldsymbol{\lambda},\partial\boldsymbol{\mu},\partial\textbf d$ 均为 $\textbf 1$

目标函数 Hessian 矩阵

很明显为 $\textbf 0$.

约束 Hessian 矩阵

$\textbf g_{1}$ 对 $\partial \boldsymbol{\lambda} \partial \boldsymbol{\lambda}$

$$
h_1: 2 a_{ymk}^2 + 2 a_{xmk}^2
$$

其余皆为 $\textbf 0$.

代码解析

对于此 warm-start 问题, Apollo 给出了多个求解器下的接口类.

先对基于 IPOPT 的 DualVariableWarmStartIPOPTInterface 类的头文件进行解析.

#define tag_L 3
class DualVariableWarmStartIPOPTInterface : public Ipopt::TNLP {
 public:
  DualVariableWarmStartIPOPTInterface(
      size_t horizon, double ts, const Eigen::MatrixXd& ego,
      const Eigen::MatrixXi& obstacles_edges_num, const size_t obstacles_num,
      const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b,
      const Eigen::MatrixXd& xWS,
      const PlannerOpenSpaceConfig& planner_open_space_config);
  virtual ~DualVariableWarmStartIPOPTInterface() = default;

//从get_nlp_info到finalize_solution均为IPOPT接口,除了矩阵对齐以外可以参考上面的公式.没什么可以讲的.

  /** Method to return some info about the nlp */
  bool get_nlp_info(int& n, int& m, int& nnz_jac_g, int& nnz_h_lag,
                    IndexStyleEnum& index_style) override;
  /** Method to return the bounds for my problem */
  bool get_bounds_info(int n, double* x_l, double* x_u, int m, double* g_l,
                       double* g_u) override;
  /** Method to return the starting point for the algorithm */
  bool get_starting_point(int n, bool init_x, double* x, bool init_z,
                          double* z_L, double* z_U, int m, bool init_lambda,
                          double* lambda) override;
  /** Method to return the objective value */
  bool eval_f(int n, const double* x, bool new_x, double& obj_value) override;
  /** Method to return the gradient of the objective */
  bool eval_grad_f(int n, const double* x, bool new_x, double* grad_f) override;
  /** Method to return the constraint residuals */
  bool eval_g(int n, const double* x, bool new_x, int m, double* g) override;
  /** Method to return:
   *   1) The structure of the jacobian (if "values" is nullptr)
   *   2) The values of the jacobian (if "values" is not nullptr)
   */
  bool eval_jac_g(int n, const double* x, bool new_x, int m, int nele_jac,
                  int* iRow, int* jCol, double* values) override;
  /** Method to return:
   *   1) The structure of the hessian of the lagrangian (if "values" is
   * nullptr) 2) The values of the hessian of the lagrangian (if "values" is not
   * nullptr)
   */
  bool eval_h(int n, const double* x, bool new_x, double obj_factor, int m,
              const double* lambda, bool new_lambda, int nele_hess, int* iRow,
              int* jCol, double* values) override;
  /** @name Solution Methods */
  /** This method is called when the algorithm is complete so the TNLP can
   * store/write the solution */
  void finalize_solution(Ipopt::SolverReturn status, int n, const double* x,
                         const double* z_L, const double* z_U, int m,
                         const double* g, const double* lambda,
                         double obj_value, const Ipopt::IpoptData* ip_data,
                         Ipopt::IpoptCalculatedQuantities* ip_cq) override;

  void get_optimization_results(Eigen::MatrixXd* l_warm_up,
                                Eigen::MatrixXd* n_warm_up) const;
  
  //这里使用了ADOL-C库用于求解AD,即数值微分.用于自动求解Jacobian与Hessian矩阵.这里的接口后面学习后再分享出来.
  //***************    start ADOL-C part ***********************************
  /** Template to return the objective value */
  template <class T>
  bool eval_obj(int n, const T* x, T* obj_value);

  /** Template to compute constraints */
  template <class T>
  bool eval_constraints(int n, const T* x, int m, T* g);

  /** Method to generate the required tapes */
  void generate_tapes(int n, int m, int* nnz_h_lag);
  //***************    end   ADOL-C part ***********************************

 private:
  int num_of_variables_;
  int num_of_constraints_;
  //时刻跨度k
  int horizon_;
  //单位时间间隔
  double ts_;
  //自车位置向量
  Eigen::MatrixXd ego_;
  //lambda数目
  int lambda_horizon_ = 0;
  //mu数目
  int miu_horizon_ = 0;
  //d数目
  int dual_formulation_horizon_ = 0;

  //计算输出lambda
  Eigen::MatrixXd l_warm_up_;
  //计算输出mu
  Eigen::MatrixXd n_warm_up_;
  //轴距
  double wheelbase_;

  //自车宽度
  double w_ev_;
  //自车长度
  double l_ev_;
  //自车形状矩阵g
  std::vector<double> g_;
  //自车后轴中心与几何中心间距
  double offset_;
  //每个障碍物的边数(1或者是4)
  Eigen::MatrixXi obstacles_edges_num_;
  int obstacles_num_;
  int obstacles_edges_sum_;

  // lagrangian l start index
  int l_start_index_ = 0;

  // lagrangian n start index
  int n_start_index_ = 0;

  // lagrangian d start index
  int d_start_index_ = 0;

  // obstacles_A
  Eigen::MatrixXd obstacles_A_;

  // obstacles_b
  Eigen::MatrixXd obstacles_b_;

  // states of warm up stage
  Eigen::MatrixXd xWS_;

  //d的标定系数
  double weight_d_;

  //***************    start ADOL-C part ***********************************
  double* obj_lam;
  unsigned int* rind_L; /* row indices    */
  unsigned int* cind_L; /* column indices */
  double* hessval;      /* values */
  int nnz_L;
  int options_L[4];
  //***************    end   ADOL-C part ***********************************
};

Apollo 中还存在基于 OSQP 求解器的 DualVariableWarmStartOSQPInterface 类.
我大致扫了一眼, 注意到有几点主要不同. 大概率都是为了提升可求解性吧. 待后面实际运行对比.

1. 变量删除 $\textbf d$, 默认其为 0.
2. 改变目标函数为 $||A^T \lambda||_2^2$.
3. 不考虑时间序列. 只算某个时刻下的.

类似地还存在基于 IPOPT 的 QP 求解 DualVariableWarmStartIPOPTQPInterface. 是常规 IPOPT 与 OSQP 的结合. 变量删除 $\textbf d$, 默认其为 0. 改变目标函数为 $||A^T \lambda||_2^2$.