TASK系列解析文章

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;

• 优化变量数量：n
• 约束函数数量：m
• 雅可比矩阵非0项数量：nnz_jac_g
• 黑塞矩阵非0项数量：nnz_h_lag

2.get_bounds_info()定义约束边界约束

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

• 自变量的下边界：x_l
• 自变量的上边界： x_u
• 约束函数下边界：g_l
• 约束函数的上边界：g_u

通过代码可知变量$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()定义初值

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

这部分在OptimizeByQP有具体介绍。
依据是否启动热启动warm-start，来确定是否将之前QP的结果作为NLP初始值。$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()求解目标函数

  /** 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}minf=& \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\_acc}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\_acc}{\dot{s}}_i^4\kappa \dot{\kappa }\\ +& 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\_acc}{\dot{s}}_i^3{\kappa }^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\_acc}{\dot{s}}_i^4\kappa \dot{\kappa }+\sum _{i=0}^{n-1}4w_{lat\_acc}{\dot{s}}_i^3{\kappa }^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()求解约束函数

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

• 变量值：x
• 约束函数值：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})\Delta t^3)\\ {\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})\Delta t^2)\\ {\dot{s}}_0-speed\_limit(s_0)\\ ⋮\\ {\dot{s}}_{n-1}-speed\_limit(s_{n-1})\\ 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()求解约束雅可比矩阵

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

• 变量值：x
• 雅可比矩阵非0元素数量：nele_jac
• 雅可比矩阵值：values

设 $f:{\mathbb{R}}^n\to {\mathbb{R}}^n$ 是一个可微函数，则 $f$ 在点 $\mathbf{x}=(x_1,x_2,\cdots ,x_n{)}^T$ 处的雅可比矩阵为：

$$\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)$$

其中，$f_i$ 表示 $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}$$

• 速度限制约束：$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$$

雅可比矩阵(维度:[4(N-1)+3N]*5N)如下：
$$\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()求解黑塞矩阵

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

• 变量值：x
• 拉格朗日乘数：lambda
• 黑塞矩阵值：values
• 目标函数因数：obj_factor

黑塞矩阵的基本形式如下所示：

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

其中，$f(\mathbf{x})$ 是二次可微函数，$\frac{{\partial }^{2}f}{\partial x_i\partial x_j}$ 表示 $f(\mathbf{x})$ 对 $x_i$ 和 $x_j$ 的二阶偏导数，$H$ 表示黑塞矩阵。

目标函数的二阶偏导只有个别项是非0的：
$$\begin{array}{rl}\frac{\partial f}{\partial s_i^2}& =2w_{s-ref}+2w_{lat\_acc}[{\dot{s}}_i^4\cdot ({\kappa }^{\prime }(s_i){)}^2+{\dot{s}}_i^4\cdot \kappa (s_i)\cdot {\kappa }^{\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\_acc}{s_i^{\prime }}^3\cdot \kappa (s_i)\cdot {\kappa }^{\prime }(s_i)\\ \frac{\partial f}{\partial {s_i^{\prime }}^2}& =12w_{lat\_acc}{s_i^{\prime }}^2\cdot {\kappa }^2(s_i)+2w_{\dot{s}-ref}\\ \frac{\partial f}{\partial {s_i^{\prime \prime }}^2}& =2w_a{s_i^{\prime }}^2\cdot {\kappa }^2(s_i)+\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}$$约束函数的二阶偏导数：
$$\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;

目标函数取得最小值时的优化量：x
目标函数最小值：obj_value

最后再次回顾一下流程：

• Input.输入部分包括PathData以及起始的TrajectoryPoint
• Process.
  • Snaity Check. 这样可以确保speed_data不为空，并且speed Optimizer不会接收到空数据.
  • const auto problem_setups_status = SetUpStatesAndBounds(path_data, *speed_data); 初始化QP问题。若失败，则会清除speed_data中的数据。
  • const auto qp_smooth_status = OptimizeByQP(speed_data, &distance, &velocity, &acceleration); 求解QP问题，并获得distance\velocity\acceleration等数据。 若失败，则会清除speed_data中的数据。这部分用以计算非线性问题的初始解，对动态规划的结果进行二次规划平滑。
  • const bool speed_limit_check_status = CheckSpeedLimitFeasibility();
    检查速度限制。接着或执行以下四个步骤：
    1)Smooth Path Curvature 2)SmoothSpeedLimit 3)Optimize By NLP 4)Record speed_constraint
  • 将 s/t/v/a/jerk等信息添加进 speed_data 并且补零防止fallback。
• Output.输出SpeedData, 包括轨迹的s/t/v/a/jerk。

参考

[1] Planning Piecewise Jerk Nonlinear Speed Optimizer Introduction
[2] Planning 基于非线性规划的速度规划
[3] Apollo星火计划学习笔记——Apollo速度规划算法原理与实践
[4] Apollo规划控制学习笔记