path planning algorithms and multiagent path...
TRANSCRIPT
Path Planning Algorithms and MultiAgent 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 MultiRobot Path Planning Decentralized MultiRobot 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 NPHARD. [Xavier92, 54]
– Path planning is PSPACEHARD. [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
● ExpansiveSpace Trees● RapidlyExploring Random Trees
Multiple Query
● A map is generated for multiple queries● Fill the space adequately
● Probabilistic Roadmap● Uniform sampling of Cfree● 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● RRTConnect : Grow from both initial and goal
● Expand by performing incremental motions
RRT Example
RRTConnect 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=count1; 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 { countj1, counti1 }
return null}
Reactive Method
● No prepath 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 Screenshot● 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
MultipleRobot Path Planning
● Not only find paths for robots, but coordinate those paths
● Centralized Planning● Decoupled Planning
Centralized Planning
● Different robots forming a single multibody 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● Recalculate path to real target
● Retry 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 videocard 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.91_i386.deb
● Put licence.key or license.srv file into “resources” directory
Sample Screenshot
Sample Screenshot 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
