Behavior Planner Initial Research

Behavior Planner

Purpose & Endgoal

The behavior planner is the second layer in the overall planning stack. Based on our desired route (provided by route planner) and valid paths (generated by path planner), we need to make a decision on what specific behavior the vehicle should perform. The behavior is then used to select a path for the vehicle to follow.

Basic behaviors include stopping, slowing down, speeding up, or maintaining current speed. More complex behaviors involve turning, changing lanes, yielding when necessary, following traffic lights, and adjusting speed when faced with obstacles. All these behaviors simplify down to change in velocity/acceleration sent to the control nodes.

The time frame between the car's current state and goal state is the decision horizon.

Requirements

Chooses the path which advances car towards end-destination in the most efficient manner.
Chooses the path which avoids collisions and accidents.
Chooses the path which is the most comfortable for passengers.
Follows all road laws (stop signs, yielding, roundabout).

Approach

We will use the finite state machine approach given in the paper Self-Driving Cars: A Survey and expanded upon in Finite State Machine based Vehicle System for Autonomous Driving in Urban Environments.

Inputs

Car state (position, direction, velocity)
- Used for calculations
Previous FSM state
- Used as starting point in state machine
Path Candidates (generated by path planner)
- Iterated through to select path with highest reward value
List of static obstacles modeled as circles (2d position + radius)
- Checking if traveling to goal position on a given path runs into a static obstacle
List of dynamic obstacles modeled as circles (2d position + radius + velocity vector)
- Same reasoning as static obstacles, but need velocity vector to calculate if collision may occur

Outputs

Selected behavior for vehicle output as velocity values for motors

Constants

Parameter for effective range of collision detection (σ)
Car radius for collision detection
Maximum lateral acceleration (|a_l|_max = 5000 in paper)
Safety gain for speed adjustment - influences driving style for overtaking/following a moving obstacle (k_safe = 0.8 in paper)
Reference speed for path - speed limit (v_curve = 50 in paper)
TBD

Algorithm

We start by filtering out paths with high cost (path-planning function should remove invalid paths like those that go off the road). For the remaining paths, we run each through our state machine (starting state is the state vehicle is currently in) until we reach an accepting state. Using a reward function, we can calculate the path with the highest reward (for example, staying in motion would have a higher reward than fully stopping). Our chosen decision horizon sets how forward we want to look in our path to make our decision.

Example States: Cruise (maintain speed), Slow down (reduce speed), Stop (immediately reduce speed), Turn left (more speed in left wheel than right). Example Transition Functions: Sudden stop by front car, Detection of any object in path, Arrived at target position

This FSM model can meet our requirements and can be vastly improved with the later addition of a SVM model, which is a trained network that outputs what decision a human would make given a situation.