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

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

## Smooth Local Planning

### Parametric Curves

Boundary Conditions

- Boundary conditions must hold on either endpoint of a path
- The starting and ending conditions of the path

Kinematic Constraints

- Maximum curvature along path cannot be exceeded
- Ensures that car can drive along path

Parametric Curves

- Parametric curve r can be described by a set of parameterized equations
- Parameter denotes path traversal,can be arc length or unitless
- Example: Cubic spline formulation for x and y

Path Optimization

- Want to optimize path according to cost functionalf
- Parametric curves allow for optimizing over parameter space, which simplifies optimization formulation

Non-Parametric Path

- Reactive planner used non-parametric paths underlying each trajectory
- Path was represented as a sequence of points rather than parameterized curves

**Path Parameterization Examples**

- Two common parameterized curves are quintic splines and cubic spirals
- Both allow us to satisfy boundary conditions, and can be optimized parametrically
- Quintic Splines
- x and y are defined by 5th order splines
- Closed form solution available for (x,y,0,K) boundary conditions

$$x(u) = \alpha_5 u^5 + \alpha_4 u^4+ \alpha_3 u^3 + \alpha_2 u^2 + \alpha_1 u + \alpha_0 $$

$$y(u) = \beta_5 u^5 + \beta_4 u^4+ \beta_3 u^3 + \beta_2 u^2 + \beta_1 u + \beta_0 \quad u \in [0,1] $$ - Challenging to constrain curvature due to nature of spline’s curvature
- Due to potential discontinuities in curvature or its derivatives

- Polynomial Spirals
- Spirals are defined by their curvature as a function of arc length
- Closed form curvature definition allows for simple curvature constraint checking
- Curvature is well-behaved between sampled points as well due to polynomial formulation

- Spiral position does not have a closed form solution
- Fresnel integrals need to be evaluated numerically
- This can be done using Simpson’s rule

### Path Planning Optimization

Position Integrals and Simpson’s Rule

- Simpson’s rule has improved accuracy over other methods
- Divides the integrationinterval into n regions, and evaluates the function at each region boundary

Applying Simpson’s Rule

- Applying Simpson’s rule with n=8
- $\theta(s)$ has a closed form solution
- Substituting our integrand for x(s) and y(s) into Simpson’s rule gives us our approximations $x_s(s)$ and $y_s(s)$

Boundary Conditions via Simpson’s Rule

- Using our Simpson’s approximations, we can now write out the full boundary conditions in terms of spiral parameters
- Can now generate a spiral that satisfies boundary conditions by optimizing its spiral parameters and its length, sy

### Conformal Lattice Planning

Conformal Lattice

- Goal is to plan a feasible collision-free path to goal
- Conformal lattice exploits road structure to speed up planning
- Lattice paths are laterally offset from a goal point along road

Goal Horizon

- Short lookahead improves computation time,but reduces ability to avoid obstacles
- Goal point is dynamically calculated based on speed and other factors
- Endpoints are sampled laterally offset from goal according to the heading along the road

### Velocity Profile Generation

Behavioural Planner Reference Velocity

- Need to compute reference velocity
- Can use the speed limit of the road as a starting point
- Behavioural planner maneuver will also influence reference velocity
- E.g. a stopping maneuver requires us to stop

Dynamic Obstacles

- Lead dynamic obstacles regulate our speed to prevent collisions
- Time to collision is an important metric to preserve when driving with lead vehicles
- Need to reach the red point at lead vehicle speed to ensure there is no collision

Curvature and Lateral Acceleration

- Curvature recorded at intermediate points, $K_i$
- Velocity bounded by maximum lateral acceleration from comfort rectangle
- Final velocity selected as minimum of BP reference, lead vehicle speed and curvature speed limit

Trapezoidal Profile

- Alternative profile is trapezoidal, car decelerates to slower speed before stopping
- Useful for stop sign scenarios

- Deceleration chosen to be well within comfort rectangle to maximize passenger comfort