EE 451 - Path and Trajectory Planning

OutlinePath and Trajectory Planning

EE 451 - Path and Trajectory Planning

H.I. Bozma

Electric Electronic Engineering

Bogazici University

November 26, 2018

H.I. Bozma EE 451 - Path and Trajectory Planning


Path and Trajectory PlanningIntroductionPath Planning AlgorithmsProbabilistic Roadmap Methods (PRM)Trajectory PlanningPotential Fields



IntroductionPath Planning AlgorithmsProbabilistic Roadmap Methods (PRM)Trajectory PlanningPotential Fields

Getting robot to move

◮ So far - Kinematics: The map btw configuration space DOFand workspace position

◮ Velocity kinematics: The map btw configuration spacevelocities and workspace velocities

◮ How to get the robot move and do what we want it to do?

◮ Assume: Initial and final configurations of the robot is given.




Getting robot to move - Motion planning




Path vs Trajectory Planning

◮ Path: A sequence of points (either in configuration orworkspace)

◮ Trajectory: A sequence of points with timing




Planning Considerations - Configuration (Joint) Space




Planning Considerations - Workspace




Obstacles

◮ No consideration of restrictions due to the workspace(Obstacles!)

◮ Path and Trajectory Planning: Collision free paths ortrajectories

◮ Computationally complex!




Notation

◮ Configuration space Q

◮ Workspace W

◮ Obstacles in workspace Oi ⊂ W

◮ All obstacles O = ∪Oi

◮ Robot with q ∈ Q → A(q) ⊂ W

◮ Configuration space obstacles QO = {q ∈ Q | A(q) ∩ O 6= ∅}

◮ Free configuration space Qfree = Q \ QO

◮ Initial configuration qs ∈ Qfree , Final configuration qf ∈ Qfree




Problem Statement:

Given qs , qf ∈ Qfree , find a collision-free path γ : [0, 1] → Qfree

such that γ(0) = qs and γ(1) = qf




Approaches

◮ Not feasible to generate Qfree completely!

◮ Search algorithm – Incremental generation of Qfree

◮ Roadmap methods◮ Potential fields




RM - General Approach

◮ Generate a representation for Qfree

◮ Generate 1-D network of curves (time as varying parameter)where each arc → Collision-free path btw two points

◮ Generate a path in Qfree from qs to qa◮ Generate a path in Qfree from qf to qb◮ Generate a path in Qfree from qa to qb




Probabilistic Roadmap Methods (PRM)

◮ Sampling the configuration space: A set of randomconfigurations are generated → Nodes

◮ Connect pairs of configurations: Plan trajectories betweennodes

◮ Enhancement: If trajectories are disconnected, generate newpaths to ensure connectivity




Example - PRM Approach




◮ Advantages

◮ No problems of local minima!

◮ Problems

◮ Computational◮ Trajectory planning◮ Not reactive




Why Trajectory Planning?

◮ In PRM’s, additional step of trajectory planning since we havea sequence of points along the path!

◮ Discrete time computation:◮ γ : [0, 1] → Qfree s.t. γ(0) = qs and γ(1) = qf◮ Map this to time taken to do the task tf − t0

◮ Convert to a sequence of joint configurations

◮ Inverse kinematics◮ Teach and playback




Planning a Trajectory

◮ Point to point – From q(0) → q(tf )

◮ Velocity constraints q(0) = κ0 q(tf ) = κtf (Usually bothκ0 = κtf = 0

◮ Acceleration constraints q(0) = α0 q(tf ) = αtf (Usually bothα0 = αtf = 0




Point to Point Trajectories

◮ Cubic polynomial trajectories q(t) =∑4

i=0 ai ti -

Discontinuties in jerk (Derivative of acceleration)

◮ Quintic polynomial trajectories q(t) =∑5

i=0 ai ti

◮ Linear segments with parabolic blends (LSPB)

◮ Minimum time trajectory - A special case of LSPB




Cubic Polynomial Trajectory - Example




Quintic Polynomial Trajectory - Example




LSPB Trajectory - Example




Minimum Time Trajectory - Example




Multi-Point Trajectories

◮ A sequence of points – Endpoints are taken appropriately!




Potential Field

◮ Imagine a surface ϕ defined on workspace on which the robotis visualized to move down the gradient

◮ Robot at position b ∈ R3 with radius ρr

◮ Final configuration - Attractive (minimal point)

◮ Obstacle boundaries - Obstacles with locations oi and radiiρi > 0 Repelling

◮ Goal: One minimum (attractive) only with every other criticalpoint being nondegenerate!




APF Definition

ϕ : F → [0, 1] : Constructed as the composition:

ϕ(b) = σd ◦ σ ◦ ϕ(b) (1)

ϕ : F → [0,∞): Encodes goal point and obstacles

ϕ(b)△=

γk(b)

β(b)k ∈ Z+ (2)

Goal: γ : F → [0,∞) and β : F → [0,∞):

γ(b)△= (b − g)T (b − g)

Obstacles: The distances to other obstaclesβ(b)

△=

∏Nj=1 βj(b) where ∀j , βj(b) = (b − oj)− (ρr + ρj)

2 andThe parameter k : Balance between goal attraction & obstacles’repulsion!




APF Definition (cont)

Freespace boundary ∂F : Zero level set of β−1(0)

Since ϕ blows up on ∂F → Not admissible.

Remedy: It is squashed by the function σ : [0,∞] → [0, 1]:σ(x) = x

1+x.

The resulting function becomes admissible, but degenerate goalpoint gRemedy: Sharpening function σd : [0, 1] → [0, 1] is appliedσd(x) = x1/k .

ϕ becomes admissible and has non-degenerate minimum at b = g .




Application:

1. b = −∇bϕ(b) with b(0) = bs

2. Note that b = 0 if b = g (Stop condition)

3. Practical: Update control law at discrete instances k

• b(k + 1) = b(k)− τk∇bϕ(b(k)) (τk is step size)• k = k + 1• Check ‖b(k)− g‖ > ǫ

else return b

4. Go to (2)




General Performance

◮ Advantages

◮ Generate the torque trajectory to reach the goal◮ Reactive - As obstacles changes, so does the trajectory

◮ Problems◮ Stuck in local minima◮ May be difficult to compute attactive & repulsive forces


Documents

EE 451 - Path and Trajectory Planning