Path Planning for Robotics - Computerphile

Key Takeaways

So we're going to talk about robotics, in particular path planning with robots. So currently I think there is a tendency to think our robotics is more engineering than computer science, but there is a lot that we do as computer scientists about how the intelligence builds. There's a lot of things around learning systems built for robots. And it gives us a really nice playground because the problems are much more much more difficult I think when you think about uh just data sets or things that are built in simulations because the world is noisy, the world is uncertain, and there there is a lot of things that we don't understand uh how the robot should work. The problem is finding the end point, is it? >> Finding a goal, yeah. That's the that's the problem of a mobile robot anyway. It tries to Or does it Or does it have the goal, and it's how it gets there is the question. What >> How it gets there is the question. So uh we do have I think um situations where we need to decide where the goal is, and I think that is uh maybe earlier in the um cycle where you decide on where to go, and then there is the problem of how you would go there without colliding into something or without uh disturbing anyone who's around. So we could talk about um the sense plan act cycle again. Um so we had in the earlier video talked about it, and we typically talk about sensing then planning and acting. In the sensing you basically talk about um getting the raw data from the environment. Robots have sensors, and they look at the world to capture some form of raw data. There is perception here, which I think um kind of lies in between sensing and planning sometimes. It's sort of like working out making sense of what the sensors have told you. Yeah. So you may think of perception as part of sensing as well depending on um your perspectives. So perceiving is about making sense of what you see, that raw data, and it can um be focusing on different problems such as uh what are the objects around me, um but also where am I, so the localization problem, uh where do I want to go, um and and what am I uh in for example, the mapping problem, uh what is the representation of this world that I am, which we haven't uh talked about a lot. What we do uh in in the algorithms that we will discuss today, we do assume that there is a map of the environment, then we choose to discretize it. So the planning here is uh the part that we are going to talk about. We are given a starting point, which is hopefully where the robot is at the moment, and a goal position, and you want to sort out how you go there most effectively. And then the acting part um is how you actuate your motors, and how you uh actuate that path. >> I'm presuming it's a kind of looping situation here where it keeps continually checking, right? >> Yes, exactly. So basically the robot deals with the physical world through sensing, and the actions change the physical world. So all of this is actually a cycle, a pipeline uh where you use information from the uh preceding blocks. Okay, so let's assume uh we have a map. I haven't shown you a map. Uh so I don't know if I should show you one. >> [laughter] >> And then maybe demonstrate uh what the robot could do in a simulated environment. Okay. So I have kicked off a ROS launch file. Robot operating system. So this is the middleware we uh we tend to use for um programming most of our robots. So on the right we're seeing the reality of this simulation, and on the left we're seeing the robot's perception. >> Yes, it's the robot's perspective, exactly. This is something I use in my teaching, so this is a um ready-made uh environment for the TurtleBots that we also use in our teaching, and this is a tiny simulated TurtleBot over here. Yeah, that is that is our robot here in this large environment. The environment has a lot of rooms and some obstacles around. If we look at these files, actually uh I have created a map for you, and this is the map that the robot would be given. So it is what we call an occupancy grid, and it's uh shows you what are the free areas, which are the white areas here, and the obstacle areas, which are the black areas, and there is some unknown areas where we don't actually we haven't gone to, we don't know what it is. So that map is added to this visualization tool, so this is called Rviz, and this is uh actually showing uh where the robot is in a slightly shifted way, so the robot is kind of lost, but not too much. And I can uh use this tool to give it a goal. So this is using the ROS navigation stack to compute a goal. So currently it's not able to compute any uh any paths, I think because the robot is seemingly on an obstacle on a wall, but I can Oh, actually it did. Uh yeah. >> So what's going on with the with the purple and the red there? Is that its perception of what's nearby? Um so the purple red areas are what we call uh cost maps. So the robot is actually creating uh certain costs on the grid, and those are uh defining the no-go zones. They are also around the robot because we inflate the robotic structure so that it becomes uh free to move around obstacles. Um and it is slightly off here because it did think that it was at a different place, and uh as you can see, the robot in reality, in its own world, thinks that it is in the middle of the room, but in fact it's it's a little bit shifted. So I can uh give it a better position estimate, which would hopefully um clear this up, and I can clear all the costs on the map so that it's a little bit more clean, and uh these green things around the robot are our particles. So we are running a particle filter in the background to localize the robot. So if I give it another goal, I think you covered it in the SLAM video, the robot through looking at uh what's happening around it would converge to a better localization. And it does still have this artificial uh cost map because it it has no way to actually forget something that it has seen. So from its own perspective it thinks there are there are obstacles around there. >> kind of shadows on the outside, right? >> Yes, yes. So it is an artifact uh of the mislocalization at the beginning, uh but now it is it is really nicely localized. So here what I do is um the robot we know where the robot is, and it is pretty accurate, and I can give it a goal like this. So this is a uh position and then orientation, and the robot would compute a path, which is about the planning problem we will be talking about today. So this one is actually uh using an A* algorithm, which we also covered many years ago, uh so we will also talk about that. What it will do is prioritize nodes that are going roughly in the right direction. That kind of demonstrates uh the problem. Um and as we talked about, there is a map in that in that demo. So let's assume we have a map. And in our uh purposes, the map is a rectangle, >> [laughter] >> empty. The goal is from a start position, start pose, to go to a goal. And what we are interested in uh in robotics at least is to move a body through the start configuration to the goal configuration. So what we are interested in are typically X, Y, theta, which is the position of the mobile robot because it is not flying, and the rotation. So this is a 2D obviously in this instance, yeah. >> So for the you can extend it. Some path planning um ideas, they can all be extended, and uh the second part what we are going to discuss uh will be mostly about the challenges of these algorithms when you go on to higher degrees of freedom such as manipulators, and why we require maybe different algorithms for for planning. Um so for the purposes of this uh video, I will be talking only about the XY, but this holds for higher dimensions. Um and the the aim of path planning is cuz I have a lot of colors to find this path, which is allowing me to navigate from a start and a goal. A start position to a start pose to a goal pose. Yeah. Um and for this very simple example, this is actually quite quite easy. This is very This is even analytical, isn't it? There is there is a mathematical function that I can define because I know these positions in 2D, and this is the function that would lead me to it. Why we're not doing it in robotics is because the world is typically more complex. Um and also maybe another concept we should mention is um motion planning. So uh dealing with robotics, you would um sometimes hear the term motion planning and path planning. Um And while it's not defined >> you don't you can't move in every particular direction? You have to take into account how your wheels turn. >> Yes, that is one aspect. Um there there not a clear cut definition between motion and path planning. But motion planning is about also uh um how you're moving through this path. For example, if there are velocity constraints rotational constraints etc. Um path planning is more focused on how you are um geometrically moving on this. Um So that is our problem. Uh so if the um So maybe I do draw the scene again. Uh if the goal is to move from start to goal and if I can't actually uh have an analytical solution, I'll do think. Uh what I can do is uh to make um use of graph-based algorithms. Uh and and path planning is very computer science uh and in fact uh most of the uh path planning algorithms are proposed by computer scientists for roboticists. So if we don't have an analytical solution, if we want to numerically solve it through uh graph-based methods, what we can do is we can populate this area with nodes. Right? Uh What can I do? Initially, uh the first thing that comes in your mind could be just I randomly put the nodes in places. So I put a node on my starting point and then I randomly select a place and put a node over there. So the cost to go to this node is zero. Um and let's say this is 10 maybe. Uh the distance, the cost of traversing this path, and the cost to go to this node from this to here would be 10. I could continue this process such as I can have something here and then I could find the closest node and I would draw uh edge in between them. Let's say the cost of this trajectory is one, so this could be one. Then I could maybe have something like um this to here and the cost would be three and another two here and then the cost would be five to go to this node from here. And now we can see uh actually these are not that far away. So if I had a path between these, the cost instead of uh 10 would be six to go to this node, which means that I actually cancel out this edge. Don't go this way in order to reach this node. Go this way and you can you can have paths that are not really very useful taking you maybe to the uh other directions and they can they can populate your graph. Adding more and more nodes, let's say this is two. So this is seven and if you get um closer to the goal you could add a add a um I maybe not overlapping with it but add a node around the goal and you could assume that within a threshold you have achieved your job. So cost of going here would be eight. And this is not really optimal. Um you could possibly have something better if you had selected a node here, which would be one and one maybe. So the cost to go here is one. So this should be updated to two. This should be updated to four. This should be updated to uh six and seven. So this is closer to being an optimal solution. >> [laughter] >> That's right. So how how long do you spend adding nodes until you stop? >> Exactly. So these are This is not the best idea that you randomly add things. Like I could I could just play around here, right? I could add things and then the graph would circle on itself and uh I would be losing a lot of time. So that is not the best best best idea. A better idea is obviously something that we all know, A*. A* makes use of heuristics. So typically it works on a grid-based representation in in the robot world. That's why we talk about grid maps. So the maps that we create we don't use a continuous representation, but we discretize it. We select a grid size, we select a resolution for it. We lose some information over there and we say this will be the space the state space that my algorithm will operate on. So assuming that this is my start and this is my goal A* basically tries to put nodes into these grid cells and select a path that is moving toward the goal. So in general, such a discretization also uh limits your action space. And for illustration purposes, we assume that the robot can go sideways, up and down and diagonally and we assign costs over there. So um because this is like one one one one, the cost of going to these cells would be one and this would be uh the square root of two. So something like 1.4. Uh but people demonstrated it as 10 and 14 typically. So that you would have a decent uh way of showing how you choose which node to expand. And the whole point of A* is choosing which node we are going to because a um different uh may maybe more naive expansion algorithm would go in breadth like the Dijkstra for example. It would expand the node and going in a way >> out everything when you don't need to know everything. You don't need to because we know that the goal is here. So what I know also is that I should probably move in that direction. So yeah, possibly the best way to go to this goal from this one is to go diagonally and then move move this way. And what A* does is it's uh computes three costs in doing so. One is the cost that has already incurred. So I have taken this path. So this is the what we call a G cost 14 and there is a H cost, the heuristic cost, which is uh the distance or the cost that I have to incur in order to reach the goal. And this second H uh cost is always underestimating the reality. So you could have something like an Euclidean distance or something real, but you shouldn't overestimate the cost so that your algorithm uh is admissible. Um so in here in this sense, I think we go three diagonal, one sideways, which means that our heuristic cost is 52 if I compute it correctly for this one. Which makes the cost for this cell the sum of these costs, which I will call the F cost, which would be 66. Okay? And for this one, uh it is similar. So I would go probably diagonally and two sideways. So the cost that I have incurred until now is 10 and what I have to do is 62, which makes the total cost here 72. So clearly at this moment, this one is a better choice. And for this one, we have to go maybe diagonally all the way. So it is uh 14 * 4. So 10 is the G cost and 14 * 4 uh 56. Yes? Is it 56? Okay. Yeah, it's all right. It's uh it's Then it is 66. Then you have to make a decision between these two. So what we are doing is we are actually filling all these cells which are adjacent to it and then going to cells which has the minimum F cost at every stage. And it will hopefully lead me through a direct path in a much uh quicker way because I choose to not expand these ones. So I'm kind of making my tree expand in that direction. Hopefully the math is good. >> [laughter] [gasps] >> We lost that ability. Okay. So so that is the A* algorithm and that is actually what is used in the simulation that you have seen. It is a very um very good algorithm and it does work when you have obstacles around. Um and in here if we had an obstacle um it would have probably found out different ways eventually. Maybe it would go this way and then choose these things. So probably this is this is a shorter path from this. So even though this looks better or this or this looks better, after a few steps it's it should find us because A* algorithm would give you uh the optimal solution uh with the restriction of your resolution. So it will work. Only on the grid. So >> Yeah, because in a fine grid you would get a solution that is more closer closer to the to the optimal solution. If I have such a coarse grid, obviously this is not the real optimal, but it is an approximation. Um But that presumably that's a trade-off against how much time you have to compute. >> Yeah. Yeah. Yeah. So those are all Uh that brings us to uh the issue of whether this algorithm is usable at all times. So, we had a very small grid here. Imagine I have a grid of 100 by 100, which gives me 10 to the power of four for the number of uh nodes that I have in this graph. Uh if I had a um three-dimensional space, so if my robot would swim or it would uh it would fly, then I would have 100 * 100 * 100 for the same grid size, which makes the complexity of my algorithm 10 to the power of six. What happens if I have a manipulator arm, which has six degrees of freedom, six joints that I can control? Then it would be 100 * 100 * 100 * 100 * I would have every >> Yeah. degree of Yeah, it would give you something like 10 to the power of 12 for your grid size because you have 100 dimensions uh for each degree of freedom, and you have six of them. So, 10 to the power of 12, which makes it like exponentially grow in the number of nodes that you have to deal with, which makes your algorithm less effective. For the most recent tricks that you get to basically call some of those or bring it down that number, yeah? So, what we do when we tend to deal with manipulators is actually at the algorithmic level. So, we said at the beginning it's not a very good idea to randomly select these nodes, but maybe it is if the you know Yeah. if the resolution is really too fine. Or or if something like even if we are in this zone and instead of this we really create a very fine grid, then your dimension is uh also increasing. So, maybe we should do something random. And that brings us to uh the rapidly exploring random trees algorithm. Okay. So, that algorithm does select nodes randomly, hence the name random, but it does it in a intelligent way. So, semi-random. Yeah, you constrain it a little bit. So, you try to go towards randomly in the right direction. Randomly in the right direction and not really making the uh making the edges too long, too far away. So, it's not completely random in the space that's uh like in in this >> So, constraining it to your proximity in some way. Yes, so instead of like selecting a node just here, select something around it. So, you could expand in the locality of your current node. So, you you do expand the tree in a wiser way. And it is actually a um I think simple algorithm. The implementation is very short. Again, we think that this is the starting configuration, and this is the goal configuration. Uh and we assume that um we have obstacles around. Right? So, let's say I have these maybe obstacles around me. Let's say there's something on the top. I don't know, something here. So, we do have a space that we have to navigate in between. So, you have to find a path instead of drawing something really uh linear. So, what happens here is uh you choose an node randomly. So, you assume your node is Actually, I do have some props. Let's say you choose a node just here, which would be normally connected by a line from the from the starting node, because we do have a node here. There's no node here yet because we haven't seen it, but this is far far away. So, what we do is we select a distance, and we project this node to fall onto that distance. So, we move it closer to this, and then we then draw an edge. And this is my node around. And then you do it um from this node. So, you put something here. This is far away, so you project it to be within to be within the maximum distance. And then your tree becomes becomes closer. So, let's say if I had another obstacle here, and I choose something like this and the projection actually falls onto the obstacle, I basically ignore it. I don't add it to the graph because I there's no way I can traverse to this. So, the validity checks could be also pretty simple because you do still have a known map. Um so, what this algorithm would do would be expanding the search tree, and sometimes in directions that are in the opposite way, but this doesn't really matter uh because those won't be used, and ultimately reaching a target. So, if you put something here, so if you get close enough, you say this is okay. And this is uh rapidly expanding. That's why the name name name is like that. So, basically this algorithm uh is not complete as the A* uh so, it doesn't give you an optimal solution. So, possibly the optimal solution would be something like this, but it did find a path. And quickly. And quickly. Uh it's not guaranteed to find a path, obviously, because it's probabilistic, uh but we say um this algorithm is probabilistically complete because if you have um the number of nodes reaching toward infinity, you would have a solution. You're guaranteed to have a solution if the number of nodes was infinity. So, that's why we call it uh probabilistically complete, but it's not optimal. Um what can we do to make it optimal uh is the algorithm called RRT*? And uh that actually makes a maybe uh clever adjustment to uh to this algorithm. So, it selects a node somewhere, and instead of connecting this to only the closest node, it looks into a radius or even something like like this in this example, and looks at the costs. If connecting this node to this one gives you a better solution, it connects it to this one regardless of the of the distance within a certain certain tolerance. So, this would give you something like this, and this node should have been actually maybe linked to this because cost of going to through through through this node is three, but you could have got done it in two steps. So, this is the RRT* algorithm. There are also variants which you start at two ends. So, you start populating tree from the starting configuration, but you can start another tree at the goal configuration. So, they expand toward each other, and these are all uh algorithms that we use to make this search better. And because these are short, you can sort out the action problem to traverse between these nodes, these configurations. Do you end up using a combination of these things depending on the problem, or is it is the one that you sort of favor for everything? So, it it it depends on the problem. Um So, for mobile robotics, for very simple applications in the lectures as well, we do uh we do use the A* um algorithm, which comes out of the box. Uh for many purposes that is sufficient. Uh for manipulators, um you do a combination of things. So, we did talk about A*, which is classified under um search-based algorithms. Um and these ones are RRT, RRT*, and other road map uh based algorithms, they are classified under uh sampling-based algorithms. There are algorithms that are, for example, learning-based, which uses reinforcement learning. There are algorithms that are optimization-based, uh which looks at how you can optimize through a uh cost function or through certain criteria. And I think the um state of the art is to use a combination of these. So, for manipulators, we do generally use a combination uh of different methodologies, so hybrid approaches uh do hold. Sampling-based algorithms, uh there are many more than this. You can find implementations in the Open Motion Planning Library. Uh and after selecting maybe a feasible path, one approach is to use optimization-based techniques to make it really nicer, to fine-tune the uh the final path. the probabilistically good one. If the space looks very much alike, then you're kind of in trouble because this algorithm, which uses a base filter, actually uses the similarities in space. So, the robot always tries to find something