大家好，我已经把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() 中主要添加了如下约束：
- 起始点函数值约束 f(0) = 0。等式约束。
- 起始点速度约束 f'(0) = v0 = 初始速度。等式约束。
- 单调性约束，即随时间t 增大，s = f(t) 一定是不变或增大（单调递增），因为车会停下或前进，不后退。等式约束。
- 分界点处各阶导数连续平滑性约束，这里用了0~3阶导数。等式约束。
- 采样时刻t 对应的自车s 坐标范围约束。不等式约束。
- 采样时刻t 对应的自车速度范围约束。不等式约束。
- 采样时刻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() 与上面相同。
添加约束到此结束。
扫一扫
6351
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
