流程图

OpenSpacePlanner算法与周边模块关系

OpenSpacePlanner与PublicRoadPlanner关系

Apollo中的PublicRoadPlanner涵盖了OpenSpacePlanner。
OpenSpacePlanner用来处理public road所包含的2个场景：

• u-turn
• valet parking

在apollo/modules/planning/conf/planning_config.pb.txt 配置文件中，
PublicRoadPlanner 支持 VALET_PARKING 场景，而该场景分为2个stage：

• VALET_PARKING_APPROACHING_PARKING_SPOT、
• VALET_PARKING_PARKING

VALET_PARKING_PARKING stage 包含task：OPEN_SPACE_TRAJECTORY_PROVIDER ，该task就是OpenSpacePlanner的工作。

Hybird A*流程

Hybird A*外部调用入口

代码目录：apollo/modules/planning/tasks/optimizers/open_space_trajectory_generation。代码文件：
（1）open_space_trajectory_provider.h：
OpenSpaceTrajectoryProvider类，在OpenSpaceTrajectoryProvider::GenerateTrajectoryThread()中会调用open_space_trajectory_optimizer_->Plan()。
（2）open_space_trajectory_optimizer.h：
OpenSpaceTrajectoryOptimizer类，有一个std::unique_ptr warm_start_ 成员变量。在 OpenSpaceTrajectoryOptimizer::Plan()中，会调用 warm_start_->Plan()。

Hybird A*内部流程

• 碰撞检测
• ReedShepp曲线加速搜索
• 扩展相邻的节点
• 计算节点的代价
• 路径后处理
  – 路径分割
  – 轨迹平滑，计算动态信息，完善轨迹
  – 计算动态信息，完善轨迹

Hybird A*代码逻辑

主函数Plan

apollo-master\modules\planning\planning_open_space\coarse_trajectory_generator\hybrid_a_star.cc

    bool HybridAStar::Plan(double sx, double sy, double sphi, double ex, double ey, double ephi,const std::vector<double>& XYbounds,const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,HybridAStartResult* result,const std::vector<std::vector<common::math::Vec2d>>&soft_boundary_vertices_vec,bool reeds_sheep_last_straight) {reed_shepp_generator_->reeds_sheep_last_straight_ = reeds_sheep_last_straight;// clear containers//每次规划，清空之前的缓存数据open_set_.clear();close_set_.clear();open_pq_ = decltype(open_pq_)();final_node_ = nullptr;PrintCurves print_curves;std::vector<std::vector<common::math::LineSegment2d>>obstacles_linesegments_vec;//构造障碍物轮廓线段for (const auto& obstacle_vertices : obstacles_vertices_vec) {size_t vertices_num = obstacle_vertices.size();std::vector<common::math::LineSegment2d> obstacle_linesegments;for (size_t i = 0; i < vertices_num - 1; ++i) {common::math::LineSegment2d line_segment =common::math::LineSegment2d(obstacle_vertices[i], obstacle_vertices[i + 1]);obstacle_linesegments.emplace_back(line_segment);}obstacles_linesegments_vec.emplace_back(obstacle_linesegments);}obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);for (size_t i = 0; i < obstacles_linesegments_vec_.size(); i++) {for (auto linesg : obstacles_linesegments_vec_[i]) {std::string name = std::to_string(i) + "roi_boundary";print_curves.AddPoint(name, linesg.start());print_curves.AddPoint(name, linesg.end());}}Vec2d sposition(sx, sy);Vec2d svec_to_center((vehicle_param_.front_edge_to_center() -vehicle_param_.back_edge_to_center()) / 2.0,(vehicle_param_.left_edge_to_center() -vehicle_param_.right_edge_to_center()) / 2.0);Vec2d scenter(sposition + svec_to_center.rotate(sphi));Box2d sbox(scenter, sphi, vehicle_param_.length(), vehicle_param_.width());print_curves.AddPoint("vehicle_start_box", sbox.GetAllCorners());Vec2d eposition(ex, ey);print_curves.AddPoint("end_position", eposition);Vec2d evec_to_center((vehicle_param_.front_edge_to_center() -vehicle_param_.back_edge_to_center()) / 2.0,(vehicle_param_.left_edge_to_center() -vehicle_param_.right_edge_to_center()) / 2.0);Vec2d ecenter(eposition + evec_to_center.rotate(ephi));Box2d ebox(ecenter, ephi, vehicle_param_.length(), vehicle_param_.width());print_curves.AddPoint("vehicle_end_box", ebox.GetAllCorners());//构造规划的起点和终点，并检查其有效性XYbounds_ = XYbounds;
// load nodes and obstaclesstart_node_.reset(new Node3d({sx}, {sy}, {sphi}, XYbounds_, planner_open_space_config_));end_node_.reset(new Node3d({ex}, {ey}, {ephi}, XYbounds_, planner_open_space_config_));AINFO << "start node" << sx << "," << sy << "," << sphi;AINFO << "end node " << ex << "," << ey << "," << ephi;if (!ValidityCheck(start_node_)) {AERROR << "start_node in collision with obstacles";AERROR << start_node_->GetX()<< "," << start_node_->GetY()<< "," << start_node_->GetPhi();print_curves.PrintToLog();return false;}if (!ValidityCheck(end_node_)) {AERROR << "end_node in collision with obstacles";print_curves.PrintToLog();return false;}double map_time = Clock::NowInSeconds();//使用动态规划DP来计算目标点到某点的启发代价（以目标点为DP的起点），即A*中的H//生成graph的同时获得了目标点到图中任一点的cost，作为缓存，这就是DPMap的用处grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_, obstacles_linesegments_vec_);ADEBUG << "map time " << Clock::NowInSeconds() - map_time;// load open set, pqopen_set_.insert(start_node_->GetIndex());open_pq_.emplace(start_node_, start_node_->GetCost());// Hybrid A* beginssize_t explored_node_num = 0;size_t available_result_num = 0;auto best_explored_num = explored_node_num;auto best_available_result_num = available_result_num;double astar_start_time = Clock::NowInSeconds();double heuristic_time = 0.0;double rs_time = 0.0;double node_generator_time = 0.0;double validity_check_time = 0.0;size_t max_explored_num =planner_open_space_config_.warm_start_config().max_explored_num();size_t desired_explored_num = std::min(planner_open_space_config_.warm_start_config().desired_explored_num(),planner_open_space_config_.warm_start_config().max_explored_num());static constexpr int kMaxNodeNum = 200000;std::vector<std::shared_ptr<Node3d>> candidate_final_nodes;while (!open_pq_.empty() &&open_pq_.size() < kMaxNodeNum &&available_result_num < desired_explored_num &&explored_node_num < max_explored_num) {std::shared_ptr<Node3d> current_node = open_pq_.top().first;open_pq_.pop();const double rs_start_time = Clock::NowInSeconds();std::shared_ptr<Node3d> final_node = nullptr;//true：如果生成了一条从当前点到目标点的ReedShepp曲线，就找到了最短路径//false：否则，继续Hybrid A*扩展节点if (AnalyticExpansion(current_node, &final_node)) {if (final_node_ == nullptr ||final_node_->GetTrajCost() > final_node->GetTrajCost()) {final_node_ = final_node;best_explored_num = explored_node_num + 1;best_available_result_num = available_result_num + 1;}available_result_num++;}explored_node_num++;const double rs_end_time = Clock::NowInSeconds();rs_time += rs_end_time - rs_start_time;close_set_.insert(current_node->GetIndex());if (Clock::NowInSeconds() - astar_start_time >planner_open_space_config_.warm_start_config().astar_max_search_time() &&available_result_num > 0) {break;}size_t begin_index = 0;size_t end_index = next_node_num_;std::unordered_set<std::string> temp_set;for (size_t i = begin_index; i < end_index; ++i) {const double gen_node_time = Clock::NowInSeconds();//一个grid内的最后一个路径点叫node，该grid内可以有多个路径点，//该node的next_node一定在相邻的其他grid内std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);node_generator_time += Clock::NowInSeconds() - gen_node_time;// boundary check failure handleif (next_node == nullptr) {continue;}// check if the node is already in the close setif (close_set_.count(next_node->GetIndex()) > 0) {continue;}// collision checkconst double validity_check_start_time = Clock::NowInSeconds();if (!ValidityCheck(next_node)) {continue;}validity_check_time += Clock::NowInSeconds() - validity_check_start_time;if (open_set_.count(next_node->GetIndex()) == 0) {const double start_time = Clock::NowInSeconds();CalculateNodeCost(current_node, next_node);const double end_time = Clock::NowInSeconds();heuristic_time += end_time - start_time;temp_set.insert(next_node->GetIndex());open_pq_.emplace(next_node, next_node->GetCost());}}open_set_.insert(temp_set.begin(), temp_set.end());}if (final_node_ == nullptr) {AERROR << "Hybird A* cannot find a valid path";print_curves.PrintToLog();return false;}AINFO << "open_pq_.empty()" << (open_pq_.empty() ? "true" : "false");AINFO << "open_pq_.size()" << open_pq_.size();AINFO << "desired_explored_num" << desired_explored_num;AINFO << "min cost is : " << final_node_->GetTrajCost();AINFO << "max_explored_num is " << max_explored_num;AINFO << "explored node num is " << explored_node_num<< "available_result_num " << available_result_num;AINFO << "best_explored_num is " << best_explored_num<< "best_available_result_num is " << best_available_result_num;AINFO << "cal node time is " << heuristic_time<< "validity_check_time " << validity_check_time<< "node_generator_time " << node_generator_time;AINFO << "reed shepp time is " << rs_time;AINFO << "hybrid astar total time is "<< Clock::NowInSeconds() - astar_start_time;print_curves.AddPoint("rs_point", final_node_->GetXs().front(), final_node_->GetYs().front());if (!GetResult(result)) {AERROR << "GetResult failed";print_curves.PrintToLog();return false;}for (size_t i = 0; i < result->x.size(); i++) {print_curves.AddPoint("warm_path", result->x[i], result->y[i]);}// PrintBox print_box("warm_path_box");// for (size_t i = 0; i < result->x.size(); i = i + 5) {//   print_box.AddAdcBox(result->x[i], result->y[i], result->phi[i]);// }// print_box.PrintToLog();print_curves.PrintToLog();return true;
}

碰撞检测

详见HybridAStar::ValidityCheck

bool HybridAStar::ValidityCheck(std::shared_ptr<Node3d> node) {
...
}

ReedsShepp曲线加速搜索

HybridAStar::AnalyticExpansion()

//尝试使用ReedsShepp曲线连接当前点与目标点，若成功，则Hybrid A*规划完成
//允许返回false;只返回一次true
bool HybridAStar::AnalyticExpansion(std::shared_ptr<Node3d> current_node,std::shared_ptr<Node3d>* candidate_final_node) {std::shared_ptr<ReedSheppPath> reeds_shepp_to_check =std::make_shared<ReedSheppPath>();//ReedsShepp曲线都是从当前点到终点的if (!reed_shepp_generator_->ShortestRSP(current_node, end_node_,reeds_shepp_to_check)) {return false;}//ReedsShepp曲线段的碰撞检测与越界检测if (!RSPCheck(reeds_shepp_to_check)) {return false;}// load the whole RSP as nodes and add to the close set//将连接到目标点的一段ReedsShepp曲线封装成node，放入Hybrid A*的集合中*candidate_final_node = LoadRSPinCS(reeds_shepp_to_check, current_node);return true;
}

扩展相邻的节点

//扩展节点的重点在于把车辆运动学模型的约束考虑进去，根据限定的steering angle 去搜索相邻的grid。
//扩展节点，扩展一个node就是扩展了一个grid，但是会产生多个在同一grid内的路径点
std::shared_ptr<Node3d> HybridAStar::Next_node_generator(std::shared_ptr<Node3d> current_node, size_t next_node_index) {double steering = 0.0;double traveled_distance = 0.0;//首先，根据next_node_index与next_node_num_的对比是可以区分运动方向的//这里的if-else就是区分运动正反方向讨论的（前进和倒车）//其次，车辆在当前的姿态下，既可以向左转、又可以向右转，那么steering angle的取值范围其实是//[-max_steer_angle_, max_steer_angle_]，在正向或反向下，能取next_node_num_/2个有效值//即，把[-max_steer_angle_, max_steer_angle_]分为（next_node_num_/2-1）份//所以，steering = 初始偏移量 + 单位间隔 × index//steering angle的正负取决于车的转向，而非前进的正反if (next_node_index < static_cast<double>(next_node_num_) / 2) {steering = -max_steer_angle_ +(2 * max_steer_angle_ /(static_cast<double>(next_node_num_) / 2 - 1))* static_cast<double>(next_node_index);traveled_distance = step_size_;} else {size_t index = next_node_index - next_node_num_ / 2;steering = -max_steer_angle_+ (2 * max_steer_angle_/ (static_cast<double>(next_node_num_) / 2 - 1))* static_cast<double>(index);traveled_distance = -step_size_;}// take above motion primitive to generate a curve driving the car to a// different gridstd::vector<double> intermediate_x;std::vector<double> intermediate_y;std::vector<double> intermediate_phi;double last_x = current_node->GetX();double last_y = current_node->GetY();double last_phi = current_node->GetPhi();intermediate_x.push_back(last_x);intermediate_y.push_back(last_y);intermediate_phi.push_back(last_phi);//从当前grid前进到下一个grid，一个grid内可能有多个路径点for (size_t i = 0; i < arc_length_ / step_size_; ++i) {const double next_phi = last_phi +traveled_distance /vehicle_param_.wheel_base() *std::tan(steering);const double next_x = last_x +traveled_distance *std::cos((last_phi + next_phi) / 2.0);const double next_y = last_y +traveled_distance *std::sin((last_phi + next_phi) / 2.0);intermediate_x.push_back(next_x);intermediate_y.push_back(next_y);//看车辆运动学模型——自行车模型intermediate_phi.push_back(common::math::NormalizeAngle(next_phi));last_x = next_x;last_y = next_y;last_phi = next_phi;}// check if the vehicle runs outside of XY boundaryif (intermediate_x.back() > XYbounds_[1] ||intermediate_x.back() < XYbounds_[0] ||intermediate_y.back() > XYbounds_[3] ||intermediate_y.back() < XYbounds_[2]) {return nullptr;}std::shared_ptr<Node3d> next_node = std::shared_ptr<Node3d>(new Node3d(intermediate_x, intermediate_y, intermediate_phi, XYbounds_,planner_open_space_config_));next_node->SetPre(current_node);next_node->SetDirec(traveled_distance > 0.0);next_node->SetSteer(steering);return next_node;
}

计算节点的代价

// 分别计算路径代价和启发代价，就是A*中的G和H。这里计算启发代价的方法很巧妙，用了DP，请参看 grid_a_star_heuristic_generator_->GenerateDpMap() 处的注释。
void HybridAStar::CalculateNodeCost(std::shared_ptr<Node3d> current_node, std::shared_ptr<Node3d> next_node) {//A*中走过的轨迹的代价Gnext_node->SetTrajCost(current_node->GetTrajCost() + TrajCost(current_node, next_node));// evaluate heuristic costdouble optimal_path_cost = 0.0;//A*中从当前点到目标点的启发式代价H，采用了动态规划DP来计算（以目标点为DP的起点）optimal_path_cost += HoloObstacleHeuristic(next_node);next_node->SetHeuCost(optimal_path_cost);
}

路径后处理

//将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段，分别逆向翻转轨迹点
//然后重新拼接在一起，就是最终可以发布供车行驶的轨迹
bool HybridAStar::GetTemporalProfile(HybridAStartResult* result) {std::vector<HybridAStartResult> partitioned_results;if (!TrajectoryPartition(*result, &partitioned_results)) {AERROR << "TrajectoryPartition fail";return false;}ADEBUG << "PARTION SIZE " << partitioned_results.size();//将分段的轨迹拼接起来HybridAStartResult stitched_result;for (const auto& result : partitioned_results) {std::copy(result.x.begin(), result.x.end() - 1,std::back_inserter(stitched_result.x));std::copy(result.y.begin(), result.y.end() - 1,std::back_inserter(stitched_result.y));std::copy(result.phi.begin(), result.phi.end() - 1,std::back_inserter(stitched_result.phi));std::copy(result.v.begin(), result.v.end() - 1,std::back_inserter(stitched_result.v));std::copy(result.a.begin(), result.a.end(),std::back_inserter(stitched_result.a));std::copy(result.steer.begin(), result.steer.end(),std::back_inserter(stitched_result.steer));}stitched_result.x.push_back(partitioned_results.back().x.back());stitched_result.y.push_back(partitioned_results.back().y.back());stitched_result.phi.push_back(partitioned_results.back().phi.back());stitched_result.v.push_back(partitioned_results.back().v.back());*result = stitched_result;return true;
}

路径分割

//将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段，并完善轨迹的静态、动态信息
bool HybridAStar::TrajectoryPartition(const HybridAStartResult& result,std::vector<HybridAStartResult>* partitioned_result) {const auto& x = result.x;const auto& y = result.y;const auto& phi = result.phi;if (x.size() != y.size() || x.size() != phi.size()) {AERROR << "states sizes are not equal when do trajectory partitioning of ""Hybrid A Star result";return false;}size_t horizon = x.size();partitioned_result->clear();partitioned_result->emplace_back();auto* current_traj = &(partitioned_result->back());double heading_angle = phi.front();const Vec2d init_tracking_vector(x[1] - x[0], y[1] - y[0]);double tracking_angle = init_tracking_vector.Angle();bool current_gear =std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle))<(M_PI_2);//此时的result只有路径静态信息，x,y,phi//将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段for (size_t i = 0; i < horizon - 1; ++i) {heading_angle = phi[i];const Vec2d tracking_vector(x[i + 1] - x[i], y[i + 1] - y[i]);tracking_angle = tracking_vector.Angle();bool gear =std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) <(M_PI_2);if (gear != current_gear) {current_traj->x.push_back(x[i]);current_traj->y.push_back(y[i]);current_traj->phi.push_back(phi[i]);partitioned_result->emplace_back();current_traj = &(partitioned_result->back());current_gear = gear;}current_traj->x.push_back(x[i]);current_traj->y.push_back(y[i]);current_traj->phi.push_back(phi[i]);}current_traj->x.push_back(x.back());current_traj->y.push_back(y.back());current_traj->phi.push_back(phi.back());const auto start_timestamp = std::chrono::system_clock::now();// Retrieve v, a and steer from pathfor (auto& result : *partitioned_result) {//2种不同的方式获取轨迹动态信息，v,a,steer。区别: 前者用数值优化的方法，后者用相邻点静态信息if (FLAGS_use_s_curve_speed_smooth) {//使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steerif (!GenerateSCurveSpeedAcceleration(&result)) {AERROR << "GenerateSCurveSpeedAcceleration fail";return false;}} else {//根据result中的静态信息x,y,phi，利用相邻点、逐点求动态信息v,a,steerif (!GenerateSpeedAcceleration(&result)) {AERROR << "GenerateSpeedAcceleration fail";return false;}}}const auto end_timestamp = std::chrono::system_clock::now();std::chrono::duration<double> diff = end_timestamp - start_timestamp;ADEBUG << "speed profile total time: " << diff.count() * 1000.0 << " ms.";return true;
}

轨迹平滑，计算动态信息，完善轨迹

//使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steer
bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {AINFO << "GenerateSCurveSpeedAcceleration";// sanity checkCHECK_NOTNULL(result);if (result->x.size() < 2 || result->y.size() < 2 || result->phi.size() < 2) {AERROR << "result size check when generating speed and acceleration fail";return false;}if (result->x.size() != result->y.size() ||result->x.size() != result->phi.size()) {AERROR << "result sizes not equal";return false;}// get gear infodouble init_heading = result->phi.front();const Vec2d init_tracking_vector(result->x[1] - result->x[0],result->y[1] - result->y[0]);const double gear =std::abs(common::math::NormalizeAngle(init_heading - init_tracking_vector.Angle())) < M_PI_2;// get path lenghsize_t path_points_size = result->x.size();double accumulated_s = 0.0;result->accumulated_s.clear();auto last_x = result->x.front();auto last_y = result->y.front();for (size_t i = 0; i < path_points_size; ++i) {double x_diff = result->x[i] - last_x;double y_diff = result->y[i] - last_y;accumulated_s += std::sqrt(x_diff * x_diff + y_diff * y_diff);result->accumulated_s.push_back(accumulated_s);last_x = result->x[i];last_y = result->y[i];}// assume static initial stateconst double init_v = 0.0;const double init_a = 0.0;// minimum time speed optimization// TODO(Jinyun): move to confsSpeedData speed_data;// TODO(Jinyun): explore better time horizon heuristicconst double path_length = result->accumulated_s.back();const double total_t =std::max(gear ? 1.5 *(max_forward_v_ * max_forward_v_ +path_length * max_forward_acc_) /(max_forward_acc_ * max_forward_v_): 1.5 *(max_reverse_v_ * max_reverse_v_ +path_length * max_reverse_acc_) /(max_reverse_acc_ * max_reverse_v_),10.0);if (total_t + delta_t_ >= delta_t_ * std::numeric_limits<size_t>::max()) {AERROR << "Number of knots overflow. total_t: " << total_t<< ", delta_t: " << delta_t_;return false;}const size_t num_of_knots = static_cast<size_t>(total_t / delta_t_) + 1;//使用QP优化方法求frenet系下的轨迹PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t_, {0.0, std::abs(init_v), std::abs(init_a)});// set end constraintsstd::vector<std::pair<double, double>> x_bounds(num_of_knots,{0.0, path_length});const double max_v = gear ? max_forward_v_ : max_reverse_v_;const double max_acc = gear ? max_forward_acc_ : max_reverse_acc_;const auto upper_dx = std::fmax(max_v, std::abs(init_v));std::vector<std::pair<double, double>> dx_bounds(num_of_knots,{0.0, upper_dx});std::vector<std::pair<double, double>> ddx_bounds(num_of_knots,{-max_acc, max_acc});x_bounds[num_of_knots - 1] = std::make_pair(path_length, path_length);dx_bounds[num_of_knots - 1] = std::make_pair(0.0, 0.0);ddx_bounds[num_of_knots - 1] = std::make_pair(0.0, 0.0);// TODO(Jinyun): move to confsstd::vector<double> x_ref(num_of_knots, path_length);piecewise_jerk_problem.set_x_ref(ref_s_weight_, std::move(x_ref));piecewise_jerk_problem.set_dx_ref(ref_v_weight_, max_v * 0.8);piecewise_jerk_problem.set_weight_ddx(acc_weight_);piecewise_jerk_problem.set_weight_dddx(jerk_weight_);piecewise_jerk_problem.set_x_bounds(std::move(x_bounds));piecewise_jerk_problem.set_dx_bounds(std::move(dx_bounds));piecewise_jerk_problem.set_ddx_bounds(std::move(ddx_bounds));piecewise_jerk_problem.set_dddx_bound(max_acc_jerk_);// solve the problemif (!piecewise_jerk_problem.Optimize()) {AERROR << "Piecewise jerk speed optimizer failed!";return false;}// extract outputconst std::vector<double>& s = piecewise_jerk_problem.opt_x();const std::vector<double>& ds = piecewise_jerk_problem.opt_dx();const std::vector<double>& dds = piecewise_jerk_problem.opt_ddx();// assign speed point by gearspeed_data.AppendSpeedPoint(s[0], 0.0, ds[0], dds[0], 0.0);const double kEpislon = 1.0e-6;const double sEpislon = 1.0e-6;for (size_t i = 1; i < num_of_knots; ++i) {if (s[i - 1] - s[i] > kEpislon) {ADEBUG << "unexpected decreasing s in speed smoothing at time "<< static_cast<double>(i) * delta_t_<< "with total time " << total_t;break;}speed_data.AppendSpeedPoint(s[i], delta_t_ * static_cast<double>(i), ds[i],dds[i], (dds[i] - dds[i - 1]) / delta_t_);// cut the speed data when it is about to meet end conditionif (path_length - s[i] < sEpislon) {break;}}// combine speed and path profileDiscretizedPath path_data;for (size_t i = 0; i < path_points_size; ++i) {common::PathPoint path_point;path_point.set_x(result->x[i]);path_point.set_y(result->y[i]);path_point.set_theta(result->phi[i]);path_point.set_s(result->accumulated_s[i]);path_data.push_back(std::move(path_point));}HybridAStartResult combined_result;// TODO(Jinyun): move to confsconst double kDenseTimeResoltuion = 0.5;const double time_horizon = speed_data.TotalTime() +kDenseTimeResoltuion * 1.0e-6;if (path_data.empty()) {AERROR << "path data is empty";return false;}for (double cur_rel_time = 0.0; cur_rel_time < time_horizon;cur_rel_time += kDenseTimeResoltuion) {common::SpeedPoint speed_point;if (!speed_data.EvaluateByTime(cur_rel_time, &speed_point)) {AERROR << "Fail to get speed point with relative time " << cur_rel_time;return false;}if (speed_point.s() > path_data.Length()) {break;}common::PathPoint path_point = path_data.Evaluate(speed_point.s());combined_result.x.push_back(path_point.x());combined_result.y.push_back(path_point.y());combined_result.phi.push_back(path_point.theta());combined_result.accumulated_s.push_back(path_point.s());if (!gear) {combined_result.v.push_back(-speed_point.v());combined_result.a.push_back(-speed_point.a());} else {combined_result.v.push_back(speed_point.v());combined_result.a.push_back(speed_point.a());}}combined_result.a.pop_back();// recalc step sizepath_points_size = combined_result.x.size();// load steering from phifor (size_t i = 0; i + 1 < path_points_size; ++i) {double discrete_steer =(combined_result.phi[i + 1] - combined_result.phi[i]) *vehicle_param_.wheel_base() /(combined_result.accumulated_s[i + 1] -combined_result.accumulated_s[i]);discrete_steer =gear ? std::atan(discrete_steer) : std::atan(-discrete_steer);combined_result.steer.push_back(discrete_steer);}*result = combined_result;return true;
}

计算动态信息，完善轨迹

//使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steer
bool HybridAStar::GenerateSpeedAcceleration(HybridAStartResult* result) {AINFO << "GenerateSpeedAcceleration";// Sanity Checkif (result->x.size() < 2 || result->y.size() < 2 || result->phi.size() < 2) {AERROR << "result size check when generating speed and acceleration fail";return false;}const size_t x_size = result->x.size();// load velocity from position// initial and end speed are set to be zerosresult->v.push_back(0.0);for (size_t i = 1; i + 1 < x_size; ++i) {double discrete_v = (((result->x[i + 1] - result->x[i]) / delta_t_) *std::cos(result->phi[i]) +((result->x[i] - result->x[i - 1]) / delta_t_) *std::cos(result->phi[i])) /2.0 +(((result->y[i + 1] - result->y[i]) / delta_t_) *std::sin(result->phi[i]) +((result->y[i] - result->y[i - 1]) / delta_t_) *std::sin(result->phi[i])) /2.0;result->v.push_back(discrete_v);}result->v.push_back(0.0);// load acceleration from velocityfor (size_t i = 0; i + 1 < x_size; ++i) {const double discrete_a = (result->v[i + 1] - result->v[i]) / delta_t_;result->a.push_back(discrete_a);}// load steering from phifor (size_t i = 0; i + 1 < x_size; ++i) {double discrete_steer = (result->phi[i + 1] - result->phi[i]) *vehicle_param_.wheel_base() / step_size_;if (result->v[i] > 0.0) {discrete_steer = std::atan(discrete_steer);} else {discrete_steer = std::atan(-discrete_steer);}result->steer.push_back(discrete_steer);}return true;
}

参考资料：

https://apollo.baidu.com/community/article/4
https://blog.csdn.net/linxigjs/article/details/103419697