Thursday, March 29, 2012

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

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
    dist = abs(b.pos – eb[b.i + 1].pos)
    if dist > c
    if dist < c_min
  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.

Tuesday, March 20, 2012

Collision Avoidance in Mining Conference

I gave a presentation on the Transmin Rocklogic rockbreaker collision avoidance system at the 3rd Collision Avoidance in Mining Conference held in Fremantle.

The speakers were:
  • Simon Ridge - Department of Mines and Petroleum: provided an overview of regulation and risk management in WA.
  • Stewart Bell - Department of Employment, Economic Development and Innovation: provided an overview of collision incidents in Queensland, and made an announcement indicating Queensland may make collision avoidance technology mandatory.
  • David Quayle - Industrea: discussed systems integration issues in mining, and contrasted it with aerospace and the automotive industry.
  • Patrick Glynn - CSIRO: provided an overview of the development of collision avoidance technology in mining, and discussed CSIRO's old LHD automation project.
  • Luke Schelosky - SafeMine: discussed SafeMine's proximity / collision detection products.
  • Marcus Punch: discussed functional safety standards (e.g. SIL), the standards that the industry is moving towards, and the "Proven-In-Use" clause for the latest 61508.2 standard.
  • Matthew Watson - Rio Tinto Iron Ore: discussed issues in rolling out technology and how it affects operators ('users'), managers, and maintenance personal.
  • Rhys Sherborne - APS: gave an overview of Automated Positioning System machine guidance products.
  • Harland Attwood - Javpac: introduced a new physical barrier product designed to separate heavy vehicle traffic.
  • Peter Woodford - LSM: discussed cameras and operator visibility.
  • Elliot Duff - CSIRO: gave an overview of projects at the Autonomous Systems Laboratory.
  • Nicky Guenther - SICK: presented a new range of SICK products for proximity detection and provided a comparison with RADAR products.
  • Adrian Boeing - Transmin: Rocklogic collision avoidance system, and installation case study.
  • Robin Burgess-Limerick - UQ: Operator cognitive load.
  • Natalie Tindale - MB solutions: Operator fatigue.
For me the highlight was CSIRO's Autonomous Systems Labs robotics and SLAM work, particularly on the North Parkes mine site. Since they don't have a YouTube video of their work on the ASL channel, here is something similar: