From Coursera, State Estimation and Localization for Self-Driving Cars by University of Toronto

https://www.coursera.org/learn/motion-planning-self-driving-cars

## Reactive Planning in Static Environments

### Trajectory Propagation

Varying Input for Obstacle Avoidance

- To avoid obstacles,we require more complex maneuvers
- We can vary the steering input according to a steering function to navigate complex scenarios
- Main objective of local planning is to compute the control inputs (or trajectory) required to navigate to goal point without collision

### Collision Checking

**Collision Checking Challenges**

- Computationally intensive
- Requires perfect information to guarantee safety
- Needs to be approximated,and must be robust to noise

**Swath-based checking**

- Area occupied by car along path generated by rotating the car’s footprint by each $x, y, \theta$ along the path
- Swath along path is the union of each rotated and translated footprint

$$S = \cup_{p\in P} F(x(p), y(p), \theta(p))$$- $P$ is a path composed of a set of points $p$
- $F$ denotes a function that returns the set points contained in a footprint rotated by theta, and translated by x and y.

- Swath can then be checked for collisions
- check this entire set to see if there are any obstacles inside it. If there are, the path contains a collision and otherwise it’s collision-free.

- Discretized Example
- Initial state of the vehicle in the occupancy grid, with base link at the origin
- Will need to rotate and translate to get the new footprint at point $(1.0,2.0,\pi/2)$
- To compute the occupancy grid index for each point in the footprint, add half the width/height of the occupancy grid,and divide by the grid resolution $\delta$
- Swath is then the union of these indices

$$x_i = \frac{x(p)+\frac X2}{\delta} \qquad y_i = \frac{y(p)+\frac Y2}{\delta} \qquad S = S\cup (x_i,y_i)$$

- Lattice Planner Swaths
- Swath based methods are useful for lattice planners, as the swath sets can be computed offline
- Online collision checking is then simplified using lookup tables

- Speed and Robustness
- Need to improve speed
- Need to be robust to noise
- Use conservative approximations to solve both of these problems
- Want algorithmic speedup without sacrificing path quality

**Circle-based collision checking**

Conservative Approximations

- Conservative approximations may report a collision evenif there isn’t one,but will never miss a collision if it were to actually happen
The car can be completely encapsulated by three circles

Circle approximation is effective because it is fast to check if an occupancy grid point lies within a circle of radiusr centered at $(x_c,y_c)$

- If obstacle in occupancy gridlies within circle, a collision is reported
- Otherwise, due to conservative approximation, no collision is possible

**Pitfalls posed by imperfect information and discretization errors**

Discretization Resolution

- Collision checking accuracy is impacted by the resolution of our discretization
- Higher fidelity collision checking requires a finer resolution for occupancy grids and path points, and will require more computational resources

### Trajectory Rollout Algorithm

Trajectory rollout algorithm steps include:

- Trajectory propagation
- Collision checking
- Path selection

**Trajectory Rollout Planner**

- Uses trajectory propagation to generate candidate set of trajectories
- Among collision-free trajectories, select trajectory that makes the most progress to goal

**Trajectory Set Generation**

- Each trajectory corresponds to a fixed control input to our model
- Typically uniformly sampled across range of possible inputs

- More sampled trajectories leads to more maneuverability
- Fewer sampled trajectories improves computation time

**Trajectory Propagation**

- Holding the velocity constant and varying the steering angle gives candidate set of trajectories

**Objective Function**

- Rewarding progress to a goal point is the ultimate goal of motion planning

$$J = \alpha_1 |x_n - x_{goal} | + \alpha_2 \sum^n_{i=1} k^2_i + \alpha_3 \sum^n_{i=1} |x_i - P_{center}(x_i) |$$ - where $k_i$ is the curvature, $P_{center}$ is the deviation from centerline

**Receding Horizon Example**

- Steering angle bounded by $|\delta| \leq \pi/4$
- First trajectory corresponds to $\delta = -\pi/4$
- Second trajectory corresponds to $\delta = -\pi/8$
- Positive steering angle trajectories are symmetrical with the negative steering angle trajectories

- Time discretization of 0.1s, with a 2s planning horizon
- Using the vehicle footprint, compute the swath for each trajectory

- Only 1s of 2s trajectory is executed at each planning iteration
- Objective is evaluated across all collision-free trajectories
- Only the most progress one will be chosed to execute

- Planning horizon end time recedes towards time point when goal is reached
- This process is continued until goal is reached
- This planner is greedy and sub-optimal, but is fast enough to allow for online planning

### Dynamic Windowing

Bicycle Model + Acceleration Constraints

- Higher order terms handled by adding constraints
- More comfort for passengers，but less maneuverability

Constraint in Terms of Steering Angle

- Angular acceleration constraint may prevent us from selecting certain maneuvers based on current angular velocity
- Change in steering angle between planning cycles is bounded
- Similar logic applies for changes in linear velocity inputs between planning cycles