Path Planning Algorithms and MultiAgent Path...

Path Planning Algorithms and Multi­Agent Path Planning

Fuat Geleri

10.2006

Outline

Path Planning Basics Complexity of Path Planning Simulation Related Utilities

Collision Checking Integration

Sampling Algorithms Single Robot Path Planning Multi Robot Path Planning

Centralized Multi­Robot Path Planning Decentralized Multi­Robot Path Planning

Webots on Linux Environment

Path Planning Basics

– Path– Configuration– Work Space– Configuration Space (Cspace)

• Cell Decomposition• Roadmap (Skeletonization)

– Free, Obstacle, Unknown Space– Dimension and Degrees of Freedom

Complexity of Path Planning

– In 3D work space finding exact solution is NP­HARD. [Xavier92, 54]

– Path planning is PSPACE­HARD. [Reif79,55]– The compexity increases exponentially with,

• Number of DOF  [Canny88, 9]• Number of agents

Simulation Related Utilities

Simulation

Collision Checking

Integration

Simulation

● Extremely simplified simulation loop

while(1){    process_input();    update_objects();    render_world();}

update_objects(){    for (each_object)        save_old_position();        calc new_object_position              {based on velocity accel. etc.}        if (collide_with_other_objects())             new_object_position = old_position();             {or if destroyed object remove it etc.}}

Simulator Independent Design

SIMULATOR

INFO GATE

CONTROL GATE

SHELL

Simulation Module

SHELL

SIMULATION MODULE

+INITIALIZE+SIMULATE

-PRE STEP-STEP

-POST STEP

DRIVERS

PLANNING

General Flow

simulator : SampleSimulator()world : SampleWorld(objects)drivers : List<SampleDriver>simulation : SampleSimulation(world, drivers)infoGate : SampleInfoGate(simulator, objects)controlGate : SampleControlGate(simulator)

while( simulation_continues )infoGate.prepare()    // get current configuration informationcontrolGate.preStep()  // clear old control inputs, set to defaults

for( IDriver driver : simulation.drivers()){IControlInput ci = driver.nextControlInput(); // calculate && provide next CIcontrolGate.setControlInput(driver.info().robotId(), ci);

}controlGate.step(); // make according assignmentscontrolGate.postStep(); // send control inputs

simulator.step();  // apply control inputs}

Collision Detection

● Sampling based algorithms depend on this● Most of the processing time consumed in collision 

checking● We used “Sphere Subdivision Algorithm”

Sphere Subdivision Algorithm

● Sphere subdivision algorithm is applied

Sphere Subdivision Algorithm

● The collision detection algorithmboolean checkCollision( head_from, head_to)

if( doesCollide( head_from, head_to ) )if( head_from has any child )

for each child if( checkCollision(head_to, child ) )

return true;else

if( head_to has any child )for each child

if( checkCollision(head_from, child) )return true

elsereturn true

elsereturn false;

Collision Checking Design

Collidable collision tree head : Collision Tree Node configuration

Collision Tree Node circular list of Collision Tree Node

Collision Checker checkCollision( Collidable from, Collidable to)

Local Planner

● A local planner for each robot● Generates random configuration● Checks whether the configuration leads 

collisions● Checks connecting two configurations lead 

collisions● Separates world specific info from path planner

Random Configuration

● Random configuration composed of● Configuration in the current world

● Position● Orientation

● Robot specific configuration● Velocity● Acceleration

Local Planner Collision Checks

● Appropriate Collision Checker is used● Single collision check

● Whether robot is in collision for given configuration

● Continuous collision check● Robot collides while translating from one 

configuration to the other● Divide path to single check points

Continuous Collision Check

boolean isConnectable( Configuration from, to )if( !isCollisionFree(from) or !

isCollisionFree(to) )return false

distance = distance(from, to)difference = difference(from, to)k = 1while(true)

if( distance < RESOLUTION )return true

elsedistance = distance / 2scale(difference, 0.5)append(start, difference)if( !isCollisionFree(start) )

return falsescale(difference, 2)

for( int i = 1; i < k ; i++ ) append(start, difference)if(! isCollisionFree(start) )

return false

//revert back to half..scale(difference, .5f)k = k * 2 start = from

k = 1

k = 2

k = 2

Integrators

● Differential equation● Derivative for a configuration, and “t”

● Integrator● Euler's Method● Runge Kutta 2● Runge Kutta 4● Runge Kutta Cash Karp● Runge Kutta Fehlberg

Differential Equation

● IDifferentialEquationdouble[] derivative(double t, double[] configuration)

● Differential equation represents the change of the system

● For a given “t” and configuration, the function returns the derivatives

● Example:Configuration : Position and velocityt : time=> derivative[]{velocity, acceleration}

Integration Methods● Euler's Method

● Simple but not enough approximation

● Runge Kutta● Using different approximations and average them

● Runge Kutta Cash Karp and Fehlberg● Uses dynamic step size● Updates step size according to error amount

● Runge Kutta 4 gave good results for me

Sampling Based Algorithms

● Incomplete but efficient and practical

● Types● Multiple Query

● Probabilistic Roadmaps● Single Query

● Expansive­Space Trees● Rapidly­Exploring Random Trees

Multiple Query

● A map is generated for multiple queries● Fill the space adequately

● Probabilistic Roadmap● Uniform sampling of C­free● Local planner attempts connections● Biased sampling

Multiple Query Example

Obstacle Based Sampling

Lazy PRM

● Defer collision checks, and save big amount of time consumed while checking collisions

● Check edges which have high possibility to have collisions

● Only make collision checks when a path found● Save clean edges, remove colliding ones● Continue search if found path is not collision free

● Make more sampling● Not focus on one edge while performing 

“isConnectable” collision checks● Collision resolution of edges decremented 

together

Single Query

● Suited for high dimensions● Find a path as quickly as possible

● RRTs● Grow from an initial state● RRT­Connect : Grow from both initial and goal

● Expand by performing incremental motions

RRT Example

RRT­Connect Example

Path Smoothing

Eliminate extra oscillations on the path

Path Smoothing Algorihtm

//Path : List<IConfiguration>Path smoothPath(Path path){

Path result = new Path(path)

int []cutBetween = findSmootherPath(result)while( cutBetween != null )

//clear the betweenpath.subList(cutBetween[0]+1, 

cutBetween[1]).clear()cutBetween = findSmootherPath(result)

return result;}

int[] findSmootherPath(Path path){count = path length//go till middle of the path for( i=0; i<Math.ceil(count/2f); i++ )

node_i = node( i )node_back_i = node( count – i ­ 1 )

//go  backfor( j=count­1; j>i+1; j­­ )

node_j = node( j )if connectable( node_i, node_j )

return { i, j }

node_back_j = node( count – j ­ 1)if connectable( node_back_i, 

node_back_j )return { count­j­1, count­i­1 }

return null}

Reactive Method

● No pre­path calculation required● May stuck local minima● RBot implementation

● My additions● Noise added● Statistics collected● Descriptor file structure modelled

RBot

double[] calculateControlInputs(RboTRobot robot, List<RboTRobot> neighbors)

{this.robot = robot;this.communication = robot.communication;this.neighbors = neighbors;

//calculateKValues(); //user gives..

double Vx = getGradient_dx();double Vy = getGradient_dy();

return new double[]{Vx, Vy};}

Gradient Calculation

● General Logic● Given robot and its neighbours● Calculate a force over the robot

● Target attracting the robot● Obstacles repelling it

● Make the force admissible

Example Screen­shot● Test results for the figure

Task difficulty : 7.960365244645906Total initial distance from target: 600.0Total travelled distance : 1380.0Normalized robot path length : 2.3Total iteration count   : 459

Multiple­Robot Path Planning

● Not only find paths for robots, but coordinate those paths

● Centralized Planning● Decoupled Planning

Centralized Planning

● Different robots forming a single multi­body robot

● Cartesian product of the configuration spaces of all the robots● 3 robots, with 2 dimension = 6 dimension

● Goes to high dimensions

Centralized Method's Implementation

● Single robot sampling based algorithms applied● Special Local Planner

● Behave robots as a single body, but process them as separate

● Coordination● Each edge will be traversed in the same amount 

of time● We know velocity, and position of each robot at 

every time instance● Special collision checking

Local Planner

● Configuration composed of● List of single robot configurations

● Single robot configuration composed of● Robot configuration● World configuration

● For now only world configuration is used● Adapted methods

● “distance”, “difference”, “duplicate”, “append”, “scale”, “generateRandomConfiguration”, “isCollisionFree”, and “isConnectable”

Path Planning Algorithms

● No update required● Local planner adaptation solves all the problems

Robot Coordination

● Each robot should take the next edge in their path at the same amount of time

● Collision checks are done in this manner● So

t = x / Vfor each robot, choose Vmax of robot

find t_min = Xr / Vr_maxt = max( t_min )so V for each robot achieved with no collision

Decoupled Planning

● Think robots as single and alone● Calculate collision free path for each● Coordinate by tuning velocities● Assign priorities● See others as dynamical obstacles

● Basic obstacle avoidance ● Reactive movements ● Help of sensors

Decoupling

● Complete path searches● Find path of first robot● Find the second accordingly● Till collision free path of each robot found● If no path found

● Go back and find another path for the last● Iterative path searches

● Add another edge to a robot's path construct● Add edge to next robot's that will not collide 

with previous ones

Our Algorithm

● Use sensors● Understand the side of possible collision● Make suitable escape movement

● Go back and opposite direction and retry● Try for some while

● Time out● Make another random path

● Trace it a while● Re­calculate path to real target

● Re­try the path● Method shown good results in Manufacturing 

Applications

Velocity Tuning vs Sensor Usage

● Velocity tuning algorithms and other predictive algorithms ● Knows

● Position of each object● Velocity of each object

● Not an absolute decoupling● Our sensor based algorithm 

● Does not need to know● Position of dynamical obstacles● Velocity of dynamical obstacles or other robots

● Absolute decoupling● Quite effective

Webots on Linux

● Ubuntu 6.0● Nvidia video­card is recommended

●  How to install Graphics Driver (Intel ­ Nvidia)● www.ubuntuguide.org● Intel ­ Failure in “direct rendering“

● Has solution but tricky● Webots 5

● www.cyberbotics.com● dpkg webots_5.1.9­1_i386.deb

● Put licence.key or license.srv file into “resources” directory

Sample Screen­shot

Sample Screen­shot 2

Directory Structure

● /usr/local/webots/resources● locate license.key or license.srv file here

● /usr/local/webots/projectscontests  default  robots  samples

● contests : contest related projects' files● default : empty example controller and world● robots : sample robots files● samples : example worlds and controllers

Windows vs Linux

● Problems on hardware support for Intel video card● Causes slow 3D view● Do not install Nvidia drivers for Intel video card

● If installed remove them and restart● Take backup of “/etc/X11/xorg.conf” beforehand

● Different directory structure● Update of make/ant files required

Conclusion

● Path Planning is a challenging task with many different applications

● Each application may device its own path planning strategy

● A generic path planning libray may provide solution or guidelines for other path planners

Questions..

Thank you

Fuat Gelerifuatgeleri@gmail.com