文章目录

TASK系列解析文章
- - OptimizeByNLP
  - - 1.get_nlp_info()定义问题规模
    - 2.get_bounds_info()定义约束边界约束
    - 3.get_starting_point()定义初值
    - 4.eval_f()求解目标函数
    - 5.eval_grad_f()求解梯度
    - 6.eval_g()求解约束函数
    - 7.eval_jac_g()求解约束雅可比矩阵
    - 8.eval_h()求解黑塞矩阵
    - [9. finalize_solution()](#9. finalize_solution())
参考

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这个类的对象。下面是该类构造函数中所含有的参数。

cpp 复制代码

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）是一个用于大规模非线性优化的开源软件包。它可用于解决如下形式的非线性规划问题：
min ⁡ x ∈ R n f ( x ) s.t. g L ≤ g ( x ) ≤ g U x L ≤ x ≤ x U , x ∈ R n \begin{aligned} \min_{x\in\mathbb{R}^{n}}&& f(x) \\ \text{s.t.}&& g^{L}\leq g(x)\leq g^{U} \\ &&x^{L}\leq x\leq x^{U}, \\ &&x\in\mathbb{R}^{n} \end{aligned} x∈Rnmins.t.f(x)gL≤g(x)≤gUxL≤x≤xU,x∈Rn
g L {g^L} gL和 g U {g^U} gU是约束函数的上界和下界， x L {x^L} xL和 x U {x^U} xU是优化变量的上界和下界。

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

1.get_nlp_info()定义问题规模

cpp 复制代码

/** 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()定义约束边界约束

cpp 复制代码

  /** 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 x x的边界约束（5n x 1）为：
x L = [ s i n i t s ( 1 ) − l o w e r ⋮ s ( n − 1 ) − l o w e r s ˙ i n i t 0 ⋮ 0 s ¨ i n i t s ¨ min ⁡ ⋮ s ¨ min ⁡ 0 ⋮ 0 0 ⋮ 0 ] , x U = [ s i n i t s ( 1 ) − u p p e r ⋮ s ( n − 1 ) − u p p e r s ˙ i n i t s ˙ max ⁡ ⋮ s ˙ max ⁡ s ¨ i n i t s ¨ max ⁡ ⋮ s ¨ max ⁡ inf ⁡ ⋮ inf ⁡ inf ⁡ ⋮ inf ⁡ ] {x^L} = \left[ {\begin{array}{ccccccccccccccc}{{s_{init}}}\\{{s_{(1) - lower}}}\\ \vdots \\{{s_{(n - 1) - lower}}}\\{{{\dot s}{init}}}\\0\\ \vdots \\0\\{{{\ddot s}{init}}}\\{{{\ddot s}{\min }}}\\ \vdots \\{{{\ddot s}{\min }}}\\0\\ \vdots \\0\\0\\ \vdots \\0\end{array}} \right],{x^U} = \left[ {\begin{array}{ccccccccccccccc}{{s_{init}}}\\{{s_{(1) - upper}}}\\ \vdots \\{{s_{(n - 1) - upper}}}\\{{{\dot s}{init}}}\\{{{\dot s}{\max }}}\\ \vdots \\{{{\dot s}{\max }}}\\{{{\ddot s}{init}}}\\{{{\ddot s}{\max }}}\\ \vdots \\{{{\ddot s}{\max }}}\\{\inf }\\ \vdots \\{\inf }\\{\inf }\\ \vdots \\{\inf }\end{array}} \right] xL= sinits(1)−lower⋮s(n−1)−lowers˙init0⋮0s¨inits¨min⋮s¨min0⋮00⋮0 ,xU= sinits(1)−upper⋮s(n−1)−uppers˙inits˙max⋮s˙maxs¨inits¨max⋮s¨maxinf⋮infinf⋮inf

g ( x ) g(x) g(x)的边界约束（[4(n-1)+3n] x 1）为：
g L = [ 0 ⋮ 0 s ′ ′ ′ min ⁡ ⋮ s ′ ′ ′ min ⁡ 0 ⋮ 0 0 ⋮ 0 − inf ⁡ ⋮ − inf ⁡ 0 ⋮ 0 − inf ⁡ ⋮ − inf ⁡ ] , g U = [ s ˙ max ⁡ ⋅ Δ t ⋮ s ˙ max ⁡ ⋅ Δ t s ′ ′ ′ max ⁡ ⋮ s ′ ′ ′ max ⁡ 0 ⋮ 0 0 ⋮ 0 0 ⋮ 0 inf ⁡ ⋮ inf ⁡ 0 ⋮ 0 ] {g^L} = \left[ {\begin{array}{ccccccccccccccc}0\\ \vdots \\0\\{{{s'''}{\min }}}\\ \vdots \\{{{s'''}{\min }}}\\0\\ \vdots \\0\\0\\ \vdots \\0\\{ - \inf }\\ \vdots \\{ - \inf }\\0\\ \vdots \\0\\{ - \inf }\\ \vdots \\{ - \inf }\end{array}} \right],{g^U} = \left[ {\begin{array}{ccccccccccccccc}{{{\dot s}{\max }} \cdot \Delta t}\\ \vdots \\{{{\dot s}{\max }} \cdot \Delta t}\\{{{s'''}{\max }}}\\ \vdots \\{{{s'''}{\max }}}\\0\\ \vdots \\0\\0\\ \vdots \\0\\0\\ \vdots \\0\\{\inf }\\ \vdots \\{\inf }\\0\\ \vdots \\0\end{array}} \right] gL= 0⋮0s′′′min⋮s′′′min0⋮00⋮0−inf⋮−inf0⋮0−inf⋮−inf ,gU= s˙max⋅Δt⋮s˙max⋅Δts′′′max⋮s′′′max0⋮00⋮00⋮0inf⋮inf0⋮0

3.get_starting_point()定义初值

cpp 复制代码

  /** 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 {X_0} X0为5n x 1的向量。
X 0 = [ s Q P 0 ⋮ s Q P n − 1 s ˙ Q P 0 ⋮ s ˙ Q P n − 1 s ¨ Q P 0 ⋮ s ¨ Q P n − 1 0 ⋮ 0 0 ⋮ 0 ] {X_0} = \left[ {\begin{array}{ccccccccccccccc}{s_{QP}^0}\\ \vdots \\{s_{QP}^{n - 1}}\\{\dot s_{QP}^0}\\ \vdots \\{\dot s_{QP}^{n - 1}}\\{\ddot s_{QP}^0}\\ \vdots \\{\ddot s_{QP}^{n - 1}}\\0\\ \vdots \\0\\0\\ \vdots \\0\end{array}} \right] X0= sQP0⋮sQPn−1s˙QP0⋮s˙QPn−1s¨QP0⋮s¨QPn−10⋮00⋮0

4.eval_f()求解目标函数

cpp 复制代码

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

• 变量值：x

• 目标函数值：obj_val
m i n f = ∑ i = 0 n − 1 w s − r e f ( s i − s − r e f i ) 2 + w s ˙ − r e f ( s ˙ i − s ˙ − r e f ) 2 + w s ¨ s ¨ i 2 + ∑ i = 0 n − 2 w s ′ ′ ′ ( s ¨ i + 1 − s ¨ i Δ t ) 2 + ∑ i = 0 n − 1 w l a t _ a c c l a t _ a c c i 2 + w s o f t s _ s l a c k _ l o w e r i + w s o f t s _ s l a c k _ u p p e r i + w t a r g e t − s ( s n − 1 − s t a r g e t ) 2 + w t a r g e t − s ˙ ( s ˙ n − 1 − s ˙ t a r g e t ) 2 + w t a r g e t − s ¨ ( s ¨ n − 1 − s ¨ t a r g e t ) 2 \begin{aligned}minf=&\sum_{i=0}^{n-1}w_{s-ref}(s_i-s_-ref_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}^{'''}}(\frac{\ddot{s}{i+1}-\ddot{s}i}{\Delta t})^2\\&+\sum{i=0}^{n-1}w{lat\acc}lat\acc_i^2+w{soft}s\slack\lower_i+w{soft}s\slack\upper_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{aligned} minf=i=0∑n−1ws−ref(si−s−refi)2+ws˙−ref(s˙i−s˙−ref)2+ws¨s¨i2+i=0∑n−2ws′′′(Δts¨i+1−s¨i)2+i=0∑n−1wlat_acclat_acci2+wsofts_slack_loweri+wsofts_slack_upperi+wtarget−s(sn−1−starget)2+wtarget−s˙(s˙n−1−s˙target)2+wtarget−s¨(s¨n−1−s¨target)2

5.eval_grad_f()求解梯度

cpp 复制代码

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

梯度的定义：
∇ x = d e f [ ∂ ∂ x 1 , ∂ ∂ x 2 , ⋯ , ∂ ∂ x ] T = ∂ ∂ x \nabla_x\overset{\mathrm{def}}{=}\left[\frac{\partial}{\partial x_1},\frac{\partial}{\partial x_2},\cdots,\frac{\partial}{\partial x}\right]^T=\frac{\partial}{\partial\boldsymbol{x}} ∇x=def[∂x1∂,∂x2∂,⋯,∂x∂]T=∂x∂

对目标函数求偏导，可得：
∂ f ∂ s = 2 ∑ i = 0 n − 1 w s − r e f ( s i − s − r e f i ) + ∑ i = 0 n − 1 2 w l a t _ a c c s ˙ i 4 κ κ ˙ + 2 w t a r g e t − s ( s n − 1 − s t a r g e t ) + 2 w t a r g e t − s ˙ ( s ˙ n − 1 − s ˙ t a r g e t ) + 2 w t a r g e t − s ¨ ( s ¨ n − 1 − s ¨ t a r g e t ) ∂ f ∂ s ˙ = 2 ∑ i = 0 n − 1 w s ˙ − r e f ( s ˙ i − s ˙ − r e f ) + ∑ i = 0 n − 1 4 w l a t _ a c c s ˙ i 3 κ 2 ∂ f ∂ s ¨ = 2 ∑ i = 0 n − 1 w s ¨ s ¨ i − 2 w s ′ ′ ′ 2 ( s ¨ 1 − s ¨ 0 ) Δ t 2 + ∑ i = 1 n − 2 2 w s ′ ′ ′ ( 2 s ¨ i − s ¨ i + 1 − s ¨ i − 1 ) Δ t 2 + 2 w s ′ ′ ′ 2 ( s ¨ n − 1 − s ¨ n − 2 ) Δ t 2 ∂ f ∂ s _ s l a c k _ l o w e r = n ⋅ w s o f t ∂ f ∂ s _ s l a c k _ u p p e r = n ⋅ w s o f t \begin{aligned} \frac{\partial f}{\partial s}=&2\sum_{i=0}^{n-1}w_{s-ref}(s_i-s_-ref_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}^{'''}}{\frac{2(\ddot s_1-\ddot s_0)}{\Delta t^2}}+\sum_{i=1}^{n-2}2w_{{s}^{'''}}{\frac{(2\ddot s_i-\ddot s_{i+1}-\ddot s_{i-1})}{\Delta t^2}}+2w_{{s}^{'''}}{\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{aligned} ∂s∂f=+∂s˙∂f=∂s¨∂f=∂s_slack_lower∂f=∂s_slack_upper∂f=2i=0∑n−1ws−ref(si−s−refi)+i=0∑n−12wlat_accs˙i4κκ˙2wtarget−s(sn−1−starget)+2wtarget−s˙(s˙n−1−s˙target)+2wtarget−s¨(s¨n−1−s¨target)2i=0∑n−1ws˙−ref(s˙i−s˙−ref)+i=0∑n−14wlat_accs˙i3κ22i=0∑n−1ws¨s¨i−2ws′′′Δt22(s¨1−s¨0)+i=1∑n−22ws′′′Δt2(2s¨i−s¨i+1−s¨i−1)+2ws′′′Δt22(s¨n−1−s¨n−2)n⋅wsoftn⋅wsoft

所以，目标函数的梯度为
∂ f ∂ X = 2 ∑ i = 0 n − 1 w s − r e f ( s i − s − r e f i ) + 2 ∑ i = 0 n − 1 w s ˙ − r e f ( s ˙ i − s ˙ − r e f ) + 2 ∑ i = 0 n − 1 w s ¨ s ¨ i − 2 w s ′ ′ ′ 2 ( s ¨ 1 − s ¨ 0 ) Δ t 2 + ∑ i = 1 n − 2 2 w s ′ ′ ′ ( 2 s ¨ i − s ¨ i + 1 − s ¨ i − 1 ) Δ t 2 + 2 w s ′ ′ ′ 2 ( s ¨ n − 1 − s ¨ n − 2 ) Δ t 2 + ∑ i = 0 n − 1 2 w l a t _ a c c s ˙ i 4 κ κ ˙ + ∑ i = 0 n − 1 4 w l a t _ a c c s ˙ i 3 κ 2 + 2 w t a r g e t − s ( s n − 1 − s t a r g e t ) + 2 w t a r g e t − s ˙ ( s ˙ n − 1 − s ˙ t a r g e t ) + 2 w t a r g e t − s ¨ ( s ¨ n − 1 − s ¨ t a r g e t ) + 2 n ⋅ w s o f t \begin{aligned} \frac{\partial f}{\partial X}=&2\sum_{i=0}^{n-1}w_{s-ref}(s_i-s_-ref_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}^{'''}}{\frac{2(\ddot s_1-\ddot s_0)}{\Delta t^2}}+\sum_{i=1}^{n-2}2w_{{s}^{'''}}{\frac{(2\ddot s_i-\ddot s_{i+1}-\ddot s_{i-1})}{\Delta t^2}}+2w_{{s}^{'''}}{\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{aligned} ∂X∂f=−+++2i=0∑n−1ws−ref(si−s−refi)+2i=0∑n−1ws˙−ref(s˙i−s˙−ref)+2i=0∑n−1ws¨s¨i2ws′′′Δt22(s¨1−s¨0)+i=1∑n−22ws′′′Δt2(2s¨i−s¨i+1−s¨i−1)+2ws′′′Δt22(s¨n−1−s¨n−2)i=0∑n−12wlat_accs˙i4κκ˙+i=0∑n−14wlat_accs˙i3κ22wtarget−s(sn−1−starget)+2wtarget−s˙(s˙n−1−s˙target)+2wtarget−s¨(s¨n−1−s¨target)2n⋅wsoft

6.eval_g()求解约束函数

cpp 复制代码

  /** 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 ) = [ s 1 − s 0 ⋮ s n − 1 − s n − 2 s ¨ 1 − s ¨ 0 Δ t ⋮ s ¨ n − 1 − s ¨ n − 2 Δ t s 1 − ( s 0 + s ˙ 0 Δ t + 1 2 s ¨ 0 Δ t 2 + 1 6 ( s ¨ 1 − s ¨ 0 Δ t ) Δ t 3 ) ⋮ s n − 1 − ( s n − 2 + s ˙ n − 2 Δ t + 1 2 s ¨ n − 2 Δ t 2 + 1 6 ( s ¨ n − 1 − s ¨ n − 2 Δ t ) Δ t 3 ) s ˙ 1 − ( s ˙ 0 + s ¨ 0 Δ t + 1 2 ( s ¨ 1 − s ¨ 0 Δ t ) Δ t 2 ) ⋮ s ˙ n − 1 − ( s ˙ n − 2 + s ¨ n − 2 Δ t + 1 2 ( s ¨ n − 1 − s ¨ n − 2 Δ t ) Δ t 2 ) s ˙ 0 − s p e e d _ l i m i t ( s 0 ) ⋮ s ˙ n − 1 − s p e e d _ l i m i t ( s n − 1 ) s 0 − s _ s o f t _ l o w e r 0 + s _ s l a c k _ l o w e r ⋮ s n − 1 − s _ s o f t _ l o w e r n − 1 + s _ s l a c k _ l o w e r s 0 − s _ s o f t _ u p p e r 0 − s _ s l a c k _ u p p e r ⋮ s n − 1 − s _ s o f t _ u p p e r n − 1 − s _ s l a c k _ u p p e r ] g(x) = \left[ {\begin{array}{ccccccccccccccc}{{s_1} - {s_0}}\\ \vdots \\{{s_{n - 1}} - {s_{n - 2}}}\\{\frac{{{{\ddot s}1} - {{\ddot s}0}}}{{\Delta t}}}\\ \vdots \\{\frac{{{{\ddot s}{n - 1}} - {{\ddot s}{n - 2}}}}{{\Delta t}}}\\{{s_1} - ({s_0} + {{\dot s}0}\Delta t{\rm{ + }}\frac{{\rm{1}}}{{\rm{2}}}{{\ddot s}0}\Delta {t^{\rm{2}}}{\rm{ + }}\frac{{\rm{1}}}{{\rm{6}}}(\frac{{{{\ddot s}1} - {{\ddot s}0}}}{{\Delta t}})\Delta {t^{\rm{3}}})}\\ \vdots \\{{s{n - 1}} - ({s{n - 2}} + {{\dot s}{n - 2}}\Delta t{\rm{ + }}\frac{{\rm{1}}}{{\rm{2}}}{{\ddot s}{n - 2}}\Delta {t^{\rm{2}}}{\rm{ + }}\frac{{\rm{1}}}{{\rm{6}}}(\frac{{{{\ddot s}{n - 1}} - {{\ddot s}{n - 2}}}}{{\Delta t}})\Delta {t^{\rm{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})}\\ \vdots \\{{{\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})}\\ \vdots \\{{{\dot s}{n - 1}} - speed\limit({s{n - 1}})}\\{{s_0} - s\_soft\_lowe{r_0} + s\_slack\lower}\\ \vdots \\{{s{n - 1}} - s\_soft\lowe{r{n - 1}} + s\_slack\_lower}\\{{s_0} - s\_soft\_uppe{r_0} - s\_slack\upper}\\ \vdots \\{{s{n - 1}} - s\_soft\uppe{r{n - 1}} - s\_slack\_upper}\end{array}} \right] g(x)= s1−s0⋮sn−1−sn−2Δts¨1−s¨0⋮Δts¨n−1−s¨n−2s1−(s0+s˙0Δt+21s¨0Δt2+61(Δts¨1−s¨0)Δt3)⋮sn−1−(sn−2+s˙n−2Δt+21s¨n−2Δt2+61(Δts¨n−1−s¨n−2)Δt3)s˙1−(s˙0+s¨0Δt+21(Δts¨1−s¨0)Δt2)⋮s˙n−1−(s˙n−2+s¨n−2Δt+21(Δts¨n−1−s¨n−2)Δt2)s˙0−speed_limit(s0)⋮s˙n−1−speed_limit(sn−1)s0−s_soft_lower0+s_slack_lower⋮sn−1−s_soft_lowern−1+s_slack_lowers0−s_soft_upper0−s_slack_upper⋮sn−1−s_soft_uppern−1−s_slack_upper

7.eval_jac_g()求解约束雅可比矩阵

cpp 复制代码

  /** 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 : R n → R n f:\mathbb{R}^n\to\mathbb{R}^n f:Rn→Rn 是一个可微函数，则 f f f 在点 x = ( x 1 , x 2 , ⋯ , x n ) T \boldsymbol{x}=(x_1,x_2,\cdots,x_n)^T x=(x1,x2,⋯,xn)T 处的雅可比矩阵为：

J = ( ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ⋯ ∂ f 1 ∂ x n ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ⋯ ∂ f 2 ∂ x n ⋮ ⋮ ⋱ ⋮ ∂ f n ∂ x 1 ∂ f n ∂ x 2 ⋯ ∂ f n ∂ x n ) \boldsymbol{J}=\begin{pmatrix} \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} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial x_1} & \frac{\partial f_n}{\partial x_2} & \cdots & \frac{\partial f_n}{\partial x_n} \end{pmatrix} J= ∂x1∂f1∂x1∂f2⋮∂x1∂fn∂x2∂f1∂x2∂f2⋮∂x2∂fn⋯⋯⋱⋯∂xn∂f1∂xn∂f2⋮∂xn∂fn

其中， f i f_i fi 表示 f f f 的第 i i i 个分量函数， ∂ f i ∂ x j \frac{\partial f_i}{\partial x_j} ∂xj∂fi 表示 f i f_i fi 对第 j j j 个自变量 x j x_j xj 的偏导数。

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

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

单调性约束： g 1 = s i + 1 − s i g_1=s_{i+1}-s_i g1=si+1−si ∂ g 1 ∂ s i = − 1 , ∂ g 1 ∂ s i + 1 = 1 \frac{\partial g_1}{\partial s_i}=-1,\frac{\partial g_1}{\partial s_{i+1}}=1 ∂si∂g1=−1,∂si+1∂g1=1
加加速度jerk约束: g 2 = s ¨ i + 1 − s ¨ i Δ t g_2 = \frac {\ddot s_{i+1}-\ddot s_i}{\Delta t} g2=Δts¨i+1−s¨i ∂ g 2 ∂ s i = − 1 Δ t , ∂ g 2 ∂ s i + 1 = 1 Δ t \frac{\partial g_2}{\partial s_i}=-\frac{1}{\Delta t},\frac{\partial g_2}{\partial s_{i+1}}=\frac{1}{\Delta t} ∂si∂g2=−Δt1,∂si+1∂g2=Δt1
位置等式约束： g 3 = s i + 1 − ( s i + s i ′ ∗ Δ t + 1 3 ∗ s i ′ ′ ∗ Δ t 2 + 1 6 ∗ s i + 1 ′ ′ ∗ Δ t 2 ) g_3=s_{i+1}-(s_i+s_i^{\prime}*\Delta t+\frac13*s_i^{\prime\prime}*\Delta t^2+\frac16*s_{i+1}^{\prime\prime}*\Delta t^2) g3=si+1−(si+si′∗Δt+31∗si′′∗Δt2+61∗si+1′′∗Δt2)
∂ g 3 ∂ s i = − 1 ∂ g 3 ∂ s i + 1 = 1 ∂ g 3 ∂ s ˙ i = − Δ t ∂ g 3 ∂ s ¨ i = − Δ t 2 3 ∂ g 3 ∂ s ¨ i + 1 = − Δ t 2 6 \begin{gathered} \begin{aligned}\frac{\partial g_3}{\partial s_i}=-1\end{aligned} \\ \frac{\partial g_3}{\partial s_{i+1}}=1 \\ \begin{aligned}\frac{\partial g_3}{\partial \dot s_i}&=-\Delta t\end{aligned} \\ \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{gathered} ∂si∂g3=−1∂si+1∂g3=1∂s˙i∂g3=−Δt∂s¨i∂g3=−3Δt2∂s¨i+1∂g3=−6Δt2
速度等式约束： g 4 = s i + 1 ′ − ( s i ′ + 1 2 ∗ s i ′ ′ ∗ Δ t + 1 2 ∗ s i + 1 ′ ′ ∗ Δ t ) g_4=s_{i+1}^{\prime}-(s_i^{\prime}+\frac12*s_i^{\prime\prime}*\Delta t+\frac12*s_{i+1}^{\prime\prime}*\Delta t) g4=si+1′−(si′+21∗si′′∗Δt+21∗si+1′′∗Δt)
∂ g 4 ∂ s ˙ i = − 1 ∂ g 4 ∂ s ˙ i + 1 = 1 ∂ g 4 ∂ s ¨ i = − Δ t 2 ∂ g 4 ∂ s ¨ i + 1 = − Δ t 2 \begin{gathered} \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{gathered} ∂s˙i∂g4=−1∂s˙i+1∂g4=1∂s¨i∂g4=−2Δt∂s¨i+1∂g4=−2Δt
速度限制约束： g 5 = s ˙ i − s p e e d _ l i m i t ( s i ) g_5={{{\dot s}_i} - speed\limit({s_i})} g5=s˙i−speed_limit(si)
∂ g 5 ∂ s ˙ i = 1 , ∂ g 5 ∂ s i = − d s p e e d _ l i m i t ( s i ) d s i \frac{\partial g_5}{\partial \dot s_i}=1,\frac{\partial g_5}{\partial s{i}}=-\frac{d speed\_limit({s_i})}{ds_i} ∂s˙i∂g5=1,∂si∂g5=−dsidspeed_limit(si)
软约束lower: g 6 = s i − s _ s o f t _ l o w e r i + s _ s l a c k _ l o w e r i g_6=s_i-s\_soft\_lower_i+s\_slack\_lower_i g6=si−s_soft_loweri+s_slack_loweri
∂ g 6 ∂ s i = 1 , ∂ g 6 ∂ s _ s l a c k _ l o w e r i = 1 \frac{\partial g_6}{\partial s_i}=1,\frac{\partial g_6}{\partial s\_slack\_lower_i}=1 ∂si∂g6=1,∂s_slack_loweri∂g6=1
软约束upper: g 7 = s i − s _ s o f t _ u p p e r i − s _ s l a c k _ u p p e r i g_7=s_i-s\_soft\_upper_i-s\_slack\_upper_i g7=si−s_soft_upperi−s_slack_upperi
∂ g 7 ∂ s i = 1 , ∂ g 7 ∂ s _ s l a c k _ u p p e r i = − 1 \frac{\partial g_7}{\partial s_i}=1,\frac{\partial g_7}{\partial s\_slack\_upper_i}=-1 ∂si∂g7=1,∂s_slack_upperi∂g7=−1

雅可比矩阵(维度:[4(N-1)+3N]*5N)如下：
[ − 1 1 ⋱ ⋱ − 1 1 − 1 Δ t 1 Δ t ⋱ ⋱ − 1 Δ t 1 Δ t − 1 1 ⋱ ⋱ − 1 1 − Δ t ⋱ − Δ t − Δ t 2 3 − Δ t 2 6 ⋱ ⋱ − Δ t 2 3 − Δ t 2 6 − 1 1 ⋱ ⋱ − 1 1 − Δ t 2 − Δ t 2 ⋱ ⋱ − Δ t 2 − Δ t 2 − v _ f ( s i ) ′ ⋱ − v _ f ( s i ) ′ 1 ⋱ 1 1 ⋱ 1 1 ⋱ 1 1 ⋱ 1 − 1 ⋱ − 1 ] \left[ {\begin{array}{ccccccccccccccc}{\begin{array}{ccccccccccccccc}{ - 1}&1&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - 1}&1\end{array}}&{}&{}&{}&{}\\{}&{}&{\begin{array}{ccccccccccccccc}{ - \frac{1}{{\Delta t}}}&{\frac{1}{{\Delta t}}}&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - \frac{1}{{\Delta t}}}&{\frac{1}{{\Delta t}}}\end{array}}&{}&{}\\{\begin{array}{ccccccccccccccc}{ - 1}&1&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - 1}&1\end{array}}&{\begin{array}{ccccccccccccccc}{ - \Delta t}&{}&{}\\{}& \ddots &{}\\{}&{}&{ - \Delta t}\end{array}}&{\begin{array}{ccccccccccccccc}{ - \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}{ccccccccccccccc}{ - 1}&1&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - 1}&1\end{array}}&{\begin{array}{ccccccccccccccc}{ - \frac{{\Delta t}}{2}}&{ - \frac{{\Delta t}}{2}}&{}&{}\\{}& \ddots & \ddots &{}\\{}&{}&{ - \frac{{\Delta t}}{2}}&{ - \frac{{\Delta t}}{2}}\end{array}}&{}&{}\\{\begin{array}{ccccccccccccccc}{ - v\_f({s_i})'}&{}&{}\\{}& \ddots &{}\\{}&{}&{ - v\_f({s_i})'}\end{array}}&{\begin{array}{ccccccccccccccc}1&{}&{}\\{}& \ddots &{}\\{}&{}&1\end{array}}&{}&{}&{}\\{\begin{array}{ccccccccccccccc}1&{}&{}\\{}& \ddots &{}\\{}&{}&1\end{array}}&{}&{}&{\begin{array}{ccccccccccccccc}1&{}&{}\\{}& \ddots &{}\\{}&{}&1\end{array}}&{}\\{\begin{array}{ccccccccccccccc}1&{}&{}\\{}& \ddots &{}\\{}&{}&1\end{array}}&{}&{}&{}&{\begin{array}{ccccccccccccccc}{ - 1}&{}&{}\\{}& \ddots &{}\\{}&{}&{ - 1}\end{array}}\end{array}} \right] −11⋱⋱−11−11⋱⋱−11−v_f(si)′⋱−v_f(si)′1⋱11⋱1−Δt⋱−Δt−11⋱⋱−111⋱1−Δt1Δt1⋱⋱−Δt1Δt1−3Δt2−6Δt2⋱⋱−3Δt2−6Δt2−2Δt−2Δt⋱⋱−2Δt−2Δt1⋱1−1⋱−1

8.eval_h()求解黑塞矩阵

cpp 复制代码

  /** 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 = ( ∂ 2 f ∂ x 1 2 ∂ 2 f ∂ x 1 ∂ x 2 ⋯ ∂ 2 f ∂ x 1 ∂ x n ∂ 2 f ∂ x 2 ∂ x 1 ∂ 2 f ∂ x 2 2 ⋯ ∂ 2 f ∂ x 2 ∂ x n ⋮ ⋮ ⋱ ⋮ ∂ 2 f ∂ x n ∂ x 1 ∂ 2 f ∂ x n ∂ x 2 ⋯ ∂ 2 f ∂ x n 2 ) H = \begin{pmatrix} \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} \\ \vdots & \vdots & \ddots & \vdots \\ \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{pmatrix} H= ∂x12∂2f∂x2∂x1∂2f⋮∂xn∂x1∂2f∂x1∂x2∂2f∂x22∂2f⋮∂xn∂x2∂2f⋯⋯⋱⋯∂x1∂xn∂2f∂x2∂xn∂2f⋮∂xn2∂2f

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

目标函数的二阶偏导只有个别项是非0的：
∂ f ∂ s i 2 = 2 w s − r e f + 2 w l a t _ a c c [ s ˙ i 4 ⋅ ( κ ′ ( s i ) ) 2 + s ˙ i 4 ⋅ κ ( s i ) ⋅ κ ′ ′ ( s i ) ] + 2 w t a r g e t − s + 2 w t a r g e t − s ˙ + 2 w t a r g e t − s ¨ ∂ f ∂ s i ∂ s i ′ = 8 w l a t _ a c c s i ′ 3 ⋅ κ ( s i ) ⋅ κ ′ ( s i ) ∂ f ∂ s i ′ 2 = 12 w l a t _ a c c s i ′ 2 ⋅ κ 2 ( s i ) + 2 w s ˙ − r e f ∂ f ∂ s i ′ ′ 2 = 2 w a s i ′ 2 ⋅ κ 2 ( s i ) + 4 w j Δ t 2 + 2 w a ∂ f ∂ s i ′ ′ ∂ s i + 1 ′ ′ = − 2 w j Δ t 2 \begin{aligned} \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{aligned} ∂si2∂f∂si∂si′∂f∂si′2∂f∂si′′2∂f∂si′′∂si+1′′∂f=2ws−ref+2wlat_acc[s˙i4⋅(κ′(si))2+s˙i4⋅κ(si)⋅κ′′(si)]+2wtarget−s+2wtarget−s˙+2wtarget−s¨=8wlat_accsi′3⋅κ(si)⋅κ′(si)=12wlat_accsi′2⋅κ2(si)+2ws˙−ref=2wasi′2⋅κ2(si)+Δt24wj+2wa=−Δt22wj约束函数的二阶偏导数：
∂ g ∂ s i 2 = − s p e e d _ l i m i t ′ ′ ( s i ) \begin{aligned} &\frac{\partial g}{\partial{s_i}^2}=-speed\_limit^{\prime\prime}(s_i) \end{aligned} ∂si2∂g=−speed_limit′′(si)

9. finalize_solution()

cpp 复制代码

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