Skip to content

Behavior Planner Overview

Jump to bottom
eganj edited this page Feb 3, 2022 · 3 revisions

Finite State Machine

Diagram

FSM-V1-1

Required Inputs

  • Perception Data
    • Recognized vehicles in both incoming and outgoing lanes, position and velocity.
    • Recognized pedestrians in and around road, position and velocity.
    • Static obstacles which are blocking current route.
  • Route Data
    • Stop Signs
    • Yield Signs (Possibly perception's role instead?)
    • Roundabout Sign / Entrance Location
    • Current global route (needed for path-planner)
    • Destination Location
  • Vehicle Info
    • Position
    • Velocity
  • FSM
    • Previous state of machine

Output

  • Will publish current state after FSM run as a ROS message
  • Will send goal position, velocity, and global route to the motion planner

Process

  1. FSM initially starts at the lane-keeping state.
  2. Route planner and perception passes required data, and vehicle info is also retrieved.
  3. Each transition function condition is checked. Note that since overlap can occur between conditions (i.e. multiple conditions can be true), precedence is given to those that are the closest to Nova. For example, a stopped vehicle is closer to Nova than the stop sign, so the function that will be used will be the stopped vehicle transition function. Full precedence ordering is given below.
  4. State is transitioned to if necessary. Otherwise, current state is kept.
  5. Path planner takes FSM output and gives the path of lowest cost to controls. Can plug path back into FSM for validation, but would be potentially expensive and unnecessary. If no valid path found, then emergency stop.
  6. Controls executes path.
  7. Either based on route global data update or perception data update (FSM listens for both), run FSM again starting from current state.
  8. Process continues until either emergency stop occurs or destination is reached.

Function Precedence Ordering

Line-Keeping State

  • All "Slow Down To Stop" Functions - Indicate a more urgent need to decrease vehicle speed.
    • Function precedence based on vehicle distance to each function's "target" (i.e. stop sign, pedestrian, etc.)
  • All "Decrease Speed" Functions - Less urgent as nothing is directly in vehicle's path
    • Function precedence based on vehicle distance to each function's target

Slow Down To Stop

  • V = 0 - If vehicle has reached 0 velocity, transition to stopped state.
  • Pedestrian Exits Road / Vehicle Starts Moving - Higher precedence given to condition that initially triggered call to slow down to stop.

Stopped Functions are mutually exclusive and cannot be true at same time.

Decrease Speed

  • Object entered the lane - Has highest precedence as now urgency has increased.
  • Rest of self-loop functions are based on what object is closest to vehicle.
  • Rest of exit functions are based on what object last triggered this state.

To Be Explored Egan brought up valid concerns over handling traffic. The case most relevant to our demo is right-of-way. Our current approach would wait until no other cars are at the intersection/sign (no obstacles detected), but in real life a driver would yield to drivers who reached before him.

Technically, the motion planner's output isn't required as input for the FSM. We could use it, but I don't really see how it would help improve correctness. This could be something we look into as well.

Demo 2 Expected Behavior Outline

This is a sample outline of the expected base behaviors for Nova during Demo-2. This assumes no other drivers or pedestrians present on the route.

Grey Sector

  • Drive Straight
  • Left Turn
  • Drive Straight
  • Right Turn
    • Stop Sign
  • Drive Straight
    • Yield
    • Stop Sign

Red Sector

  • Cross Intersection (Left Turn)
  • Right Turn
  • Drive Straight
    • Curve
    • Speed Bumps
  • Left Turn

Orange Sector

  • Drive Straight
  • Right Turn
  • Drive Straight
    • Yield
    • Stop Sign
  • Cross Intersection (Forward)
  • Drive Straight
    • Yield

Yellow Sector

  • Drive Straight
    • Curve
    • Yield

Green Sector

  • Drive Straight
    • Yield
    • Stop Sign
  • Cross Intersection (Forward)
  • Drive Straight
    • Yield

Blue Sector

  • Drive Straight
    • Yield
    • Curve
    • Yield
    • Stop Sign
  • Cross Intersection (Left Turn)
  • Drive Straight
    • Speed Bumps

Indigo Sector

  • Drive Straight
    • Yield
    • Yield
  • Take Roundabout Counterclockwise
    • Yield
    • Right Turn
    • Right Turn
  • Drive Straight
    • Curve
    • Yield

Purple Sector

  • Drive Straight
    • Stop Sign
  • Cross Intersection (Forward)
  • Drive Straight
    • Stop Sign
  • Cross Intersection (Forward)
  • Right Turn
  • Drive Straight
    • Stop Sign
  • Cross Intersection (Left Turn)
  • Drive Straight
  • Right Turn
  • Drive Straight

Links https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9268341&tag=1 https://github.com/A2Amir/Behavior-Planning-by-Finite-State-Machine https://www.researchgate.net/figure/Autonomous-Ground-Vehicle-Finite-State-Machine-for-High-Level-Decision-Making_fig4_324416578

🏠 Home

General

Development & Simulation

Software Design

Outdated or Uncategorized

Clone this wiki locally