论坛 › 技术讨论 Technology Answers › Planning 基于非线性规划的速度规划

c0d3n4m 发表于 2022-1-29 11:50

Planning 基于非线性规划的速度规划

在上一篇文章基于二次规划的速度规划算法中，我们讲了如何利用二次规划（QP）算法将动态规划产生的st曲线平滑，生成最终的st曲线。在二次规划的目标函数中，我们看到了曲率对速度的惩罚。在曲率大的地方通过减小速度从而较小向心加速度，从而提高驾驶过程的舒适性。但是曲率是s的函数，而我们优化的变量正是s，所以在二次规划之前通过动态规划的粗轨迹提前计算出每个时间ti时的曲率，进而得到每个时间ti的曲率惩罚权重pi。这种方法如果粗轨迹和最终平滑后的轨迹接近，那么曲率的惩罚位置较为正确，如果平滑结果和粗轨迹相差较大，那么曲率惩罚就不会作用到曲率大的位置，产生相反的效果。
所以apollo在考虑向心加速度的约束，设计了一种基于非线性规划（NLP）的速度规划算法，并应用IPOPT进行求解。在参考线平滑的文章中介绍了IPOPT的求解方法。
按照IPOPT的求解过程，我们首先定义优化变量。
定义优化变量

优化变量的定义和二次规划的速度规划相似，包含了每个固定间隔时间点的位置、速度、加速度。除此之外非线性规划中如果打开了软约束FLAGS_use_soft_bound_in_nonlinear_speed_opt，还会有几个关于s软约束的松弛变量,总共5n个优化变量

https://www.zhihu.com/equation?tex=s_0%2Cs_1%2C%5Cdots%2Cs_%7Bn-1%7D%2C++%5Cdot+s_0%2C%5Cdot+s_1%2C%5Cdots%2C%5Cdot+s_%7Bn-1%7D%2C+%5Cddot+s_0%2C%5Cddot+s_1%2C%5Cdots%2C%5Cddot+s_%7Bn-1%7D%2C+s%5C_lower_0%2Cs%5C_lower_1%2C%5Cdots%2Cs%5C_lower_%7Bn-1%7D%2C+s%5C_upper_0%2Cs%5C_upper_1%2C%5Cdots%2Cs%5C_upper_%7Bn-1%7D
定义目标函数

接下来我们给出速度规划的目标函数：

https://www.zhihu.com/equation?tex=cost+%3D+%5Csum_%7Bi+%3D+0%7D%5E%7Bn-1%7Dw_%7Bref%7D%28s_i-s%5C_ref_i%29%5E2%2B+w_v%28%5Cdot+s_i-v%5C_ref%29%5E2%2Bw_a%5Cddot+s_i%5E2%2B+w_j%28%5Cfrac%7B%5Cddot+s_%7Bi%2B1%7D-%5Cddot+s_i%7D%7B+%5CDelta+t%7D%29%5E2%2B%5C%5C+w_%7Blat%5C_acc%7Dlat%5C_acc_i%5E2%2B%5C%5C+w_%7Bsoft%7Ds%5C_lower_i%2Bw_%7Bsoft%7Ds%5C_upper_i%5C%5C
其中s_lower_i和s_upper_i分别是s边界约束的松弛变量
s_ref_i和v_ref分别为位置和速度的参考值，位置参考值来自于动态规划求得的粗st曲线，速度参考值为planning中定义的巡航速度，由用户设置FLAGS_default_cruise_speed
lat_acc为向心加速度，其通过速度平方乘以曲率计算

https://www.zhihu.com/equation?tex=acc%5C_lat_i%3D%5Cdot+s_i%5E2%2Akappa%28s%29
问题就落到需要得到曲率与s的关系式。
曲率曲线平滑

在路径规划中我们也用的是二次规划（http://piecewise_jerk_path_optimizer.cc），得到的是s关于l的jerk恒定的分段多项式曲线。但是这个曲线s关于l是连续可导的，但是s关于曲率k是不可导的。ipopt中我们的目标函数是要一个二阶可导函数，所以在ipopt求解之前，apollo还对曲率关于s的曲线用二次规划做了分段多项式曲线的拟合，产生二阶可导的s关于曲率k的关系式，这里依然用PiecewiseJerkPathPr
oblem去做平滑拟合：
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
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);

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();
...
}
在速度曲线类文章中我们介绍了恒定加加速度曲线类，这里我们借用这个类保存曲率与s的曲线，这样可以利用类中的Evaluate()函数计算曲率的导数来计算目标函数梯度
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 - opt_ddx) / delta_s;
    smoothed_path_curvature.AppendSegment(j, delta_s);
}
smoothed_path_curvature_ = smoothed_path_curvature;
参考曲线平滑

apollo中还对动产规划产生的参考曲线做了平滑，作为NLP问题的迭代初始解和s的参考值，然而QP的速度规划中是直接用动态规划的解插值得到s的参考值，不知道这一步直接用动态规划解插值是否也可以？
从OptimizeByQP()中设置的权重来看，单纯是为了平滑
//QP的速度规划中是直接用动态规划的解插值
modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc：   
SpeedPoint sp;
    reference_speed_data.EvaluateByTime(curr_t, &sp);
    const double path_s = sp.s();
    x_ref.emplace_back(path_s);

Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByQP(
    SpeedData* const speed_data, std::vector<double>* distance,
    std::vector<double>* velocity, std::vector<double>* acceleration) {
std::array<double, 3> init_states = {s_init_, s_dot_init_, s_ddot_init_};
PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots_, delta_t_,
                                                   init_states);
piecewise_jerk_problem.set_dx_bounds(
      0.0, std::fmax(FLAGS_planning_upper_speed_limit, init_states));
piecewise_jerk_problem.set_ddx_bounds(s_ddot_min_, s_ddot_max_);
piecewise_jerk_problem.set_dddx_bound(s_dddot_min_, s_dddot_max_);
piecewise_jerk_problem.set_x_bounds(s_bounds_);

// TODO(Jinyun): parameter tunnings
const auto& config =
      config_.piecewise_jerk_nonlinear_speed_optimizer_config();
piecewise_jerk_problem.set_weight_x(0.0);
piecewise_jerk_problem.set_weight_dx(0.0);
piecewise_jerk_problem.set_weight_ddx(config.acc_weight());
piecewise_jerk_problem.set_weight_dddx(config.jerk_weight());

std::vector<double> x_ref;
for (int i = 0; i < num_of_knots_; ++i) {
    const double curr_t = i * delta_t_;
    // get path_s
    SpeedPoint sp;
    speed_data->EvaluateByTime(curr_t, &sp);
    x_ref.emplace_back(sp.s());
}
piecewise_jerk_problem.set_x_ref(config.ref_s_weight(), std::move(x_ref));

// Solve the problem
if (!piecewise_jerk_problem.Optimize()) {
    const std::string msg =
      "Speed Optimization by Quadratic Programming failed. "
      "st boundary is infeasible.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
}

*distance = piecewise_jerk_problem.opt_x();
*velocity = piecewise_jerk_problem.opt_dx();
*acceleration = piecewise_jerk_problem.opt_ddx();
return Status::OK();
}
约束定义

约束包含了车辆必须一直往前走，所以有

https://www.zhihu.com/equation?tex=s_%7Bi%2B1%7D%3Es_i
速度、jerk的约束(似乎少了对于加速度的约束？)：

https://www.zhihu.com/equation?tex=%5Cfrac%7B%5Cddot+s_%7Bi%2B1%7D-%5Cddot+s_i%7D%7B%5CDelta+t%7D%5Cleq+jerk_%7Bmax%7D%5C%5C+%5Cdot+s_i+%5Cleq+speed%5C_limit%28s_i%29
限速曲线平滑

在速度约束里，关于速度的约束也是和s有关的，速度边界来自于地图和speed_decider中，所以是不可导的。apollo这里的处理和曲率一样，先对速度约束进行分段多项式拟合产生PiecewiseJerkTrajectory1d曲线，再进行计算：
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};
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 - opt_ddx) / delta_s;
    smoothed_speed_limit.AppendSegment(j, delta_s);
}

smoothed_speed_limit_ = smoothed_speed_limit;

return Status::OK();
}
对于位置的软约束：

https://www.zhihu.com/equation?tex=s_i-s%5C_lower_i%2Bslack%5C_lower_i+%5Cgeq+0%5C%5C+s_i-s%5C_upper_i-slack%5C_upper_i+%5Cleq+0%5C%5C
这里简单说一下软约束的用处，通过加入松弛变量允许超出约束边界，但是超出约束的话会在目标函数中对松弛变量作出惩罚，主要避免约束过紧导致求不出来解。
约束边界主要来自于边界的类型：
Status PiecewiseJerkSpeedNonlinearOptimizer::SetUpStatesAndBounds(
    const PathData& path_data, const SpeedData& speed_data) {

// Set s boundary
if (FLAGS_use_soft_bound_in_nonlinear_speed_opt) {
    s_bounds_.clear();
    s_soft_bounds_.clear();
    // TODO(Jinyun): move to confs
    for (int i = 0; i < num_of_knots_; ++i) {
      double curr_t = i * delta_t_;
      double s_lower_bound = 0.0;
      double s_upper_bound = total_length_;
      double s_soft_lower_bound = 0.0;
      double s_soft_upper_bound = total_length_;
      for (const STBoundary* boundary : st_graph_data.st_boundaries()) {
      double s_lower = 0.0;
      double s_upper = 0.0;
      if (!boundary->GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
          continue;
      }
      SpeedPoint sp;
      switch (boundary->boundary_type()) {
          case STBoundary::BoundaryType::STOP:
          case STBoundary::BoundaryType::YIELD:
            s_upper_bound = std::fmin(s_upper_bound, s_upper);
            s_soft_upper_bound = std::fmin(s_soft_upper_bound, s_upper);
            break;
          case STBoundary::BoundaryType::FOLLOW:
            s_upper_bound =
                std::fmin(s_upper_bound, s_upper - FLAGS_follow_min_distance);
//这里用的是粗st曲线对应时间的速度定义边界，如果采用QP优化后的speed_data是不是更好呢？
            if (!speed_data.EvaluateByTime(curr_t, &sp)) {
            const std::string msg =
                  "rough speed profile estimation for soft follow fence failed";
            AERROR << msg;
            return Status(ErrorCode::PLANNING_ERROR, msg);
            }
            s_soft_upper_bound =
                std::fmin(s_soft_upper_bound,
                        s_upper - FLAGS_follow_min_distance -
                              std::min(7.0, FLAGS_follow_time_buffer * sp.v()));
            break;
          case STBoundary::BoundaryType::OVERTAKE:
            s_lower_bound = std::fmax(s_lower_bound, s_lower);
            s_soft_lower_bound = std::fmax(s_soft_lower_bound, s_lower + 10.0);
            break;
          default:
            break;
      }
      }
...
}
还有各个散点之间微分关系的约束，同二次规划一样：

https://www.zhihu.com/equation?tex=%5Cdddot+s_%7Bi-%3Ei%2B1%7D%3D%5Cfrac%7B%28%5Cddot+s_%7Bi%2B1%7D-%5Cddot+s_i%29%7D%7B%5CDelta+t%7D+%5C%5C+%5Cdot+s_%7Bi%2B1%7D%3D%5Cdot+s_i%2B%5Cfrac%7B%5CDelta+t+%5Cddot+s_%7Bi%7D%7D%7B2%7D%2B%5Cfrac%7B%5CDelta+t%5E2+%5Cdddot+s_%7Bi-%3Ei%2B1%7D%7D%7B2%7D+%5C%5C+s_%7Bi%2B1%7D%3Ds_i%2B%5CDelta+t+%5Cdot+s_i%2B%5Cfrac%7B%5CDelta+t%5E2%5Cddot+s_i%7D%7B3%7D%2B%5Cfrac%7B%5CDelta+t%5E3%5Cddot+s_%7Bi%2B1%7D%7D%7B6%7D
目标函数的梯度

目标函数对于s_i的偏导数如下，其中kappa函数为分段恒jerk多项式函数，所以它的导数dkappa也可以求得

https://www.zhihu.com/equation?tex=%5Cfrac%7B%5Cdelta+cost%7D%7Bds_i%7D%3D2%28w_%7Bref%7Ds_i-s%5C_ref_i%29%2B+2w_%7Blat%5C_acc%7D%5Cdot+s_i%5E4%5Ccdot+kappa%28s_i%29%5Ccdot+dkappa%28s_i%29
目标函数对于s'_i的偏导数：

https://www.zhihu.com/equation?tex=%5Cfrac%7B%5Cdelta+cost%7D%7Bd%5Cdot+s_i%7D%3D2w_%7Bv%7D%28%5Cdot+s_i-v%5C_ref_i%29%2B+4w_%7Blat%5C_acc%7D%5Cdot+s_i%5E3%5Ccdot+kappa%28s_i%29%5E2
目标函数对于s''_i的偏导数：

https://www.zhihu.com/equation?tex=%5Cfrac%7B%5Cdelta+cost%7D%7Bd%5Cddot+s_i%7D%3D-%5Cfrac%7B2w_%7Bj%7D%28%5Cddot+s_%7Bi%2B1%7D-%5Cddot+s_i%29%7D%7B%5CDelta+t%5E2%7D%2B2w_a%5Cddot+s_i
对于松弛变量的偏导数：

https://www.zhihu.com/equation?tex=%5Cfrac%7B%5Cdelta+cost%7D%7Bds%5C_lower_i%7D%3Dw_%7Bsoft%7D%5C%5C+%5Cfrac%7B%5Cdelta+cost%7D%7Bds%5C_upper_i%7D%3Dw_%7Bsoft%7D
约束的雅克比矩阵

对于速度约束：

https://www.zhihu.com/equation?tex=g_1%3D%5Cdot+s_i+-+speed%5C_limit%28s_i%29%5C%5C+%5Cfrac%7B%5Cdelta+g_1%7D%7Bd%5Cdot+s_i%7D%3D1%5C%5C+%5Cfrac%7B%5Cdelta+g_1%7D%7Bd+s_i%7D%3D-speed%5C_limit%28s_i%29++
对于位置软约束：

https://www.zhihu.com/equation?tex=g_2%3Ds_i-s%5C_lower_i%2Bslack%5C_lower_i+%5C%5C+%5Cfrac%7B%5Cdelta+g_2%7D%7Bd+s_i%7D%3D1%5C%5C+%5Cfrac%7B%5Cdelta+g_2%7D%7Bd+slack%5C_lower_i%7D%3D1+

https://www.zhihu.com/equation?tex=g_3%3Ds_i-s%5C_upper_i%2Bslack%5C_upper_i+%5C%5C+%5Cfrac%7B%5Cdelta+g_2%7D%7Bd+s_i%7D%3D1%5C%5C+%5Cfrac%7B%5Cdelta+g_2%7D%7Bd+slack%5C_upper_i%7D%3D-1+
对于jerk约束：

https://www.zhihu.com/equation?tex=g_3%3D%5Cfrac%7B%5Cddot+s_%7Bi%2B1%7D-%5Cddot+s_i%7D%7B%5CDelta+t%7D%5C%5C+%5Cfrac%7B%5Cdelta+g_3%7D%7Bd+%5Cddot+s_i%7D%3D%5Cfrac%7B-1%7D%7B%5CDelta+t%7D%5C%5C+%5Cfrac%7B%5Cdelta+g_3%7D%7Bd+%5Cddot+s_%7Bi%2B1%7D%7D%3D%5Cfrac%7B1%7D%7B%5CDelta+t%7D%5C%5C
微分关系的等式约束

https://www.zhihu.com/equation?tex=g_4%3D+s_%7Bi%2B1%7D-s_i-%5CDelta+t+%5Cdot+s_i-%5Cfrac%7B%5CDelta+t%5E2%5Cddot+s_i%7D%7B3%7D-%5Cfrac%7B%5CDelta+t%5E3%5Cddot+s_%7Bi%2B1%7D%7D%7B6%7D%5C%5C+%5Cfrac%7B%5Cdelta+g_4%7D%7Bd++s_i%7D%3D-1%5C%5C+%5Cfrac%7B%5Cdelta+g_4%7D%7Bd++s_%7Bi%2B1%7D%7D%3D1%5C%5C+%5Cfrac%7B%5Cdelta+g_4%7D%7Bd++dot+s_%7Bi%7D%7D%3D-%5CDelta+t%5C%5C+%5Cfrac%7B%5Cdelta+g_4%7D%7Bd+%5Cddot+s_%7Bi%7D%7D%3D-%5Cfrac%7B%5CDelta+t%5E2%7D%7B3%7D%5C%5C+%5Cfrac%7B%5Cdelta+g_4%7D%7Bd+%5Cddot+s_%7Bi%2B1%7D%7D%3D-%5Cfrac%7B%5CDelta+t%5E3%7D%7B6%7D%5C%5C

https://www.zhihu.com/equation?tex=g_5%3D+%5Cdot+s_%7Bi%2B1%7D-%5Cdot+s_i-%5CDelta+t+%5Cddot+s_%7Bi%7D-%5Cfrac%7B%5CDelta+t+%28%5Cddot+s_%7Bi%2B1%7D-%5Cddot+s_%7Bi%7D%29%7D%7B2%7D%5C%5C+%5Cfrac%7B%5Cdelta+g_5%7D%7Bd++%5Cdot+s_i%7D%3D-1%5C%5C+%5Cfrac%7B%5Cdelta+g_5%7D%7Bd++%5Cdot+s_%7Bi%2B1%7D%7D%3D1%5C%5C+%5Cfrac%7B%5Cdelta+g_5%7D%7Bd+%5Cddot+s_%7Bi%7D%7D%3D-+%5Cfrac%7B%5CDelta+t%7D%7B2%7D%5C%5C+%5Cfrac%7B%5Cdelta+g_5%7D%7Bd+%5Cddot+s_%7Bi%2B1%7D%7D%3D-%5Cfrac%7B%5CDelta+t%7D%7B2%7D%5C%5C
对于位置约束

https://www.zhihu.com/equation?tex=g_6%3Ds_%7Bi%2B1%7D-s_i%5C%5C+%5Cfrac%7B%5Cdelta+g_6%7D%7Bds_%7Bi%2B1%7D%7D%3D1%5C%5C+%5Cfrac%7B%5Cdelta+g_6%7D%7Bds_i%7D%3D-1
设定迭代初值

迭代初值采用的是动态规划计算的粗st曲线的位置、速度、加速度：
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, warm_start_velocity,
         warm_start_acceleration}));
    }
    ptr_interface->set_warm_start(warm_start);
}
仿真验证与结果分析

使用Dreamview进行仿真，PNC_Monitor来观测仿真结果。由于NLP的速度规划相对于QP的速度规划主要优化在于对向心加速度的约束更为精准，所以这里在sunnyvale中选取一个掉头路段进行测试：


分别用QP算法和NLP算法的速度规划对以上路段进行仿真，设置初始速度为3m/s。为了看出对曲率优化的效果，把位置s相对于轨迹曲率kappa的曲线也展式出来了。结果如下，其中SPEED_HEURISTIC_OPTIMIZER为动态规划产生的速度曲线：



QP算法速度优化结果



NLP算法速度优化结果

从二次规划算法的优化结果中，二次规划算法的减速过程并没有在弯内减速，而是在弯前减速，然后在弯中加速。这是由于动态规划的速度规划目标函数中没有考虑曲率的权重，所以其速度由于小于巡航速度就有了加速的过程。二次规划算法获得动态规划的粗st曲线后是由动态规划的时间t对应的曲率，所以如果二次规划产生减速曲线后就导致路径曲率约束的前移。
从NLP算法仿真结果中，在弯前由于低于巡航速度进行了一定的加速，弯中进行了减速，弯后重新加速，符合一般驾驶的预期。NLP是直接将路径s-关于曲率的关系直接加到目标函数的设计中，而不是像二次规划算法目标函数是t关于曲率的关系。
动态规划和二次规划算法的目标函数大家可以参考我之前写的文章

Doris232 发表于 2022-1-29 11:58

楼主有空可以说说这块的仿真验证怎么做吗？目前就是把Apollo里面record好的包拿来跑，请问如何就单独的planning进行仿真呢？

mypro334 发表于 2022-1-29 12:00

dreamview有一个sim control，可以做planning仿真

查看完整版本: Planning 基于非线性规划的速度规划