Robot Motion Planning Robotics meet Computer Science

Motion Planning

Information required Knowledge of spatial arrangement of wkspace. E.g., location of obstaclesKnowledge of spatial arrangement of wkspace. E.g., location of obstacles Full knowledge full motion planningFull knowledge full motion planning Partial knowledge combine planning and executionPartial knowledge combine planning and execution motion planning = collection of problems

Motion problem The Problem:The Problem: – –Given an initial position PO init – –Given a goal position PO goal – –Generate: continuous path t from PO init to PO goal t is a continuous sequence of Pos’

5 Work Space and Configuration Space Reference Point 2 Degrees of freedom 3 Degrees of freedom

Components of the problem A: single rigid object - the robot - moving in Euclidean space W (the wkspace).A: single rigid object - the robot - moving in Euclidean space W (the wkspace). W = R N, N=2,3 W = R N, N=2,3 B i, I=1,…,q. Rigid objects in W. The obstaclesB i, I=1,…,q. Rigid objects in W. The obstacles A B2B2B2B2 B1B1B1B1

The Environment of Motion Planning and Obstacle Avoidance

Configuration Space Idea 1. 1.represent robot as point in space 2. 2.map obstacles into this space 3. 3.transform problem from planning object motion to planning point motion A B2B2B2B2 B1B1B1B1

Obstacles in C (cont.) Def: Connected Component q 1, q 2 belong to the same connected component of C free iff they are connected by a free path Objective of Motion Planning: generate a free path between 2 configurations if one exists or report that no free path exists.

Slide 10 Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curvesRoadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cellsCell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential functionPotential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function

Planning Approaches 1- Roadmap Captures connectivity of C free in a network of 1-D curves called “the roadmaps.” Once a roadmap is constructed: use a standard path. Roadmap Construction Methods: 1) Visibility Graph, 2) Voronoi Diagram, 3) Freeway Net and 4) Silhouette.

Roadmaps Examples q init q goal Visibility graph in 2D CS. Nodes: initial and final config + vertices of C-obstacles. Voronoi diagram with polygonal C-obstacles q init q goal

Roadmap Visibility graphVisibility graph Shakey Project, SRI [Nilsson, 1969] Shakey Project, SRI [Nilsson, 1969] Voronoi DiagramVoronoi Diagram Introduced by computational geometry researchers. Generate paths that maximizes clearance. Applicable mostly to 2- D configuration spaces. Introduced by computational geometry researchers. Generate paths that maximizes clearance. Applicable mostly to 2- D configuration spaces.

Cell Decomposition Decompose the free space into simple regions called cellsDecompose the free space into simple regions called cells Construct a non-directed graph representing adjacencies: the conectivity graphConstruct a non-directed graph representing adjacencies: the conectivity graph Search for a path forming a “channel”Search for a path forming a “channel” Two variations:Two variations: –Exact: union of cells is exactly the free space –Approximate: union included in the free space

Cell Decomposition: Example q init q goal

Trapezoidal Decomposition

Adjacency Graph Nodes: cellsNodes: cells Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent.Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent.

Quadtree Decomposition

Octree Decomposition

fullmixed empty Approximation Cell Decomposition Full cellFull cell Empty cellEmpty cell Mixed cellMixed cell –Mixed –Uncertain Configuration Space

Slide 21 Potential Fields Initially proposed for real-time collision avoidance [Khatib 1986]. Hundreds of papers published.Initially proposed for real-time collision avoidance [Khatib 1986]. Hundreds of papers published. A potential field is a scalar function over the free space.A potential field is a scalar function over the free space. To navigate, the robot applies a force proportional to the negated gradient of the potential field.To navigate, the robot applies a force proportional to the negated gradient of the potential field. A navigation function is an ideal potential field thatA navigation function is an ideal potential field that –has global minimum at the goal –has no local minima –grows to infinity near obstacles –is smooth

Slide 22 Attractive & Repulsive Fields

A* Algorithm “I believe this is this way takes me shortest to the destination…. Lets give it a try” “Hey I got struck… I’ll choose another path” Add all possible moves in an open list.Add all possible moves in an open list. Make the best move as per open list statusMake the best move as per open list status Add all executed moves in the closed listAdd all executed moves in the closed list

Chromosome - representation ChromosomeChromosome –An encoded expression of potential solutions. (usually in the form of string) Path representationPath representation –Encode the order of visiting points as a string ExampleExample – –

Chromosome - population Population – a set of chromosomesPopulation – a set of chromosomes Chromosomes are generated randomly. i.e. the population contains a set of random solutionsChromosomes are generated randomly. i.e. the population contains a set of random solutions Each robot is equipped with one populationEach robot is equipped with one population

Operator - Crossover Exchange information from two selected chromosomesExchange information from two selected chromosomes –Crossover point: the first identical node in both parents –Example: Parent1: Parent2: Child1: Child2:

Operator - Mutation Mutation rate is low (for escaping local optimum)Mutation rate is low (for escaping local optimum) 1.Randomly selects one gene (node) to mutate 2.Its following nodes are picked randomly from sequentially connected nodes

Evaluation - Fitness Fitness = Tour Length + Waiting TimeFitness = Tour Length + Waiting Time (Static) (Dynamic) (Static) (Dynamic)

ANN with Back Propagation Algorithm “Whenever this type of situation arrives… Always make this move” “Hey rules failed… I’m struck… OK make random moves till you are out” Frame input/output pairs for every situation comprising of robot position, goal position and environmentFrame input/output pairs for every situation comprising of robot position, goal position and environment Learn these and use them in decision makingLearn these and use them in decision making Make random moves when position deterioratesMake random moves when position deteriorates

Genetic Algorithms “Show me some random paths so that I may decide” “OK this path is the best to go till a point and this path the best for the other part of the journey… Let me mix them both…” Generate random complete and incomplete solutions: source to nowhere, nowhere to goal and source to goalGenerate random complete and incomplete solutions: source to nowhere, nowhere to goal and source to goal Try to mix paths to attain optimalityTry to mix paths to attain optimality Generate random paths between needed pointsGenerate random paths between needed points

Problem Formulation for Point Robot InputInput –Robot represented as a point in the plane –Obstacles represented as polygons –Initial and goal positions OutputOutput –A collision-free path between the initial and goal positions

Breadth-First Search

Slide 40 Path Non-existence Problem Obstacle Goal Initial Robot

Find planning path from o to x

initial position (1, 1), goal symbol * and obstacles are marked by boxes