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.
Now, 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 the environment
that connects a robot's starting state to some goal state.
And 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're going to cover two popular approaches
for creating that graph, search-based algorithms,
like A* star and sampling-based algorithms like RRT and RRT*.
And I think this video will sort of nicely
set a base understanding of how we
can use graphs to plane a trajectory through a known
environment.
And it's something on which you can build when 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.
And 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.
And 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're trying
to dictate precisely how the robot moves
through the environment.
With path planning, we're 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 three 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.
Now, 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.
Let's get started with a simple map that's similar to the one
that we generated in the last video.
It's just a rectangle.
And assume that the starting pose is up here
and the goal pose is over here.
A minimum distance solution can be solved for directly
by connecting the start and the goal with a straight line,
as long as there's no obstacles in the way.
Now, if the robot moves along this path,
it's going to 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 nodes,
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 zero since we're already there.
So I'll put a zero inside the node.
Then we can move in a random direction
and place a node in the graph at our new location.
The edge between the 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 completely random direction.
Place another edge.
And the cost of this node is the total of three units.
And we can continue to do this on sort of a random walk
until we get to the goal.
And 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,
and 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 it and keep the smallest one.
Basically, we're revising our estimate on how many units
it takes to reach that particular node.
We now have this graph of nodes or locations on a map
and how much it costs to get to each node.
But 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 any way 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.
It doesn't guarantee an optimal path,
but it will continue to approach optimal as the number of nodes
go 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.
And 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 units
and diagonal moves are 14.
Now, this is similar to the random approach
that we just did, except we're methodically
working our way through each cell
and calculating the cost to reach it.
And if it's the shortest path to that node,
then we update the cost and the tree to reflect it.
And 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 it would be computationally expensive
since it's kind of a brute force method of checking
every possible node.
So to improve this, researchers came up with the A* algorithm.
This was in 1968 to give Shakey the Robot the ability
to determine where to go on its own, which
was a first for general purpose mobile robots.
This search-based method still adds 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.
And 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 units.
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
units to go for a total of 42.
It makes 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
that we found.
Now, this was a fast introduction to A*.
And I was going to make an animation that
shows how this still works in the presence of obstacles.
But honestly, I couldn't do better than
what Sebastian Lague 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.
So search-based algorithms give us
a way to build up a tree by adding nodes
in some kind of an 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 dimensions
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, things like determining the path
for multi-jointed robot manipulators,
or for really large low dimensional state bases,
ones 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 it's first helpful to realize
that in our map, and probably most maps,
there are sections where a path could continue
in a straight 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 total number of nodes,
and therefore the total number of calculations.
So the question becomes how do we
pick the location of the 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 how 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.
And RRT does this by randomly selecting a node anywhere
in the state space.
And once we have this random node,
we want to connect it to the nearest
node 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 just
place the new node right there.
Also, if there's an obstacle between the nearest node
and where we want to place the new node,
then that one's just 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.
Let's move on.
We randomly select a new node and find which
existing node it's 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 the algorithm is called rapidly
exploring random trees.
When there are large unexplored areas, like we have,
then there's going to be a good chance that a random node is
selected in that area.
And 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's on its way to the goal,
since a different node would be nearest.
In this way, one path is always rapidly making its way
towards 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.
Now, 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 zigzag as it makes its way.
But we did find a solution, and importantly,
a solution that is likely using 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.
Now, 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 approach infinity.
And in most cases, a much, much smaller,
finite number of samples.
Now, if we do want a solution that's closer to optimal,
we have to add some additional steps into the RRT algorithm.
And we get our RRT*.
For RRT* the node selection process is exactly the same.
We choose a random node.
Find the nearest neighbor.
And if there's 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, however, 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's going to 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 the visualization and the RRT*
function from the navigation toolbox.
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 right here.
This next node is going to appear here.
If this was our 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's a shorter path back to the start.
And it reconnected some of the other nodes
within the search radius.
So our 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 zigzaggy 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.
Now, 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.
And this is the power of our 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.
But 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's 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.
And 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's 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 if you want to check out my channel, Control System
Lectures, I cover more control theory topics there as well.
Thanks for watching.
And I'll see you next time.