Generalized Predictive Planning for Autonomous Vehicles Scott - - PowerPoint PPT Presentation

2017/9/24 1

Generalized Predictive Planning for Autonomous Vehicles

Scott Pendleton and Marcelo H. Ang Jr.

Department of Mechanical Engineering National University of Singapore

2

Why Autonomous Vehicles? (Singapore Perspectives)

• Reduce car ownership

– Ride sharing, delivery, logistics

• Efficient use of resources

– Car, road infrastructure, less parking spaces

• Public transportation

– Last mile/first mile problem – Urban driving as opposed to highways

• Improved Productivity & Safety,

“greener”

3

Availability Accessibility Affordability

Autonomy Ride Sharing

• Multiple vehicle classes: Operational advantages for each vehicle

class favor different environments. A combined multi-class service can extend the operational area. True point-to-point service coverage is achievable.

• Disruptive technology: Automation can enable new ways of thinking

about automobiles and transportation systems in general. In particular, it can provide affordable, convenient, on-demand mobility.

Autonomous Mobility‐on‐Demand

• Vehicle sharing for first-and-last-mile transportation

INTRODUCTION & MOTIVATION

4

Environments

• Road
• Pedestrian

5

SMART=NUS Fleet

6

What we can confidently do?

• Reactive control with guaranteed safety

(lowest layer – always on)

• Mapping and Localization
• Local planning

– RRT* variant – POMDP

• Execution & Control

– More accurate path following using kinematic constraints

7

Mobility on Demand using Multi‐Class Autonomous Vehicles

8

• One North:

– Jan 2015 – 6 km route – Sept 2016 – 12 km route – 23 June 2017 – 55 km ‐NUS & Science Pk

8

• 9 vehicles

– SMART-NUS: 1 – Nutonomy: 6 – Delphi : 1 – A*STAR: 1

9

One North – Live Testing

10

Pedestrian crossing Signalized Intersection Complex intersection Road construction Road construction and jay walking

One North – May 2017

11

Public Deployment at the Chinese & Japanese Gardens (Oct 2014)

‐ Long Term Vehicle Testing ‐ To raise awareness ‐ To gain public acceptance 6 Days 360 km 500 Visitors 220 Trips 225 Surveys 98% “would ride again”

12

13

• ur autonomous mobility scooter

14

Our Planning Framework

• Interface planning modules with

perception and control modules

• Incorporate acceleration constraints
• Establish replanning timing/retriggering
• Safety mechanism design for predictive

planning

PREDICTIVE PLANNING FRAMEWORK

15

Planning Framework Overview

PREDICTIVE PLANNING FRAMEWORK

16

Planning Framework Overview

• Booking System & Mission Planner
• Mobile phone access to webserver for handling mission

requests as {Pickup Station, Dropoff Station}

• Dijkstra search over directed graph of reference path

segments

• Mapping/Localization
• Vertical features extracted from 3D point cloud gathered

from 2D LIDAR “rolling window” accumulation over time

• Obstacle Detection
• SVM performed over spatio‐temporal features of object

clusters from 2D LIDAR

PREDICTIVE PLANNING FRAMEWORK

17

Planning Framework Overview

• Cost Map Generator
• Obstacle avoidance cost set for grid locations in a 3D cost

map layered by time dimension, up to a time horizon

• Goal Generator
• Goal state set at constant distance ahead along route plan
• Steering Control
• Pure‐pursuit steering find constant radius arc target to

forward waypoint

• Speed Control
• Proportional Integral (PI) controller with switching

mechanism for throttle vs. braking

PREDICTIVE PLANNING FRAMEWORK

18

Trajectory Planner

• Control and Path Guided RRT* (CPG‐RRT*)

– Use RG, path guided sample biasing, and min‐jerk edge connection

PREDICTIVE PLANNING FRAMEWORK

• Same structure of RRT*,

but redefine subfunctions:

– “Nearest” is RG NN search – “SampleFree” uses biasing – “Line” uses an min‐jerk profile interpolation along Dubins car paths – “Steer” and “CollisionFree” are built off the “Line” function

19

Trajectory Planner: SampleFree

PREDICTIVE PLANNING FRAMEWORK

• Retain previous iteration knowledge by Φi‐1
• Bias toward route plan by Φpp
• SampleGoal

for greedy search

• RG Sample for

efficient exploration

20

Trajectory Planner: Line

PREDICTIVE PLANNING FRAMEWORK

• Controllable trajectory generation to enforce:

– Minimum turning radius (Dubins curves) – Velocity bounds – Acceleration bounds

• Edges are min‐jerk optimal for comfort

– Minimizes – Known to be 5th degree polynomial for position

• Trajectory defined over Dubins x Velocity x Time

– Configuration space

21

Trajectory Planner: Line

PREDICTIVE PLANNING FRAMEWORK

• First, solve for Dubins curve in SE(2) space
• Then, solve for position, velocity, and

acceleration w.r.t time by system of equations for boundary conditions:

• Known: pinit , vinit , ainit , pfinal , vfinal . set afinal = 0
• Solve for constants b0 … b5

22

Trajectory Planner: Line

PREDICTIVE PLANNING FRAMEWORK

• Polynomial solutions found quickly
• Bounds checked over time interval at endpoints and roots

23

Replan Timing

PREDICTIVE PLANNING FRAMEWORK

• Each plans is generated while previous plan is executed

24

Safety Checking

PREDICTIVE PLANNING FRAMEWORK

• Each solution plan is rechecked against an updated
• bservation before execution
• A new variant of braking Inevitable Collision State (ICSb)

is applied for passive safety:

– A braking maneuver must exist from the commit state following the solution trajectory to satisfy dynamic minimum braking distance – Otherwise, velocity profile of solution is overridden by constant deceleration profile up to braking distance

• “Clear zone” applied to command stop when obstacles are

very close

25

Control Interfacing

PREDICTIVE PLANNING FRAMEWORK

• Planner must know next commit state as root for plan tree

– Control and/or localization error may affect true pose – s1 is expected commit state at end of trajectory Φ0 , but instead arrive at s1’ – Where to begin plan Φ2? Introduce pose correction factor! – Start plan Φ2 from state s2+ w Δs1 (we use w = 0.5)

26

Control Interfacing

PREDICTIVE PLANNING FRAMEWORK

• Pose correction in practice:

– Red is odometry trace (series of vectors) – Yellow is commit path – Overlap correlates with velocity undershoot, gap for overshoot

27

Summary: Planning Framework

PREDICTIVE PLANNING FRAMEWORK

• Predictive planning framework

– Real‐time replanning in space‐time

• Trajectory planning algorithm (CPG‐RRT*)

– Generates min‐jerk controllable edge connections – Biased sampling for

• Near previous solution trajectory
• Near pure pursuit steering trajectory to route plan
• Near goal
• Reachable configuration space
• Passive safety assurances through adapted

braking Inevitable Collison State Avoidance (ICSb)

28

Software Overview

Fleet Management System Server Booking App Multi-Class Autonomous Vehicles Users Onboard Verification VEHICLE PLATFORM DEVELOPMENT

29

Software Overview

VEHICLE PLATFORM DEVELOPMENT

30

Hardware Overview

• Common Sensor Suite
• IMU & wheel encoders for odometry
• 1 2D LIDAR for Mapping & Localization (M&L) – fuse

w/odom

• ≥1 2D LIDAR for Obstacle Detection (OD)
• Similar Power Management & Off‐the‐shelf

Computers

• Ubuntu 14.04, ROS Indigo, i7 processor, 16GB RAM, SSD
• Differing Actuation Mechanisms to Control:
• Steering
• Braking/Throttle
• Gear Selection (Forward/Reverse)

VEHICLE PLATFORM DEVELOPMENT

31

Hardware Overview

Start with a personal mobility scooter, then add…

VEHICLE PLATFORM DEVELOPMENT

32

Hardware Overview

Start with a golf car, then add…

VEHICLE PLATFORM DEVELOPMENT

33

Hardware Overview

Start with a road car, then add…

VEHICLE PLATFORM DEVELOPMENT

34

Safety Overrides

• User Button

Controls:

• Pause
• Auto
• Manual
• E‐stops, onboard and

remote

• Visualizations
• nboard show

perception data and planned path

• Audio cues for station

arrival/departure

VEHICLE PLATFORM DEVELOPMENT

35

Experiment Setup

• Look for positive emergent behaviors
• Compare against baseline planning method:
• Decoupled spatial path and velocity planning
• Enlarge obstacle bounds forward based on velocity to

treat environment as static

• Trigger replanning only when at a stop due to blockage
• Test Scenarios:
• Pedestrian navigation
• T‐junction
• Defensive driving
• Overtaking

VEHICLE PLATFORM DEVELOPMENT

36

Experiment Setup

VEHICLE PLATFORM DEVELOPMENT

• Planning visualization

37

• Video available on YouTube: search “FMAutonomy” channel

Predictive Planning Video

https://youtu.be/eVVGZxp03Hc EXPERIMENTAL VALIDATION

38

• Reactive Control – Guaranteed Safety as a

Baseline

• Generalize predictive planning

– Plans coupled spatial path and velocity – Demonstrated over varied vehicle types and environments in high‐risk scenarios

• Reachability Guidance

– Speed improvement by factor of 9‐10

• Predictive Planning Framework

– CPG‐RRT* (biased sampling and min‐jerk edges) – Modified ICSb passive safety assurances

What have we achieved?

39

Towards Mapless Navigation

39

• You are “here”

(blue circle)

• Go to #02‐16
• Giving

intelligence to robot

– To read maps – Navigation to points in the map

What’s Next?

40

Learning how to drive

• Cars and people

around

• Moving

directions

• Relative

positions

• Speeds
• Intermediate

Goal

40

• Steering
• Brake
• Throttle

What’s Next?

41

Marcelo H ANG Jr mpeangh@nus.edu.sg