Home

Implementation notes

Navigation mesh path finding

We use navigation mesh built from the static map to find a global path..

Triangulation mesh

One of the ways to build the navigation mesh is with an algorithm called Delaunay triangulation. It is very fast, but can only work for static obstacles that are known ahead of time.

The advantage of Delaunay triangulation is that it can "fit" to the border shape of occupied cells, so it can generate very sparse navigation graph even in a complex environment.

However, it is very costly to compute Delaunay triangulation, so we cannot run it every tick. We need a dynamic path finding algorithm for avoidance.

Quad tree mesh

Another way of building the navigation mesh is to use quad tree. The advantage of quad tree is that it is inherently local, which means you don't have to look at the next cells to determine the cell size you are currently looking at. It may open up a possibility to update the mesh in real time with moving obstacles, if we can achieve dynamic update based on only moved objects.

Another good thing about quad tree is that it doesn't have to simplify the border lines like triangulation.

The downside of quad tree is that it tends to have more graph nodes than triangulation, so the amount of memory to store the mesh and the cost of searching a path tend to be larger.

Although it is less space efficient, it can generate decently sparse search graph.

Dynamic update of quad tree

We can update the quad tree every tick relatively cheaply. This will allow us to make global path planning work with dynamic obstacles.

In the picture below, green squares are free leaves, red are static obstacles and purple are dynamic obstacles. You can see that individual agent tend to choose a different path from the others since the same path tend to intersect with each other.

This method seems the best approach so far, since the random tree based approach descibed later tend to be more costly, and they don't scale well for many agents. With quad tree navigation mesh, it needs to be updated only once in a tick, and all the agents can take advantage of that to make globally valid path.

Dynamic update of QTree

Now we update only parts of navigation mesh around the map that has changes. In the video below, only the flashing cells are considered in map update. It reduces the amount of computation significantly.

Random Tree based path finding

We use algorithms with randomly sampled tree to avoid dynamic objects. The type of the algorithm is called StateSampler and implemented as a trait.

• ForwardKinematicSampler
• RRT
• RRT*

ForwardKinematicSampler

This is a simple kinematic based forward prediction from randomly chosen steering and drive speed.

While it guarantees feasible path for noholonomic constrained drive system and very cheap to compute, it is very slow to find the solution.

RRT

A Rapidly exploring Random tree, abbreviated as RRT, is a kind of random tree that can find a solution much faster than ForwardKinematicSampler with the same number of iterations. It has also a nice statistical property that the whole search space is covered asymptotically.

However, it is not guaranteed to be optimal or feasible fro nonholonomic system. Also, once the solution is found, it will never be refined further, even if we continue searching.

RRT*

An improvement over RRT, RRT* is an algorithm that can "refine" the existing search tree. It generate more or less "smooth" path, but it is not guaranteed to be feasible path in nonholonomic constraint.

It requires querying neighboring nodes quickly, so I use grid map to optimize it.

Collision detection

In avoidance, we need to detect collision (interesction with predicted motion between agents).

Binary search collision checking

It is very costly to compare collision of two OBBs in every state in every potential path. We use binary search approach to reduce this check cost.

The idea is illustrated by the pseudocode below.

fn bsearch_collision(obj1, motion, obj2) -> bool {
   collision_internal(obj1, motion, obj2, 0).hit
}

fn collision_internal(obj1, motion, obj2, level) -> (hit, max_level) {
    let bounding_sphere = obj1.radius + obj2.radius + motion.length();
    if bounding_sphere < (obj1 + motion / 2.).distance(obj2) {
        return (false, level);
    }
    if level < MAX_LEVEL {
        collision_internal(obj, motion / 2., obj2, level + 1) ||
        collision_internal(obj + motion / 2., motion / 2., obj2, level + 1)
    } else {
        obj1.intersects(obj2)
    }
}

The actual code is obviously a lot more complex.

One thing to be careful with this is that we need to check the last state outside of recursion, because obj1 will never reach obj2 with this recursion. In particular, it would intersect with the obstacle like below. In this visualization, green rectangle is the obstacle, and the white lines and rectangles are the search tree and potential agent states. The white rectangles and the green should never intersect, but it does.

(Other unimportant info about the visualization: the thickness of the white lines indicate how many recursions were performed, and the dots along the lines are the points that intersection checking was performed.)

In order to fix this issue, we need to modify bsearch_collision function like below.

fn bsearch_collision(obj1, motion, obj2) -> bool {
   let (hit, max_level) = collision_internal(obj1, motion, obj2, 0);
   if hit {
       // If we detected the collision somewhere in the recursion, we can return immediately
       true
   } else if max_level == MAX_LEVEL {
       // If we had to recurse all the way, but are still not sure if intersection happened,
       // Check intersection with full motion
       (obj1 + motion).intersects(obj2)
   } else {
       // Otherwise, it did not hit
       false
   }
}

The result looks like this. It does not have intersection at all.