目录
PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
get_optimization_results & finalize_solution
PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
piecewise_jerk_speed_optimizer速度优化器构造凸优化问题得到进行速度规划. 这里介绍非线性速度优化器piecewise_jerk_nonlinear_speed_optimizer.
先讲下为什么需要使用非线性速度规划， 如下图，基于二次规划的速度规划中，pi是曲率关于时间t的函数，但实际上路径的曲率是与s相关的。二次规划在原先动态规划出来的粗糙ST曲线上将关于s的曲率惩罚转化为关于t的曲率惩罚，如此，当二次规划曲线与动态规划曲线差别不大，规划出来基本一致；若规划差别大，则会差别很大。就如图所示，规划出来的区间差别较大。限速/曲率的函数是关于s的函数，而s是我们要求的优化量，只能通过动态规划进行转化，如此就会使得二次规划的速度约束不精确。为了使得限速更加精细，Apollo提出了一种基于非线性规划的速度规划方法, 限速/曲率函数是关于s的非线性函数。
这个task的主要功能是对上一步搜索出的速度轨迹进行平滑，从而得到一条平滑、舒适的速度轨迹，里面涉及到二次优化平滑及非线性优化。主要包含以下及部分：
利用二次优化进行速度点集平滑
//利用二次优化进行速度点集平滑，首先得到一个初始平滑的速度点集 //这里相当于做了一次PIECEWISE_JERK_SPEED_OPTIMIZER const auto qp_start = std::chrono::system_clock::now(); const auto qp_smooth_status = OptimizeByQP(speed_data, &distance, &velocity, &acceleration); const auto qp_end = std::chrono::system_clock::now(); std::chrono::duration<double> qp_diff = qp_end - qp_start; ADEBUG << "speed qp optimization takes " << qp_diff.count() * 1000.0 << " ms";
先利用二次优化的方法对速度点集进行初次平滑。一些优化问题中的参数设定如下：
delta_t：0.1 ;total_length：等于path的总长度; total_time：7.0
优化问题的代价函数构造方式如下：
优化变量为ST坐标系下的s值，s一阶导及s二阶导, s三阶导。可以看到，与路径规划的优化问题构造形式还是比较类似的，除了这里用到了s_ref作为s值的标准，s_ref是根据上一步生成的离散点集，用更小的单位（delta_t=0.1s）来进行线性差值。这样也就引入了一次项，因此利用osqp优化库来求解的时候，要将q向量进行赋值。
对于优化问题中的限制约束，这里和path的规划限制完全是一样的，在Apollo的代码中可以看到，两个task都用了优化问题基类下的CalculateAffineConstraint函数，因此两者的限制约束是完全一样的，只是对于变量的上下限值，变量的初始值有所出入。
void PiecewiseJerkProblem::CalculateAffineConstraint( std::vector<c_float>* A_data, std::vector<c_int>* A_indices, std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds, std::vector<c_float>* upper_bounds)
查看speedlimit是否可行
如果当前时刻的速度已经超越了在这个位置的道路限速，则会返回错误。只有在满足该条件下才能进行后面的非线性优化，否则直接输出上一步的二次优化结果。
bool PiecewiseJerkSpeedNonlinearOptimizer::CheckSpeedLimitFeasibility() { // a naive check on first point of speed limit static constexpr double kEpsilon = 1e-6; const double init_speed_limit = speed_limit_.GetSpeedLimitByS(s_init_); if (init_speed_limit + kEpsilon < s_dot_init_) { AERROR << "speed limit [" << init_speed_limit << "] lower than initial speed[" << s_dot_init_ << "]"; return false; } return true; }
对道路曲率进行平滑
对生成的path点的曲率值进行平滑，代价函数如下所示, 其中x_i表示道路点的曲率值，这一步曲率的平滑是为后面的速度非线性优化做铺垫。得到光滑的kappa-s curve, 优化变量是每个s处的kappa,kappa_d, kappa_dd，参考kappa_ref由path_data计算得出
使用了PiecewiseJerkPathProblem类(PIECEWISE_JERK_PATH_OPTIMIZER的主要类)，
//对道路曲率进行平滑 Status PiecewiseJerkSpeedNonlinearOptimizer::SmoothPathCurvature( const PathData& path_data) { // using piecewise_jerk_path to fit a curve of path kappa profile // TODO(Jinyun): move smooth configs to gflags // 得到光滑的kappa-s curve, 优化变量是每个s处的kappa,kappa_d, kappa_dd // 参考kappa_ref由path_data计算得出 const auto& cartesian_path = path_data.discretized_path(); const double delta_s = 0.5; std::vector<double> path_curvature; for (double path_s = cartesian_path.front().s(); path_s < cartesian_path.back().s() + delta_s; path_s += delta_s) { const auto& path_point = cartesian_path.Evaluate(path_s); path_curvature.push_back(path_point.kappa()); } const auto& path_init_point = cartesian_path.front(); std::array<double, 3> init_state = {path_init_point.kappa(), path_init_point.dkappa(), path_init_point.ddkappa()}; PiecewiseJerkPathProblem piecewise_jerk_problem(path_curvature.size(), delta_s, init_state); piecewise_jerk_problem.set_x_bounds(-1.0, 1.0); piecewise_jerk_problem.set_dx_bounds(-10.0, 10.0); piecewise_jerk_problem.set_ddx_bounds(-10.0, 10.0); piecewise_jerk_problem.set_dddx_bound(-10.0, 10.0); piecewise_jerk_problem.set_weight_x(0.0); piecewise_jerk_problem.set_weight_dx(10.0); piecewise_jerk_problem.set_weight_ddx(10.0); piecewise_jerk_problem.set_weight_dddx(10.0); //TODOLIU:这里为什么要把path_curvature当成x_ref塞进去 piecewise_jerk_problem.set_x_ref(10.0, std::move(path_curvature)); if (!piecewise_jerk_problem.Optimize(1000)) { const std::string msg = "Smoothing path curvature failed"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } std::vector<double> opt_x; std::vector<double> opt_dx; std::vector<double> opt_ddx; opt_x = piecewise_jerk_problem.opt_x(); opt_dx = piecewise_jerk_problem.opt_dx(); opt_ddx = piecewise_jerk_problem.opt_ddx(); PiecewiseJerkTrajectory1d smoothed_path_curvature( opt_x.front(), opt_dx.front(), opt_ddx.front()); for (size_t i = 1; i < opt_ddx.size(); ++i) { double j = (opt_ddx[i] - opt_ddx[i - 1]) / delta_s; smoothed_path_curvature.AppendSegment(j, delta_s); } smoothed_path_curvature_ = smoothed_path_curvature; return Status::OK(); }
对speedLimit进行平滑
对道路限速speedLimit进行平滑优化，相同的代价函数及限制约束条件。概括来讲，这一步和对道路曲率进行平滑的目的相同，都是尽可能将后面非线性速度优化的输入构造的尽可能平滑，从而生成较好的速度优化效果。
我们具体讨论一下为什么需要先进行限速平滑。限速的函数并非直接可以得到，它来自于告警地图限速，交通规则限速， 向心加速度限速，绕行障碍物限速，将所有的限速函数相加，得到下图的限速函数，很明显，该函数既不连续也不可导，所以需要对其进行平滑处理。
对于限速曲线的平滑，Apollo采样分段多项式进行平滑，之后采样二次规划的方式进行求解。限速曲线的目标函数如下， 如此，我们就有了连续且可导的限速曲线。
//对speedLimit进行平滑 Status PiecewiseJerkSpeedNonlinearOptimizer::SmoothSpeedLimit() { // using piecewise_jerk_path to fit a curve of speed_ref // TODO(Hongyi): move smooth configs to gflags double delta_s = 2.0; std::vector<double> speed_ref; for (int i = 0; i < 100; ++i) { double path_s = i * delta_s; double limit = speed_limit_.GetSpeedLimitByS(path_s); speed_ref.emplace_back(limit); } std::array<double, 3> init_state = {speed_ref[0], 0.0, 0.0}; PiecewiseJerkPathProblem piecewise_jerk_problem(speed_ref.size(), delta_s, init_state); piecewise_jerk_problem.set_x_bounds(0.0, 50.0); piecewise_jerk_problem.set_dx_bounds(-10.0, 10.0); piecewise_jerk_problem.set_ddx_bounds(-10.0, 10.0); piecewise_jerk_problem.set_dddx_bound(-10.0, 10.0); piecewise_jerk_problem.set_weight_x(0.0); piecewise_jerk_problem.set_weight_dx(10.0); piecewise_jerk_problem.set_weight_ddx(10.0); piecewise_jerk_problem.set_weight_dddx(10.0); piecewise_jerk_problem.set_x_ref(10.0, std::move(speed_ref)); if (!piecewise_jerk_problem.Optimize(4000)) { const std::string msg = "Smoothing speed limit failed"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } std::vector<double> opt_x; std::vector<double> opt_dx; std::vector<double> opt_ddx; opt_x = piecewise_jerk_problem.opt_x(); opt_dx = piecewise_jerk_problem.opt_dx(); opt_ddx = piecewise_jerk_problem.opt_ddx(); PiecewiseJerkTrajectory1d smoothed_speed_limit(opt_x.front(), opt_dx.front(), opt_ddx.front()); for (size_t i = 1; i < opt_ddx.size(); ++i) { double j = (opt_ddx[i] - opt_ddx[i - 1]) / delta_s; smoothed_speed_limit.AppendSegment(j, delta_s); } smoothed_speed_limit_ = smoothed_speed_limit; return Status::OK(); }
利用IPOPT进行速度非线性优化
由函数PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByNLP实现。
Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByNLP( std::vector<double>* distance, std::vector<double>* velocity, std::vector<double>* acceleration)
参数配置
default_task_config: { task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER piecewise_jerk_nonlinear_speed_optimizer_config { acc_weight: 2.0 jerk_weight: 3.0 lat_acc_weight: 1000.0 s_potential_weight: 0.05 ref_v_weight: 5.0 ref_s_weight: 100.0 soft_s_bound_weight: 1e6 use_warm_start: true } }
基于非线性规划的速度规划步骤与之前规划步骤基本一致， 采样方式为等间隔的时间采样。slower 与supper为松弛变量，防止求解失败。
构造非线性优化器实例
这里new了一个PiecewiseJerkSpeedNonlinearIpoptInterface类的对象实例，这个类继承了IPOPT库，通过这个接口类，可以将规划问题的代价函数、限制条件等转成IPOPT库求解时所需要的形式。这一步通过代码可以看出将规划问题的初始参数全部传入进去，包括规划初始点信息、优化问题的维度、单位、优化变量的上下限值等等。
// Set optimizer instance auto ptr_interface = new PiecewiseJerkSpeedNonlinearIpoptInterface( s_init_, s_dot_init_, s_ddot_init_, delta_t_, num_of_knots_, total_length_, s_dot_max_, s_ddot_min_, s_ddot_max_, s_dddot_min_, s_dddot_max_);
设置s的上下界范围
将st_graph_data处理过后的s_boundary赋值进接口类的对象中，
ptr_interface->set_safety_bounds(s_bounds_);
设置path_curvature 和speed_limit
传入平滑后的path_curvature曲线和speed_limit曲线,
ptr_interface->set_curvature_curve(smoothed_path_curvature_); ptr_interface->set_speed_limit_curve(smoothed_speed_limit_);
设置warm_start
这一步warm_start设置的就是之前QP优化后的变量结果，因此NLP是基于QP后的结果再进行的一次优化
// TODO(Jinyun): refactor warms start setting API if (config.use_warm_start()) { const auto& warm_start_distance = *distance; const auto& warm_start_velocity = *velocity; const auto& warm_start_acceleration = *acceleration; if (warm_start_distance.empty() || warm_start_velocity.empty() || warm_start_acceleration.empty() || warm_start_distance.size() != warm_start_velocity.size() || warm_start_velocity.size() != warm_start_acceleration.size()) { const std::string msg = "Piecewise jerk speed nonlinear optimizer warm start invalid!"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } std::vector<std::vector<double>> warm_start; std::size_t size = warm_start_distance.size(); for (std::size_t i = 0; i < size; ++i) { warm_start.emplace_back(std::initializer_list<double>( {warm_start_distance[i], warm_start_velocity[i], warm_start_acceleration[i]})); } ptr_interface->set_warm_start(warm_start); }设置s_reference
这一步设置优化变量s的参考值，可以使用QP优化出来的distance向量.， 也可以使用默认值
if (FLAGS_use_smoothed_dp_guide_line) { ptr_interface->set_reference_spatial_distance(*distance); // TODO(Jinyun): move to confs ptr_interface->set_w_reference_spatial_distance(10.0); } else { std::vector<double> spatial_potantial(num_of_knots_, total_length_); ptr_interface->set_reference_spatial_distance(spatial_potantial); ptr_interface->set_w_reference_spatial_distance( config.s_potential_weight()); }
设置权重
设置优化变量的权重值，包括加速度、加加速度、向心加速度权重。设置参考速度及参考速度权重，这边的参考速度直接设为了cruise_speed。
ptr_interface->set_w_overall_a(config.acc_weight()); ptr_interface->set_w_overall_j(config.jerk_weight()); ptr_interface->set_w_overall_centripetal_acc(config.lat_acc_weight()); ptr_interface->set_reference_speed(cruise_speed_); ptr_interface->set_w_reference_speed(config.ref_v_weight());
构造IPOPT优化问题
构造接口类的智能指针来存放优化问题的具体形式 ，并调用IPOPT进行求解. 将构造好的接口类指针ptr_interface传入problem中，再调用OptimizeTNLP对优化问题进行求解。
Ipopt::SmartPtr<Ipopt::TNLP> problem = ptr_interface; Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory(); app->Options()->SetIntegerValue("print_level", 0); app->Options()->SetIntegerValue("max_iter", 1000); Ipopt::ApplicationReturnStatus status = app->Initialize(); if (status != Ipopt::Solve_Succeeded) { const std::string msg = "Piecewise jerk speed nonlinear optimizer failed during initialization"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } const auto start_timestamp = std::chrono::system_clock::now(); status = app->OptimizeTNLP(problem);
ptr_interface构造优化问题
Overview
对于此问题， 我们根据代码将问题用公式写出， 并使用ipopt的调用方式进行展示。
求解问题：
PiecewiseJerkSpeedNonlinearIpoptInterface() 原理解析. 介绍ptr_interface中对于优化问题是如何构造的， 将优化问题的数学表达式写出来，看对于非线性速度优化，Apollo是如何设计优化问题的。
-
代价函数：
-
代价函数与QpSpeedOptimizer中的代价函数很类似，只是多了关于向心加速度的最后一项，因此这一步其实是在考虑向心加速度的基础上对QP的结果又一次平滑。
f(s→)=wref−s⋅∑i=0n−1(si−s−refi)2+wref−v⋅∑i=0n−1(s˙i−v−ref)2+wa⋅∑i=0n−1s¨i2+wj⋅∑i=0n−2(s¨i+1−s¨ideltat)2+wc−a⋅∑i=0n−1(s˙i⋅s˙i∙ kappa i)2+wtar_s(xnp−1−xtar)2+wtar_v(x˙2∗np−2−vtar)2+wtar_a(x¨3∗np−3−atar)2+wsoft_s_low∑i=0n−1slow_slack+wsoft_s_upper∑i=0n−1supper_slack
-
约束条件：
-
约束条件也与QP中的约束十分相似，唯一不同之处在于对于speed_limit的非线性约束。到这，我们可以知道为什么在进行非线性优化这步之前，需要对path的曲率以及speed_limit曲线进行平滑了，因为在这里都得用上它们平滑后的结果，曲率和限速越平滑，优化出的速度曲线也就越平滑。其起始点约束是通过待优化变量的上下限实现的。
-
vmax∗dt>=si+1−si>=0.0, np−1 s monotone constraints
-
|s¨i+1−s¨i|/dt<=s⃛max, np−1 jerk bound constraint
-
si+1−si−s˙i∗dt−13∗s¨i∗dt2−16∗s¨i+1∗dt2=0， np−1 position equality constraints
-
s˙i+1−s˙i−12∗dt∗s¨i−12∗dt∗s¨i+1=0， np−1 velocity equality constraints
-
s˙i−v_bound_func_(si)<=0.0，，np−1speed limit constraints
-
si−sisoft_low_slack+silow_slack>=0.0, np−1 soft safety low boundary constraints
-
si−sisoft_upper_slack+siupper_slack>=0.0, np−1 soft safety upper boundary constraints
定义点的数目num_of_points为np.
get_nlp_info
bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_nlp_info( int &n, int &m, int &nnz_jac_g, int &nnz_h_lag, IndexStyleEnum &index_style)
-
待优化变量n=5∗np
-
x=[s0,s1,...vnp−1,|v0,s1,...vnp−1,|a0,a1,...anp−1,|s0low_slack,s1low_slack,...snp−1low_slack,|s0low_slack,s1low_slack,...snp−1low_slack]T
-
n=5∗np ,即每个时间dt处的 s,s˙,s¨,slow_slack,supper_slack .
-
约束数量 m=7(np−1)
-
s的单调性约束 np−1个
-
vmax∗dt>=si+1−si>=0.0, np−1 s monotone constraints
-
jerk的上下界约束 np−1个
-
|s¨i+1−s¨i|/dt<=s⃛max, np−1 jerk bound constraint
-
位置相等约束，即位置连续约束 np−1个
-
si+1−si−s˙i∗dt−13∗s¨i∗dt2−16∗s¨i+1∗dt2=0， np−1 position equality constraints
-
速度相等约束, np−1个
-
s˙i+1−s˙i−12∗dt∗s¨i−12∗dt∗s¨i+1=0， np−1 velocity equality constraints
-
speed_limit约束，np−1个
-
s˙i−v_bound_func_(si)<=0.0，，np−1speed limit constraints
-
s上下界松弛约束，2(np−1)个
-
si−sisoft_low_slack+silow_slack>=0.0, np−1 soft safety low boundary constraints
-
si−sisoft_upper_slack+siupper_slack>=0.0, np−1 soft safety upper boundary constraints
-
雅可比矩阵非0项数量 nnz_jac_g=19(np−1)
-
s的单调性约束产生的非零元素 2(np−1)个
-
jerk的上下界约束产生的非零元素 2(np−1)个
-
位置相等约束，即位置连续约束产生的非零元素 5(np−1)个
-
速度相等约束产生的非零元素 4(np−1)个
-
speed_limit约束产生的非零元素 2(np−1)个
-
s上下界松弛约束产生的非零元素 4(np−1)个
-
黑塞矩阵非0项数量nnz_h_lag=5np−1
-
因为目标函数里面有5*n_p个待优化变量, 且每一项为为二次函数，求解二阶导后不为0.
get_bounds_info
bool PiecewiseJerkSpeedNonlinearIpoptInterface::get_bounds_info( int n, double *x_l, double *x_u, int m, double *g_l, double *g_u)
-
待优化变量的上下界
-
x_l<=x<=x_u
-
待优化变量s,s˙,s¨的上下边界
// bounds for variables
// s
x_l[0] = s_init_;
x_u[0] = s_init_;
for (int i = 1; i < num_of_points_; ++i) {
x_l[i] = safety_bounds_[i].first;
x_u[i] = safety_bounds_[i].second;
}
// s_dot
x_l[v_offset_] = s_dot_init_;
x_u[v_offset_] = s_dot_init_;
for (int i = 1; i < num_of_points_; ++i) {
x_l[v_offset_ + i] = 0.0;
x_u[voffset_ + i] = LARGE_VELOCITY_VALUE;
}
// s_ddot
x_l[a_offset_] = s_ddot_init_;
x_u[a_offset_] = s_ddot_init_;
for (int i = 1; i < num_of_points_; ++i) {
x_l[a_offset_ + i] = s_ddot_min_;
x_u[a_offset_ + i] = s_ddot_max_;
}
-
待优化变量slow_slack,supper_slack的上下边界
for (int i = 0; i < num_of_points_; ++i) {
x_l[lower_s_slack_offset_ + i] = 0.0;
x_u[lower_s_slack_offset_ + i] = INF;
}
// upper_s_slack
for (int i = 0; i < num_of_points_; ++i) {
x_l[upper_s_slack_offset_ + i] = 0.0;
x_u[upper_s_slack_offset_ + i] = INF;
-
约束函数的上下界
-
g_l<=g(x)<=g_u
-
s的单调性约束 np−1个
-
vmax∗dt>=si+1−si>=0.0, np−1 s monotone constraints
for (int i = 0; i + 1 < num_of_points_; ++i) {
g_l[i] = 0.0;
g_u[i] = LARGE_VELOCITY_VALUE * delta_t_;
}
-
jerk的上下界约束 np−1个
-
|s¨i+1−s¨i|/dt<=s⃛max, np−1 jerk bound constraint
int offset = num_of_points_ - 1;
for (int i = 0; i + 1 < num_of_points_; ++i) {
g_l[offset + i] = s_dddot_min_;
g_u[offset + i] = s_dddot_max_;
}
-
位置相等约束，即位置连续约束 np−1个
-
si+1−si−s˙i∗dt−13∗s¨i∗dt2−16∗s¨i+1∗dt2=0， np−1 position equality constraints
offset += num_of_points_ - 1;
for (int i = 0; i + 1 < num_of_points_; ++i) {
g_l[offset + i] = 0.0;
g_u[offset + i] = 0.0;
}
-
速度相等约束, np−1个
-
s˙i+1−s˙i−12∗dt∗s¨i−12∗dt∗s¨i+1=0， np−1 velocity equality constraints
offset += num_of_points_ - 1;
for (int i = 0; i + 1 < num_of_points_; ++i) {
g_l[offset + i] = 0.0;
g_u[offset + i] = 0.0;
}
-
speed_limit约束，np−1个
-
s˙i−v_bound_func_(si)<=0.0，，np−1speed limit constraints
offset += num_of_points_ - 1;
for (int i = 0; i < num_of_points_; ++i) {
g_l[offset + i] = -INF;
g_u[offset + i] = 0.0;
}
-
s上下界松弛约束，2(np−1)个
-
si−sisoft_low_slack+silow_slack>=0.0, np−1 soft safety low boundary constraints
-
si−sisoft_upper_slack+siupper_slack>=0.0, np−1 soft safety upper boundary constraints
offset += num_of_points_;
for (int i = 0; i < num_of_points_; ++i) {
g_l[offset + i] = 0.0;
g_u[offset + i] = INF;
}
// s_i - soft_upper_s_i - upper_slack_i <= 0.0
offset += num_of_points_;
for (int i = 0; i < num_of_points_; ++i) {
g_l[offset + i] = -INF;
g_u[offset + i] = 0.0;
}
get_starting_point
bool PiecewiseJerkSpeedNonlinearIpoptInterface::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)
方式1：
目前使用的初始值，如果x_warm_start_.empty() == false, xp,xv,xa使用warm start得到的值; xlowslack,xupperslack设置成0.
如果怎么办？如果x_warm_start_.empty()==true怎么办？
这个warm start的值是通过求解qp速度规划问题(task PIECEWISE_JERK_SPEED_OPTIMIZER)，得到符合约束的速度平滑曲线，作为非线性规划的初值.
方式2：
for (int i = 0; i < num_of_points_; ++i) { x[i] = std::min(5.0 * delta_t_ * i + s_init_, s_max_); } x[0] = s_init_; for (int i = 0; i < num_of_points_; ++i) { x[v_offset_ + i] = 5.0; } x[v_offset_] = s_dot_init_; for (int i = 0; i < num_of_points_; ++i) { x[a_offset_ + i] = 0.0; } x[a_offset_] = s_ddot_init_; if (use_soft_safety_bound_) { for (int i = 0; i < num_of_points_; ++i) { x[lower_s_slack_offset_ + i] = 0.0; x[upper_s_slack_offset_ + i] = 0.0; } }
eval_f
bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_f(int n, const double *x, bool new_x, double &obj_value)
目标函数
f(s→)=wref−s⋅∑i=0n−1(si−s−refi)2+wref−v⋅∑i=0n−1(s˙i−v−ref)2+wa⋅∑i=0n−1s¨i2+wj⋅∑i=0n−2(s¨i+1−s¨ideltat)2+wc−a⋅∑i=0n−1(s˙i⋅s˙i∙ kappa i)2+wtar_s(xnp−1−xtar)2+wtar_v(x˙2∗np−2−vtar)2+wtar_a(x¨3∗np−3−atar)2+wsoft_s_low∑i=0n−1slow_slack+wsoft_s_upper∑i=0n−1supper_slack
-
第一项尽量贴近sref
// difference between ref spatial distace
for (int i = 0; i < num_of_points_; ++i) {
double s_diff = x[i] - s_ref_[i];
obj_value += s_diff * s_diff * w_ref_s_;
}
-
第二项尽量贴近v_ref
// difference between ref speed
for (int i = 0; i < num_of_points_; ++i) {
double v_diff = x[v_offset_ + i] - v_ref_;
obj_value += v_diff * v_diff * w_ref_v_;
}
-
第三项加速度的errfort尽可能小
// acceleration obj.
for (int i = 0; i < num_of_points_; ++i) {
double a = x[a_offset_ + i];
obj_value += a * a * w_overall_a_;
}
-
第四项jerk的effort尽可能小
// jerk obj.
for (int i = 0; i + 1 < num_of_points_; ++i) {
double j = (x[a_offset_ + i + 1] - x[a_offset_ + i]) / delta_t_;
obj_value += j * j * w_overall_j_;
}
-
第五项横向加速度的effort尽可能小
// centripetal acceleration obj.
for (int i = 0; i < num_of_points_; ++i) {
double v = x[v_offset_ + i];
double s = x[i];
double kappa = curvature_curve_.Evaluate(0, s);
double a_lat = v * v * kappa;
obj_value += a_lat * a_lat * w_overall_centripetal_acc_;
}
-
第六项s, v, a终端约束(此处has_end_state_target_ == false，未使用该约束)
-
wtar_s(xnp−1−xtar)2+wtar_v(x˙2∗np−2−vtar)2+wtar_a(x¨3∗np−3−atar)2
double s_diff = x[num_of_points_ - 1] - s_target_;
obj_value += s_diff * s_diff * w_target_s_;
double v_diff = x[v_offset_ + num_of_points_ - 1] - v_target_;
obj_value += v_diff * v_diff * w_target_v_;
double a_diff = x[a_offset_ + num_of_points_ - 1] - a_target_;
obj_value += a_diff * a_diff * w_target_a_;
-
第七项使用软安全约束 (有无使用有无使用?)
-
wsoft_s_low∑i=0n−1slow_slack+wsoft_s_upper∑i=0n−1supper_slack
for (int i = 0; i < num_of_points_; ++i) {
obj_value += x[lower_s_slack_offset_ + i] * w_soft_s_bound_;
obj_value += x[upper_s_slack_offset_ + i] * w_soft_s_bound_;
}
eval_grad_f
目标函数
+[wtar_s(xnp−1−xtar)2+wtar_v(x˙2∗np−2−vtar)2+wtar_a(x¨3∗np−3−atar)2]
+[wsoft_s_low∑i=0n−1slow_slack+wsoft_s_upper∑i=0n−1supper_slack]
待优化变量
x=[s0,s1,...vnp−1,|v0,s1,...vnp−1,|a0,a1,...anp−1,|s0low_slack,s1low_slack,...snp−1low_slack,|s0low_slack,s1low_slack,...snp−1low_slack]T
对f(s)针对各个变量 s,s˙,s¨,slow_slack,supper_slack 求导. f(s)是一个5np∗1的行向量.
Δf=[∂fs[0],∂fs[1],…,∂fs[np−1],|∂fv[0],∂fv[1],…,∂fv[np−1],|∂fa[0],∂fa[1],…,∂fa[np−1]|∂fslow_slack[0],∂fslow_slack[1],…,∂fslow_slack[np−1],|∂fsupper_slack[0],∂fsupper_slack[1],…,∂fsupper_slack[np−1]]T
// ref. spatial distance objective for (int i = 0; i < num_of_points_; ++i) { auto s_diff = x[i] - s_ref_[i]; grad_f[i] += 2.0 * s_diff * w_ref_s_; } // ref. speed objective for (int i = 0; i < num_of_points_; ++i) { auto v_diff = x[v_offset_ + i] - v_ref_; grad_f[v_offset_ + i] += 2.0 * v_diff * w_ref_v_; } // jerk objective double c = 2.0 / (delta_t_ * delta_t_) * w_overall_j_; grad_f[a_offset_] += -c * (x[a_offset_ + 1] - x[a_offset_]); for (int i = 1; i + 1 < num_of_points_; ++i) { grad_f[a_offset_ + i] += c * (2.0 * x[a_offset_ + i] - x[a_offset_ + i + 1] - x[a_offset_ + i - 1]); } grad_f[a_offset_ + num_of_points_ - 1] += c * (x[a_offset_ + num_of_points_ - 1] - x[a_offset_ + num_of_points_ - 2]); // acceleration objective for (int i = 0; i < num_of_points_; ++i) { grad_f[a_offset_ + i] += 2.0 * x[a_offset_ + i] * w_overall_a_; } // centripetal acceleration objective for (int i = 0; i < num_of_points_; ++i) { double v = x[v_offset_ + i]; double v2 = v * v; double v3 = v2 * v; double v4 = v3 * v; double s = x[i]; double kappa = curvature_curve_.Evaluate(0, s); double kappa_dot = curvature_curve_.Evaluate(1, s); grad_f[i] += 2.0 * w_overall_centripetal_acc_ * v4 * kappa * kappa_dot; grad_f[v_offset_ + i] += 4.0 * w_overall_centripetal_acc_ * v3 * kappa * kappa; } if (has_end_state_target_) { double s_diff = x[num_of_points_ - 1] - s_target_; grad_f[num_of_points_ - 1] += 2.0 * s_diff * w_target_s_; double v_diff = x[v_offset_ + num_of_points_ - 1] - v_target_; grad_f[v_offset_ + num_of_points_ - 1] += 2.0 * v_diff * w_target_v_; double a_diff = x[a_offset_ + num_of_points_ - 1] - a_target_; grad_f[a_offset_ + num_of_points_ - 1] += 2.0 * a_diff * w_target_a_; } if (use_soft_safety_bound_) { for (int i = 0; i < num_of_points_; ++i) { grad_f[lower_s_slack_offset_ + i] += w_soft_s_bound_; grad_f[upper_s_slack_offset_ + i] += w_soft_s_bound_; } }
eval_g
bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_g(int n, const double *x, bool new_x, int m, double *g)
s的单调性约束 np−1个
vmax∗dt>=si+1−si>=0.0, np−1 s monotone constraints
g[i]=si+1−si,i∈[0,np−1]
// s monotone constraints s_i+1 - s_i for (int i = 0; i + 1 < num_of_points_; ++i) { g[i] = x[offset + i + 1] - x[i]; }
jerk的上下界约束 np−1个
|s¨i+1−s¨i|/dt<=s⃛max, np−1 jerk bound constraint
g[i]=(s¨i+1−s¨i)/dt,i∈[np,2np−1]
位置相等约束，即位置连续约束 np−1个
si+1−si−s˙i∗dt−13∗s¨i∗dt2−16∗s¨i+1∗dt2=0， np−1 position equality constraints
g[i]=si+1−si−s˙i∗dt−13∗s¨i∗dt2−16∗s¨i+1∗dt2,i∈[2np,3np−1]
速度相等约束, np−1个
s˙i+1−s˙i−12∗dt∗s¨i−12∗dt∗s¨i+1=0， np−1 velocity equality constraints
g[i]=s˙i+1−s˙i−12∗dt∗s¨i−12∗dt∗s¨i+1,i∈[3np,4np−1]
//位置相等约束，即位置连续约束 $n_p -1$个 //速度相等约束, $n_p -1$个 //speed_limit约束，$n_p -1$个 int coffset_jerk = offset; int coffset_position = coffset_jerk + num_of_points_ - 1; int coffset_velocity = coffset_position + num_of_points_ - 1; double t = delta_t_; double t2 = t * t; double t3 = t2 * t; for (int i = 0; i + 1 < num_of_points_; ++i) { double s0 = x[i]; double s1 = x[i + 1]; double v0 = x[v_offset_ + i]; double v1 = x[v_offset_ + i + 1]; double a0 = x[a_offset_ + i]; double a1 = x[a_offset_ + i + 1]; double j = (a1 - a0) / t; g[coffset_jerk + i] = j; g[coffset_position + i] = s1 - (s0 + v0 * t + 0.5 * a0 * t2 + 1.0 / 6.0 * j * t3); g[coffset_velocity + i] = v1 - (v0 + a0 * t + 0.5 * j * t2); }
speed_limit约束，np−1个
s˙i−v_bound_func_(si)<=0.0，，np−1speed limit constraints
g[i]=s˙i−v_bound_func_(si),i∈[4np,5np−1]
// speed limit constraints int coffset_speed_limit = offset; // s_dot_i - v_bound_func_(s_i) <= 0.0 for (int i = 0; i < num_of_points_; ++i) { double s = x[i]; double s_dot = x[v_offset_ + i]; g[coffset_speed_limit + i] = s_dot - v_bound_func_.Evaluate(0, s); }
s上下界松弛约束，2(np−1)个
si−sisoft_low_slack+silow_slack>=0.0, np−1 soft safety low boundary constraints
g[i]=si−sisoft_low_slack+silow_slack,i∈[5np,6np−1]
si−sisoft_upper_slack+siupper_slack>=0.0, np−1 soft safety upper boundary constraints
g[i]=si−sisoft_upper_slack+siupper_slack,i∈[6np,7np−1]
// soft safety boundary constraints int coffset_soft_lower_s = offset; // s_i - soft_lower_s_i + lower_slack_i >= 0.0 for (int i = 0; i < num_of_points_; ++i) { double s = x[i]; double lower_s_slack = x[lower_s_slack_offset_ + i]; g[coffset_soft_lower_s + i] = s - soft_safety_bounds_[i].first + lower_s_slack; } offset += num_of_points_; int coffset_soft_upper_s = offset; // s_i - soft_upper_s_i - upper_slack_i <= 0.0 for (int i = 0; i < num_of_points_; ++i) { double s = x[i]; double upper_s_slack = x[upper_s_slack_offset_ + i]; g[coffset_soft_upper_s + i] = s - soft_safety_bounds_[i].second - upper_s_slack; }
eval_jac_g
变量值：x 雅可比矩阵非0元素数量：nele_jac 雅可比矩阵值：values
bool PiecewiseJerkSpeedNonlinearIpoptInterface::eval_jac_g( int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values)
J=[∂g∂x1⋯∂g∂xn]=[∂g1∂x1⋯∂g1∂xn⋮⋱⋮∂gm∂x1⋯∂gm∂xnn]
J=[∂g1∂x,[∂g2∂x],...,[∂gm∂x]]
求解器通过稀疏矩阵（下面两个图是稀疏矩阵的一个例子）来保存值, 求解雅可比矩阵需要对约束函数进行求偏导.
s的单调性约束 np−1个 ∂g1∂x
vmax∗dt>=si+1−si>=0.0, np−1 s monotone constraints
g[i]=si+1−si,i∈[0,np−1] define g1
∂g1∂x 是 J的前np行， g1=[g1[0],g1[1],...g1[i],...,g1[np−1]]T以此为例， 写出雅克比矩阵，
∂g1∂x=[∂g1[0]x[0]=1∂g1[0]x[1]=−100000∂g1[1]x[0]=1∂g1[1]x[0]=−100000⋱000000∂g1[i]x[i]=1∂g1[i]x[i+1]=−100000⋱00000∂g1[np−2]x[np−2]=1∂g1[np−2]x[np−1]=−1]
//指定下标 // s monotone constraints s_i+1 - s_i for (int i = 0; i + 1 < num_of_points_; ++i) { // s_i iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = i; ++non_zero_index; // s_i+1 iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = i + 1; ++non_zero_index; ++constraint_index; } //给对应下标塞值 // s monotone constraints s_i+1 - s_i for (int i = 0; i + 1 < num_of_points_; ++i) { // s_i values[non_zero_index] = -1.0; ++non_zero_index; // s_i+1 values[non_zero_index] = 1.0; ++non_zero_index; }
jerk的上下界约束 np−1个g2
|s¨i+1−s¨i|/dt<=s⃛max, np−1 jerk bound constraint
g[i]=(s¨i+1−s¨i)/dt,i∈[np,2np−1]
∂g2∂x=[∂g2[0]x[0]=1dt∂g2[0]x[1]=−1dt00000∂g2[1]x[0]=1dt∂g2[1]x[0]=−1dt00000⋱000000∂g2[i]x[i]=1dt∂g2[i]x[i+1]=−1dt00000⋱00000∂g2[np−2]x[np−2]=1dt∂g2[np−2]x[np−1]=−1dt]
//指定下标 // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max for (int i = 0; i + 1 < num_of_points_; ++i) { // a_i iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = a_offset_ + i; ++non_zero_index; // a_i+1 iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = a_offset_ + i + 1; ++non_zero_index; ++constraint_index; } //给对应下标塞值 // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max for (int i = 0; i + 1 < num_of_points_; ++i) { // a_i values[non_zero_index] = -1.0 / delta_t_; ++non_zero_index; // a_i+1 values[non_zero_index] = 1.0 / delta_t_; ++non_zero_index; }
位置相等约束，即位置连续约束 np−1个
si+1−si−s˙i∗dt−13∗s¨i∗dt2−16∗s¨i+1∗dt2=0， np−1 position equality constraints
g[i]=si+1−si−s˙i∗dt−13∗s¨i∗dt2−16∗s¨i+1∗dt2,i∈[2np,3np−1]
对于∂g3∂x的第i行：
[0...∂g3[i]s[i]=−1∂g3[i]s[i+1]=1...0 | 0...∂g3[i]v[i]=−dt∂g3[i]v[i+1]=0...0| 0...∂g3[i]a[i]=−13dt2∂g3[i]a[i+1]=−16dt2...0|0...∂g3[i]slow_slack[i]=0∂g3[i]slow_slack[i+1]=0...0 | 0...∂g3[i]supper_slack[i]=0∂g3[i]supper_slack[1]=0...0 ]T
//指定下标 // position equality constraints for (int i = 0; i + 1 < num_of_points_; ++i) { // s_i iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = i; ++non_zero_index; // v_i iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = v_offset_ + i; ++non_zero_index; // a_i iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = a_offset_ + i; ++non_zero_index; // s_i+1 iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = i + 1; ++non_zero_index; // a_i+1 iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = a_offset_ + i + 1; ++non_zero_index; ++constraint_index; } //给对应下标塞值 // position equality constraints for (int i = 0; i + 1 < num_of_points_; ++i) { // s_i values[non_zero_index] = -1.0; ++non_zero_index; // v_i values[non_zero_index] = -delta_t_; ++non_zero_index; // a_i values[non_zero_index] = -1.0 / 3.0 * delta_t_ * delta_t_; ++non_zero_index; // s_i+1 values[non_zero_index] = 1.0; ++non_zero_index; // a_i+1 values[non_zero_index] = -1.0 / 6.0 * delta_t_ * delta_t_; ++non_zero_index; }
速度相等约束, np−1个
s˙i+1−s˙i−12∗dt∗s¨i−12∗dt∗s¨i+1=0， np−1 velocity equality constraints
g[i]=s˙i+1−s˙i−12∗dt∗s¨i−12∗dt∗s¨i+1,i∈[3np,4np−1]
对于∂g4∂x的第i行：
[0...∂g3[i]s[i]=0∂g3[i]s[i+1]=0...0 | 0...∂g3[i]v[i]=−1∂g3[i]v[i+1]=1...0| 0...∂g3[i]a[i]=12dt∂g3[i]a[i+1]=−12dt...0|0...∂g3[i]slow_slack[i]=0∂g3[i]slow_slack[i+1]=0...0 | 0...∂g3[i]supper_slack[i]=0∂g3[i]supper_slack[1]=0...0 ]T
//指定下标 // velocity equality constraints for (int i = 0; i + 1 < num_of_points_; ++i) { // v_i iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = v_offset_ + i; ++non_zero_index; // a_i iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = a_offset_ + i; ++non_zero_index; // v_i+1 iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = v_offset_ + i + 1; ++non_zero_index; // a_i+1 iRow[non_zero_index] = constraint_index; jCol[non_zero_index] = a_offset_ + i + 1; ++non_zero_index; ++constraint_index; } //给对应下标塞值 // velocity equality constraints for (int i = 0; i + 1 < num_of_points_; ++i) { // v_i values[non_zero_index] = -1.0; ++non_zero_index; // a_i values[non_zero_index] = -0.5 * delta_t_; ++non_zero_index; // v_i+1 values[non_zero_index] = 1.0; ++non_zero_index; // a_i+1 values[non_zero_index] = -0.5 * delta_t_; ++non_zero_index; }
speed_limit约束，np−1个
s˙i−v_bound_func_(si)<=0.0，，np−1speed limit constraints
g[i]=s˙i−v_bound_func_(si),i∈[4np,5np−1]
需要注意的是这里求解v_bound_func(si)的一阶导，
values[non_zero_index] = -1.0 * v_bound_func_.Evaluate(1, s);
s上下界松弛约束，2(np−1)个
si−sisoft_low_slack+silow_slack>=0.0, np−1 soft safety low boundary constraints
g[i]=si−sisoft_low_slack+silow_slack,i∈[5np,6np−1]
si−sisoft_upper_slack+siupper_slack>=0.0, np−1 soft safety upper boundary constraints
g[i]=si−sisoft_upper_slack+siupper_slack,i∈[6np,7np−1]
eval_h
• 变量值：·x· • 拉格朗日乘数：·lambda· • 黑塞矩阵值：·values· • 目标函数因数：·obj_factor·
bool PiecewiseJerkSpeedNonlinearIpoptInterface::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)
黑塞矩阵Hession Matrix
H=[∂2f∂x12∂2f∂x1∂x2⋯∂2f∂x1∂xn∂2f∂x2∂x1∂2f∂x22⋯∂2f∂x2∂xn⋮⋮⋱⋮∂2f∂xn∂x1∂2f∂xn∂x2⋯∂2f∂xn2]
拉格朗日函数：
Ipopt的拉格朗日黑塞矩阵：
目标函数
f(s→)=wref−s⋅∑i=0n−1(si−s−refi)2+wref−v⋅∑i=0n−1(s˙i−v−ref)2+wa⋅∑i=0n−1s¨i2+wj⋅∑i=0n−2(s¨i+1−s¨ideltat)2+wc−a⋅∑i=0n−1(s˙i⋅s˙i∙ kappa i)2+wtar_s(xnp−1−xtar)2+wtar_v(x˙2∗np−2−vtar)2+wtar_a(x¨3∗np−3−atar)2+wsoft_s_low∑i=0n−1slow_slack+wsoft_s_upper∑i=0n−1supper_slack
约束
vmax∗dt>=si+1−si>=0.0, np−1 s monotone constraints
|s¨i+1−s¨i|/dt<=s⃛max, np−1 jerk bound constraint
si+1−si−s˙i∗dt−13∗s¨i∗dt2−16∗s¨i+1∗dt2=0， np−1 position equality constraints
s˙i+1−s˙i−12∗dt∗s¨i−12∗dt∗s¨i+1=0， np−1 velocity equality constraints
s˙i−v_bound_func_(si)<=0.0，，np−1speed limit constraints
si−sisoft_low_slack+silow_slack>=0.0, np−1 soft safety low boundary constraints
si−sisoft_upper_slack+siupper_slack>=0.0, np−1 soft safety upper boundary constraints
目标函数的二阶偏导数(推导略)
∂2f∂si2=2wref_s+2wlat_acc[s˙i4⋅(kappa′(si))2+s˙i4⋅kappa(si)⋅kappa′′(si)]
∂2f∂si∂si′=8wlat_accsi′3⋅kappa(si)⋅kappa′(si)
∂2f∂si′2=12wlat_accsi′2⋅kappa2(si)+2wref−v
∂2f∂si′′2=2wjdt2+2wa
约束函数的二阶偏导
∂2g∂si2=−v_bound_func′′(si)
同样是稀疏矩阵的存储形式，按照行列映射的value index进行塞值，值得注意的是hessen矩阵只需要塞下三角矩阵，这个映射是通过hessian_mapper_使用一个unordered_map实现的。需要注意的是传入的kappa(si),kappa′(si),kappa′′(si),v_bound_func′′(si)并非显式表达式， 而是传入函数。
下面塞的是目标函数中对横向加速度产生的二阶导，需要乘上目标函数因数obj_factor
2wlat_acc[s˙i4⋅(kappa′(si))2+s˙i4⋅kappa(si)⋅kappa′′(si)]
8wlat_accsi′3⋅kappa(si)⋅kappa′(si)
12wlat_accsi′2⋅kappa2(si)+2wref−v
// speed by curvature objective for (int i = 0; i < num_of_points_; ++i) { auto s_index = i; auto v_index = v_offset_ + i; auto s = x[s_index]; auto v = x[v_index]; auto kappa = curvature_curve_.Evaluate(0, s); auto kappa_dot = curvature_curve_.Evaluate(1, s); auto kappa_ddot = curvature_curve_.Evaluate(2, s); auto v2 = v * v; auto v3 = v2 * v; auto v4 = v3 * v; auto h_s_s_obj = 2.0 * kappa_dot * kappa_dot * v4 + 2.0 * kappa * kappa_ddot * v4; auto h_s_s_index = hessian_mapper_[to_hash_key(s_index, s_index)]; values[h_s_s_index] += h_s_s_obj * w_overall_centripetal_acc_ * obj_factor; auto h_s_v_obj = 8.0 * kappa * kappa_dot * v3; auto h_s_v_index = hessian_mapper_[to_hash_key(s_index, v_index)]; values[h_s_v_index] += h_s_v_obj * w_overall_centripetal_acc_ * obj_factor; auto h_v_v_obj = 12.0 * kappa * kappa * v2; auto h_v_v_index = hessian_mapper_[to_hash_key(v_index, v_index)]; values[h_v_v_index] += h_v_v_obj * w_overall_centripetal_acc_ * obj_factor; }
下面塞的是目标函数中位置偏差项产生的二阶导，需要乘上目标函数因数obj_factor
2wref_s
// spatial distance reference objective for (int i = 0; i < num_of_points_; ++i) { auto h_s_s_index = hessian_mapper_[to_hash_key(i, i)]; values[h_s_s_index] += 2.0 * w_ref_s_ * obj_factor; }
下面塞约束函数产生的二阶导数，需要乘上对应位置的拉格朗日因子lambda[i]
−v_bound_func′′(si)
int lambda_offset = 4 * (num_of_points_ - 1); for (int i = 0; i < num_of_points_; ++i) { auto s_index = i; auto s = x[s_index]; auto v_bound_ddot = v_bound_func_.Evaluate(2, s); auto h_s_s_constr = -1.0 * v_bound_ddot; auto h_s_s_index = hessian_mapper_[to_hash_key(s_index, s_index)]; values[h_s_s_index] += h_s_s_constr * lambda[lambda_offset + i]; }
下面塞的是目标函数中位置偏差项产生的二阶导，需要乘上目标函数因数obj_factor
2wref_a
for (int i = 0; i < num_of_points_; ++i) { auto a_index = a_offset_ + i; auto h_index = hessian_mapper_[to_hash_key(a_index, a_index)]; values[h_index] += 2.0 * w_overall_a_ * obj_factor; }
下面塞的是jerk项产生的二阶导，
2wjdt2 , 但是为什么这里对三个位置进行了填充，不就一个位置吗？但是为什么这里对三个位置进行了填充，不就一个位置吗？
auto c = 2.0 / delta_t_ / delta_t_ * w_overall_j_ * obj_factor; for (int i = 0; i + 1 < num_of_points_; ++i) { auto a0_index = a_offset_ + i; auto a1_index = a_offset_ + i + 1; auto h_a0_a0_index = hessian_mapper_[to_hash_key(a0_index, a0_index)]; values[h_a0_a0_index] += c; auto h_a0_a1_index = hessian_mapper_[to_hash_key(a0_index, a1_index)]; values[h_a0_a1_index] += -c; auto h_a1_a1_index = hessian_mapper_[to_hash_key(a1_index, a1_index)]; values[h_a1_a1_index] += c; }
下面塞又速度项产生的二阶导，
2wref−v
for (int i = 0; i < num_of_points_; ++i) { auto v_index = i + v_offset_; auto key = to_hash_key(v_index, v_index); auto index = hessian_mapper_[key]; values[index] += 2.0 * w_ref_v_ * obj_factor; }
如果有终端约束， 则由cost终端约束产生的二阶导，
if (has_end_state_target_) { // target s auto s_end_index = num_of_points_ - 1; auto s_key = to_hash_key(s_end_index, s_end_index); auto s_index = hessian_mapper_[s_key]; values[s_index] += 2.0 * w_target_s_ * obj_factor; // target v auto v_end_index = 2 * num_of_points_ - 1; auto v_key = to_hash_key(v_end_index, v_end_index); auto v_index = hessian_mapper_[v_key]; values[v_index] += 2.0 * w_target_v_ * obj_factor; // target a auto a_end_index = 3 * num_of_points_ - 1; auto a_key = to_hash_key(a_end_index, a_end_index); auto a_index = hessian_mapper_[a_key]; values[a_index] += 2.0 * w_target_a_ * obj_factor; }
get_optimization_results & finalize_solution
得到最终结果，并打印求解中间信息。最终输出的是最优的是
[s[0], s[1] ...s[np-1], | v[0], v[1] ...v[np-1] |,a[0], a[1] ...a[np-1]]
void PiecewiseJerkSpeedNonlinearIpoptInterface::get_optimization_results( std::vector<double> *ptr_opt_s, std::vector<double> *ptr_opt_v, std::vector<double> *ptr_opt_a) { *ptr_opt_s = opt_s_; *ptr_opt_v = opt_v_; *ptr_opt_a = opt_a_; } void PiecewiseJerkSpeedNonlinearIpoptInterface::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) { opt_s_.clear(); opt_v_.clear(); opt_a_.clear(); for (int i = 0; i < num_of_points_; ++i) { double s = x[i]; double v = x[v_offset_ + i]; double a = x[a_offset_ + i]; opt_s_.push_back(s); opt_v_.push_back(v); opt_a_.push_back(a); } }
扫一扫
1225
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
