# [MOOC] Apollo Lesson 6: Planning

**Published:** January 31, 2021
**Tags:** Computer Vision, Apollo


---

My note for lesson 6 of MOOC course: Self-Driving Fundamentals - Featuring Apollo. Content: Identify several different approaches Apollo uses to develop trajectories for autonomous vehicles.

In planning, we incorporate HP maps, localization and prediction to build a trajectory for the vehicle. The first step is **route navigation**, which focuses on how to go from A to B on the map. This step is like the navigation app on your phone. After route planning, we zoom into **trajectory planning**, which make subtle decisions to avoid obstacles and create a smooth ride for passengers.

## Routing

Routing is to find the best path from A to B on a map.

**Inputs:**

- The map: Apollo provides map data, including the road network and realtime traffic information.
- Our current position on the map
- Our destination

**Output:** The route to the destination.

## World to Graph

Apollo uses graph to represent the map.

- **Nodes:** Sections of road
- **Edges:** Connections between those sections

**Map → Map Graph → Best Route on Graph → Best Route on Map**

We can use **A\*** algorithm to search on the graph.

## 3D trajectory

3D trajectory is made of 2D position (waypoint) and time.

![3D trajectory](/posts-data/notes/apollo-self-driving-car-course/l6-planning/waypoints.png)

Real-world planning faces multiple constraints:

- The trajectory should be collision-free
- We want passengers to feel comfortable
- The trajectory should be physically viable for the vehicle

## Frenet coordinates

Cartesian coordinates aren't optimal for a vehicle because it's hard to know how far the car has traveled or whether it's deviated from the center of the lane.

![Car in Cartesian coordinates](/posts-data/notes/apollo-self-driving-car-course/l6-planning/cartesian-coor.png)

Therefore, we need Frenet coordinates, which describe the position of a car with respect to the road. Frenet has 2 axes: longitudinal axis and lateral axis.

![Car in Frenet coordinates](/posts-data/notes/apollo-self-driving-car-course/l6-planning/frenet.png)

## Path-velocity decoupled planning

**Trajectory Planning = Path Planning + Speed Planning**

- **Path Planning:** Generate candidate curves. Rank each path by smoothness, safety, deviation from the lane center, .. and select the best path.
- **Speed Planning:** Determine what speed of vehicle in each waypoint. → **Speed profiler**.

### ST graph

ST graph is a tool which can help us design and select the speed profile. In an ST graph, **S** represents the longitudinal displacement of the vehicle and **T** represents time.

In order to build the best speed profile, we discretize the ST graph into multiple cells. Suppose the prediction module predicts that a car will cut into our lane from time t0 to t1. Since the car will occupy positions S0 to S1 during that time period, we draw a yellow rectangle to block this area on ST graph. In order to avoid a collision, our speed profile must not intersect this rectangle.

![Speed profile construction](/posts-data/notes/apollo-self-driving-car-course/l6-planning/speed-profile.png)

In real-world situations, many constraints can affect the optimization process for speed profile, such as speed limit, physical properties of the car, and the obstacles.

### Optimization

From discrete solution, we use quadratic programming to generate a smooth speed profile.

### End-to-end example

Suppose we are control a car to avoid an obstacle.

**Generate candidate paths → Evaluate paths using a cost function → Select the lowest cost path → Use ST graph to create speed profile → Use quadratic programming to smooth the path and speed profile → Bind results to construct a trajectory.**

## Lattice programming

Lattice programming is a trajectory generation approach using Frenet coordinates. We decompose 3D problem into different 2D problems: 3D trajectory → the longitudinal trajectory with timestamps (ST trajectory) + the lateral offset relative to the longitudinal trajectory (SL trajectory).

- **Step 1:** Project our environment onto a longitudinal axis and a lateral axis and develop the ST and SL trajectory independently.
- **Step 2:** Combine the results.

Source of Lattice programming in Apollo: [https://github.com/ApolloAuto/apollo/blob/master/modules/planning/planner/lattice/lattice_planner.cc](https://github.com/ApolloAuto/apollo/blob/master/modules/planning/planner/lattice/lattice_planner.cc).