【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(二)

文章目录

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

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()定义问题规模

/** 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 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()定义初值

  /** 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()求解目标函数

  /** 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()求解梯度

  /** 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()求解约束函数

  /** 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()求解约束雅可比矩阵

  /** 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。 # 参考 \[1\] [Planning Piecewise Jerk Nonlinear Speed Optimizer Introduction](https://daobook.github.io/apollo/docs/technical_documents/planning_piecewise_jerk_nonlinear_speed_optimizer.html) \[2\] [Planning 基于非线性规划的速度规划](https://zhuanlan.zhihu.com/p/457315646) \[3\] [Apollo星火计划学习笔记------Apollo速度规划算法原理与实践](https://blog.csdn.net/sinat_52032317/article/details/128456949) \[4\] [Apollo规划控制学习笔记](https://blog.csdn.net/qq_42027654/article/details/126453968?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522169337858016800215045907%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=169337858016800215045907&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-2-126453968-null-null.142%5Ev93%5EchatgptT3_2&utm_term=Apollo%E8%A7%84%E5%88%92%E6%8E%A7%E5%88%B6&spm=1018.2226.3001.4187)