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