1 of 37

ME5751�Robotics Motion Planning

Yizhe Chang chang@cpp.edu

Lecture Note Set #17

​

2 of 37

Motion Planning, PRM

  • Review of motion planning concepts
    • Overview
    • C-Space
    • Three-steps of planning
    • MP algorithm metrics
  • Probabilistic roadmap
    • Overview
    • Single query PRM
    • Random exploring random tree (next lecture)
    • Multiple query PRM (next lecture)

​

3 of 37

Goal for robotic motion planning

  • To construct a collision-free path from some initial configuration to some goal configuration for a robot within a workspace containing obstacles

4 of 37

Motion planning overview

5 of 37

Motion planning overview

  • Inputs
    • Geometry of robots and obstacles
    • Kinematics/Dynamics of robots
    • Start and Goal configurations
  • Outputs
    • Continuous sequence of configurations connecting the start and goal configurations

6 of 37

Motion planning overview

    • Moving obstacles
    • Multiple robots
    • Movable objects
    • Assembly planning
    • Exploration
    • Nonholonomic constraints
    • Dynamic constraints

​

    • Stability constraints
    • Uncertainty in model, control and sensing
    • Exploiting task mechanics (under-actuated systems)
    • Physical models and deformable objects
    • Integration with higher-level planning

​

Extensive topics:

7 of 37

Motion planning overview

  • The configuration space
    • To facilitate motion planning, the configuration space was defined as a tool that can be used with planning algorithms.
  • ---(Latombe 1991)

8 of 37

The configuration space

  •  

9 of 37

Define C

  • Inflation: Plan paths for a point robot § Instead of using a robot of fixed dimensions/size, “ grow ” the obstacles to reflect how close the robot can get.

10 of 37

Discretize C

  • Cell decomposition
    • Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells

11 of 37

Discretize C

  • Roadmap
    • Represent the connectivity of the free space by a network of 1-D curves

12 of 37

Discretize C

  • Potential field
    • Define a function over the free space that has a global minimum at the goal configuration and follow its steepest descent

13 of 37

Search C

  • Given a discretization of C, a search can be carried out using a Graph Search or gradient descent, etc.
    • Example: Find a path from D to G

14 of 37

Metrics

  • Metrics for which to compare planning algorithms:

​

  • 1. Speed or Complexity
  • 2. Completeness
  • 3. Optimality
  • 4. Feasibility of solutions

15 of 37

Metric: Speed/Complexity

  • Same definition as complexity of any algorithm: O(n), O(n2), O(logn)

​

  • E.g.: Binary search complexity?
  • Given a sorted n-element array [1, 3, 5, 6, … , 123], find if element 10 is in the array or not

​

  • Complexity is?

16 of 37

Metric: Speed/Complexity

  • Same definition as complexity of algorithm O(n), O(n2), O(logn)

​

  • E.g.: Binary search complexity?
  • Given a sorted n-element array [1, 3, 5, 6, … , 123], find if element 10 is in the array or not

​

  • Complexity is? (logn)

17 of 37

Metrics: completeness

  • A complete algorithm is one that is guaranteed to find a solution if one exists, or determine if no solution exists.
  • Time Consuming!
    • An exhaustive search will search every possible path to see if it is a feasible solution.
    • A complete planner usually requires exponential time in the number of degrees of freedom, objects, etc.

18 of 37

Metric: completeness

  • Resolution completeness:
  • A resolution complete planner discretizes the space and returns a path whenever one exists in the discretized representation

19 of 37

Metric: completeness

  • A probabilistically complete planner returns a path with high probability if a path exists. It may not terminate if no path exists.

​

​

​

  • Weaker form of completeness, but usually faster.

20 of 37

Optimality

  • Resolution of Discretization can lead to sub-optimal solutions

21 of 37

Optimality

  • Some algorithms will only guarantee sub-optimal solutions (e.g. Greedy Search).

22 of 37

Feasibility

  • Not all planners take into account the exact model of the robot or environment.
  • E.g. Proportional control for differential drive robot

23 of 37

Motion Planning, PRM

  • Review of motion planning concepts
    • Overview
    • C-Space
    • Three-steps of planning
    • MP algorithm metrics
  • Probabilistic roadmap
    • Overview
    • Single query PRM
    • Random exploring random tree (next lecture)
    • Multiple query PRM (next lecture)

​

24 of 37

SQ PRM Algorithm overview

  • Add start configuration cstart to roadmap R( N, E )
  • Loop
    • Randomly select an existing node c to expand
    • Randomly generate a new node c ’ from c
    • If edge e from c to c ’ is collision-free, add (c’, e) to R
    • If c ’ belongs to endgame region, return path(R)
    • Return if stopping criteria is met

​

  • To get the path, simply search a path on the roadmap R from last c’ to the cstart

25 of 37

Single-query probabilistic roadmap

  • Single-Query PRMs
    • Try to only sample a subspace of F that is relevant to the problem.
    • Probabilistically complete assuming C is expansive [Hsu et. al. 2000].
    • Very fast for many applications (allow for on-the-fly planning).

26 of 37

SQ PRM Algorithm overview

  • E.G.

​

27 of 37

SQ PRM Algorithm overview

  • Iteration 1

28 of 37

SQ PRM Algorithm overview

  • Iteration 2

29 of 37

SQ PRM Algorithm overview

  • Iteration 3

30 of 37

SQ PRM Algorithm overview

  • Iteration 11

31 of 37

SQ PRM Algorithm overview

  • Construct path

32 of 37

SQ PRM Algorithm overview

  • Construct path

33 of 37

Sampling strategy: node selection/generation

  • Step 3: randomly select node c to expand
  • Step 4: Randomly Generate new Node c ’ from c
    • One could pick the next node for expansion by picking from all nodes in the roadmap with equal probability.
    • This is easy to implement, but leads to poor expansion -> Clustering

34 of 37

Sampling strategy: improvement for wheel kinematics

35 of 37

Sampling strategy: End game region

  • We define the endgame region E, to be the set of configurations that have a simple connection to the goal configuration
    • For each planning problem, we can define a unique method of making simple connections.
    • This method will inherently define E.

36 of 37

Sampling strategy: End game region

  • Given the complexity of most configuration spaces, it is very difficult to model E.
  • In practice, we develop a simple admissibility test to calculate if a configuration c ’ belongs to the E
  • At every iteration of the algorithm, this test is used to determine if newly generated configurations are connected to the goal configuration.

37 of 37

Sampling strategy: End game region

  • Several endgame definitions exist:
  • 1. The set of all configurations within some radius r of the goal configuration
  • 2. The set of all configurations that have “simple” , collision-free connection with the goal configuration.
    • Example: Use circular arc for differential drive robots.