One of the easiest tasks for us humans to do is move from one place to another without even meticulously planning our way. We get up from our seats and walk to the cafe, get our drink and come back to our seats. During this entire “action” that we performed, we hardly give any thought about the “trajectory” that we have to follow. For example, while walking or running, when we see that there is a banana peel or a pothole in our “path”, we tend to avoid it. This is something that comes to us naturally as humans. But for a robot, this is a complex task. There is a lot of planning required to enact certain trajectories that come naturally to humans.

Let us understand the planning problem of our robot, which now has to get coffee from a cafe that is at some distance away from its chair. From our robot’s perspective, the “search space” can either be “discrete” or “continuous”. Discrete search space is easier to work with but is not viable as we cannot perfectly discretize a high dimensional search space and the movement of the robot in a discrete space gets tricky. The better option would be to employ algorithms to search a continuous search space and obtain better results. But since the search space is continuous, you will have to “sample” the search space and look for an optimal solution. An optimal solution is the solution having the least cost.

Now that we have decided that we will be working with continuous search spaces, the most general algorithm to search for a solution is to sample the search space and connect nodes that are “connectable”. Now, this might sound simple, but there are two things that complicate our problem. One, what is the probability that you will find a solution since you are randomly sampling the search space? Two, what are connectable nodes?

The first question involves how we sample our search space. Theoretically, the probability that we find a solution goes to one as the number of samples goes to infinity. This makes sense right! As you completely search the space, the more probable you are to find the solution. But the caveat here is that you don’t know how fast you will find the solution. In more technical terms, the rate of convergence of the algorithm is not guaranteed. There are many algorithms where once an initial solution is found, no matter the cost of the initial solution, sampling is done “around” the initial path as samples that might improve the solution lie in close proximity to the initial solution.

Let us take the example of an Ellipsoid sampler as used in the algorithms Informed RRT*, Batch Informed Trees* etc.

The second question involves how we evaluate the cost of our path. Cost function is generally something that tells us “how expensive” is connecting one node to another. Generally, it is hard to compute. This is so because the whole dynamics and kinematics of the robot have to be taken into account, which increases in complexity as the number of dimensions of your search space increases. Hence, we try to estimate our cost and use “heuristics” to guide our search and delay computing our cost as much as possible.

Heuristic-based search of an implicit Randomly Generated Graph (RGG) is used in Batch Informed Trees* where the actual cost of connecting two nodes is delayed until we are sure that adding the node would lead to a better solution. Instead of the actual cost an estimated cost is used. In addition to the estimated costs, various heuristics such as cost to come, cost to go, cost to come through the tree etc. are used.

Now that we have kind of understood and solved the questions, we can sample our search space and construct an RGG. The RGG will now be searched using our heuristic functions. A Tree from start node to the goal node will be constructed. If adding any node to the tree might improve the solution we do so by either rewiring the tree or deleting some edges and vertices of the tree and adding new ones.

Once an initial solution has been found, the algorithm will asymptotically converge to the optimal solution, thus enabling our robot to go and get its coffee.

What’s Next

Now that we have a path from our chair to the coffee table, some questions arise. Is the path smooth for the robot to travel? How does the robot tackle situations where a person comes in between all of a sudden? Could we have improved our search?

Path smoothening is done keeping in mind the dynamics of the robot, so that the trajectory generated is continuous. Research is being done to incorporate the generation of smooth paths.

Algorithms like Dynamic A*(D*) or Anytime Dynamic A* take into consideration the collision data, i.e when the need arises to dynamically change the path, the algorithm uses previously stored data to arrive at a solution much quicker, instead of re-searching the whole space again.

The guiding force behind the search of our algorithm are the heuristic functions. But there may come a time, when the heuristic you use hinders the search. Hence we need adaptive heuristics that can adapt to the given problem or space.

There is always a need for intelligent systems. Data driven approaches to path planning are also being researched upon. Research in this field will continue until the day a robot can fetch a cup of coffee as naturally as we do.

References

1. Combining the Two-Layers PageRank Approach with the APA Centrality in Networks with Data - Scientific Figure on ResearchGate. Available here [accessed 15 Mar, 2021]
2. Gammell, Jonathan D., Siddhartha S. Srinivasa, and Timothy D. Barfoot. “Batch informed trees (BIT*): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs.” 2015 IEEE international conference on robotics and automation (ICRA). IEEE, 2015.
3. Strub, Marlin P., and Jonathan D. Gammell. “Adaptively Informed Trees (AIT*): Fast asymptotically optimal path planning through adaptive heuristics.” 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020.
4. Gammell, Jonathan D., and Marlin P. Strub. “A survey of asymptotically optimal sampling-based motion planning methods.” arXiv preprint arXiv:2009.10484 (2020).
5. Gammell, Jonathan D., Siddhartha S. Srinivasa, and Timothy D. Barfoot. “Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic.” 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2014.