如何建设彩票网站,企业网站内容,潍坊专升本教育机构,企业网站内容如何搭建文章目录 TASK系列解析文章OptimizeByNLP1.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. f… 文章目录 TASK系列解析文章OptimizeByNLP1.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() 参考 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_DECIDERSPEED_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中求解问题所需要的一些函数并完成对这些函数的实现。
IPOPTInterior 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_{i0}^{n-1}w_{s-ref}(s_i-s_-ref_i)^2w_{\dot{s}-ref}(\dot{s}_i-\dot{s}_-ref)^2w_{\ddot{s}}\ddot{s}_i^2\sum_{i0}^{n-2}w_{{s}^{}}(\frac{\ddot{s}_{i1}-\ddot{s}_i}{\Delta t})^2\\\sum_{i0}^{n-1}w_{lat\_acc}lat\_acc_i^2w_{soft}s\_slack\_lower_iw_{soft}s\_slack\_upper_i\\w_{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})^2 \end{aligned} minfi0∑n−1ws−ref(si−s−refi)2ws˙−ref(s˙i−s˙−ref)2ws¨s¨i2i0∑n−2ws′′′(Δts¨i1−s¨i)2i0∑n−1wlat_acclat_acci2wsofts_slack_loweriwsofts_slack_upperiwtarget−s(sn−1−starget)2wtarget−s˙(s˙n−1−s˙target)2wtarget−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}} ∇xdef[∂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_{i0}^{n-1}w_{s-ref}(s_i-s_-ref_i)\sum_{i0}^{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_{i0}^{n-1}w_{\dot{s}-ref}(\dot{s}_i-\dot{s}_-ref)\sum_{i0}^{n-1}4w_{lat\_acc}{\dot s_i^3}{\kappa^2}\\ \frac{\partial f}{\partial \ddot s}2\sum_{i0}^{n-1}w_{\ddot{s}}\ddot{s}_i-2w_{{s}^{}}{\frac{2(\ddot s_1-\ddot s_0)}{\Delta t^2}}\sum_{i1}^{n-2}2w_{{s}^{}}{\frac{(2\ddot s_i-\ddot s_{i1}-\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∂f2i0∑n−1ws−ref(si−s−refi)i0∑n−12wlat_accs˙i4κκ˙2wtarget−s(sn−1−starget)2wtarget−s˙(s˙n−1−s˙target)2wtarget−s¨(s¨n−1−s¨target)2i0∑n−1ws˙−ref(s˙i−s˙−ref)i0∑n−14wlat_accs˙i3κ22i0∑n−1ws¨s¨i−2ws′′′Δt22(s¨1−s¨0)i1∑n−22ws′′′Δt2(2s¨i−s¨i1−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_{i0}^{n-1}w_{s-ref}(s_i-s_-ref_i)2\sum_{i0}^{n-1}w_{\dot{s}-ref}(\dot{s}_i-\dot{s}_-ref)2\sum_{i0}^{n-1}w_{\ddot{s}}\ddot{s}_i\\ -2w_{{s}^{}}{\frac{2(\ddot s_1-\ddot s_0)}{\Delta t^2}}\sum_{i1}^{n-2}2w_{{s}^{}}{\frac{(2\ddot s_i-\ddot s_{i1}-\ddot s_{i-1})}{\Delta t^2}}2w_{{s}^{}}{\frac{2(\ddot s_{n-1}-\ddot s_{n-2})}{\Delta t^2}}\\ \sum_{i0}^{n-1}2w_{lat\_acc}{\dot s_i^4}{\kappa}{\dot\kappa}\sum_{i0}^{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−2i0∑n−1ws−ref(si−s−refi)2i0∑n−1ws˙−ref(s˙i−s˙−ref)2i0∑n−1ws¨s¨i2ws′′′Δt22(s¨1−s¨0)i1∑n−22ws′′′Δt2(2s¨i−s¨i1−s¨i−1)2ws′′′Δt22(s¨n−1−s¨n−2)i0∑n−12wlat_accs˙i4κκ˙i0∑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−(s0s˙0Δt21s¨0Δt261(Δts¨1−s¨0)Δt3)⋮sn−1−(sn−2s˙n−2Δt21s¨n−2Δt261(Δts¨n−1−s¨n−2)Δt3)s˙1−(s˙0s¨0Δt21(Δts¨1−s¨0)Δt2)⋮s˙n−1−(s˙n−2s¨n−2Δt21(Δts¨n−1−s¨n−2)Δt2)s˙0−speed_limit(s0)⋮s˙n−1−speed_limit(sn−1)s0−s_soft_lower0s_slack_lower⋮sn−1−s_soft_lowern−1s_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_1s_{i1}-s_i g1si1−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_{i1}}1 ∂si∂g1−1,∂si1∂g11 加加速度jerk约束: g 2 s ¨ i 1 − s ¨ i Δ t g_2 \frac {\ddot s_{i1}-\ddot s_i}{\Delta t} g2Δts¨i1−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_{i1}}\frac{1}{\Delta t} ∂si∂g2−Δt1,∂si1∂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_3s_{i1}-(s_is_i^{\prime}*\Delta t\frac13*s_i^{\prime\prime}*\Delta t^2\frac16*s_{i1}^{\prime\prime}*\Delta t^2) g3si1−(sisi′∗Δt31∗si′′∗Δt261∗si1′′∗Δ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_{i1}}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}_{i1}}-\frac{\Delta t^2}6 \end{gathered} ∂si∂g3−1∂si1∂g31∂s˙i∂g3−Δt∂s¨i∂g3−3Δt2∂s¨i1∂g3−6Δt2 速度等式约束 g 4 s i 1 ′ − ( s i ′ 1 2 ∗ s i ′ ′ ∗ Δ t 1 2 ∗ s i 1 ′ ′ ∗ Δ t ) g_4s_{i1}^{\prime}-(s_i^{\prime}\frac12*s_i^{\prime\prime}*\Delta t\frac12*s_{i1}^{\prime\prime}*\Delta t) g4si1′−(si′21∗si′′∗Δt21∗si1′′∗Δ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}_{i1}}1 \\ \frac{\partial g_4}{\partial\ddot{s}_i}-\frac{\Delta t}2 \\ \frac{\partial g_4}{\partial\ddot{s}_{i1}}-\frac{\Delta t}2 \end{gathered} ∂s˙i∂g4−1∂s˙i1∂g41∂s¨i∂g4−2Δt∂s¨i1∂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})} g5s˙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∂g51,∂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_6s_i-s\_soft\_lower_is\_slack\_lower_i g6si−s_soft_loweris_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∂g61,∂s_slack_loweri∂g61 软约束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_7s_i-s\_soft\_upper_i-s\_slack\_upper_i g7si−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∂g71,∂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()求解黑塞矩阵 /** 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_{i1}^{\prime\prime}}-\frac{2w_j}{\Delta t^2}\end{aligned} ∂si2∂f∂si∂si′∂f∂si′2∂f∂si′′2∂f∂si′′∂si1′′∂f2ws−ref2wlat_acc[s˙i4⋅(κ′(si))2s˙i4⋅κ(si)⋅κ′′(si)]2wtarget−s2wtarget−s˙2wtarget−s¨8wlat_accsi′3⋅κ(si)⋅κ′(si)12wlat_accsi′2⋅κ2(si)2ws˙−ref2wasi′2⋅κ2(si)Δt24wj2wa−Δ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() /** 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以及起始的TrajectoryPointProcess. 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规划控制学习笔记
