Apollo规划代码ros移植-Lattice规划框架

---

前言

作为一名研究生，我开始接触了自动驾驶，目前在团队负责路径规划部分，学习了基础的规划算法后，就立志要学习并移植Baidu Apollo 的路径规划算法，此次从Lattice开始入手。
为了完成这次的的Baidu Apollo 的Lattice路径规划算法，花费了我一个多月时间，终于把原理和代码写了出来。因为我们的是基于ros的低速无人车，所以整个开发环境都是基于ros，但是Apollo不是。所以才历经磨难。我将此部分拆分为几个板块，希望能帮助到你们！代码我最后也会附上。这都不重要了，话不多说，开始你的ros Lattice规划路程吧！！！！
补充一句，我们的ros环境下的高精地图是Lanelet框架搭建的。

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的内容其实不难理解：

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

七、横向与纵向轨迹合并

        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坐标系转为世界坐标系，这样轨迹就出现了。

总结

第一步先了解大概的框架，里面涉及的详细内容，我觉得还是一篇一篇来介绍，帮助大家更快理解如何自己写出这个Lattice，当然，还有很多bug等我取修复。。。后面再见
二次规划: Apollo规划代码ros移植-Lattcie的二次规划.

这里有核心我移植的参考代码，注意只是我们测试车整体的规划部分的一小部分，里面的代码仅供参考，谨慎下载，代码不能直接运行，可以参考里面的代码，里面包括了Lattice离散空间采样规划。

代码下载链接: Lattice离散空间采样规划.