[Apollo Study Notes] - Planning module TASK PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER (2)

TASK series analysis articles

1.【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER
2.【Apollo学习笔记】——规划模块TASK之PATH_REUSE_DECIDER
3.【Apollo学习笔记】——规划模块TASK之PATH_BORROW_DECIDER
4.【Apollo学习笔记】——规划模块TASK之PATH_BOUNDS_DECIDER
5.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_PATH_OPTIMIZER
6.【Apollo学习笔记】——规划模块TASK之PATH_ASSESSMENT_DECIDER
7.【Apollo学习笔记】——规划模块TASK之PATH_DECIDER
8.【Apollo学习笔记】——规划模块TASK之RULE_BASED_STOP_DECIDER
9.【Apollo学习笔记】——规划模块TASK之SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER
10.【Apollo学习笔记】——规划模块TASK之SPEED_HEURISTIC_OPTIMIZER
11.【Apollo学习笔记】——规划模块TASK之SPEED_DECIDER
12.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER
13.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)
14.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(二)

续接上文：【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)

OptimizeByNLP

这部分是具体的非线性规划代码实现。

在代码中首先可以看到构造了PiecewiseJerkSpeedNonlinearIpoptInterface这个类的对象。下面是该类构造函数中所含有的参数。

PiecewiseJerkSpeedNonlinearIpoptInterface::
    PiecewiseJerkSpeedNonlinearIpoptInterface(
        const double s_init, const double s_dot_init, const double s_ddot_init,
        const double delta_t, const int num_of_points, const double s_max,
        const double s_dot_max, const double s_ddot_min,
        const double s_ddot_max, const double s_dddot_min,
        const double s_dddot_max)
    : curvature_curve_(0.0, 0.0, 0.0),
      v_bound_func_(0.0, 0.0, 0.0),
      s_init_(s_init),
      s_dot_init_(s_dot_init),
      s_ddot_init_(s_ddot_init),
      delta_t_(delta_t),
      num_of_points_(num_of_points),
      s_max_(s_max),
      s_dot_max_(s_dot_max),
      s_ddot_min_(-std::abs(s_ddot_min)),
      s_ddot_max_(s_ddot_max),
      s_dddot_min_(-std::abs(s_dddot_min)),
      s_dddot_max_(s_dddot_max),
      v_offset_(num_of_points),
      a_offset_(num_of_points * 2) {
    
    }

PiecewiseJerkSpeedNonlinearIpoptInterface继承自基类Ipopt::TNLP。为了利用IPOPT的接口TNLP求解问题，需要构建一个接口类来重写TNLP中求解问题所需要的一些函数，并完成对这些函数的实现。

IPOPT（Interior Point Optimizer）是一个用于大规模非线性优化的开源软件包。它可用于解决如下形式的非线性规划问题：
$$\begin{array}{rlr}\underset{x\in {\mathbb{R}}^n}{\min }& & f(x)\\ \text{s.t.}& & g^L\le g(x)\le g^U\\ & & x^L\le x\le x^U,\\ & & x\in {\mathbb{R}}^n\end{array}$$
$g^L$和$g^U$是约束函数的上界和下界，$x^L$和$x^U$是优化变量的上界和下界。

IPOPT的求解由以下几个函数构成：

1.get_nlp_info()定义问题规模

/** 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;

• Number of optimization variables: n
• Number of constraint functions: m
• Number of non-zero entries in the Jacobian matrix: nnz_jac_g
• Number of non-zero entries in the Hessian matrix:nnz_h_lag

2.get_bounds_info() defines constraint boundary constraints

  /** 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;

• Lower bound of the independent variable: x_l
• Upper bound of the independent variable: x_u
• Lower bound of the constrained function: g_l
• Upper bound of the constrained function:g_u

The variable xx can be known through the code$x$的边界约束（5n x 1）为：
$$x^L=\left[\begin{array}{c}{s}_{init}\\ s_{(1)-lower}\\ ⋮\\ s_{(n-1)-lower}\\ {\dot{s}}_{init}\\ 0\\ ⋮\\ 0\\ {\ddot{s}}_{init}\\ {\ddot{s}}_{\min }\\ ⋮\\ {\ddot{s}}_{\min }\\ 0\\ ⋮\\ 0\\ 0\\ ⋮\\ 0\end{array}\right],x^U=\left[\begin{array}{c}{s}_{init}\\ s_{(1)-upper}\\ ⋮\\ s_{(n-1)-upper}\\ {\dot{s}}_{init}\\ {\dot{s}}_{\max }\\ ⋮\\ {\dot{s}}_{\max }\\ {\ddot{s}}_{init}\\ {\ddot{s}}_{\max }\\ ⋮\\ {\ddot{s}}_{\max }\\ \inf \\ ⋮\\ \inf \\ \inf \\ ⋮\\ \inf \end{array}\right]$$

$g(x)$的边界约束（[4(n-1)+3n] x 1）为：
$$g^L=\left[\begin{array}{c}0\\ ⋮\\ 0\\ {s^{\prime \prime \prime }}_{\min }\\ ⋮\\ {s^{\prime \prime \prime }}_{\min }\\ 0\\ ⋮\\ 0\\ 0\\ ⋮\\ 0\\ -\inf \\ ⋮\\ -\inf \\ 0\\ ⋮\\ 0\\ -\inf \\ ⋮\\ -\inf \end{array}\right],g^U=\left[\begin{array}{c}{\dot{s}}_{\max }\cdot \Delta t\\ ⋮\\ {\dot{s}}_{\max }\cdot \Delta t\\ {s^{\prime \prime \prime }}_{\max }\\ ⋮\\ {s^{\prime \prime \prime }}_{\max }\\ 0\\ ⋮\\ 0\\ 0\\ ⋮\\ 0\\ 0\\ ⋮\\ 0\\ \inf \\ ⋮\\ \inf \\ 0\\ ⋮\\ 0\end{array}\right]$$

3.get_starting_point() defines the initial value

  /** 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;

This part is introduced in detail in OptimizeByQP .
Depending on whether warm-start is started, it is determined whether to use the previous QP results as the NLP initial value. $X_0$为5n x 1的向量。
$$X_0=\left[\begin{array}{c}{s}_{QP}^0\\ ⋮\\ s_{QP}^{n-1}\\ {\dot{s}}_{QP}^0\\ ⋮\\ {\dot{s}}_{QP}^{n-1}\\ {\ddot{s}}_{QP}^0\\ ⋮\\ {\ddot{s}}_{QP}^{n-1}\\ 0\\ ⋮\\ 0\\ 0\\ ⋮\\ 0\end{array}\right]$$

4.eval_f() solves the objective function

  /** Method to return the objective value */
  bool eval_f(int n, const double *x, bool new_x, double &obj_value) override;

• 变量值：x
• 目标函数值：obj_val
$$\begin{array}{rl}myf=& \sum _{i=0}^{n-1}{w}_{s-ref}(s_i-s_{-}re{f}_i{)}^2+w_{\dot{s}-ref}({\dot{s}}_i-{\dot{s}}_{-}ref{)}^2+w_{\ddot{s}}{\ddot{s}}_i^2+\sum _{i=0}^{n-2}{w}_{s^{{}^{\prime \prime \prime }}}(\frac{{\ddot{s}}_{i+1}-{\ddot{s}}_i}{\Delta t}{)}^2\\ & +\sum _{i=0}^{n-1}{w}_{lat\_a}lat\_ac{c}_i^2+w_{soft}s\_slack\_lowe{r}_i+w_{soft}s\_slack\_uppe{r}_i\\ & +w_{target-s}(s_{n-1}-s_{target}{)}^2+w_{target-\dot{s}}({\dot{s}}_{n-1}-{\dot{s}}_{target}{)}^2+w_{target-\ddot{s}}({\ddot{s}}_{n-1}-{\ddot{s}}_{target}{)}^2\end{array}$$

5.eval_grad_f()求解梯度

  /** Method to return the gradient of the objective */
  bool eval_grad_f(int n, const double *x, bool new_x, double *grad_f) override;

• 变量值：x
• 梯度值：grad_f

梯度的定义：
$${\nabla }_x\stackrel{\mathrm{def}}{=}{\left[\frac{\partial }{\partial x_1},\frac{\partial }{\partial x_2},\cdots ,\frac{\partial }{\partial x}\right]}^T=\frac{\partial }{\partial \mathbf{x}}$$

对目标函数求偏导，可得：
$$\begin{array}{rl}\frac{\partial f}{\partial s}=& 2\sum _{i=0}^{n-1}{w}_{s-ref}(s_i-s_{-}re{f}_i)+\sum _{i=0}^{n-1}2w{\_}_{lat\_a}{\dot{s}}_i^{4}K\dot{K}\\ +& 2w{\_}_{target-s}(s_{n-1}-s_{target})+2w{\_}_{target-\dot{s}}({\dot{s}}_{n-1}-{\dot{s}}_{target})+2w{\_}_{target-\ddot{s}}({\ddot{s}}_{n-1}-{\ddot{s}}_{target})\\ \frac{\partial f}{\partial \dot{s}}=& 2\sum _{i=0}^{n-1}{w}_{\dot{s}-ref}({\dot{s}}_i-{\dot{s}}_{-}ref)+\sum _{i=0}^{n-1}4w_{lat\_a}{\dot{s}}_i^3{K}^2\\ \frac{\partial f}{\partial \ddot{s}}=& 2\sum _{i=0}^{n-1}{w}_{\ddot{s}}{\ddot{s}}_i-2w{\_}_{s^{{}^{\prime \prime \prime }}}\frac{2({\ddot{s}}_1-{\ddot{s}}_0)}{\Delta t^2}+\sum _{i=1}^{n-2}2w{\_}_{s^{{}^{\prime \prime \prime }}}\frac{(2{\ddot{s}}_i-{\ddot{s}}_{i+1}-{\ddot{s}}_{i-1})}{\Delta t^2}+2w{\_}_{s^{{}^{\prime \prime \prime }}}\frac{2({\ddot{s}}_{n-1}-{\ddot{s}}_{n-2})}{\Delta t^2}\\ \frac{\partial f}{\partial s\_slack\_lower}=& n\cdot w_{soft}\\ \frac{\partial f}{\partial s\_slack\_upper}=& n\cdot w_{soft}\end{array}$$

所以，目标函数的梯度为
$$\begin{array}{rl}\frac{\partial f}{\partial X}=& 2\sum _{i=0}^{n-1}{w}_{s-ref}(s_i-s_{-}re{f}_i)+2\sum _{i=0}^{n-1}{w}_{\dot{s}-ref}({\dot{s}}_i-{\dot{s}}_{-}ref)+2\sum _{i=0}^{n-1}{w}_{\ddot{s}}{\ddot{s}}_i\\ -& 2w{\_}_{s^{{}^{\prime \prime \prime }}}\frac{2({\ddot{s}}_1-{\ddot{s}}_0)}{\Delta t^2}+\sum _{i=1}^{n-2}2w{\_}_{s^{{}^{\prime \prime \prime }}}\frac{(2{\ddot{s}}_i-{\ddot{s}}_{i+1}-{\ddot{s}}_{i-1})}{\Delta t^2}+2w{\_}_{s^{{}^{\prime \prime \prime }}}\frac{2({\ddot{s}}_{n-1}-{\ddot{s}}_{n-2})}{\Delta t^2}\\ +& \sum _{i=0}^{n-1}2w{\_}_{lat\_a}{\dot{s}}_i^{4}K\dot{K}+\sum _{i=0}^{n-1}4w_{lat\_a}{\dot{s}}_i^3{K}^2\\ +& 2w{\_}_{target-s}(s_{n-1}-s_{target})+2w{\_}_{target-\dot{s}}({\dot{s}}_{n-1}-{\dot{s}}_{target})+2w{\_}_{target-\ddot{s}}({\ddot{s}}_{n-1}-{\ddot{s}}_{target})\\ +& 2n\cdot w_{soft}\end{array}$$

6.eval_g() solves the constraint function

  /** Method to return the constraint residuals */
  bool eval_g(int n, const double *x, bool new_x, int m, double *g) override;

• Variable values: x
• Constraint function values:g
$$g(x)=\left[\begin{array}{c}{s}_1-s_0\\ ⋮\\ s_{n-1}-s_{n-2}\\ \frac{{\ddot{s}}_1-{\ddot{s}}_0}{\Delta t}\\ ⋮\\ \frac{{\ddot{s}}_{n-1}-{\ddot{s}}_{n-2}}{\Delta t}\\ s_1-(s_0+{\dot{s}}_0\Delta t+\frac{1}{2}{\ddot{s}}_0\Delta t^2+\frac{1}{6}(\frac{{\ddot{s}}_1-{\ddot{s}}_0}{\Delta t})\Delta t^3)\\ ⋮\\ s_{n-1}-(s_{n-2}+{\dot{s}}_{n-2}\Delta t+\frac{1}{2}{\ddot{s}}_{n-2}\Delta t^2+\frac{1}{6}(\frac{{\ddot{s}}_{n-1}-{\ddot{s}}_{n-2}}{\Delta t}\left)\Delta t^3\right)\\ {\dot{s}}_1-({\dot{s}}_0+{\ddot{s}}_0\Delta t+\frac{1}{2}(\frac{{\ddot{s}}_1-{\ddot{s}}_0}{\Delta t})\Delta t^2)\\ ⋮\\ {\dot{s}}_{n-1}-({\dot{s}}_{n-2}+{\ddot{s}}_{n-2}\Delta t+\frac{1}{2}(\frac{{\ddot{s}}_{n-1}-{\ddot{s}}_{n-2}}{\Delta t}\left)\Delta t^2\right)\\ {\dot{s}}_0-speed\_limit(s_0)\\ ⋮\\ {\dot{s}}_{n-1}-speed\_limit\left(s_{n-1}\right)\\ s_0-s\_soft\_lowe{r}_0+s\_slack\_lower\\ ⋮\\ s_{n-1}-s\_soft\_lowe{r}_{n-1}+s\_slack\_lower\\ s_0-s\_soft\_uppe{r}_0-s\_slack\_upper\\ ⋮\\ s_{n-1}-s\_soft\_uppe{r}_{n-1}-s\_slack\_upper\end{array}\right]$$

7.eval_jac_g() solves the constrained Jacobian matrix

  /** 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;

• Variable values: x
• Number of non-zero elements of the Jacobian matrix: nele_jac
• Jacobian matrix values:values

设 $f:{\mathbb{R}}^n\to R^n$ is a differentiable function, then$f$ 在点 $\mathbf{x}=(x_1,x_2,\cdots ,x_n{)}^{}$The Jacobian matrix at${}^{T\; is:}$

$$\mathbf{J}=\left(\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}& \cdots & \frac{\partial f_2}{\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial x_1}& \frac{\partial f_n}{\partial x_2}& \cdots & \frac{\partial f_n}{\partial x_n}\end{array}\right)$$

Among them, $f_i$means $f$ 的第 $i$ 个分量函数，$\frac{\partial f_i}{\partial x_j}$ 表示 $f_i$ 对第 $j$ 个自变量 $x_j$ 的偏导数。

求解器通过稀疏矩阵来保存值。

稀疏矩阵的例子

由约束矩阵可知，共有7个类型的函数，分别求偏导可得：

• 单调性约束：$g_1=s_{i+1}-s_i$$$\frac{\partial g_1}{\partial s_i}=-1,\frac{\partial g_1}{\partial s_{i+1}}=1$$

• 加加速度jerk约束:$g_2=\frac{{\ddot{s}}_{i+1}-{\ddot{s}}_i}{\Delta t}$$$\frac{\partial g_2}{\partial s_i}=-\frac{1}{\Delta t},\frac{\partial g_2}{\partial s_{i+1}}=\frac{1}{\Delta t}$$

• 位置等式约束：$g_3=s_{i+1}-(s_i+s_i^{\prime }\ast \Delta t+\frac{1}{3}\ast s_i^{\prime \prime }\ast \Delta t^2+\frac{1}{6}\ast s_{i+1}^{\prime \prime }\ast \Delta t^2)$
  $$\begin{array}{c}\begin{array}{r}\frac{\partial g_3}{\partial s_i}=-1\end{array}\\ \frac{\partial g_3}{\partial s_{i+1}}=1\\ \begin{array}{rl}\frac{\partial g_3}{\partial {\dot{s}}_i}& =-\Delta t\end{array}\\ \frac{\partial g_3}{\partial {\ddot{s}}_i}=-\frac{\Delta t^2}{3}\\ \frac{\partial g_3}{\partial {\ddot{s}}_{i+1}}=-\frac{\Delta t^2}{6}\end{array}$$

• 速度等式约束：$g_4=s_{i+1}^{\prime }-(s_i^{\prime }+\frac{1}{2}\ast s_i^{\prime \prime }\ast \Delta t+\frac{1}{2}\ast s_{i+1}^{\prime \prime }\ast \Delta t)$
  $$\begin{array}{c}\frac{\partial g_4}{\partial {\dot{s}}_i}=-1\\ \frac{\partial g_4}{\partial {\dot{s}}_{i+1}}=1\\ \frac{\partial g_4}{\partial {\ddot{s}}_i}=-\frac{\Delta t}{2}\\ \frac{\partial g_4}{\partial {\ddot{s}}_{i+1}}=-\frac{\Delta t}{2}\end{array}$$

• Speed ​​limit constraint: $g_5={\dot{s}}_i-speed\_limit(s_i)$
  $$\frac{\partial g_5}{\partial {\dot{s}}_i}=1,\frac{\partial g_5}{\partial s_i}=-\frac{dspeed\_limit(s_i)}{d{s}_i}$$

• 软约束lower:$g_6=s_i-s\_soft\_lowe{r}_i+s\_slack\_lowe{r}_i$
  $$\frac{\partial g_6}{\partial s_i}=1,\frac{\partial g_6}{\partial s\_slack\_lowe{r}_i}=1$$

• 软约束upper:$g_7=s_i-s\_soft\_uppe{r}_i-s\_slack\_uppe{r}_i$
  $$\frac{\partial g_7}{\partial s_i}=1,\frac{\partial g_7}{\partial s\_slack\_uppe{r}_i}=-1$$

The solvent(s: [4(N-1)+3N]*5N) is:
$$\left[\begin{array}{ccccc}\begin{array}{cccc}-1& 1& & \\ & \ddots & \ddots & \\ & & -1& 1\end{array}& & & & \\ & & \begin{array}{cccc}-\frac{1}{\Delta t}& \frac{1}{\Delta t}& & \\ & \ddots & \ddots & \\ & & -\frac{1}{\Delta t}& \frac{1}{\Delta t}\end{array}& & \\ \begin{array}{cccc}-1& 1& & \\ & \ddots & \ddots & \\ & & -1& 1\end{array}& \begin{array}{ccc}-\Delta t& & \\ & \ddots & \\ & & -\Delta t\end{array}& \begin{array}{cccc}-\frac{\Delta t^2}{3}& -\frac{\Delta t^2}{6}& & \\ & \ddots & \ddots & \\ & & -\frac{\Delta t^2}{3}& -\frac{\Delta t^2}{6}\end{array}& & \\ & \begin{array}{cccc}-1& 1& & \\ & \ddots & \ddots & \\ & & -1& 1\end{array}& \begin{array}{cccc}-\frac{\Delta t}{2}& -\frac{\Delta t}{2}& & \\ & \ddots & \ddots & \\ & & -\frac{\Delta t}{2}& -\frac{\Delta t}{2}\end{array}& & \\ \begin{array}{ccc}-v\_f(s_i{)}^{\prime }& & \\ & \ddots & \\ & & -v\_f(s_i{)}^{\prime }\end{array}& \begin{array}{ccc}1& & \\ & \ddots & \\ & & 1\end{array}& & & \\ \begin{array}{ccc}1& & \\ & \ddots & \\ & & 1\end{array}& & & \begin{array}{ccc}1& & \\ & \ddots & \\ & & 1\end{array}& \\ \begin{array}{ccc}1& & \\ & \ddots & \\ & & 1\end{array}& & & & \begin{array}{ccc}-1& & \\ & \ddots & \\ & & -1\end{array}\end{array}\right]$$

8.eval_h() solves the Hessian matrix

  /** 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;

• Variable values: x
• Lagrange multiplier: lambda
• Hessian matrix values: values
• Objective function factors:obj_factor

The basic form of the Hessian matrix is ​​as follows:

$$H=\left(\begin{array}{cccc}\frac{{\partial }^{2}f}{\partial x_1^2}& \frac{{\partial }^2}{\partial x_1\partial x_2}& \cdots & \frac{{\partial }^2}{\partial x_1\partial x_n}\\ \frac{{\partial }^{2}f}{\partial x_2\partial x_1}& \frac{{\partial }^2}{\partial x_2^2}& \cdots & \frac{{\partial }^2}{\partial x_2\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{{\partial }^2}{\partial x_n\partial x_1}& \frac{{\partial }^2}{\partial x_n\partial x_2}& \cdots & \frac{{\partial }^2}{\partial x_n^2}\end{array}\right)$$

where, $f\left(x\right)$ is a quadratic differentiable function,$\frac{{\partial }^2}{\partial x_i\partial x_j}$represents $f(\mathbf{x})$ 对 $x_i$and $x_j$The second partial derivative of , $H$ represents the Hessian matrix.

Only individual terms of the second-order partial derivative of the objective function are non-zero:
$$\begin{array}{rl}\frac{\partial f}{\partial s_i^2}& =2w{\_}_{s-ref}+2w{\_}_{lat\_a}[{\dot{s}}_i^4\cdot ({Mr}^{\prime }(s_i){)}^2+{\dot{s}}_i^4\cdot k(s_i)\cdot K^{\prime \prime }(s_i)]\\ & +2w{\_}_{target-s}+2w{\_}_{target-\dot{s}}+2w{\_}_{target-\ddot{s}}\\ \frac{\partial f}{\partial s_i\partial s_i^{\prime }}& =8w_{lat\_a}{s_i^{\prime }}^3\cdot k\left(s_i\right)\cdot K^{\prime }\left(s_i\right)\\ \frac{\partial f}{\partial {s_i^{\prime }}^2}& =12w_{lat\_a}{s_i^{\prime }}^2\cdot K^2(s_i)+2w{\_}_{\dot{s}-ref}\\ \frac{\partial f}{\partial {s_i^{\prime \prime }}^2}& =2w{\_}_a{s_i^{\prime }}^2\cdot K^2\left(s_i\right)+\frac{4w_j}{\Delta t^2}+2w{\_}_a\\ \frac{\partial f}{\partial s_i^{\prime \prime }\partial s_{i+1}^{\prime \prime }}& =-\frac{2w{\_}_j}{\Delta t^2}\end{array}$$Second-order partial derivative of the constraint function:
$$\begin{array}{rl}& \frac{\partial g}{\partial {s_i}^2}=-speed\_limi{t}^{\prime \prime }(s_i)\end{array}$$

9. finalize_solution()

  /** @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;

The amount of optimization when the objective function obtains the minimum value: x
Minimum value of the objective function:obj_value

Finally, review the process again:

• Input . The input part includes PathData and the starting TrajectoryPoint
• Process.
  • Snaity Check . This ensures that speed_data is not empty and that the speed Optimizer does not receive empty data.
  • const auto problem_setups_status = SetUpStatesAndBounds(path_data, *speed_data);Initialize QP problem. If it fails, the data in speed_data will be cleared.
  • const auto qp_smooth_status = OptimizeByQP(speed_data, &distance, &velocity, &acceleration);Solve the QP problem and obtain distance\velocity\acceleration and other data. If it fails, the data in speed_data will be cleared. This part is used to calculate the initial solution to the nonlinear problem and perform quadratic programming smoothing on the results of dynamic programming .
  • const bool speed_limit_check_status = CheckSpeedLimitFeasibility();
    Check speed limits. Then or perform the following four steps:
    1)Smooth Path Curvature 2)SmoothSpeedLimit 3)Optimize By NLP 4)Record speed_constraint
  • Add s/t/v/a/jerk and other information into speed_data and pad it with zeros to prevent fallback.
• Output . Output SpeedData, including the s/t/v/a/jerk of the trajectory.

reference

[1] Planning Piecewise Jerk Nonlinear Speed ​​Optimizer Introduction
[2] Planning Speed ​​planning based on nonlinear planning
[3] Apollo Spark Plan study notes - Apollo speed planning algorithm principle and practice
[4] Apollo planning control study notes