In the last video, we covered simultaneous localization and mapping. And, as the name suggests, we ended up with a map of the environment in the form of a binary occupancy grid. For this video, we’re going to explore some of the ways that we can use a map like this for motion planning, that is finding a trajectory through this environment that connects a robot’s starting state to some goal state. We’re going to briefly cover what motion planning means and how we can use a graph to solve this planning problem, and then we’ll cover two popular approaches for creating that graph - search-based algorithms like A* and sampling-based algorithms like RRT and RRT*. I think this video will sort of nicely set a base understanding of how we can use graphs to plan a trajectory through a known environment. And it’s something on which you can build on as you go out and learn more about these planning methods. So, I hope you stick around for it. I’m Brian, and welcome to a MATLAB Tech Talk.
We want to find a path from a starting pose to a goal pose. If we’re talking about a robot that moves along the ground, there may be three states that make up its pose, x and y location and its orientation.
A path is a sequence of pose states that smoothly connect the start and the goal. Determining this sequence is called path planning. Now, path planning is just a subset of the larger motion planning problem. With motion planning, we’re not just concerned with the sequence of poses, but also their derivatives, like velocity, acceleration, rotation rate and so on. So, with motion planning, we are trying to dictate precisely how the robot moves through the environment. With path planning, we are just concerned with the path that it takes and not how fast it accelerates or moves through it.
Of course the size of the pose vector depends on the specifics of your system and environment. Instead of 3 states, it could be just two, x and y if the robot is omnidirectional and orientation doesn’t matter. Or, in the case of a robotic arm with multiple actuators, the pose could consist of dozens of states.
For this video, the examples I’m going to use focus on path planning and for a robot that is omnidirectional, so just two pose states. This will simplify the explanation and, hopefully, you’ll be able to see that these techniques can be extrapolated to higher dimensional systems.
Alright, let’s get started with a simple map that is similar to what we generated in the last video. It’s just a rectangle.
Assume, the starting pose is here, and the goal pose is over here. A minimum-distance solution can be solved for directly by connecting the start and goal with a straight line as long as there are no obstacles in the way. If the robot moves along this path, it will reach the goal in the shortest distance possible.
Now, analytically solving for the shortest path like this is trivial for our simple environment. And this type of solution could even work for environments with some obstacles and constraints as well.
But for many problems the obstacles and the dynamics of the system are too complex to generate an optimal solution analytically. So, we approach it by solving the problem numerically. And as I said at the beginning of this video, we’re going to focus on graph-based methods to numerically find the path with the shortest length.
Before we get into any particular algorithm, let me show you the general idea behind graph-based solutions with this simple map. Graph-based algorithms work by discretizing the environment - that is breaking it up into discrete points or node - and then finding the shortest distance to the goal considering only these nodes.
Let’s approach this problem in a random way. The starting location is the first node in our graph and it has a cost of 0 since we’re already there. So I’ll put a zero inside of the node. Then we can move in a random direction and place a node in the graph at our new location. The edge between nodes is how far we traveled and the cost of getting to this node is the length of that one edge. Now we take another step in a random direction, place another edge, and the cost of this node is a total of 3 units. We can continue to do this, on a sort of random walk, until we get to the goal. The cost of this particular random path is 10 units. We’ve found a path that works, even though it’s definitely not the shortest path.
So, we can start again, taking a new random step, adding a node and connecting it with an edge and recording its cost. And if we happen to reach a node that we visited before, we can compare the cost between the two different paths that we took to reach that node and keep the smallest one. Basically, revising our estimate of how many units it takes to reach that node.
We now have this graph of nodes, or locations on the map, and how much it costs to get to each node. We should recognize here that we don’t actually need to build a fully interconnected graph, just a tree, which is a subset of a graph. A graph can have nodes connected in anyway you want, but in a tree each node only has a single parent. If you can get to a node two different ways, it doesn’t make sense to keep the path that is longer if you’re looking for the shortest path. So, you can remove the edge for the longer path, keeping a tree structure.
In this way, a tree would start at the location of the robot and the branches would venture out to other states, but never recombine.
Now, to find a shorter distance to the goal, we can just keep randomly wandering the environment, updating the tree until we find a branch that gets to the goal with a cost that is low enough. This doesn’t guarantees an optimal path but it will continue to approach optimal as the number of nodes goes to infinity.
Of course, building up a tree through random wandering is not the best solution. So, this is where path planning algorithms come in, they provide more efficient ways to build this tree.
I want to start with the so-called search-based methods, that build up the tree by adding nodes in an ordered pattern. One way to accomplish this in practice is to start with a grid-based map, like the occupancy grid that we have and go cell by cell and determine the cost, or the distance the robot would have to go in order to reach that cell. Here, I’m claiming adjacent moves are 10 and diagonal moves are 14. This is similar to the random approach we just did, except we methodically work our way through each cell, and calculating the cost to reach it, and if it’s the shortest path to that node updating the cost and the tree. Once we’ve covered every single cell in the grid, the optimal path is simply the sequence of cells that produce the minimum cost at the goal.
This will produce an optimal solution, at least optimal at the resolution of the grid, but you can see that this would be computationally expensive since it’s kind of a brute force method of checking every possible node. To improve this, researches came up with the A* algorithm in 1968 to give Shakey the Robot the ability to determine where to go on its own - a first for general-purpose mobile robots. This search-based method still add nodes in an ordered way, but it does so by prioritizing the nodes that are more likely to produce the optimal path, and searching there first. It does this by keeping track of some other heuristic like the straight line distance from a node to the goal, in addition to the cost of the node. The sum of these two numbers is the absolute minimum cost of the path. If there was a straight line shot to the goal, then you could imagine how the total path length for, say, this node would be 48. We’ve already gone 10 and we have a minimum of 38 left to go. Therefore, this other node should be prioritized, even though its current cost is 14, since there is the potential of only having 28 more to go for a total of 42, it makes more sense to keep trying this path.
So, in this way, A* allows us to search through the nodes in a way that will get us to the goal without necessarily having to add every node into our tree. In fact, once we get to the goal we know that we took the optimal path since every other path would have a cost plus distance to go that is greater than the path we found.
Now, this was a fast introduction to A* and I was going to make an animation that shows how this still works well in the presence of obstacles, but honestly I couldn’t do better than what Sebastian Lague (Leg-you) already has on his YouTube channel which I’ve linked below. His video and animation on A* is amazing and I recommend you check it out if you’d like to know more about this method.
Alright, so search-based algorithms gives us a way to build up a tree by adding nodes in some kind of ordered pattern. But one problem with these types of algorithms, even efficient ones like A*, is that they become very computationally expensive as the size and dimension of the state space increases. You can imagine how the number of grid-points grow exponentially as the number of dimensions increase. Which can slow everything down. So, they tend to not be used for high dimensional state spaces, like determining the path for multi-jointed robot manipulators, or for really large low-dimensional state spaces, one that might have millions or more grid cells. This is where the so-called sampling-based algorithms are useful.
To understand how sampling-based algorithms work, I think first it’s helpful to realize that in our map, and probably most maps, there are sections where a path could continue in a single direction for some distance before it needs to make a turn. With A* we have to calculate every single grid cell between these two points - so multiple nodes in the tree. However, if we only checked the far distant node, and there weren’t any obstacles in the way, then we could calculate the straight line cost for just this one node. This reduces the number of nodes, and therefore the number of total calculations.
So the question becomes, how do we pick the location of these sparse nodes so that we still reach the goal? And one answer is to randomly select them, or sample them, hence the name. Now, I know I said that randomly selecting nodes is not the best approach to build out a tree, but we’re going to choose a random node location a little bit differently. Rather than extending the path through some kind of random walk, which could allow the path to circle back on itself and take a long time to explore in the direction of the goal, we’re going to be smarter about randomization and focus on rapidly exploring random trees (RRT) and a version of it that can approach an optimal solution called RRT*.
Let’s go into how the basic RRT algorithm works. From the starting node, we need to place a new node in our tree. RRT does this by randomly selecting a node anywhere in the state space. Once we have this random node, we want to connect it to the nearest node to it in our tree. Which, since we're just starting is the first node. But we don’t want to place it too far away because the chance of the path crossing an obstacle or just traveling too far in the wrong direction is greater with a longer edge. So, we specify a maximum distance that the new node can be away from the nearest node.
Now, a quick aside. If the random node is closer than the max distance then we place the new node right there. Also, if there is an obstacle between the nearest node and where we want to place the new node, then that one is ignored completely, nothing is added to the tree, and we move on. This keeps the tree from attempting to cross through any walls or other obstacles.
Ok, so let’s move on. We randomly select a new node and find which existing node its nearest to. Again, this happens to be the starting node. So we already have two paths in our tree and both are venturing out into different parts of the state space. A new random node, and again we connect it to the nearest, which is growing one path even further into the open space. This is why this algorithm is called rapidly exploring random trees. When there are large unexplored areas, like we have, there is a good chance that a random node is selected in that area. This has the effect of adding new nodes in places that cause the paths to venture into unexplored areas quickly at the beginning, hence rapidly exploring. And a random node that is in a direction opposite of the goal doesn’t affect the path that is on its way to the goal since a different node would be nearest. In this way, one path is always rapidly making its way toward the goal and the others are rapidly making their way into other open areas of the state space. And then later on, as the unexplored area starts to fill up, the random node selection tends to just fill the tree out with more branches.
We can continue doing this process until the path gets to within some threshold of the goal. At that point, we have a viable path, albeit probably not an optimal one since this method tends to zig zag as it makes its way. But we did find a solution, and importantly a solution that likely uses far fewer nodes than would have been necessary for A* since the nodes can be spaced further apart. Again we set that with the maximum connection distance.
RRT can work really well for situations where you’re just looking for a valid path and not necessarily the shortest distance. As long as the start and goal nodes are reachable, this method is guaranteed to find a path as the number of samples approaches infinity - and in most cases a much much smaller finite number of samples.
Now, if we do want a solution that is closer to optimal, we have to add some additional steps into the RRT algorithm and we get RRT*. For RRT*, the node selection process is exactly the same. We choose a random node, find the nearest neighbor, and if there is no obstacle in the way we place a new node at the random node or at the max connection distance - whichever is smaller. The difference comes from where we connect this node to the existing tree. We don’t necessarily connect it to the nearest node. Instead, we check for other nodes within some specified search radius, and determine if we can reconnect these local nodes in a way that maintains the tree structure but also minimizes the total path length. So, here, I’m connecting it to this other node since that will produce a shorter path back to the start.
Let’s watch RRT* work with a slightly more complex environment. Here, I’m using MATLAB to do this visualization and the RRT* function from the Navigation tool box. At the beginning, you can see that it’s rapidly exploring the environment. And doing so with just a couple dozen nodes. So far, it’s not different than RRT, but let me pause it. This next node is going to appear here. If this was RRT, it would connect that node to this one, since its nearest. But watch what happens. That node appears and it’s connected all the way down here since that is a shorter path back to the start. And it reconnected some of the other nodes within the search radius. So RRT* is always trying to shorten the paths. Let’s continue.
Here it reaches the goal and just like with RRT the path is a bit zig zaggy, at first. But as we continue adding more nodes, and the existing paths are reconnected, you can see how they get refined over time, becoming shorter and straighter. We can stop adding nodes whenever we’re happy with the result. Not only do we have a near-optimal path to the goal, but our tree has generated near optimal paths to anywhere in the environment. At least, as long as the environment doesn’t change. This is the power of RRT*.
Now again, this was a super quick introduction to sampling-based algorithms, and I’ve left better sources of information in the description of this video. That’s where I’m going to leave it for now with just the briefest of introductions. The idea here, was not to tell you everything you need to know about path planning as there are dozens of variations of just RRT alone, but hopefully you have a sense of how a tree can help us plan a path through an environment, and some of the search-based and sampling-based ways we can approach building that tree.
In this video, we dealt with planning a path through a static environment. However, often other objects and obstacles are moving through the environment as well and planning algorithms need to react to those changes. Part of solving this problem is tracking extended objects - these are objects that are large enough that a sensor returns multiple detections of it. And that is what we’re going to cover in the next video.
If you don’t want to miss that or any other future tech talk videos, don’t forget to subscribe to this channel. And you want to check out my channel, control system lectures, I cover more control theory topics there as well. Thanks watching, and I’ll see you next time.
