Generalized Predictive Planning for Autonomous Vehicles

2017/9/24 1

Generalized Predictive Planning for Autonomous Vehicles

Scott Pendleton and Marcelo H. Ang Jr.

Department of Mechanical EngineeringNational 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

AvailabilityAccessibility

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 theChinese & Japanese Gardens (Oct 2014)

‐ Long Term Vehicle Testing

‐ To raise awareness‐ To gain public acceptance

6 Days360 km

500 Visitors220 Trips225 Surveys

98% “would ride again”

12

13

our 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 OverviewPREDICTIVE 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


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


18

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

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


• 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


• Retain previous iteration knowledge by Φi‐1

• Bias toward route plan by Φpp

• SampleGoalfor greedy search

• RG Sample for efficient exploration

20

Trajectory Planner: Line


• 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



• 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



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

23

Replan Timing


• Each plans is generated while previous plan is executed

24

Safety Checking


• Each solution plan is rechecked against an updated observation 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


• 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


• 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 FrameworkPREDICTIVE 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 OverviewVEHICLE 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)


31

Hardware OverviewStart with a personal mobility scooter, then add…


32

Hardware OverviewStart with a golf car, then add…


33

Hardware OverviewStart with a road car, then add…


34

Safety Overrides• User Button Controls:• Pause• Auto• Manual

• E‐stops, onboard and remote

• Visualizations onboard show perception data and planned path

• Audio cues for station arrival/departure


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


36

Experiment SetupVEHICLE 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 peoplearound

• Movingdirections

• Relativepositions

• Speeds• IntermediateGoal

40

• Steering• Brake• Throttle

What’s Next?

41

Marcelo H ANG [email protected]

Generalized Predictive Planning for Autonomous Vehicles

Documents