Probabilistic Methods for Kinodynamic Path Planning Based On Advanced Lectures by: Paul Elliott, Aisha Walcott, Nathan Ickes and Stanislav Funiak 16.412/6.834J Cognitive Robotics February 11, 2004 Outline ? Probabilistic roadmaps ? Planning in the real world ? Planning amidst moving obstacles ? RRT-based planners ? Conclusions Outline ? Probabilistic roadmaps ? Planning in the real world ? Planning amidst moving obstacles ? RRT-based planners ? Conclusions Applicability of Lazy PRM to Spheres By Paul Elliott Zvezda Service Module Place Start and Goal Goal Start Place nodes Select a set of Neighbors A* search 16 18 18 2 1 2 A* search A* search Check Nodes Check Nodes Check Nodes A* search Check Edges A* Search Lazy PRM Algorithm ? Build Roadmap ? Start and Goal Nodes ? Uniform Dist Nodes ? Nearest Neighbors Build Roadmap Shortest Path (A*) Check Nodes Check Edges Remove Colliding node/edge Node Enhancement Collision Collision No path found q init , q goal Lazy PRM Algorithm ? Shortest Path (A*) ? Heuristic = distance to the goal ? Path length = distance between nodes Build Roadmap Shortest Path (A*) Check Nodes Check Edges Remove Colliding node/edge Node Enhancement Collision Collision No path found q init , q goal Lazy PRM Algorithm ? Check Nodes & Edges ? Search from Start and End for collisions ? First check Nodes then Edges Build Roadmap Shortest Path (A*) Check Nodes Check Edges Remove Colliding node/edge Node Enhancement Collision Collision No path found q init , q goal Lazy PRM Algorithm ? Remove Node/Edge ? For Nodes, remove all edges ? For Edges, just remove the edge Build Roadmap Shortest Path (A*) Check Nodes Check Edges Remove Colliding node/edge Node Enhancement Collision Collision No path found q init , q goal Lazy PRM Algorithm ? Node Enhancement ? Add ? uniformly ? Add ? clustered around midpoints of removed edges Build Roadmap Shortest Path (A*) Check Nodes Check Edges Remove Colliding node/edge Node Enhancement Collision Collision No path found q init , q goal Where PRMs Fall Short ? Using PRM 1. Construct roadmap 2. A* finds path in roadmap 3. Must derive control inputs from path ? Path itself is not enough: need control inputs ? Cannot always execute an arbitrary path Path Planning in the Real World Real World Robots ? Have inertia ? Have limited controlability ? Have limited sensors ? Face a dynamic environment ? Face an unreliable environment Static planners (ex. PRM) are not sufficient Outline ? Probabilistic roadmaps ? Planning in the real world ? Planning amidst moving obstacles ? RRT-based planners ? Conclusions Two Approaches to Path Planning Kinematic: only concerned with motion, without regard to the forces that cause it ? Performs well for systems where position can be controlled directly ? Does not work well for systems with significant inertia Kinodynamic: incorporates dynamic constraints ? Plans velocity as well as position Representing Static State ? Configuration space represents the position and orientation of a robot ? Sufficient for static planners like PRM Example: Steerable car Configuration space (x, y, θ) x y θ Representing Dynamic State ? State space incorporates the dynamic state of a robot x y θ Example: Steerable car State space X = (x, y, θ, x, y, θ) ... Representing Dynamic State ? Working in state space allows planner to incorporate dynamic constraints on path ? Examples: maximum velocity, Examples: minimum turning radius ? Working in state space doubles the dimensionality of the planning problem ? Math becomes exponentially harder Incorporating Dynamic Constraints ? Robot actuators can apply limited force ? For some states, collision is unavoidable ? Path planner should avoid these states Obstacle Imminent collision region ? Collision regions: X coll ? Clearly illegal ? Region of Imminent Collision: X ric ? Where robot’s actuators cannot prevent a collision ? Free Space: X free = X – (X coll + X ric ) ? Collision-free planning involves finding paths that lie entirely in X free Regions in State Space Xcoll Xric Xfree ? Not all degrees of freedom are controllable ? Consider a steerable car ? Equation of Motion: G( s, s ) = 0 ? Constraint is a function of state and time derivative of state Constraints on Maneuvering ?System has 3 dof (x, y, θ), but only one controllable dof (steering angle) . . Constraints on Maneuvering ? Nonholonomic: fewer controllable DOFs than total DOFs ? Nonholonomic systems cannot execute an arbitrary path in configuration space ? This is a problem for PRM and other configuration space planners Outline ? Probabilistic roadmaps ? Planning in the real world ? Planning amidst moving obstacles ? RRT-based planners ? Conclusions Planning Amidst Moving Obstacles Moving Obstacles Planner (MOP): A PRM extension that accounts for both kinematic and dynamic constraints of a robot navigating amidst moving obstacles Problem ? Kinodynamic motion planning amidst moving obstacles with known trajectories ? Example: Asteroid avoidance problem Asteroid Avoidance Problem http://antwrp.gsfc.nasa.gov/apod/astropix.html Autonomous Vehicle “Spacecraft” Moving Obstacles “Asteroids” Known trajectories Docking station ?Path-planning among moving obstacles with known trajectories MOP Overview Similar to PRM, except ? Does not pre-compute the roadmap ? Incrementally constructs the roadmap by extending it from existing nodes ? Roadmap is a directed tree rooted at initial state × time point and oriented along time axis MOP Overview For each query, the planner incrementally builds new roadmap in state × time space ? Why? The environment includes moving obstacles that change location (state) continuously with time ? Each node in the roadmap must be indexed by both its state and the time it is attained ? Ex: node n is valid at time t, however at time t+δ node n collides with a moving obstacle Building the Roadmap Nodes (state, time) Collision- free? Randomly choose existing node 1. Randomly choose an existing node Select control input u at random 2. Randomly select control input u 3. Randomly select integration time interval δ∈[0, δ max ] Integrate equations of motion from an existing node with respect to u for some time interval δ 4. Integrate equations of motion Building the Roadmap (cont.) Collision-free trajectory 5. If edge is collision-free then Store control input u with new edge u 6. Store control input with new edge Add new node 7. Add new node to roadmap Result: Any trajectory along tree satisfies motion constraints and is collision-free! Nodes (state, time) Solution Trajectory Start state and time (s start , t start ) Goal state and time (s goal , t goal ) 1. If goal is reached then 2. Proceed backwards from the goal to the start MOP details: Inputs and Outputs Planning Query: ? Let (s start , t start ) denote the robot’s start point in the state × time space, and (s goal , t goal ) denote the goal ? t goal ∈I goal , where I goal is some time interval in which the goal should be reached Solution Trajectory: ? Finite sequence of fixed control inputs applied over a specified duration of time ? Avoids moving obstacles by indexing each state with the time when it is attained ? Obeys the dynamic constraints MOP details: Roadmap Construction ? Objective: obtain new node (s’, t’) ? s’ = the new state in the robot’s state space ? t’ = t + δ, current time plus the integration time Each iteration: 1. Select an existing node (s, t) in the roadmap at random 2. Select control input u at random 3. Select integration time δ at random from [0, δ max ] MOP details: Roadmap Construction 3. Integrate control inputs over time interval 4. Edge between (s, t) and (s’, t’) is checked for collision with static obstacles and moving obstacles 5. If collision-free, store control input u with the new edge 6. (s’, t’) is accepted as new node MOP details: Uniform Distribution Ensure Uniform Distribution of Space: ? Why? If existing roadmap nodes were selected uniformly, the planner would pick a node in an already densely sampled region ? Avoid oversampling of any region by dividing the state×time space into bins Achieving Uniform Node Distribution Space 1. Equally divide space 2. Denote each section as a bin; number each bin bin 1 bin 2 bin 3 bin 4 bin 5 bin 6 bin 7 bin 8 bin 9 bin 10 bin 11 bin 12 bin 13 bin 14 . . . . . . . . . *bins store roadmap nodes that lie in their region Array Equal-sized bins bin 1 bin 2 bin 3 …. Existing nodes 3. Create an array of bins Achieving Uniform Node Distribution Planner randomly picks a bin with at least one node At that bin, the planner picks a node at random bin 1 bin 2 bin 3 …. Achieving Uniform Node Distribution Insert new node into its corresponding bin Achieving Uniform Node Distribution Demonstration of MOP ?Point–mass robot moving in a plane ?State s = (x, y, , ) ? x ? y (s start , t start ) (s goal , t goal ) Moving obstacles Static obstacle Point-mass robot Demonstration of MOP Summary ? MOP algorithm incrementally builds a roadmap in the state×time space ? The roadmap is a directed tree oriented along the time axis ? By including time the planner is able to generate a solution trajectory that ? avoids moving and static obstacles ? obeys the dynamic constraints ? Bin technique to ensure that the space is explored somewhat uniformly Outline ? Probabilistic roadmaps ? Planning in the real world ? Planning amidst moving obstacles ? RRT-based planners ? Conclusions Planning with RRTs ? RRTs: Rapidly-exploring Random Trees ? Similar to MOP ? Incrementally builds the roadmap tree ? Integrates the control inputs to ensure that the kinodynamic constraints are satisfied ? Different exploration strategy from MOP ? Extends to more advanced planning techniques How it Works ? Build a rapidly-exploring random tree in state space (X), starting at s start ? Stop when tree gets sufficiently close to s goal Goal Start Building an RRT ? To extend an RRT: ? Pick a random point a in X ? Find b, the node of the tree closest to a ? Find control inputs u to steer the robot from b to a a b u Building an RRT ? To extend an RRT (cont.) ? Apply control inputs u for time δ, so robot reaches c ? If no collisions occur in getting from a to c, add c to RRT and record u with new edge a b u c Executing the Path Once the RRT reaches s goal ? Backtrack along tree to identify edges that lead from s start to s goal ? Drive robot using control inputs stored along edges in the tree Principle Advantage ? RRT quickly explores the state space: ? Nodes most likely to be expanded are those with largest Voronoi regions Advanced RRT Algorithms ? Single RRT biased towards the goal ? Bidirectional planners ? RRT planning in dynamic environments Example: Simple RRT Planner ? Problem: ordinary RRT explores X uniformly →slow convergence ? Solution: bias distribution towards the goal (Courtesy of the Motion Strategy Library at the University of Illinois. Used with permission.) Goal-biased RRT BIASED_RANDOM_STATE() 1 toss←COIN_TOSS() 2 if toss = heads then 3 Return s goal 4 else 5 Return RANDOM_STATE() Goal-biased RRT (Courtesy of the Motion Strategy Library at the University of Illinois. Used with permission.) The world is full of… ? If too much bias, the planner may get trapped in a local minimum A different strategy: ? Pick a point near s goal ? Based on distance from goal to the nearest v in G ? Gradual bias towards s goal Also, can apply sampling methods for PRMS local minima Rather slow convergence Bidirectional Planners ? Build two RRTs, from start and goal state ? Complication: need to connect two RRTs ? local planner will not work (dynamic constraints) ? bias the distribution, so that the trees meet (Courtesy of the Motion Strategy Library at the University of Illinois. Used with permission.) (Courtesy of the Motion Strategy Library at the University of Illinois. Used with permission.) Bidirectional Planner Algorithm Bidirectional Planner Example Bidirectional Planner Example Demos ? Holonomic ? Removing an object from a cage ? Nonholonomic/kinodynamic ? Steerable car moving in forward direction ? Car with limited controllability (Courtesy of the Motion Strategy Library at the University of Illinois. Used with permission.) Conclusions ? Path planners for real-world robots must account for dynamic constraints ? Building the roadmap tree incrementally ? ensures that the kinodynamic constraints are satisfied ? avoids the need to reconstruct control inputs from the path ? allows extensions to moving obstacles problem Conclusions ? MOP and RRT planners are similar ? Well-suited for single-query problems ? RRTs benefit from the ability to steer a robot toward a point ? RRTs explore the state more uniformly ? RRTs can be biased towards a goal or to grow into another RRT