Elastic Band - Realtime pathfinding deformation

I've covered global path planners before including A* path planning and Dijkstra. These path planners will give you a result, but if something alters the desired path (such as an object crosses the path, which you need to avoid) you need to recompute the whole path again. Algorithms such as D* lite allow efficient re-computation. However, the elastic band method allows the existing path to be used, and just adjusted to handle deformations. Or, more formally, elastic band path planning enables realtime modifications to a precomputed path that consider additional obstacles (or cost functions) not considered during the original paths computation.

Elastic band - Global path in blue, bubbles as colored circles,
 and the red path is the one generated by the elastic band algorithm.

The easiest way to describe it in games terminology is to have an entire path turned into particles that follow boid-like (flocking) rules. The best part, is that since it is essentially based on particles, means that you can re-use code from a particle system or physics engine you may have available. If your from the games background, you can use player/entity interchangeably with my use of the word 'robot'.

The 'elastic band' is a created from a set of 'bubbles'. A bubble contains a radius, a position, the coordinates of the closest obstacle to the bubble and a velocity.

The way it works is that an 'elastic band' is initialised with the partial path from the global path planner. The centre of each bubble is assigned to a point along the global path. (For optimisation purposes, a sparse version of the global path is typically used).

If the elastic band does not contain the robots current position, then the robots present position is inserted, provided it will align with the desired path.

For every bubble in the path we determine the total external and internal forces acting on it, and apply them and create/remove bubbles as necessary.
This is done by:

• Creating a new bubble if the distance to the next bubble is greater than a predefined constant. In effect this constant determines the discretization of the path.
  
• Determining the closest obstacle to the bubble and calculating the radius. This is achieved by using a spatial partitioning algorithm to determine the closest obstacle by its Euclidean distance. The radius is assigned to this distance, limited to a maximum.
  
• Calculating the external repulsion force from the closest obstacle. The magnitude of this force is scaled to be a function of its distance limited by an upper bound, such that closer obstacles exert a greater pressure. (i.e. (max - r) / r )
  
• Calculating the internal cohesion force from the previous and following bubbles in the path. This is simply the sum of the normalised vectors to the previous and following bubbles centres.
  
• Updating the bubble state by applying the forces and velocities to calculate the new bubble position. I used a higher order integrator with hand-tuned dampening terms. In addition, a low pass filter is applied to the position update to reduce the influence of the forces.
  

Finally, bubbles are removed if they are too close to each other. The entire band is then checked for continuity, ensuring that each bubble is greater than the robots size. If this criteria is not fulfilled the robot is halted, as it will not be able to get to its destination without colliding.

The bubble that is a set distance infront of the robot is selected to generate the next trajectory input. The angle between the current bubble and the next bubble is calculated and used to modulate the vehicles desired velocity. This causes the vehicle to reduce its speed when it approaches a sharp turn.

In pseudo-code the algorithm is as follows:

eb[0] = path.start
i = 0
for each point along path
  if dist(eb[i],point)> c
    eb.add(point)

while robot not at path.goal
  for each bubble b in eb
    f_e = b.pos – closestObstacle(b)
    f_i = (b.pos – eb[b.i – 1].pos) + (b.pos – eb[b.i + 1].pos)
    b.integrate(f_e+f_i,b.vel) //update velocity from forces
    b.dampen(b.vel) //filter state
    b.integrate(b.vel,b.pos)
    dist = abs(b.pos – eb[b.i + 1].pos)
    if dist > c
      eb.add(midpoint(b,eb[b.i+1]))
    if dist < c_min
       eb.remove(b)
  tmp_goal = eb.closestbubble(path.robot + lookahead) //set goal from robot position
  set robot trajectory to tmp_goal

Thanks to Sushil who did all the hard work in implementing this for MAGIC2010.