How does a robot plan a path in its environment?

Original article can be found here (source): Artificial Intelligence on Medium

How does a robot plan a path in its environment?

Recently, I’ve been working on my own mobile robot, Pumpkin, with a Master’s student in my research lab. Since we’re trying to replace some of the default packages that my robot uses from the library ROS, we’ve been learning about different algorithms used throughout a typical robot stack. As someone who works on planning and reinforcement learning algorithms but decidedly not robots, there’s been a sharp learning curve for me. A robot needs to know how to localize itself in its environment — or figure out where it is, build a map of its environment on the fly if it doesn’t have one already, avoid obstacles that might pop up randomly, control its motors to change its speed or direction, or come up with a plan that solves a task efficiently.

My robot Pumpkin

As you’d suspect, one really important part of a robot is its ability to plan out a path from one location to another given a map of its environment. Why would it have to do this? Maybe it has to move across a room to deliver some package or maybe it has to escort a person to some building. In any case, whenever a robot has to go from a start location to a goal location to complete a task, it has to come up with a path plan for how to move around its environment. In robotics papers, you’ll often see a messy map like the one below with a start location and a goal location. This is a nice illustration of the classic problem in mobile robotics that we call path planning. In other words, how can a robot figure out a path plan that gets it from the start location to the goal location?

Source: A robotics paper by Andrew Howard, Gaurav Sukhatme, and Lynne Parker

Disclaimer: In the past, I’ve written a few posts with colorful diagrams and lengthy explanations. Admittedly, because posts like that involve a ton of work, I ended up never posting anything. Going forward, I plan to write no-frills posts that are a little more rough and casual. Why? Well, not only does it make it easier for me to write posts that reinforce my own understanding of a concept, but I’m also pretty sure they can be just as informative without all of the bells and whistles that my other posts had. And now back to our regularly scheduled programming…

However, like always, there are a few subtleties that we have to keep in mind:

  1. The path plan should actually work on a robot. If the path plan makes the robot turn at sharp angles but the robot can’t move at sharp angles (like a car), that path plan shouldn’t be allowed.
  2. The path plan should be as close to optimal as possible. While it’s nice to find any path plan that gets the robot from a start location to a goal location, that isn’t enough, unfortunately. We’d like something that’s somewhat efficient. Not only will it help the robot complete its task as fast as possible, but it’ll also conserve its precious battery life.
  3. The path plan should avoid colliding with walls. This obviously goes without saying. Robots can be pretty expensive, and crashing is never a good thing. My little robot alone cost me well over a thousand bucks.

One of the most popular algorithms for coming up with a path plan that tries to satisfies these conditions is called Rapidly-exploring Random Trees (RRT). Since a picture is worth a thousand words, check out the diagram below. Let’s suppose the robot has to go from a start location (the red dot) to a goal location (the green dot) in a simple map without any obstacles. Basically, we’ll start off with a tree that has a root node representing the start position of the robot. After that, we’ll build the tree up gradually. How? We’ll take a bunch of random samples of the map, make a new node for each random sample, and insert each new node into the tree somehow. Once the tree has a node that’s close enough to the goal position of the robot, we’re done.

Source: The original RRT paper by Steven LaValle

Since I know that seems pretty vague, let’s add some more detail to this rough sketch. To start us off, let’s go over each parameter that we’ll send to RRT.

  • Map: the map of the environment that’s partitioned into an obstacle region and an obstacle-free region. It’ll look like that map I posted above where the obstacle region is anything that’s gray and the obstacle-free region is anything that’s white.
  • Start Position: the start position of the robot in its environment. This is just the red dot in that map.
  • Goal Region: the goal region of the robot in its environment. And this is just the green dot in that map.
  • Number of Iterations: the number of iterations performed by RRT.

Let’s go over each step of RRT. First, we’ll initialize an empty tree. Next, we’ll insert the root node that represents the start position into the tree. At this point, we’ll just have a tree with a single node that represents the start position. Finally, we’ll repeat these steps until we hit the number of iterations or reach the goal, whichever one comes first.

  1. Sample a random position from the obstacle-free region of the map.
  2. Create a node that’s associated with the random position.
  3. Find a node already in the tree that’s closest to the random position.
  4. Calculate a path from the random position to the position of the node that would work actually be feasible on the robot.
  5. Continue to the next iteration if the path collides with something.
  6. Insert the node that’s associated with the random position into the tree with the node (the node nearest to it) as its parent node.
  7. Return the tree once the random position is within some distance of the goal position.

As a heads up, if the tree doesn’t ever get near the goal region by the time we’ve hit the number of iterations, we’ll just return the tree we’ve built so far.

Source: mjkaufer via Reddit

But how do we get the path from the start location to the goal location once we’ve built the tree? All we have to do is start at the node that represents the goal location and go back up the tree until we get to the node that represents the start location. That’ll give us the path that gets the robot from the start location to the goal location. Easy.

And is it possible to reuse the tree for new goal locations within the same map? Of course! If there’s already a node in the tree that’s near the new goal location, we’re good to go. However, if there’s nothing close to that new goal location yet, we can just keep sampling until we’ve hit a node that’s near it. As long as the environment doesn’t change, you can just keep building on the same tree for any new goal locations.

Source: Realtime Robotics

Without further ado, here’s a rough version of the algorithm for RRT!

function RRT(map, startPosition, goalRegion, numIterations):
tree = initializeEmptyTree()

insertRootNode(tree, startPosition)
for i = 1 to numIterations:
randomPosition = sample(map)
randomNode = createNode(tree, randomPosition)
nearestNode = findNearestNode(tree, randomPosition)
path = calculatePath(nearestNode, randomNode) if (hasCollision(map, path)):
continue

insertNewNode(tree, nearestNode, randomNode)
if (randomPosition is within goalRegion):
return tree
return tree

There’s one more thing that’s important to cover! Earlier, I mentioned we’re looking for a path that works on an actual robot, is optimal, and avoids obstacles. While this path will work on an actual robot and avoids obstacles, is it optimal? It turns out that RRT isn’t guaranteed to produce optimal paths unfortunately. In another blog post, I’ll discuss RRT*, another version of RRT, that always generates an optimal path eventually. But that’s for next time.