文章目录
流程图
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 obstacles
start_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, pq
open_set_.insert(start_node_->GetIndex());
open_pq_.emplace(start_node_, start_node_->GetCost());
// Hybrid A* begins
size_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 handle
if (next_node == nullptr) {
continue;
}
// check if the node is already in the close set
if (close_set_.count(next_node->GetIndex()) > 0) {
continue;
}
// collision check
const 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 grid
std::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 boundary
if (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*中走过的轨迹的代价G
next_node->SetTrajCost(
current_node->GetTrajCost() + TrajCost(current_node, next_node));
// evaluate heuristic cost
double 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 path
for (auto& result : *partitioned_result) {
//2种不同的方式获取轨迹动态信息，v,a,steer。区别: 前者用数值优化的方法，后者用相邻点静态信息
if (FLAGS_use_s_curve_speed_smooth) {
//使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steer
if (!GenerateSCurveSpeedAcceleration(&result)) {
AERROR << "GenerateSCurveSpeedAcceleration fail";
return false;
}
} else {
//根据result中的静态信息x,y,phi，利用相邻点、逐点求动态信息v,a,steer
if (!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 check
CHECK_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 info
double 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 lengh
size_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 state
const double init_v = 0.0;
const double init_a = 0.0;
// minimum time speed optimization
// TODO(Jinyun): move to confs
SpeedData speed_data;
// TODO(Jinyun): explore better time horizon heuristic
const 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 constraints
std::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 confs
std::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 problem
if (!piecewise_jerk_problem.Optimize()) {
AERROR << "Piecewise jerk speed optimizer failed!";
return false;
}
// extract output
const 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 gear
speed_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 condition
if (path_length - s[i] < sEpislon) {
break;
}
}
// combine speed and path profile
DiscretizedPath 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 confs
const 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 size
path_points_size = combined_result.x.size();
// load steering from phi
for (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 Check
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;
}
const size_t x_size = result->x.size();
// load velocity from position
// initial and end speed are set to be zeros
result->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 velocity
for (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 phi
for (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
扫一扫
540
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
