Apollo规划代码ros移植-Lattice规划框架_移植lattice到ros仿真

---

前言

学习了基础的规划算法后，就立志要学习并移植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;
}
1
2
3
4
5
6
7
8
9
10

首先，进行纵向规划，纵向规划为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);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12

纵向规划有三种模式：巡航模式，停止模式，根车和超车模式。我在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));
            }
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

轨迹是由多项式曲线生成，就是根据首末点状态，进行多项式曲线的系数求解，这里总结一下：
纵向规划：
巡航模式：四次多项式（起始点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;
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

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;
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82

合并的原理也简单，其实就是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