大家好，我已經把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() 與上面相同。
添加限制到此結束。
![上下位機：嵌入式工程中的默契（代碼解析）[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)