大家好，我已經把CSDN上的部落格遷移到了知乎上，歡迎大家在知乎關注我的專欄慢慢悠悠小馬車（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相學習。
QpSplinePathOptimizer 的入口函數在該類的Process()，在Process()中構造了QpSplinePathGenerator類的執行個體path_generator，軌迹生成工作由path_generator.Generate()完成。檔案路徑：apollo3_5/modules/planning/tasks/optimizers/qp_spline_path/qp_spline_path_optimizer.cc
Status QpSplinePathOptimizer::Process(const SpeedData& speed_data,
const ReferenceLine& reference_line,
const common::TrajectoryPoint& init_point,
PathData* const path_data) {
QpSplinePathGenerator path_generator(spline_solver_.get(), reference_line,
qp_spline_path_config_,
reference_line_info_->AdcSlBoundary());
path_generator.SetDebugLogger(reference_line_info_->mutable_debug());
path_generator.SetChangeLane(reference_line_info_->IsChangeLanePath());
double boundary_extension = 0.0;
bool is_final_attempt = false;
//第一次調用Generate()時，boundary_extension=0.0。
bool ret = path_generator.Generate(
reference_line_info_->path_decision()->obstacles().Items(), speed_data,
init_point, boundary_extension, is_final_attempt, path_data);
...
boundary_extension = qp_spline_path_config_.cross_lane_lateral_extension();
is_final_attempt = true;
//若生成軌迹失敗，則将boundary_extension增大為cross_lane_lateral_extension()（預設值1.2m)
//以放松優化過程中的限制條件，使得更易求解，第二次調用Generate()
ret = path_generator.Generate(
reference_line_info_->path_decision()->obstacles().Items(), speed_data,
init_point, boundary_extension, is_final_attempt, path_data);
...
}
return Status::OK();
}
QpSplinePathGenerator::Generate()中，主要有以下關鍵步驟：初始化樣條曲線InitSpline()，初始化用來計算自車軌迹橫向限制的類QpFrenetFrame::Init()，添加限制AddConstraint()，添加目标函數AddKernel()，優化問題求解器Solve()，最後将求得的軌迹點封裝成DiscretizedPath。接下來一一分析。
//boundary_extension第一次嘗試=0，第二次嘗試增大了
bool QpSplinePathGenerator::Generate( ... ) {
...
if (!InitSpline(start_s, end_s)) { ... }
QpFrenetFrame qp_frenet_frame(reference_line_, speed_data, init_frenet_point_,
qp_spline_path_config_.time_resolution(),
evaluated_s_);
if (!qp_frenet_frame.Init(obstacles)) { ... }
if (!AddConstraint(qp_frenet_frame, boundary_extension)) { ... }
AddKernel();
bool is_solved = Solve();
...
// extract data
//将求得的軌迹點封裝成DiscretizedPath
...
}
InitSpline()主要完成對spline segment和縱向s采樣點的初始化。
bool QpSplinePathGenerator::InitSpline(const double start_s, const double end_s) {
uint32_t number_of_spline = static_cast<uint32_t>(
(end_s - start_s) / qp_spline_path_config_.max_spline_length() + 1.0);
number_of_spline = std::max(1u, number_of_spline);
common::util::uniform_slice(start_s, end_s, number_of_spline, &knots_);
// spawn a new spline generator
//産生number_of_spline條order階spline
spline_solver_->Reset(knots_, qp_spline_path_config_.spline_order());
// set evaluated_s_
uint32_t constraint_num = static_cast<uint32_t>(
(end_s - start_s) / qp_spline_path_config_.max_constraint_interval() + 1);
common::util::uniform_slice(start_s, end_s, constraint_num - 1, &evaluated_s_);
return (knots_.size() > 1) && !evaluated_s_.empty();
//難道max_spline_length和max_constraint_interval可以不相等嗎？
//難道constraint_num和number_of_spline可以不相等嗎？
}
将縱向區間[start_s, end_s] 按照qp_spline_path_config_.max_spline_length()和qp_spline_path_config_.max_constraint_interval()的設定，均勻分割後存入knots_和evaluated_s_，每一段都會對應一條qp_spline_path_config_.spline_order()次多項式曲線。在這裡，我認為knots_和evaluated_s_ 這2個記錄s軸采樣點的vector是應該完全相同的。不明白代碼中為何有2種定義？
QpFrenetFrame::Init() 根據已知條件，計算對自車橫向軌迹的限制。
- HDMap和道路參考線對自車橫向軌迹的限制
- 靜态動态障礙物及相應的decision對自車橫向軌迹的限制
bool QpFrenetFrame::Init(const std::vector<const Obstacle*>& obstacles) {
//根據SpeedData，計算自車縱向行經軌迹，存入vector<SpeedPoint>
//即discretized_vehicle_location_
if (!CalculateDiscretizedVehicleLocation()) { ... }
//根據HDMap和道路參考線，計算自車軌迹的橫向限制
if (!CalculateHDMapBound()) { ... }
//計算靜态、動态障礙物及相應的decision對自車軌迹的橫向限制
if (!CalculateObstacleBound(obstacles)) { ... }
return true;
}
Init()内調用了3個函數。
CalculateDiscretizedVehicleLocation()根據SpeedData按時間周遊計算了自車的縱向軌迹相關資訊，存入vector<SpeedPoint>，即discretized_vehicle_location_。目前，SpeedData的來源我還沒有搞清楚。
bool QpFrenetFrame::CalculateDiscretizedVehicleLocation() {
for (double relative_time = 0.0; relative_time < speed_data_.TotalTime();
relative_time += time_resolution_) {
SpeedPoint veh_point;
if (!speed_data_.EvaluateByTime(relative_time, &veh_point)) { ... }
veh_point.set_t(relative_time);
discretized_vehicle_location_.push_back(std::move(veh_point));
}
return true;
}
1. HDMap和道路參考線對自車橫向軌迹的限制
CalculateHDMapBound()計算了沿縱軸s均勻采樣所得點集中 各個點的橫向限制，即左右邊界。
//根據HDMap和道路參考線，計算自車軌迹的橫向限制
//hdmap_bound_初始化為vector {evaluated_s_.size(), std::make_pair(-inf, inf)}
bool QpFrenetFrame::CalculateHDMapBound() {
const double adc_half_width = vehicle_param_.width() / 2.0;
for (uint32_t i = 0; i < hdmap_bound_.size(); ++i) {
double left_bound = 0.0;
double right_bound = 0.0;
bool suc = reference_line_.GetLaneWidth(evaluated_s_[i], &left_bound, &right_bound);
if (!suc) {
AWARN << "Extracting lane width failed at s = " << evaluated_s_[i];
right_bound = FLAGS_default_reference_line_width / 2;
left_bound = FLAGS_default_reference_line_width / 2;
}
//按照右手坐标系，自車前方為s軸正方向，hdmap_bound_[i].first是右側邊界，second是左側邊界
hdmap_bound_[i].first = -right_bound + adc_half_width;
hdmap_bound_[i].second = left_bound - adc_half_width;
//如果右邊界>左邊界，不合理，則縮短縱向s上限feasible_longitudinal_upper_bound_到目前考察點
if (hdmap_bound_[i].first >= hdmap_bound_[i].second) {
...
feasible_longitudinal_upper_bound_ =
std::min(evaluated_s_[i], feasible_longitudinal_upper_bound_);
...
}
}
return true;
}
2. 靜态動态障礙物及相應的decision對自車橫向軌迹的限制
CalculateObstacleBound()的處理分為2部分：靜态障礙物和動态障礙物。首先判斷障礙物是否有LateralDecision，若沒有，則忽略，因為對path沒有影響（這個階段的path重點考慮橫向運動）。LateralDecision主要是指橫向的nudge，具體decision相關資訊可查閱apollo3_5/modules/planning/proto/decision.proto。
//計算靜态、動态障礙物及相應的decision對自車軌迹的橫向限制
bool QpFrenetFrame::CalculateObstacleBound(const std::vector<const Obstacle*>& obstacles) {
for (const auto ptr_obstacle : obstacles) {
//path是橫向軌迹，若沒有LateralDecision，就對path優化沒有影響，主要指nudge
if (!ptr_obstacle->HasLateralDecision()) {
continue;
}
if (ptr_obstacle->IsStatic()) {
if (!MapStaticObstacleWithDecision(*ptr_obstacle)) { ... }
} else {
if (!MapDynamicObstacleWithDecision(*ptr_obstacle)) { ... }
}
}
return true;
}
MapStaticObstacleWithDecision()考慮了靜态障礙物以及相應的橫向避讓措施對可行駛區域寬度的影響，處理結果static_obstacle_bound_儲存了evaluated_s_中各采樣點處的橫向可行駛範圍。
//考慮靜态障礙物以及相應的橫向避讓措施對自車軌迹橫向的限制
//static_obstacle_bound_儲存了evaluated_s_中采樣點處的橫向限制區間
bool QpFrenetFrame::MapStaticObstacleWithDecision(const Obstacle& obstacle) {
if (!obstacle.HasLateralDecision()) { ... }
const auto& decision = obstacle.LateralDecision();
if (!decision.has_nudge()) { ... }
if (!MapNudgePolygon(common::math::Polygon2d(obstacle.PerceptionBoundingBox()),
decision.nudge(), &static_obstacle_bound_)) { ... }
return true;
}
處理靜态障礙物的主要思路是将其輪廓端點映射到Frenet坐标系，結合nudge的方向計算障礙物占據的橫向範圍，由MapNudgePolygon()和MapNudgeLine()實作。
//确定障礙物輪廓對縱向s範圍内軌迹采樣點的橫向限制
bool QpFrenetFrame::MapNudgePolygon(
const common::math::Polygon2d& polygon, const ObjectNudge& nudge,
std::vector<std::pair<double, double>>* const bound_map) {
std::vector<common::SLPoint> sl_corners;
for (const auto& corner_xy : polygon.points()) {
common::SLPoint corner_sl;
if (!reference_line_.XYToSL(corner_xy, &corner_sl)) { ... }
// shift box based on buffer
// nudge decision buffer:
// --- position for left nudge
// --- negative for right nudge
//nudge.distance_l()是帶正負的，表示nudge的左右方向，同上
corner_sl.set_l(corner_sl.l() + nudge.distance_l());
sl_corners.push_back(std::move(corner_sl));
}
const auto corner_size = sl_corners.size();
for (uint32_t i = 0; i < corner_size; ++i) {
if (!MapNudgeLine(sl_corners[i], sl_corners[(i + 1) % corner_size],
nudge.type(), bound_map)) { ... }
}
return true;
}
MapNudgePolygon()中循環調用MapNudgeLine()，将障礙物的bounding box對自車軌迹的橫向限制計算，轉換為計算bounding box的4條邊對自車軌迹的橫向限制。
//确定障礙物輪廓的一條邊所對應縱向s範圍内軌迹采樣點的橫向限制
bool QpFrenetFrame::MapNudgeLine( ... ) {
const common::SLPoint& near_point = (start.s() < end.s() ? start : end);
const common::SLPoint& further_point = (start.s() < end.s() ? end : start);
//impact_index表示evaluated_s_中受影響的區間範圍，2個index可能是相等的
std::pair<uint32_t, uint32_t> impact_index = FindInterval(near_point.s(), further_point.s());
//s軸超出軌迹縱向範圍
if (further_point.s() < start_s_ - vehicle_param_.back_edge_to_center() ||
near_point.s() > end_s_ + vehicle_param_.front_edge_to_center()) {
return true;
}
const double distance = std::max(further_point.s() - near_point.s(), common::math::kMathEpsilon);
const double adc_half_width = vehicle_param_.width() / 2;
for (uint32_t i = impact_index.first; i <= impact_index.second; ++i) {
double weight = std::fabs((evaluated_s_[i] - near_point.s())) / distance;
weight = std::min(1.0, std::max(weight, 0.0));
double boundary = near_point.l() * (1 - weight) + further_point.l() * weight;
if (nudge_type == ObjectNudge::LEFT_NUDGE) {
boundary += adc_half_width;
//first是lower bound，second是upper bound
//lower bound增大，即将自車可行駛區域向左移動，即left nudge
//而upper bound不變，其初始化為INF
(*constraint)[i].first = std::max(boundary, (*constraint)[i].first);
} else {
boundary -= adc_half_width;
(*constraint)[i].second = std::min(boundary, (*constraint)[i].second);
}
//若可行駛區域寬度constraint太窄(<0.3m)，則收縮feasible_longitudinal_upper_bound_
if ((*constraint)[i].second < (*constraint)[i].first + 0.3) {
if (i > 0) {
feasible_longitudinal_upper_bound_ =
std::min(evaluated_s_[i - 1] - kEpsilonTol, feasible_longitudinal_upper_bound_);
} else {
feasible_longitudinal_upper_bound_ = start_s_;
return true;
}
...
break;
}
}
return true;
}
MapDynamicObstacleWithDecision()也是和靜态障礙物大體相似的思路。根據由SpeedData求得的自車位置資訊discretized_vehicle_location_，按照時間戳一一比對動态障礙物的軌迹點（應該是預測軌迹），計算該點處的障礙物bounding box對自車橫向軌迹的影響。
bool QpFrenetFrame::MapDynamicObstacleWithDecision(const Obstacle& obstacle) {
...
const auto& nudge = decision.nudge();
for (const SpeedPoint& veh_point : discretized_vehicle_location_) {
double time = veh_point.t();
common::TrajectoryPoint trajectory_point = obstacle.GetPointAtTime(time);
common::math::Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);
// project obs_box on reference line
std::vector<common::math::Vec2d> corners;
obs_box.GetAllCorners(&corners);
std::vector<common::SLPoint> sl_corners;
for (const auto& corner_xy : corners) {
common::SLPoint cur_point;
if (!reference_line_.XYToSL(corner_xy, &cur_point)) { ... }
// shift box base on buffer
cur_point.set_l(cur_point.l() + nudge.distance_l());
sl_corners.push_back(std::move(cur_point));
}
for (uint32_t i = 0; i < sl_corners.size(); ++i) {
common::SLPoint sl_first = sl_corners[i % sl_corners.size()];
common::SLPoint sl_second = sl_corners[(i + 1) % sl_corners.size()];
if (sl_first.s() < sl_second.s()) {
std::swap(sl_first, sl_second);
}
//MapLateralConstraint()被調用4次，因為sl_corners由bounding box而來，有4個端點
//因為是考慮橫向限制，bounding box的上下2條橫向線段端點對于求bound沒有影響
std::pair<double, double> bound = MapLateralConstraint( ... );
// update bound map
double s_resolution = std::fabs(veh_point.v() * time_resolution_);
double updated_start_s = init_frenet_point_.s() + veh_point.s() - s_resolution;
double updated_end_s = init_frenet_point_.s() + veh_point.s() + s_resolution;
//縱向s超出考察範圍
if (updated_end_s > evaluated_s_.back() || updated_start_s < evaluated_s_.front()) {
continue;
}
std::pair<uint32_t, uint32_t> update_index_range = FindInterval(updated_start_s, updated_end_s);
for (uint32_t j = update_index_range.first; j <= update_index_range.second; ++j) {
//可行駛區域的右邊界bound，取max
dynamic_obstacle_bound_[j].first = std::max(bound.first, dynamic_obstacle_bound_[j].first);
//可行駛區域的左邊界bound，取min
dynamic_obstacle_bound_[j].second = std::min(bound.second, dynamic_obstacle_bound_[j].second);
}
}
}
return true;
}
MapLateralConstraint()用來計算障礙物bounding box的一條邊對自車橫向軌迹的限制。
//傳回值pair.first表示可行駛區域的右邊界限定，second表示左邊界限定，first<second
std::pair<double, double> QpFrenetFrame::MapLateralConstraint( ... ) {
constexpr double inf = std::numeric_limits<double>::infinity();
std::pair<double, double> result = std::make_pair(-inf, inf);
//障礙物車在自車前方或後方，忽略，本函數隻考慮橫向影響
if (start.s() > s_end || end.s() < s_start) {
return result;
}
double s_front = std::max(start.s(), s_start);
double s_back = std::min(end.s(), s_end);
double weight_back = 0.0;
double weight_front = 0.0;
if (end.s() - start.s() > std::numeric_limits<double>::epsilon()) {
weight_back = (s_back - end.s()) / (end.s() - start.s());
weight_front = (s_front - start.s()) / (end.s() - start.s());
}
//将自車首尾點向障礙物車做映射，找對應點，以确定橫向偏移
//我覺得隻簡單的利用障礙物車的bounding box就夠了
//nudge的橫向距離已經在調用該函數之前納入障礙物車的start和end兩點的l坐标了
common::SLPoint front = common::math::InterpolateUsingLinearApproximation(
start, end, weight_front);
common::SLPoint back = common::math::InterpolateUsingLinearApproximation(
start, end, weight_back);
if (nudge_type == ObjectNudge::RIGHT_NUDGE) {
//确認自車橫向左方限定，取min使可行駛範圍偏右，故nudge right
result.second = std::min(front.l(), back.l());
} else {
//确認自車橫向右方限定，取max使可行駛範圍偏左，故nudge left
result.first = std::max(front.l(), back.l());
}
return result;
}
以上都是QP的前期處理，我會繼續更新文章分析接下來的AddConstraint()、AddKernel()、Solve()。敬請期待。
![上下位機：嵌入式工程中的默契（代碼解析）[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)