Path Planning Configuration Space and Potential Functionsrobots.iit.edu/uploads/2/5/7/1/25715664/mmae_540_-_lecture_9_-_path_planning...The Configuration Space •Complete specification

Path Planning –Configuration Space and

Potential FunctionsProf. Matthew Spenko

Overview

• Where are we? – Localization Problem

• What’s the best way to get to where we want to go? – Path Planning Problem

• How do we ensure we are actually following the best path? – Control Problem (with Kinematics and Dynamics) coupled with Trajectory Planning

• Path Planning – Complete Solutions• Finds a solution if it exists and signals, in a finite time, if a solution does not

exist

• Computationally expensive – grows exponentially in time as # of DOF grows linearly

• Non-complete solutions are effective

Configuration Space

The Configuration Space

• Complete specification of the location of every point on the robot

• Set of all possible configuration = configuration space, Q

• If rigid links, joint angles provide configuration

• One revolute joint arm - 𝑄 = 𝑆1 where 𝑆1 represents the unit circle

• Planar two revolute joint arm 𝑄 = 𝑆1 × 𝑆1 = 𝑇2 where 𝑇2

represents a torus

• Examples in n-dimensional Euclidean Space, ℝ𝑛

• Point in space 𝐪 = 𝑥1, 𝑥2, 𝑥3 , ℝ3, 3 DOF

• Two points on a plane 𝐪 = 𝑥1, 𝑥2, 𝑥3, 𝑥4 , ℝ2 × ℝ2 = ℝ4, 3 DOF

Obstacles and the Configuration Space

• Define the path planning problem to be:

• Find a continuous mapping 𝑐: 0,1 → 𝒬 such that no configuration in the path causes a collision between the robot and an obstacle

• Configuration space obstacle 𝒬𝒪𝑖

• Workspace obstacle 𝒲𝒪𝑖

• 𝒬𝒪𝑖 = 𝑞 ∈ 𝒬 𝑅 𝑞 ∩𝒲𝒪𝑖 ≠ 0

• Free space or free configuration space, 𝒬𝑓𝑟𝑒𝑒, is the set of configurations at which the robot does not intersect an obstacle

• 𝒬𝑓𝑟𝑒𝑒 = 𝒬\ ⋃𝑖𝒬𝒪𝑖

• Free path 𝑐: 0,1 → 𝒬𝑓𝑟𝑒𝑒

Representation of Workspace obstacle in the configuration space

Potential Functions

Potential Functions

• As DOF increase, becomes difficult to represent the configuration space.

• Potential Field method bypasses building 𝒬𝑓𝑟𝑒𝑒 a priori• Incrementally explore 𝒬𝑓𝑟𝑒𝑒 while searching for the goal

• Construct a potential field with one global minimum and zero local minimum

Potential Field

• 𝑈 𝑞 = 𝑈𝑎𝑡𝑡 𝑞 + 𝑈𝑟𝑒𝑝 𝑞

• Optimization problem – Find global minimum in 𝑈 starting from an initial configuration 𝑞0

• Gradient Descent algorithm

• Consider 𝑈 𝑞 as energy

• Gradient is force

• 𝜏 𝑞 = −𝛻𝑈 𝑞

• For 1st order problems can consider force to be velocity

• Robot terminates when gradient is 0

• Occurs at critical point, Hessian (2nd derivative)

Define Attractive and Repulsive Potential

• What properties do we want the attractive potential to have?

• Should be increasing• attractive force should be stronger the farther the robot is away from the

goal

• Should be monotonically increasing• Don’t want force to decrease at any point as the robot gets closer to the goal

Defining the Attractive Potential

• One possibility: 𝑈𝑎𝑡𝑡 𝑞 linearly related to distance such that: 𝑈𝑎𝑡𝑡 𝑞 = 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓

• What happens then?• 𝛻𝑈𝑎𝑡𝑡 𝑞 = 1 when any distance from goal

• 𝛻𝑈𝑎𝑡𝑡 𝑞 = 0 when at goal

• Problems?• Discontinuity at goal leads to instability

• Uniform “force” or “velocity” does not pull the robot in toward the goal with a stronger effort the farther it is away from the goal

• Options?

Defining the Potential Function

• Use a quadratic function• Gradually decreases the gradient so no discontinuities

• Stronger farther away

• 𝑈𝑎𝑡𝑡 =1

2𝜁𝑖 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓

2

• Attractive force equal to gradient: 𝜏 𝑞 = 𝜁𝑖 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓

• Any problems?• What happens when the robot is really far away from the goal?

• Combine the two functions

• 𝑈𝑎𝑡𝑡 =

1


2𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 ≤ 𝑑

𝑈𝑎𝑡𝑡 𝑞 = 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 −1

2𝜁𝑖𝑑

2 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 > 𝑑

Defining the Attractive Potential

• Given

• 𝑈𝑎𝑡𝑡 =

1


2𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 ≤ 𝑑

𝑈𝑎𝑡𝑡 𝑞 = 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 −1

2𝜁𝑖𝑑

2 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 > 𝑑

• The attractive force (or velocity) becomes

• 𝐹𝑎𝑡𝑡 = −𝜁𝑖 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓

−𝑑𝜁𝑖𝒪𝑖 𝑞 −𝒪𝑖 𝑞𝑓

𝒪𝑖 𝑞 −𝒪𝑖 𝑞𝑓

16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds

Attractive Force Example

• Link lengths = 1

• Using forward kinematics:

• Assume d to be sufficiently large for ease of example:

Defining the Repulsive Field

• Question - what features do we want?

• Cannot allow the robot to hit an obstacle at any cost!

• When the robot is a sufficient distance away from an obstacle, the obstacle should have little to no influence on the path.

• Define 𝑈𝑟𝑒𝑝 =

1

2𝜂𝑖

1

𝜌 𝑂𝑖 𝑞−

1

𝜌0

2

𝜌 𝑂𝑖 𝑞 ≤ 𝜌0

0 𝜌 𝑂𝑖 𝑞 > 𝜌0

• Where • 𝜂𝑖 is a scaling factor

• 𝜌 𝑂𝑖 𝑞 is a function that defines the distance from an obstacle

• 𝜌0 is some predefined position

Defining the Repulsive Field

• Taking the gradient yields:

• 𝐹𝑟𝑒𝑝 = 𝜂𝑖1

𝜌 𝑂𝑖 𝑞−

1

𝜌0

1

𝜌2 𝑂𝑖 𝑞𝛻𝜌 𝑂𝑖 𝑞

• Where 𝛻𝜌 𝑂𝑖 𝑞 represents the gradient, 𝛻𝜌 𝑥 , is evaluated at 𝑥 = 𝑂𝑖 𝑞

• If obstacle is convex

• Point 𝑏 lies on obstacle as is defined as closest point to 𝑂𝑖

• Then


Combining Attractive and Repulsive Potentials

• 𝑈 𝑞 = 𝑈𝑎𝑡𝑡 𝑞 +𝑈𝑟𝑒𝑝 𝑞


Translating forces into Torques

• Use Jacobian: 𝐉𝑇𝐅 = 𝛕

• Do this before summing torques• Summing forces =0

• Torque does not equal 0

• Find global minimum in 𝑈 𝑞 = 𝑈𝑎𝑡𝑡 𝑞 + 𝑈𝑟𝑒𝑝 𝑞

• Use gradient descent method• Start in initial configuration

• Take a small step in the direction of the negative gradient

• repeat

Implementing the Algorithm

• Do not apply the torque directly to the system

• Use the algorithm to determine direction, not magnitude of where to

go: 𝑞 𝑖 + 1 = 𝑞 𝑖 + 𝛼𝜏 𝑞

𝜏 𝑞

• Step size is 𝛼

• If 𝛼 is too small – waste computation time

• If 𝛼 is too large – risk “jumping” into obstacles

Potential FieldPotential Problems

Concave and Multiple Obstacles

• Distance function, 𝜌, not necessarily differentiable

• May be discontinuity

• Results in non-smooth path or jitter

• Ensure no overlap in region of influence of two distinct obstacle by adjusting 𝜌0

Computing Distance in the Plane

• Two methods (of many)• Mobile robot implementation – use sensors to give distance data

• Brushfire algorithm – assumes configuration space discretized into pixels

• Brushfire• Create grid

• Define 4-point or 8 point connectivity

• Seed grids with obstacles as 1

• See free space as 0

4 point 8 point

Brushfire Algorithm

• Label all 0-valued pixels adjacent to 1-valued pixels as 2 (using 4-point or 8-point to determine adjacency)

• Label all 0-valued pixels adjacent to 2-valued pixels as 3

• …

• Terminate when all pixels have an assigned value

• Each cell gives minimum distance to obstacle

• Find gradient by taking differences with all adjacent cells


Local Minima Problem

• Alternate search techniques to gradient descent that solve this problem - Randomized Path Planner – incorporates random walks

• Wavefront planner• Requires configuration space represented as grid

Wavefront Planner – Solve Local Minima Problem

• Only valid in grid spaces

• Choose 4-point or 8-point connectivity

• Give goal grid a 2

• Give all other free grids a 0

• Start from goal

• Assign all 0 pixels adjacent to 2-pixels a 3

• Assign all 0 pixels adjacent to 3-pixels a 4

• …

• Terminate when the wavefront reaches the start location pixel.

Wavefront Example (taken from 16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds)








Two possible shortest paths

What about the Links?

Documents

Path Planning Configuration Space and Potential Functionsrobots.iit.edu/uploads/2/5/7/1/25715664/mmae_540_-_lecture_9_-_path_planning...The Configuration Space •Complete specification