path planning configuration space and potential...
TRANSCRIPT
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𝜁𝑖 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓
2𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 ≤ 𝑑
𝑈𝑎𝑡𝑡 𝑞 = 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 −1
2𝜁𝑖𝑑
2 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓 > 𝑑
Defining the Attractive Potential
• Given
• 𝑈𝑎𝑡𝑡 =
1
2𝜁𝑖 𝒪𝑖 𝑞 − 𝒪𝑖 𝑞𝑓
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
16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
Combining Attractive and Repulsive Potentials
• 𝑈 𝑞 = 𝑈𝑎𝑡𝑡 𝑞 +𝑈𝑟𝑒𝑝 𝑞
16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
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
16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds
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)
Wavefront Example (taken from 16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds)
Wavefront Example (taken from 16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds)
Wavefront Example (taken from 16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds)
Wavefront Example (taken from 16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds)
Wavefront Example (taken from 16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds)
Wavefront Example (taken from 16-735, Howie Choset, with slides from Ji Yeong Lee, G.D. Hager and Z. Dodds)
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?