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