文章目录
前言
学习了基础的规划算法后，就立志要学习并移植Baidu Apollo 的路径规划算法，此次从Lattice开始入手。为了完成这次的的Baidu Apollo 的Lattice路径规划算法，花费了不少时间，终于把原理和代码写了出来。
想要可跑工程，看这里：
链接: Apollo6.0规划代码ros移植-路径规划可跑工程分享.
Lattice的执行步骤如下：
一、获取一条参考线，并将其转换为路径点格式
说白了就是参考线离散化为一个一个点。
引用一位大佬的图：
这一步被我省去了， 因为我们的参考线是根据中心线生成的（目前使用Lanelet高精地图是这样处理），本身点是离散的，不用再离散化，但是需要插值然后优化参考线路径点。
二、计算参考线上初始规划点的匹配点
就是找距离起点最近的参考线上的点。这里注意Apollo的参考线上的路径点，是一个类封装，里面有位置xy，有朝向，曲率，曲率导数等信息。
三、Frenet坐标系
可参考：
https://www.jianshu.com/p/630c19f2bb9a
四、静态和动态障碍物的ST和SL图构建
后面会有专门文章。
五、横向规划与纵向规划
原理其实不难，说白了就是分为横向规划与纵向规划。
void Trajectory1dGenerator::GenerateTrajectoryBundles(
const PlanningTarget &planning_target,
Trajectory1DBundle *ptr_lon_trajectory_bundle,
Trajectory1DBundle *ptr_lat_trajectory_bundle,
Lattice_InitialConditions Lattice_Initial, std::vector<SL_Result> SL_Static)
{
GenerateLongitudinalTrajectoryBundle(planning_target, ptr_lon_trajectory_bundle);
GenerateLateralTrajectoryBundle(SL_Static, ptr_lat_trajectory_bundle);
return;
}
首先，进行纵向规划，纵向规划为V-T规划，即速度和时间的规划。
void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(
const PlanningTarget &planning_target, Trajectory1DBundle *ptr_lon_trajectory_bundle) const
{
// cruising trajectories are planned regardlessly.~
GenerateSpeedProfilesForCruising(planning_target.cruise_speed(), ptr_lon_trajectory_bundle);
GenerateSpeedProfilesForPathTimeObstacles(ptr_lon_trajectory_bundle);
if (planning_target.has_stop_point())
{
GenerateSpeedProfilesForStopping(planning_target.stop_point(), ptr_lon_trajectory_bundle);
}
}
纵向规划有三种模式：巡航模式，停止模式，根车和超车模式。我在ros下目前只用巡航模式，后面会跟大家仔细分享这些模式的区别和用法。先入门了解一下框架。
其次，进行横向规划，横向规划分为采样点规划和二次规划，Apollo默认进入二次规划，因为速度快。
void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
Trajectory1DBundle *ptr_lat_trajectory_bundle) const
{
if (!FLAGS_lateral_optimization)
{
auto end_conditions = end_condition_sampler_.SampleLatEndConditions();
// Use the common function to generate trajectory bundles.
GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,
ptr_lat_trajectory_bundle);
}
else
{
double s_min = init_lon_state_[0];
double s_max = s_min + FLAGS_max_s_lateral_optimization;
double delta_s = FLAGS_default_delta_s_lateral_optimization;
auto lateral_bounds = ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);
// LateralTrajectoryOptimizer lateral_optimizer;
std::unique_ptr<LateralQPOptimizer> lateral_optimizer(new LateralOSQPOptimizer);
lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);
auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();
ptr_lat_trajectory_bundle->push_back(
std::make_shared<PiecewiseJerkTrajectory1d>(lateral_trajectory));
}
}
轨迹是由多项式曲线生成，就是根据首末点状态，进行多项式曲线的系数求解，这里总结一下：
纵向规划：
巡航模式：四次多项式（起始点s、v、a和末点的v、a）
停止模式：五次多项式（起始点s、v、a和末点的s、v、a）
根车和超车模式：五次多项式（起始点s、v、a和末点的s、v、a）
横向规划：
采样点的规划：五次多项式
二次规划:调用osqp求解器的库
六、轨迹评价
分为纵向轨迹评价和横向轨迹评价。
double TrajectoryEvaluator::Evaluate(
const PlanningTarget &planning_target,
const PtrTrajectory1d &lon_trajectory,
const PtrTrajectory1d &lat_trajectory,
std::vector<double> *cost_components) const
{
// Costs:
// 1. Cost of missing the objective, e.g., cruise, stop, etc.
// 2. Cost of longitudinal jerk
// 3. Cost of longitudinal collision
// 4. Cost of lateral offsets
// 5. Cost of lateral comfort
// Longitudinal costs
double lon_objective_cost = LonObjectiveCost(lon_trajectory, planning_target, reference_s_dot_);
double lon_jerk_cost = LonComfortCost(lon_trajectory);
double lon_collision_cost = LonCollisionCost(lon_trajectory);
double centripetal_acc_cost = CentripetalAccelerationCost(lon_trajectory);
// decides the longitudinal evaluation horizon for lateral trajectories.
double evaluation_horizon = std::min(FLAGS_speed_lon_decision_horizon, lon_trajectory->Evaluate(0,lon_trajectory->ParamLength()));
std::vector<double> s_values;
for (double s = 0.0; s < evaluation_horizon;
s += FLAGS_trajectory_space_resolution)
{
s_values.emplace_back(s);
}
// Lateral costs
double lat_offset_cost = LatOffsetCost(lat_trajectory, s_values);
double lat_comfort_cost = LatComfortCost(lon_trajectory, lat_trajectory);
if (cost_components != nullptr)
{
cost_components->emplace_back(lon_objective_cost);
cost_components->emplace_back(lon_jerk_cost);
cost_components->emplace_back(lon_collision_cost);
cost_components->emplace_back(lat_offset_cost);
}
return lon_objective_cost * FLAGS_weight_lon_objective +
lon_jerk_cost * FLAGS_weight_lon_jerk +
lon_collision_cost * FLAGS_weight_lon_collision +
centripetal_acc_cost * FLAGS_weight_centripetal_acceleration +
lat_offset_cost * FLAGS_weight_lat_offset +
lat_comfort_cost * FLAGS_weight_lat_comfort;
}
cost的内容其实不难理解：
- Cost of missing the objective, e.g., cruise, stop, etc.
- Cost of longitudinal jerk
- Cost of longitudinal collision
- Cost of lateral offsets
- Cost of lateral comfort
七、横向与纵向轨迹合并
DiscretizedTrajectory TrajectoryCombiner::Combine(
const std::vector<PathPoint> &reference_line, const Curve1d &lon_trajectory,
const Curve1d &lat_trajectory, const double init_relative_time)
{
DiscretizedTrajectory combined_trajectory;
double s0 = lon_trajectory.Evaluate(0, 0.0);
double s_ref_max = reference_line.back().s();
double accumulated_trajectory_s = 0.0;
PathPoint prev_trajectory_point;
double last_s = -FLAGS_numerical_epsilon;
double t_param = 0.0;
while (t_param < FLAGS_trajectory_time_length)
{
// linear extrapolation is handled internally in LatticeTrajectory1d;
// no worry about t_param > lon_trajectory.ParamLength() situation
double s = lon_trajectory.Evaluate(0, t_param);
if (last_s > 0.0)
{
s = std::max(last_s, s);
}
last_s = s;
double s_dot = std::max(FLAGS_numerical_epsilon, lon_trajectory.Evaluate(1, t_param));
double s_ddot = lon_trajectory.Evaluate(2, t_param);
if (s > s_ref_max)
{
break;
}
double relative_s = s - s0;
// linear extrapolation is handled internally in LatticeTrajectory1d;
// no worry about s_param > lat_trajectory.ParamLength() situation
double d = lat_trajectory.Evaluate(0, relative_s);
double d_prime = lat_trajectory.Evaluate(1, relative_s);
double d_pprime = lat_trajectory.Evaluate(2, relative_s);
PathPoint matched_ref_point = PathMatcher::MatchToPath(reference_line, s);
double x = 0.0;
double y = 0.0;
double theta = 0.0;
double kappa = 0.0;
double v = 0.0;
double a = 0.0;
const double rs = matched_ref_point.s();
const double rx = matched_ref_point.x();
const double ry = matched_ref_point.y();
const double rtheta = matched_ref_point.theta();
const double rkappa = matched_ref_point.kappa();
const double rdkappa = matched_ref_point.dkappa();
std::array<double, 3> s_conditions = {rs, s_dot, s_ddot};
std::array<double, 3> d_conditions = {d, d_prime, d_pprime};
CartesianFrenetConverter::frenet_to_cartesian(rs, rx, ry, rtheta, rkappa, rdkappa, s_conditions, d_conditions, &x, &y, &theta, &kappa, &v, &a);
if (prev_trajectory_point.has_x() && prev_trajectory_point.has_y())
{
double delta_x = x - prev_trajectory_point.x();
double delta_y = y - prev_trajectory_point.y();
double delta_s = std::hypot(delta_x, delta_y);
accumulated_trajectory_s += delta_s;
}
TrajectoryPoint trajectory_point;
trajectory_point.mutable_path_point()->set_x(x);
trajectory_point.mutable_path_point()->set_y(y);
trajectory_point.mutable_path_point()->set_s(accumulated_trajectory_s);
trajectory_point.mutable_path_point()->set_theta(theta);
trajectory_point.mutable_path_point()->set_kappa(kappa);
trajectory_point.set_v(v);
trajectory_point.set_a(a);
trajectory_point.set_relative_time(t_param + init_relative_time);
combined_trajectory.AppendTrajectoryPoint(trajectory_point);
t_param = t_param + FLAGS_trajectory_time_resolution;
prev_trajectory_point = trajectory_point.path_point();
}
return combined_trajectory;
}
合并的原理也简单，其实就是t采样，得到s，根据s采样，得到L，然后组合得到完整的S、L、t，也没啥好解释~~再从Frenet坐标系转为世界坐标系，这样轨迹就出现了。
效果
请看教程系列：
https://www.bilibili.com/video/BV1zj411Z7VH/?spm_id_from=333.1007.top_right_bar_window_history.content.click&vd_source=be5ccb1d324ad1a880d0e667733d8023
4570
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
