大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

EM Planner 中的QP Speed Optimizer 和 QP Path Optimizer 大同小异，在底层代码上都是依托于 Spline1dConstraint 和 AffineConstraint 2个类。文件路径为 apollo\modules\planning\math\smoothing_spline\spline_1d_constraint.cc 和 apollo\modules\planning\math\smoothing_spline\affine_constraint.cc。我在Baidu Apollo代码解析之EM Planner中的QP Path Optimizer 2 中已经介绍过了，这里还是原原本本的把代码贴一遍。一是细节有些微不同，二是我再读代码时有了新的理解。

首先，速度规划的调用入口在QpSplineStSpeedOptimizer::Process()中，QpSplineStSpeedOptimizer类继承自SpeedOptimizer类。主要思路仍然是对于要规划的目标曲线，按时间采样N段，每一段速度轨迹采用多项式拟合，整体需要满足设定的各项约束条件，求取代价最小的多项式参数。

Status QpSplineStSpeedOptimizer::Process(...) { ... constexpr double kBounadryEpsilon = 1e-2; for (auto boundary : st_graph_data.st_boundaries()) { //如果自车位置在障碍物占据范围内，即发生了碰撞，速度数据为空，不规划 if (boundary->IsPointInBoundary({0.0, 0.0}) || (std::fabs(boundary->min_t()) < kBounadryEpsilon && std::fabs(boundary->min_s()) < kBounadryEpsilon)) { speed_data->clear(); const std::string msg = "Collision found in QpSplineStSpeedOptimizer!"; return Status(ErrorCode::PLANNING_ERROR, msg); } } ... QpSplineStGraph st_graph( spline_solver_.get(), st_graph_data.path_length_by_conf(), st_graph_data.total_time_by_conf(), qp_st_speed_config_, veh_param, reference_line_info_->IsChangeLanePath()); std::pair<double, double> accel_bound = { qp_st_speed_config_.preferred_min_deceleration(), qp_st_speed_config_.preferred_max_acceleration()}; //速度规划进行了3次尝试，第2次相比第1次，放宽了加速度的上下限范围。 //第3次使用了QpPiecewiseStGraph auto ret = st_graph.Search(st_graph_data, accel_bound, reference_speed_data, speed_data); if (ret != Status::OK()) { ... accel_bound.first = qp_st_speed_config_.min_deceleration(); accel_bound.second = qp_st_speed_config_.max_acceleration(); ret = st_graph.Search(st_graph_data, accel_bound, reference_speed_data, speed_data); // backup plan: use piecewise_st_graph if (ret != Status::OK()) { ... QpPiecewiseStGraph piecewise_st_graph(...); ret = piecewise_st_graph.Search(st_graph_data, speed_data, accel_bound); ．．． } } ... }

规划的主体过程在QpSplineStGraph::Search()中，分为4个步骤：AddConstraint()，AddKernel()，Solve()，extract speed data。

讯享网Status QpSplineStGraph::Search(...) { ... // reset spline generator spline_solver_->Reset(t_knots_, qp_st_speed_config_.qp_spline_config().spline_order()); if (!AddConstraint(st_graph_data.init_point(), st_graph_data.speed_limit(), st_graph_data.st_boundaries(), accel_bound).ok()) { ... } if (!AddKernel(st_graph_data.st_boundaries(), st_graph_data.speed_limit()).ok()) { ...} if (!Solve().ok()) { ... } ... // extract output speed_data->clear(); const Spline1d& spline = spline_solver_->spline(); const double t_output_resolution = FLAGS_trajectory_time_min_interval; double time = 0.0; while (time < total_time_ + t_output_resolution) { double s = spline(time); double v = std::max(0.0, spline.Derivative(time)); double a = spline.SecondOrderDerivative(time); double da = spline.ThirdOrderDerivative(time); speed_data->AppendSpeedPoint(s, time, v, a, da); time += t_output_resolution; } return Status::OK(); }

本文首先介绍AddConstraint() 部分。文件路径：apollo\modules\planning\tasks\optimizers\qp_spline_st_speed\qp_spline_st_graph.cc

Status QpSplineStGraph::AddConstraint(...) { //添加等式约束f(0) = s0 = 0 if (!constraint->AddPointConstraint(0.0, 0.0)) { ... } //添加等式约束f'(0) = v0 = init_point_.v() if (!constraint->AddPointDerivativeConstraint(0.0, init_point_.v())) { ... } // monotone constraint //单调性不等式约束，即随t增大，s(t)一定是不变或增大，因为车会停下或前进，不后退 if (!constraint->AddMonotoneInequalityConstraint(t_evaluated_)) { ... } // smoothness constraint //添加joint points处0~3阶导连续 等式约束，函数名比较迷惑，不是只有3阶导 if (!constraint->AddThirdDerivativeSmoothConstraint()) { ... } // boundary constraint std::vector<double> s_upper_bound; std::vector<double> s_lower_bound; //求不同时刻t，自车s应该处的范围，每一个t对应一个[lower_s, upper_s] for (const double curr_t : t_evaluated_) { double lower_s = 0.0; double upper_s = 0.0; GetSConstraintByTime(boundaries, curr_t, total_path_length_, &upper_s, &lower_s); s_upper_bound.push_back(upper_s); s_lower_bound.push_back(lower_s); } //添加lower_s <= f(t) <= upper_s取值范围约束 if (!constraint->AddBoundary(t_evaluated_, s_lower_bound, s_upper_bound)) { ... } // speed constraint std::vector<double> speed_upper_bound; if (!EstimateSpeedUpperBound(init_point, speed_limit, &speed_upper_bound).ok()) { ... } ... //添加speed_lower_bound <= f'(t) <= speed_upper_bound取值范围约束 if (!constraint->AddDerivativeBoundary(t_evaluated_, speed_lower_bound, speed_upper_bound)) { ... } // acceleration constraint std::vector<double> accel_lower_bound(t_evaluated_.size(), accel_bound.first); std::vector<double> accel_upper_bound(t_evaluated_.size(), accel_bound.second); bool has_follow = false; double delta_s = 1.0; for (const auto* boundary : boundaries) { if (boundary->boundary_type() == STBoundary::BoundaryType::FOLLOW) { has_follow = true; delta_s = std::fmin( delta_s, boundary->min_s() - fabs(boundary->characteristic_length())); } } //如果在跟车（即前方有障碍物）且距前车很近，设定第1个时间间隔内加速度上限为0 if (FLAGS_enable_follow_accel_constraint && has_follow && delta_s < 0.0) { accel_upper_bound.front() = 0.0; } //添加accel_lower_bound <= f''(t) <= accel_upper_bound取值范围约束 if (!constraint->AddSecondDerivativeBoundary(t_evaluated_, accel_lower_bound, accel_upper_bound)) { ... } return Status::OK(); }

为描述方便，设要规划的速度曲线方程为s(t) = f(t)，那么s'(t) = f'(t) 就是速度。

AddConstraint() 中主要添加了如下约束：

1. 起始点函数值约束 f(0) = 0。等式约束。
2. 起始点速度约束 f'(0) = v0 = 初始速度。等式约束。
3. 单调性约束，即随时间t 增大，s = f(t) 一定是不变或增大（单调递增），因为车会停下或前进，不后退。等式约束。
4. 分界点处各阶导数连续平滑性约束，这里用了0~3阶导数。等式约束。
5. 采样时刻t 对应的自车s 坐标范围约束。不等式约束。
6. 采样时刻t 对应的自车速度范围约束。不等式约束。
7. 采样时刻t 对应的自车加速度范围约束。不等式约束。

如何调用我已经在代码中做了注释。接下来咱们一一拆解。

1 起始点函数值约束

讯享网bool Spline1dConstraint::AddPointConstraint(const double x, const double fx) { //x应该是整条曲线上的坐标，而不是某seg上的相对坐标 uint32_t index = FindIndex(x); std::vector<double> power_x; //多项式参数个数 const uint32_t num_params = spline_order_ + 1; //power_x是(相对x)的0~spline_order次方 GeneratePowerX(x - x_knots_[index], num_params, &power_x); Eigen::MatrixXd equality_constraint = Eigen::MatrixXd::Zero(1, (x_knots_.size() - 1) * num_params); uint32_t index_offset = index * num_params; for (uint32_t i = 0; i < num_params; ++i) { equality_constraint(0, index_offset + i) = power_x[i]; } Eigen::MatrixXd equality_boundary(1, 1); equality_boundary(0, 0) = fx; //添加在某一点的函数值相等约束，对5次多项式曲线，如下 //[1, x, x^2, x^3, x^4, x^5] * [a, b, c, d, e, f].T = fx //前者是power_x，中间是5次curve的系数，后者是该点处的函数值fx //为了引用矩阵计算工具，这里都转换成了矩阵形式 return AddEqualityConstraint(equality_constraint, equality_boundary); }

bool Spline1dConstraint::AddEqualityConstraint( const Eigen::MatrixXd& constraint_matrix, const Eigen::MatrixXd& constraint_boundary) { return equality_constraint_.AddConstraint(constraint_matrix, constraint_boundary); }

 Spline1dConstraint::AddEqualityConstraint() 最终还是调用了 AffineConstraint::AddConstraint()。

讯享网bool AffineConstraint::AddConstraint( const Eigen::MatrixXd& constraint_matrix, const Eigen::MatrixXd& constraint_boundary) { //假设添加的约束是Ax=b，constraint_matrix就是A，对5次多项式就是[1,p,p^2,p^3,p^4,p^5]， //p是多项式曲线上某点的横坐标。constraint_boundary就是b，即函数值。x就是未知的多项式系数 //自然A的一行对应的Ax的计算结果就是b的一行，所以A.rows==b.rows且，b.cols==1 if (constraint_matrix.rows() != constraint_boundary.rows()) { ... } if (constraint_matrix_.rows() == 0) { constraint_matrix_ = constraint_matrix; constraint_boundary_ = constraint_boundary; return true; } if (constraint_matrix_.cols() != constraint_matrix.cols()) { ... } if (constraint_boundary.cols() != 1) { ... } Eigen::MatrixXd n_matrix(constraint_matrix_.rows() + constraint_matrix.rows(), constraint_matrix_.cols()); Eigen::MatrixXd n_boundary( constraint_boundary_.rows() + constraint_boundary.rows(), 1); n_matrix << constraint_matrix_, constraint_matrix; n_boundary << constraint_boundary_, constraint_boundary; constraint_matrix_ = n_matrix; constraint_boundary_ = n_boundary; return true; }

FindIndex()用来寻找x 在分界点中的下标，以确定其所在的spline seg段。 

uint32_t Spline1dConstraint::FindIndex(const double x) const { //upper_bound(起始地址，结束地址，要查找的数值) 返回的是数值最后一个出现的位置。 //即返回“元素值>查找值”的第一个元素的位置 //因为x_knots_[0]=0，所以可以跳过，从x_knots_[1]开始查 auto upper_bound = std::upper_bound(x_knots_.begin() + 1, x_knots_.end(), x); return std::min(static_cast<uint32_t>(x_knots_.size() - 1), static_cast<uint32_t>(upper_bound - x_knots_.begin())) - 1; //最后-1，FindIndex()返回的是第一个比x大的元素的前一个元素下标index //即入参x位于x_knots_[index]～x_knots_[index+1]之间 }

2 起始点速度约束

讯享网bool Spline1dConstraint::AddPointDerivativeConstraint(const double x, const double dfx) { uint32_t index = FindIndex(x); std::vector<double> power_x; const uint32_t num_params = spline_order_ + 1; GeneratePowerX(x - x_knots_[index], num_params, &power_x); Eigen::MatrixXd equality_constraint = Eigen::MatrixXd::Zero(1, (x_knots_.size() - 1) * num_params); uint32_t index_offset = index * num_params; for (uint32_t i = 1; i < num_params; ++i) { //该行矩阵中对应该spline seg是[0, 1, 2*x, 3*x^2, 4*x^3, 5*x^4] //注意equality_constraint的长度并不是6，此处只分析某一段的长度是6 //注意equality_constraint被初始化为元素全为0的行矩阵 equality_constraint(0, index_offset + i) = power_x[i - 1] * i; } Eigen::MatrixXd equality_boundary(1, 1); equality_boundary(0, 0) = dfx; //添加在某一点的一阶导数相等约束，对5次多项式曲线，如下 //[0, 1, 2*x, 3*x^2, 4*x^3, 5*x^4] * [a, b, c, d, e, f].T = dfx //中间是5次curve的系数，后者是该点处的一阶导数值dfx return AddEqualityConstraint(equality_constraint, equality_boundary); }

最后一行调用的AddEqualityConstraint() 与上面相同。

讯享网

3 单调性约束（单调递增）

bool Spline1dConstraint::AddMonotoneInequalityConstraint( const std::vector<double>& x_coord) { ... const uint32_t num_params = spline_order_ + 1; //注意维度，有N个x，就有N-1个不等式f(xn-1) <= f(xn) Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero( x_coord.size() - 1, (x_knots_.size() - 1) * num_params); Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero(x_coord.size() - 1, 1); uint32_t prev_spline_index = FindIndex(x_coord[0]); double prev_rel_x = x_coord[0] - x_knots_[prev_spline_index]; std::vector<double> prev_coef; GeneratePowerX(prev_rel_x, num_params, &prev_coef); for (uint32_t i = 1; i < x_coord.size(); ++i) { uint32_t cur_spline_index = FindIndex(x_coord[i]); double cur_rel_x = x_coord[i] - x_knots_[cur_spline_index]; std::vector<double> cur_coef; GeneratePowerX(cur_rel_x, num_params, &cur_coef); // if constraint on the same spline if (cur_spline_index == prev_spline_index) { for (uint32_t j = 0; j < cur_coef.size(); ++j) { //在同一段spline，则多项式系数是相同的。 //记cur_rel_x-prev_rel_x=d, inequality_constraint对应该spline seg是 //[1-1, d, d^2, d^3, d^4, d^5] * [a, b, c, d, e, f].T >= 0 inequality_constraint(i - 1, cur_spline_index * num_params + j) = cur_coef[j] - prev_coef[j]; } } else { // if not on the same spline for (uint32_t j = 0; j < cur_coef.size(); ++j) { //inequality_constraint对应相邻2个spline seg是 //[-1, -px, -px^2, -px^3, -px^4, -px^5, 1, cx, cx^2, cx^3, cx^4, cx^5] // * [pa, pb, pc, pd, pe, pf, ca, cb, cc, cd, ce, cf].T >= 0 inequality_constraint(i - 1, prev_spline_index * num_params + j) = -prev_coef[j]; inequality_constraint(i - 1, cur_spline_index * num_params + j) = cur_coef[j]; } } prev_coef = cur_coef; prev_spline_index = cur_spline_index; } return inequality_constraint_.AddConstraint(inequality_constraint,inequality_boundary); }

 最后一行调用的AddConstraint() 与上面相同。

4 分界点处各阶导数连续平滑性约束

讯享网//添加joint points处0~3阶导连续 等式约束，即 //fl(xl)=fr(xr); fl'(xl)=fr'(xr); fl''(xl)=fr''(xr); fl'''(xl)=fr'''(xr)，转换为QP的形式，即 //fl(xl)-fr(xr)=0; fl'(xl)-fr'(xr)=0; fl''(xl)-fr''(xr)=0; fl'''(xl)-fr'''(xr)=0 //fl()和fr()是相邻的2段curve polynomial，xl和xr是同一个点在不同的段不同的起点下的相对坐标 bool Spline1dConstraint::AddThirdDerivativeSmoothConstraint() { //如果只有一段spline，就没有joint point了，也没有平滑一说了 if (x_knots_.size() < 3) { return false; } const uint32_t n_constraint = (static_cast<uint32_t>(x_knots_.size()) - 2) * 4; //中间连接点的个数，4代表0~3阶导都连续 const uint32_t num_params = spline_order_ + 1; Eigen::MatrixXd equality_constraint = Eigen::MatrixXd::Zero(n_constraint, (x_knots_.size() - 1) * num_params); Eigen::MatrixXd equality_boundary = Eigen::MatrixXd::Zero(n_constraint, 1); //这里的2层for循环很不直观，其实是把不同阶导数、不同的joint point、不同的系数杂糅在一起计算了 //虽高效，但太复杂抽象了，最终计算后应该是下面的形式 //每行左边6项是joint point左侧的curve系数，右边6项是joint point右侧的curve系数 //0~3行是1个joint point的0~3阶导，每4行表示一个点 // | 1 xl xl^2 xl^3 xl^4 xl^5 -1 -xr -xr^2 -xr^3 -xr^4 -xr^5 | | al | = | 0 | // | 0 1 2xl 3xl^2 4xl^3 5xl^4 0 -1 -2xr -3xr^2 -4xr^3 -5xr^4 | | bl | = | 0 | // | 0 0 2 6xl 12xl^2 20xl^3 0 0 -2 -6xr -12xr^2 -20xr^3 | * | cl | = | 0 | // | 0 0 0 6 24xl 60xl^2 0 0 0 -6 -24xr -60xr^2 | | dl | = | 0 | // | . . . . . . . . . . . . | | el | = | 0 | // | . . . . . . . . . . . . | | fl | = | 0 | // | ar | = | 0 | // | br | = | 0 | // | cr | = | 0 | // | dr | = | 0 | // | er | = | 0 | // | fr | = | 0 | //i循环x_knots_.size())-2次 for (uint32_t i = 0; i < n_constraint; i += 4) { double left_coef = 1.0; double right_coef = -1.0; double left_dcoef = 1.0; double right_dcoef = -1.0; double left_ddcoef = 1.0; double right_ddcoef = -1.0; double left_dddcoef = 1.0; double right_dddcoef = -1.0; //第n段的最后一个点相对于第n段的第一点的坐标 const double x_left = x_knots_[i / 4 + 1] - x_knots_[i / 4]; //第n段的最后一个点相对于第n+1段的第一点的坐标 const double x_right = 0.0; //j循环6次 for (uint32_t j = 0; j < num_params; ++j) { equality_constraint(i, num_params * i / 4 + j) = left_coef; equality_constraint(i, num_params * (i / 4 + 1) + j) = right_coef; if (j >= 3) { equality_constraint(i + 3, num_params * i / 4 + j) = left_dddcoef * j * (j - 1) * (j - 2); equality_constraint(i + 3, num_params * (i / 4 + 1) + j) = right_dddcoef * j * (j - 1) * (j - 2); left_dddcoef = left_ddcoef; right_dddcoef = right_ddcoef; } if (j >= 2) { equality_constraint(i + 2, num_params * i / 4 + j) = left_ddcoef * j * (j - 1); equality_constraint(i + 2, num_params * (i / 4 + 1) + j) = right_ddcoef * j * (j - 1); left_ddcoef = left_dcoef; right_ddcoef = right_dcoef; } if (j >= 1) { equality_constraint(i + 1, num_params * i / 4 + j) = left_dcoef * j; equality_constraint(i + 1, num_params * (i / 4 + 1) + j) = right_dcoef * j; left_dcoef = left_coef; right_dcoef = right_coef; } left_coef *= x_left; right_coef *= x_right; } } return equality_constraint_.AddConstraint(equality_constraint, equality_boundary); }

最后一行调用的AddConstraint() 与上面相同。 

5 采样时刻t 对应的自车s 坐标范围约束

//根据在time时刻各个boundary的类别，确定s的上界或下界 Status QpSplineStGraph::GetSConstraintByTime(...) const { *s_upper_bound = total_path_s; for (const STBoundary* boundary : boundaries) { double s_upper = 0.0; double s_lower = 0.0; if (!boundary->GetUnblockSRange(time, &s_upper, &s_lower)) { ... } //如果boundary的类型是如下3种，则寻找s的上界，s上界之上可以理解为有障碍物 //如follow场景，我们只在意s的上界 if (boundary->boundary_type() == STBoundary::BoundaryType::STOP || boundary->boundary_type() == STBoundary::BoundaryType::FOLLOW || boundary->boundary_type() == STBoundary::BoundaryType::YIELD) { *s_upper_bound = std::fmin(*s_upper_bound, s_upper); } else if (boundary->boundary_type() == STBoundary::BoundaryType::OVERTAKE) { //如果boundary的类型是OVERTAKE超车，则寻找s的下界，s下界之下可以理解为有障碍物 //即自车要超过障碍物，则自车的s一定要大于障碍物的boundary的upper *s_lower_bound = std::fmax(*s_lower_bound, s_lower); } else { ... } } return Status::OK(); }

在添加约束前，先求在各采样时刻自车可能达到的s 范围，如上。

讯享网//添加lower_bound <= f(x_coord) <= upper_bound取值范围约束 bool Spline1dConstraint::AddBoundary(...) { ... //检验lower_bound和upper_bound中的元素值是否是有效值 //将一一对应的x_coord -> lower_bound存入filtered_lower_bound_x和filtered_lower_bound //将一一对应的x_coord -> upper_bound存入filtered_upper_bound_x和filtered_upper_bound if (!FilterConstraints(x_coord, lower_bound, upper_bound, &filtered_lower_bound_x, &filtered_lower_bound, &filtered_upper_bound_x, &filtered_upper_bound)) { ... } // emplace affine constraints const uint32_t num_params = spline_order_ + 1; //inequality_constraint矩阵中绝大多数元素都是0，只有x_coord所在的一段curve对应元素有值 Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero( filtered_upper_bound.size() + filtered_lower_bound.size(), //行，即不等式个数 (x_knots_.size() - 1) * num_params); //列 Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero( filtered_upper_bound.size() + filtered_lower_bound.size(), 1); //由AddPointConstraintInRange()推来，其实就是已知val=f(x0)，range>0， //x_coord=filtered_lower_bound_x=filtered_upper_bound_x， //filtered_lower_bound=lower_bound=val-range， //filtered_upper_bound=upper_bound=val+range， //由val-range < val < val+range ==> //f(filtered_lower_bound_x) >= filtered_lower_bound //且 f(filtered_upper_bound_x) <= filtered_upper_bound //这个for循环对应f(filtered_lower_bound_x) >= filtered_lower_bound的情况， //恰好符合QP中Ax>=b的形式 for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) { //FindIndex()返回x_knots_中第一个大于入参的元素的前一个位置， //即入参位于x_knots_[index]～x_knots_[index+1]之间 uint32_t index = FindIndex(filtered_lower_bound_x[i]); //corrected_x是每一段的相对起点的x坐标 const double corrected_x = filtered_lower_bound_x[i] - x_knots_[index]; double coef = 1.0; //j=0~num_params，依次是corrected_x的0~num_params次项 //[1, x, x^2, x^3, x^4, x^5] * [a, b, c, d, e, f].T >= lower_bound for (uint32_t j = 0; j < num_params; ++j) { //对特定行、特定curve的参数列更新，因为给定一个s，其根据定义域只对应一段curve inequality_constraint(i, j + index * num_params) = coef; coef *= corrected_x; } inequality_boundary(i, 0) = filtered_lower_bound[i]; } //这个for循环对应f(filtered_upper_bound_x) <= filtered_upper_bound的情况， //为符合QP中Ax>=b的形式，将Ax<=b ==> -Ax>=-b，故coef赋初值-1，与上部分比，全为负数 for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) { uint32_t index = FindIndex(filtered_upper_bound_x[i]); const double corrected_x = filtered_upper_bound_x[i] - x_knots_[index]; double coef = -1.0; //-f(x) = [-1, -x, -x^2, -x^3, -x^4, -x^5] * [a, b, c, d, e, f].T // >= -upper_bound ==> //f(x) = [1, x, x^2, x^3, x^4, x^5] * [a, b, c, d, e, f].T // <= upper_bound for (uint32_t j = 0; j < num_params; ++j) { inequality_constraint(i + filtered_lower_bound.size(), //行 j + index * num_params) = coef; //列 coef *= corrected_x; } inequality_boundary(i + filtered_lower_bound.size(), 0) = //行变化 -filtered_upper_bound[i]; } return inequality_constraint_.AddConstraint(inequality_constraint, nequality_boundary); }

 最后一行调用的AddConstraint() 与上面相同。 

6 采样时刻t 对应的自车速度范围约束

//查找针对每个采样时刻t的速度上限 Status QpSplineStGraph::EstimateSpeedUpperBound(...) const { // use v to estimate position: not accurate, but feasible in cyclic // processing. We can do the following process multiple times and use // previous cycle's results for better estimation. const double v = init_point.v(); auto last_speed_data = GetHistorySpeed(); //看不懂这个if-else??? if(static_cast<double>(t_evaluated_.size() + speed_limit.speed_limit_points().size()) < static_cast<double>(t_evaluated_.size()) * std::log( static_cast<double>(speed_limit.speed_limit_points().size()))) { uint32_t i = 0; uint32_t j = 0; while (i < t_evaluated_.size() && j + 1 < speed_limit.speed_limit_points().size()) { //2种不同的方式粗略估计时刻t可能会达到的s，一种是根据初始速度，一种是根据上一次规划的速度 double distance = v * t_evaluated_[i]; if (!last_speed_data.empty() && distance < last_speed_data.back().s()) { SpeedPoint p; last_speed_data.EvaluateByTime(t_evaluated_[i], &p); distance = p.s(); } //根据估算的s 求匹配的speed limit ... } ... } else { ... } //如果正在变道，略增大速度上限（0.05） if (is_change_lane_) { for (uint32_t k = 0; k < t_evaluated_.size(); ++k) { speed_upper_bound->at(k) *= (1.0 + FLAGS_change_lane_speed_relax_percentage); } } const double kTimeBuffer = 1.0; const double kSpeedBuffer = 0.1; //提高前1s速度上限 for (uint32_t k = 0; k < t_evaluated_.size() && t_evaluated_[k] < kTimeBuffer; ++k) { speed_upper_bound->at(k) = std::fmax(init_point_.v() + kSpeedBuffer, speed_upper_bound->at(k)); } return Status::OK(); }

在添加约束前，先求在各采样时刻自车可能达到的速度范围，如上。 

讯享网//同AddBoundary() bool Spline1dConstraint::AddDerivativeBoundary(...) { ... if (!FilterConstraints(...)) { ... } // emplace affine constraints const uint32_t num_params = spline_order_ + 1; Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero( filtered_upper_bound.size() + filtered_lower_bound.size(), //行 (x_knots_.size() - 1) * num_params); Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero( filtered_upper_bound.size() + filtered_lower_bound.size(), 1); for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) { uint32_t index = FindIndex(filtered_lower_bound_x[i]); const double corrected_x = filtered_lower_bound_x[i] - x_knots_[index]; double coef = 1.0; //f'(x) = [0, 1, 2x, 3x^2, 4x^3, 5x^4] * [a, b, c, d, e, f].T // >= filtered_lower_bound for (uint32_t j = 1; j < num_params; ++j) { inequality_constraint(i, j + index * num_params) = coef * j; coef *= corrected_x; } inequality_boundary(i, 0) = filtered_lower_bound[i]; } for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) { uint32_t index = FindIndex(filtered_upper_bound_x[i]); const double corrected_x = filtered_upper_bound_x[i] - x_knots_[index]; double coef = -1.0; //-f'(x) = [0, -1, -2x, -3x^2, -4x^3, -5x^4] * [a, b, c, d, e, f].T // >= -filtered_upper_bound ==> //f'(x) = [0, 1, 2x, 3x^2, 4x^3, 5x^4] * [a, b, c, d, e, f].T // <= filtered_upper_bound for (uint32_t j = 1; j < num_params; ++j) { inequality_constraint(i + filtered_lower_bound.size(), j + index * num_params) = coef * j; coef *= corrected_x; } inequality_boundary(i + filtered_lower_bound.size(), 0) = -filtered_upper_bound[i]; } return inequality_constraint_.AddConstraint(inequality_constraint,inequality_boundary); }

最后一行调用的AddConstraint() 与上面相同。 

7 采样时刻t 对应的自车加速度范围约束

与6基本一致，这里简单带过。

bool Spline1dConstraint::AddSecondDerivativeBoundary(...) { ... if (!FilterConstraints(...)) { ... } ... for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) { ... //f''(x) = [0, 0, 2, 6x, 12x^2, 20x^3] * [a, b, c, d, e, f].T // >= filtered_lower_bound for (uint32_t j = 2; j < num_params; ++j) { inequality_constraint(i, j + index * num_params) = coef * j * (j - 1); coef *= corrected_x; } inequality_boundary(i, 0) = filtered_lower_bound[i]; } for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) { ... //-f''(x) = [0, 0, -2, -6x, -12x^2, -20x^3] * [a, b, c, d, e, f].T // >= -filtered_upper_bound ==> //f''(x) = [0, 0, 2, 6x, 12x^2, 20x^3] * [a, b, c, d, e, f].T // <= filtered_upper_bound for (uint32_t j = 2; j < num_params; ++j) { inequality_constraint(i + filtered_lower_bound.size(), j + index * num_params) = coef * j * (j - 1); coef *= corrected_x; } inequality_boundary(i + filtered_lower_bound.size(), 0) = -filtered_upper_bound[i]; } return inequality_constraint_.AddConstraint(inequality_constraint,inequality_boundary); }

最后一行调用的AddConstraint() 与上面相同。 

添加约束到此结束。