1

Single Robot Motion Planning

Liang-Jun ZhangCOMP790-058

Sep 22, 2008

2

Motion planning is the ability for an agent to compute its own motions in order to achieve

certain goals. All autonomous robots and digital actors should eventually have this ability

3

Piano Mover’s Problem

• 2D or 3D rigid models

4

5

Types of Robots

• Rigid robots

• Articulated robots

Manipulator, VideoHumanoid robots

6

Goal of Motion Planning

• Compute motion strategies, e.g.:– geometric paths – time-parameterized trajectories– sequence of sensor-based motion commands

• To achieve high-level goals, e.g.:– go from A to B without colliding with obstacles– assemble product P– build map of environment E– find object O

7

Plan MoveSense

8

Basic Problem Statement:

Compute a collision-free path for a rigid or articulated object among static obstacles

Inputs:•Geometry of moving object and obstacles•Kinematics of moving object (degrees of freedom)•Initial and goal configurations (placements)

Output:Continuous collision-free path connecting the initial and goal configurations

9

Types of Path Constraints

• Local constraints – Collision-free paths

• Differential constraints– A car cannot move sideways– Have bound curvature

• Global constraints– Shortest or optimal paths

Path Planning

Motion Planning

10

Is It Easy? alpha puzzle

11

Outline (Mon & Wed)

• Path planning for a point robot

• Configuration space

• Approximate cell decomposition

• Sampling-based motion planning

12

Path Planning for a Point Robot

g

s

13

Visibility Graphs

Introduced in the Shakey project at SRI in the late 60s

Can produce shortest paths in a point robot in 2D

g

s

14

Simple (Naïve) Algorithm1. Install all obstacles vertices in VG, plus the start

and goal positions2. For every pair of nodes u, v in VG3. If segment(u,v) is an obstacle edge then4. insert (u,v) into VG5. else6. for every obstacle edge e7. if segment(u,v) intersects e8. then goto 29. insert (u,v) into VG10.Search VG using BFS (any other graph search

scheme)

15

Complexity

• A point robot in 2D using visibility graphs Simple algorithm: O(n3) time Rotational sweep: O(n2 log n) Optimal algorithm: O(n2) Space: O(n2)

g

s

16

Motion Planning Framework

• Motion planning: a search problem in continuous space

Discretization

Continuous representation

Graph search

17

g

s

Issues With Visibility Graphs

Difficult to extend from point robots to rigid

or articulated robots

A L-shaped robot

18

Outline (Mon & Wed)

• Path planning for a point robot

• Configuration space

• Approximate cell decomposition

• Sampling-based motion planning

19

Configuration Space:Tool to Map a Robot to a Point

20

Example: rigid robot in 2-D workspace

• 3-parameter specification: q = (x, y, ) with [0, 2).– 3-D configuration space

robot

workspace

reference point

x

y

reference direction

21

Configuration Space (C-Space)• The configuration of a moving

object is a specification of the position of every point on the object. – Usually a configuration is expressed

as a vector of position & orientation parameters: q = (q1, q2,…,qn).

• Configuration space – C-space– The set of all possible configurations.– A configuration is a point in C-space.

q=(q1, q2,…,qn)

qq11

qq22

qq33

qqnn

22

Dimension of C-space

• The dimension of a configuration space is the minimum number of parameters needed to specify the configuration of the object completely.

• It is also called the number of degrees of freedom (dofs) of a moving object.

23

Example: Rigid Robot in 3-D Workspace

• What is the configuration space?

24

Example: rigid robot in 3-D workspace

• q = (position, orientation) = (x, y, z, ???)

• Number of dofs = 6

• Euler angles

xx

y

zz

xxyy

zz

x

y

z

xx

yy

zz

1 2 3 4

25

C = S1 x S1

θ

Φ

Topology of C-Space

• The topology of C is usually not that of a Cartesian space Rn.

0 2

2

θ

Φ

θ

Φ

26

Example: Rigid Robot in 3-D Workspace

• Number of dofs = 6• Topology: R3 x SO(3)

27

Example: An Articulated Robot

Number of dofs = 3 C-space is 3 dimensional

28

An articulated object is a set of rigid bodies connected at the joints.

An Articulated Robot Puma 560

Number of dofs = 6

29

Obstacles in C-space

Workspace Configuration Space

xyRobot

Start

Goal

Free

Obstacle C-obstacle

A 2D Translating Robot

30

Obstacles in C-space• A configuration q is collision-free, or free, if a

moving object placed at q does not intersect any obstacles in the workspace.

• The free space F is the set of free configurations.

• A configuration space obstacle (C-obstacle) is the set of configurations where the moving object collides with workspace obstacles.

31

Disc in 2-D Workspace

workspace configuration space

32

Polygonal Robot Translating in 2-D Workspace

workspace configuration space

33

Articulated Robot in 2-D Workspace

workspace configuration space

34

• Live demo of C-space of a 2-link robot

35

Fundamental Questionworkspace configuration space

Are two given points connected by a path?

36

Problem: Computing C-obstacles

• Input:– Polygonal moving object translating in 2-D

workspace – Polygonal obstacles

• Output: configuration space obstacles represented as polygons

37

Minkowski Sum

},|{ BbAabaBA

A B

38

Exercise

AB

A B = ?

39

Minkowski Sum

},|{ BbAabaBA

40

Minkowski Sum

41

Minkowski Sum

},|{ BbAabaBA

42

Minkowski Sum

• The Minkowski sum of two sets A and B, denoted by AB, is defined as A B = { a+b | a A, bB }

• Similarly, the Minkowski difference is defined as

A – B = { a–b | aA, bB }

p

q

43

Minkowski Sum of Non-convex Polyhedra

44

Configuration Space Obstacle• If P is an obstacle in the workspace and M is a

translating object. Then the C-space obstacle corresponding to P is P – M.

P -MObstacle

PRobot

M

C-obstacle

Classic result by Lozano-Perez and Wesley 1979

45

Minkowski Sum of Convex Polygons

• The Minkowski sum of two convex polygons A and B of m and n vertices respectively is a convex polygon A + B of m + n vertices.– The vertices of A + B are the “sums” of

vertices of A and B.

46

Complexity of Minkowksi Sum

• 2D convex polygons: O(n+m) • 2D non-convex polygons: O(n2m2)

– Decompose into convex polygons (e.g., triangles or trapezoids), compute the Minkowski sums, and take the union

• 3-D convex polyhedra: O(nm)• 3-D non-convex polyhedra: O(n3m3)

47

Complexity of Computing C-obstacles

• 3D rigid robots with both translational and rotational DOF– 6D C-space– Arrangement of non-linear surfaces– High combinatorial complexity

• Conclusion– Explicit computation of the boundary of C-obstacle is

difficult and impractical for robots more than 3 DOFs

48

Wednesday’s Lecture

• Path planning for a point robot

• Configuration space

• Approximate cell decomposition

• Sampling-based motion planning