Robot Motion Planning using A* (Cyrill Stachniss)

have you ever asked yourself how a robot makes a decision how to go from its current location to a target location then this lecture is for you because we will discuss motion planning for mobile robots so how do robots make decisions where to navigate in order to reach a target location so typically a robot knows where it is it knows where it should go to it has a map of the environment and knows something about its own um shape or own body so that it knows how it can execute local motion commands such as goa meter forward and that the system will execute that without bumping into walls next to the robot for example and then a motion planning algorithm generates a sequence of motion commands or a sequence of states that the robot needs to visit in order to reach the target location and it can do so in an optimal fashion optimal fashion means there's no better way for doing this in terms of the output result so there's no better trajectory than the trajectory that the algorithm actually proposes and of course this optimality as always comes with some assumptions such as which stage can be with it do we discretize our space but that's something that we're going to explore in this lecture today so typically we are interested in computing a path or trajectory which guides the robot on its fastest route to its goal location and that's what we are going to discuss today how do we make this decision where to navigate in order to reach our goal so that can be a robot such as the one shown here on that slide which navigates through the catacombs under rome and needs to reach a certain location in those catacombs but can be also any other type of robot or even yourself if you use your subnuff system in the car and the system tells you where to navigate you can do this with an algorithm called a star even though in devices used today there may be some more advanced or optimized algorithms for the problem at hand but the basic motion planning algorithm that underlies this and can be solved with nasty algorithm so more concretely we are looking today into robot motion planning using the a star algorithm and discuss what a star means how it works and how we can use this algorithm in order to build real robots that navigate through the world so the motion planning problem is a standard problem in robotics since its beginnings so there's a famous quote made by la thomp in 91 which states that motion planning is an essential task that robots need to do because in order to do something useful most of the robotic systems at least of the mobile robotic systems need to move through the environment from a to b in order to succeed not to fulfill its task so moving through the environment is a core ingredient of most robotic systems today and therefore motion planning or path planning is an important capability for those systems if we look to the let's call it classical motion planning and what are the traditional goals that we always look into so the first thing we want to ensure is that the robot never collides with an object so if we have walls or doors in the environment we want to make sure the robot will never bump into a wall always fit through the door frame and without touching on the right one left inside the door frame or collide with the with the door or any other object in the space so a collision free trajectory is a requirement that we typically have and among this um set of collision-free trajectories from a start location to a goal location we are typically interested in finding the trajectory which guide the robots on the shortest path or they can that the robot can reach its target location as quickly as possible so the shortest path is not always the fastest pass so there can be trajectories along which you can which are longer but along which you can drive faster so you may take a small detour into account if you can drive faster on that trajectory it's similar to you when you're sitting in your car and there are two roads you can take the one is a highway and the other one is a local street to the city even though the trajectory along the highway is somewhat longer you can drive faster on that highway and therefore will reach your target location faster so in for several applications we are interested in the fastest route but there are also applications where we may be interested in the shortest paths often they are similar but not necessarily the same so the traditional goal in robot motion planning is provide a collision free trajectory that allows us to reach the goal as quickly as possible this is often formulated in static worlds that means the robot knows about the world and also knows that the world does not change that means there are no dynamic obstacles around there are no humans moving around no other traffic participants for example so this is something which is a very strong assumption and typically not justified in the real world so whenever we go to real world robotics we need to take dynamic environments into account that means that environments that change while the robot is operating in this environment these can be quickly moving obstacles like other traffic participants but this could also mean that there are objects which are placed in the environment that hadn't been around when the robot was building its map for example or just data that's not stored in the map in general and how can we deal with this so how can we still navigate in an efficient or effectively so have an efficient algorithm that computes a path quickly and get a get out a path which guides us to the target location as quickly as possible and that we can still give some guarantees at least guarantees under some assumptions that the um robot will not bump into an object even as an object it didn't know before in order to do that of course we typically need to make some assumptions how objects move through the environment um if the object is much faster than the robot basically chases the robot of course this can lead to a collision a collision we cannot avoid but if you make certain assumptions about the movements of the other objects in the environment we can make certain statements at least probabilistic statements if this trajectory will lead to collision yes or no so we are interested in finding trajectories that are reliable and that we can efficiently navigate on and there have been a lot of techniques proposed in order to deal with those dynamic environments often we have to distinguish two things in here a core planning algorithm that plans general paths from a to b and then what you call like a local collision avoidance systems that tries to follow that path but is allowed to deviate from the past in order to avoid collisions and prominent examples are the dynamic window approaches which are techniques which basically take into account which changes in the velocities the system can do and this generates your local window of states that you can reach and then the question is which is the best velocity to choose in that window and you're basically choosing the velocity commands that bring you close to the goal or along the trajectory towards the goal and there's a large series of other approaches some for example optimized for very confined in spaces so where the roof is very hard for the robot to navigate others more optimized through environments where you can go as fast as possible and can smooth you can make smooth avoidance maneuvers around optical objects in the world or that make some assumptions regarding optimal or near optimal behavior while navigating through the environment so it depends a little bit on the approach that you wanna that you wanna do and the goals and your environment that you're in so we will be looking today especially here into the so-called a-star algorithm which is a very popular search algorithm so-called informed search algorithm that is used for a large variety of different planning problems not only in robotics it has been developed for robotics and is again one of the key ai techniques that we use today that we find today in a lot of applications and kind of the the gold standard planning method so it's kind of important to understand how that approach works because it allows you to generate a sequence of states that you need to visit to go from a start to a goal location and it does so in a very effective manner there are again extensions and variants of the a-star algorithm such as our star anytime real-time a-star for example which is a real-time algorithm that takes the computational resources or anytime algorithm sorry that takes the computational resources into account in order to optimize the trajectory by generate but guarantees or not guarantees but in most of the settings comes up with an initial trajectory very quickly that you can start navigating on and then optimize this trajectory on the way or the dstar family of algorithms which can be used if you do a lot of re-planning so if you don't do a plan just once but replan while the environment is changing and then they allow you for substantially more efficient updates and then there are also other approaches like marker of decision processes which you then need to take into account if the uncertainty in your motion execution plays an important role so if you assume the system can actually execute what it wants to execute then you're typically fine with your a-star algorithm but if you have uncertainties in your motion execution you have to go for other things other techniques such as mdps for example so let's have a look what are the key challenges today in motion planning so what the key challenges are we want to find an optimal path optimal means under the cost functions that i'm providing this can be shortest path shortest collision free path fastest path a path or trajectory which minimizes certain accelerations that you cure um that are very pleasant for humans if humans are driven by the autonomous vehicle like an autonomous car you just don't have the fastest trajectory you want to have also trajectory which is for example legal um or a trajectory that is very convenient to when you're sitting in the car and you don't care if you arrive at the goal two or three seconds slower as long as your ride is more pleasant for example so there may be different cost functions that you want to take into account here in this lecture we are basically looking into the shortest path or the fastest path then the second question is do you need to take into account uncertainties these can be uncertainties in the action execution so when you go for i want to go from a to b and you give a command to the system um is the command actually being executed or is something else executed so for example if you say go a meter forward and the system goes only 95 centimeters forward is this been taken into account is this actually a problem for you so this is something that needs to be considered and you may also take into account uncertainties in your perception system that your perception system is not perfect the other question is how can you quickly react to unforeseen obstacle so if a new obstacle is in your past something that you were not aware of how quickly can you react to these unforeseen obstacles or what happens if other traffic participants are moving through the environment and may collide with you even if you would stop so these are things that your planning system may take into account but that somehow depends on the application you're actually targeting if we look into most robotics [Music] navigation stacks we can see a so-called layered architecture so you have different layers of planning until control algorithms which are executed by the system running at different frequencies and having different tasks that the system is going to do so what you can see here is a classical um three-layered architecture on the highest level you have your planning algorithm so that's something that we are starting with here with our aced algorithm so how to use a map of the environment and the information where you are in the world in order to plan a path that guides the robot to its target location maybe you do that once per second or once every five seconds or maybe three times per second so but at a rather low frequency you're basically reviving your plan based on the new estimates where you are in the environment and what happens around you and then you typically communicate a sub-goal so a sub-goal let's say five meters in front of a robot to our local planning system a collision avoidance system and the collision avoidance system runs at a much higher frequency typically at the frame rate of your sensor so this can be something from 5 hertz to 30 hertz or 50 hertz maybe even checking if there is an obstacle that moves near the path that the system wants to execute so that there's a risk for a collision and if it sees that there's a risk that the robot would collide with an obstacle it plans or varies a trajectory and generates alternative trajectories that the system then picks in order to avoid a collision that's something that we often refer to as a collision avoidance system or a local planner and then this collision avoidance system actually generates typically a motion command or a transition to a next state and then you have a local controller that turns this local information into an actual steering command or control the motors of the vehicle so that the vehicle actually executes the path that or the the motion command it should execute and typically there's also a correcting control loop that for example odometers are taken into account so a sensor that counts the revolution of the wheels in order to make sure the platform actually executes what it should execute and take some corrective actions so today we will be looking more on that planning side looking a little bit also into collision avoidance or a system which solved these two tasks jointly control is something that we cover in this course but not in this lecture here today so let's look into the standard motion planning problem or planning problem starting here from the top if we look into the standard motion planning problem we typically has as our inputs start pose or start configuration where the robot is right now it doesn't necessarily only has to be a pose it could also be again a full state if velocities for example matter and you start with a given velocity you may want to take that into account as well and then there is a goal configuration um like you need to end up at a certain location with a certain velocity or i don't care about the velocity as long as you reach a certain xy configuration as fast as possible so different ways for defining the goal state it may also not just be one state could also be multiple states and then you need a description of the robot often the geometric description or to understand what's the size of the robot in order to check or combine this with the representation of the environment in order to identify the valid states so states which are which don't generate a collision with the environment or with other objects around so you typically need to have some description of the robot and some description of the environment or representation of the environment so that he can do these collision checks and in order to know what's free space what's occupied space and given that you have that information the planning algorithm should turn this input information into the output information and the output if the pass typically the shortest path or the optimal pass or minimum cost path let's maybe be a bit more general minimum cost pass and you define the cost function beforehand um that move the robot from a start configuration to a goal configuration without colliding with any obstacle so a collision free path that is optimal under some cost function and we will start here with the shortest path so there should be no other paths around that visits the sequence of states from the valid states and should reach the goal uh more quicker or faster or on a shorter trajectory that's something that shouldn't be there there may be other alternative paths which are equally good but no better paths and then we found an optimal solution our algorithm computed an optimal solution to that planning problem next thing we would i would like to look into is the configuration space and try to at least very briefly introduce the concept of configuration space so the plannings plot problem actually lives in kind of our real world and we have a typically a movement a robot let's say a physical object that moves through the environment and can control um its own configuration in order to go from a to b um so we are living in the in the real world but internally we're using something which is called the configuration space and perform the planning in that configuration space so the configuration are kind of the configurations the system can be in so for example if you have a robot which also has an orientation this orientation would be taken into account in this configuration space but it's not just that it basically the configuration configuration and configuration space or state and configuration space allows you to describe where all points the full shape of the robot actually live in the world so if you for example have a robot with a joint then you have a joint angle that you may be able to control which changes the configuration of the robot and just the points that are occupied in the real world so you need this concept of configuration in order to decide if a certain configuration is actually collision free so if you have let's say an arm and whatever the shoulder is the uh defines the position of that arm then just based on this on this shoulder position you can't decide if um this is collision free or not so my arm may be collision free and even if i don't change the configuration of my shoulder that will lead to a collision so you need to take the full configuration of the arm into account and take also the joint angle here into account and the representation of the platform itself in order to make the decision if we here are in obstacle free or an obstacle configuration and so therefore we have in the end states in configuration space and based on this state in configuration space or a configuration in short that allows us to decide if it is collision free or not quite often those configuration spaces are discretized so for example say an object can move whatever just in on a one centimeter grid and has a one degree discretization in its orientation just as an example so i can show here a very simplified um discretized configuration space so this is basically an occupancy grid map where you have cells very large cells now and so we assume that the robot can only be in the center of every grid cell and can basically move to its neighboring cells so that's a very simplified form of a discretized configuration space and this in this case is just a 2d configuration space assuming the robot can move arbitrarily in all directions for example as an omnidrive and then just can move from grid cell to grid cell again this is an approximation we discretize the space and if we are generally living in a continuous world and we discretize it it is an approximation but this is one example for discretized configuration space and then the planning system only uses this discretized configuration space for example in order to plan a path from the start location over here to a goal location over there which states does a system need to visit in order to come up with a valid trajectory that guides the robot along that path so this is an example for such a discretization in general we split up the configuration space in free space and in obstacle regions or in occupied space so then then we have a workspace which is a set of configurations that and we want to live in that part of the space that is collision-free for the system where the system can actually be so our workspace is basically an uh a general space of real numbers for example and it con this is for example our workspace over here and it contains a set of um obstacles which are shown here in this greenish gray color over here so this is basically the surrounding let's say kind of the wall so to say and you have some obstacles sitting here in between and then we have configurations that describe the robot this is for example initial configuration and a goal configuration and those conf with those configurations we can actually describe the shape of the robot this is done with this function a of q so q describes the configuration again with my arm it could be the joint angle and the position of my shoulder and then this function a of q basically maps um the the robot into this into into our space um so which parts are occupied here in that workspace um so that we can make the decision will this lead to collision with an obstacle yes or no so what we're doing technically is we are taking uh we define the free space as the configurations in configuration space so my con my um configurations in configuration space so that the a of keys the shape of the robot intersected with all obstacles gives me the empty set so these are basically all the wired regions over here so where there is no overlap with um of the robot with an obstacle and then the occupied space is simply the whole space minus the free space now this is one way how we can describe the free configuration space so the configurations that the system can can be in without colliding with obstacles that we have in that surrounding and then what we typically do or need we need to have two configurations in this free space an initial configuration our current configuration and our goal configuration so then the goal is to find a path that can connect in configuration space the initial space and the gold state so it could for example look like this this could be a trajectory here in that configuration space and that can be described by a function tower from zero to one that maps only into the free space so it's not allowed to map into the occupied space because that would then lead to a trajectory which which leads to collision so that tau of zero is my initial configuration and tau of one is the goal configuration over here so basically having a function which describes which states i need to visit in order to move from the initial configuration to my goal configuration and that means we can actually do the planning so find that function that mapping over here in our configuration space so that we basically have a point that we move through the c space which describes the trajectory from the initial configuration to the target configuration and this is kind of a more formal definition how i can perform this or how a trajectory a path or a solution the planning problem looks like in this configuration space so this configuration space or c-space is not necessarily continuous it is shown over here often it is actually discretized and there are different ways how can discretize my configuration space so the configuration space can be discretized in different manners and there are kind of two general approaches how i can do this the first one is a combinatorial approach and this is something that we will be using here in the lecture which basically says i take all possible states and i break them discretize them down into uh into smaller states like a voxel representation in the three space or a grid cell representation in the 2d space so similar to an occupancy grid map for example if you just consider a set of 2d states so this is what is called combinatorial disk or combinatorial discretization which then leads to a combinatorial planning problem the problem in here is that the uh if the the dimensionality of the configuration space actually increases so there's a robot which has a lot of different joints for example this space gets very high dimensional and the number of discretized states that i'm actually generating is exponential in the dimensionality of my search problem and this can be a problem so that's something that you would apply if you're planning in low dimensional spaces 2d spaces maybe 3d spaces then you can do this discretization or this combinatorial discretization if you go to 10 20 30 dimensional spaces this is not going to fly anymore what you then do is use sampling based techniques that means we don't necessarily discretize our state space uniformly but we sample possible configurations and we are sampling possible configurations in the in c3 so we need to have a technique to generate configuration this is the free space which are close to a given configuration for example close to our current configuration so that we then by sampling we're basically expanding the states around our current configuration and expense states sample states and then plan only in those sample states and we are expanding or sampling these states during the planning process and with the hope that with an intelligent strategy we only need to sample a small subset of the overall space and then through the sampling process don't need to discretize all the different dimensions we only need to explore the states which kind of look as if they would be promising to lead the robot to the goal and if this is the case if you have this additional information at hand then these sampling based planning techniques can be very efficient so with those two different approaches we can actually generate our c-space discretization so the next thing we need to do we need to search through these possible configurations in order to find a sequence of states in configuration space that guides me from my current configuration to my goal configuration i said already we need to search so planning can be solved by using search algorithms in order to search the configuration space for a sequence of configurations that guide me from the current configuration to my goal configuration and we have again two different concepts in third strategies that we want to distinguish here these are the uninformed search techniques and the informed search techniques so again the goal is to find a sequence of actions so a path that guides me from my initial configuration to my desired goal configuration and how do i find the sequence i can search through all the possible states that i'm expanding in my configuration space and then start from my initial configuration and hope to at some point in time end up at my goal configuration and if i found that the transition from the initial configuration to the goal configuration i also found a sequence of actions that i can actually execute in order to come from my current state to my goal state and the uninformed search algorithms are kind of the simplest form of search algorithms and they kind of form of a blind search they are blindly searching through my configuration space they may need to take into account how many steps they already have been taken from their start location until they reach a certain state and then they always try to make the decision of expanding the cheapest node for example in order to you know to explore the space further until they find the goal configuration but there are different ways how these costs can be taken into account it can be a search tree that i'm expanding i'm just looking into the the level or the depth of that search tree i may have a cost function associated with every state so individual cost functions for individual transitions between states that maybe take into account these are in different variants of this uninformed search techniques but what the uninformed search technique is is not doing it doesn't exploit any additional information about the problem or additional knowledge about the goal or things let's say upper bounds or lower bounds for example that i can generate in order to say something how cheap or how expensive a trajectory may be if i know where i am with respect to a goal configuration so the uninformed search technique don't use this they only use their cost function they're expanding their own cost function and popular algorithms in this context are breadth first search depth first search uniform cost search these are commonly used techniques which just will introduce and is with one slide basically each in order to kind of guide you towards from the uninformed search techniques towards the informed search techniques because a-star is an informed search technique in contrast to the uninformed search techniques the informed search techniques use additional information about the environment and micro configuration and that's something what we call a heuristic a heuristic is an estimate of the cost so how expensive is it in order to if i start in my current configuration and i want to reach the goal configuration to reach that goal configuration from my start configuration of course if i would have the perfect knowledge about this my planning problem would be very easy because i could just make a gradient descent in this in this cost function but what the heuristic does it provides an estimate for that so it can estimate the cost these are not the actual cost but then estimate of that cost and how can i exploit an estimate and still come up with a good solution or maybe even an optimal solution and that's what the informed search techniques are about they are using this heuristic to speed up the search so to find the goal configuration more quickly but the quality of the found solution should actually be still the optimal solution not all of the informed search techniques are optimal the first thing technique we'll discuss for example greedy search is not an optimal trajectory optimal search technique that leads to an optimal trajectory but a star for example is and for that we need to have certain constraints on how that heuristic could look like or should look like but what you can generally say that these informed search tech user heuristic use additional information in order to find a solution faster so if we have an optimal search algorithm that means an algorithm which provides me with a trajectory which is optimal that means there's no better trajectory or no better a sequence of actions guiding the robot from its current location to the target location the question is under which how do i compare algorithms under which criteria or things i'm actually evaluating and can compare different search algorithms and there are a couple of things i can take into account the four kind of most commonly used criteria for evaluating search algorithm are shown here so it's completeness optimality time complexity and space complexity so what does that mean completeness means does the algorithm actually find a solution a valid sequence of configurations from the start to the goal location in case it exists so of course i can define a planning problem where there is no path from a start configuration to a gold configuration so for example we have two rooms but there's no door between the rooms and the robot is in room number one and the goal is in room number two there's no way to reach the goal configuration and that means there you can't find a path but the question is if there is a doorway can actually guarantee that the algorithm will find it and this is what i refer to as completeness so does the algorithm find a solution if there exists one and of course we would like to have complete algorithms because we don't want the algorithm to abort and say can't find a solution although there is a solution that's something that we would like to avoid so the algorithm should report a solution if there is one the second thing is optimality and this is what i already referred to before is the result that the path that the planning algorithm actually generates is this the optimal one or is there a better one optimal under some cost function so there can be other alternative paths which are as good as the one that i have or they are worth but they can't be better if this is the case then i found an optimal path because there is no better path under the specified cost function people talk about optimal planning algorithms you always need to remind yourself that this is under the defined cost function that doesn't mean this is the best algorithm for any robot navigation problem it's just kind of the algorithm which generates the best possible solution under the assumption that have been made in those assumptions of the cost functions the discretization of the state of the configuration space and other things just to keep that in mind so be careful with the word optimal algorithm or optimal technique unless you specified what optimality means for you and then there are things like time complexity and space complexity so how long does it take to find a solution how much memory is needed to do that and in this kind of how much we typically don't refer to whatever 20 seconds or 10 gigabytes we refer to that in relation to the number of states or the number of transitions which are out there so it's a complexity for example provided in terms of the uh big annotation so it's not just kind of whatever it takes 10 seconds to compute a path it's more like it's an algorithm which has whatever is linear in the number of states and quadratic in the number of transitions for example or edges that we have in this in this search problem um so these are things that i want to take into account so in the end i won't have algorithms which are complete which are optimal have a low time the low space complexity that's kind of the best thing i want to have okay so now let's dive into some examples of uninformed and then informed search techniques and we can actually gradually build them up and then kind of combine all the different search techniques that we are now presenting so that in the end we come up with the a style grid we start here with the very basic um search algorithms breadth first search bfs or depth first search dfs where you are expanding your configuration space in a certain order not taking any cost into account so you're basically expanding a note and you can basically say um every note that you're actually expending um is has a certain cost or has a fixed cost a cost of one associated to this and you may want to find the kind of number of states that you need to visit which is the smallest number of states to reach your goal location so this is kind of a tree where you're starting so one is your initial configuration and you're expanding basically the neighboring states that you can reach from the state number one and and you stop once you've found your goal configuration so for example from one you have the neighbors two three and four that you may consider to visit and two has a neighbors five and six and the state five has the neighbors nine and ten and so on and so forth and you're basically searching through this tree that you're expanding your search tree until you find your gold configuration so you start over here and in um breadth first search you're basically passing this level by level so you start with your initial configuration then you first inspect all the neighbors so two three and four and then you inspect the next level then the neighbors of two which are five and six neighbors of three three has no neighbors nothing to do here neighbors of four is seven and eight and so then you have kind of five six seven eight is the next level where you haven't checked for the neighbor so you start checking the neighbors so from five it's nine and ten and so on and so forth until you find in this example for example state number 12 as your as your as your as your goal configuration so this is breadth first search and you're expanding this basically row by row and you can see this this is basically a cost of 0 because it's the state that i'm in 2 3 and 4 are states which i could visit with a cost of 1 so making one movement so to say from one state to its neighboring state and then the second level would require two movement steps three movement steps and so on and so forth so by searching through this space i actually have an optimal algorithm assuming that they all have kind of a cost of the same cost in which i'm expanding that it's also a complete algorithm it will simply go through all states and will find the solution if a solution exists somewhat different is depth first search here i'm basing it going level by level i'm basically checking for the first neighbor and then it's first neighbor and then that's first neighbor until there are no neighbors and then i'm basically back tracing back and expanding all the other states so i'm basically going deep in my search tree this is somewhat different properties it's definitely not an optimal solution because i may find a path by just going into the dapps to a goal configuration although there may be a shorter path just by going through a different neighbor and i guess given that i'm exploring going deep first and not explaining it level by level as for a breath breath first search then i may not come up with the optimal solution it is a complete algorithm if we are in finite spaces so if there's just a limited number of states i can explore if this is an infinite number of spaces i may run into a kind of an endless loop or an infinitively large corridor down here although there's a very short pass over here to my goal location and i may miss this and therefore will not find um a solution even though a solution exists which is maybe even a very simple path so if neighbor number eight would be actually the goal but i'm starting with two and continue forever searching this part of the tree i may not find the configuration although it would actually be pretty close so there's not an optimal solution and it's not complete in infinite spaces it has different complexities they basically depend on the branching factor so how many neighbors do we have how deep is the goal so which level is the goal so basically how far the goal is away or the maximum depth of the tree these are different parameters on which those two algorithms actually depend on and breadth first search is kind of the the absolute basic technique that we may want to use for path planning problems how often often however we don't have equal cost for all those expansions so some trajectories are more expensive and some trajectories or some transitions are cheaper and this leads us into a so-called cost sensitive search which is still an uninformed search because it only uses this cost function and it doesn't take any further information into account about the goal and what we have here is that all nodes are treated uniformly so in the same way so independent of the level for example they are in compared to bfs and dfs but they are distinguished based on a cost function so there's a cost function which tells you the transition how to go from one node to another node but the nodes itself are treated uniformly therefore it's called uniform cost search it doesn't mean we have uniform cost so this may be a bit confusing it doesn't mean we have uniform cost in that search it means all the nodes are uniform but we have a cost or different cost for transition to go from one node to the other node okay so and what this algorithm does it takes into account the accumulated cost on the way so if we have my graph over here and i have here numbers on those edges and these are costs of going from a node one to a node two would be a cost of two to go from one to six would be cost of seven so this is what those numbers mean here written to the edges and what the algorithm does it looks into the accumulated cost so the cost that i have invested so far so my path cost so far so if i'm for example the cost of this node number five if i started one would be two plus four equals six so this would have an accumulated cost of six and this node over here would have an accumulated cost of five plus six equals eleven so this is the so-called g-cost or the accumulated path cost and what the uniform cost search does it basically always expand the node which has the smallest accumula accumulated path cost so we are basically always looking oh what is the the nodes which i reached in the cheapest possible way so far and i'm trying to expend those nodes and this leads to behavior which is actually similar to a breast first surge if we have uniform costs so if all those costs would be one but often this is not the case we have different costs for different transitions and then the cost sensitive search is actually the algorithm that you should use because it expands the cheaper nodes further all the nodes which lead to a cheaper accumulated path cost and it's an optimal and complete algorithm so you will find the optimal solution and if not if the solution exists you will actually find it and this is kind of a standard approach in the uninformed search technique that you're just saying okay let's see what i pay in the end how expensive it is to reach a node and i'm always kind of expanding the cheapest note until i find my goal location because if i do this strategy and find the goal i can actually guarantee that i found the cheapest solution because there's no other cheaper path out there any other node i can reach with a lower cost and i haven't found the goal so far so the first time i find the goal i know it is actually an optimal solution and again if i use this uniform cost search but i have uniform costs so if all the costs are one which is illustrated over here this basically means we are passing through it level by level and then the uniform cost search basically degenerates um to bfs um so there's a direct relation between uniform cost search and bfs so bfs is a special case of this uniform cost search with a transition cost of one so there's a direct link between those techniques there's also another link so you may have experienced the dextrose algorithm which is an algorithm which expands the whole search space and tries um the or compute the shortest path um to from all nodes to one node for example if you start your expansion from your goal node um and if you run this search starting basically from your from your goal configuration um and expanding the whole tree and save those g cost for all the nodes that you expended so the whole um the whole configuration then this is actually dextrose algorithm we get the same result as dijkstra's algorithm would do so there's a direct link between the uniform cost search and uh and dextrose algorithm which may allow you to relate that to techniques you potentially have seen so far so this is a part of the uninformed search techniques so uninformed again means we're not exploiting any further information about um the goal or about the problem that we are using and this with this we can define rather simple search strategies and these strategies will lead to optimal solutions and some of them are also complete that's kind of great the problem with those techniques is it takes a while until i actually find my goal configuration so the question is can i do better and can i generate solutions which can exploit somewhat more information and in through that additional information can find the goal configuration quicker and that's what the informed search techniques are about so the informed search techniques are in contrast to the uninformed search techniques they don't use any they use prior they use further information about the problem in order to be better than the uninformed search techniques so we use additional knowledge and this additional knowledge is encoded in a so-called heuristic a heuristic is a cost estimate so it's a function that estimates the cost from a configuration from any input configuration to the goal configuration and this cost will not be the actual cost but it's a hint it gives you some idea how that cost could look like and of course the better that estimate is the better or the the faster i can probably find the gold configuration if your heuristic is completely random of course it is not of any help may even distract you and guide you into the wrong direction but often we can actually provide you with certain bounds on cost like whatever reaching the target configuration from my current configuration you can't do that better than a certain distance because you take the straight line distance between two configurations into account you can see there they can't be any faster or shorter path rather than a straight line distance assuming that we're living in an euclidean world and so this allows you to provide a bound for that cost and this is very very valuable which you can exploit in the search and to guide the search expanding nodes which potentially lead to the target location or to the goal location faster than the expansion of other nodes and that's exactly what the informed search techniques do they look into nodes which are promising which are most likely to lead to a goal configuration quicker so they are there these heuristics to speed up the search and one technique is greedy search in greedy search or the simplest form of greedy search we are basically trying we're only taking into account this cost estimate so how good can i actually reach uh from that note my goal location so that's kind of the simplest form of greedy search there are more advanced ones um and you're basically just checking using your estimate estimation function your heuristic how far is it from that state to the goal you're only always looking into the state which is which promises you to be the one closest to the goal under this heuristic so consider you have expanded a certain number of states and you're kind of here in that state number four and assuming our goal would be that state number 12 which we don't know how to reach it yet then what the heuristic function can do it tells you okay from 4 to 12 you have the h function tells you an estimate of the cost although you haven't expended those nodes yet so you don't know how they look like otherwise you would know the cost already so you haven't expanded them but this h function can give you a good guess on what that cost could be and then if you're having current nodes to expand to look further you would always take the node which promises you the shortest distance to the goal under this heuristic so the greedy search ignores the the accumulated past cost so far so it ignores how expensive it was to reach for it just says from all the nodes that i've seen always expand the one which guides me closer to the goal so it greedily optimizes based on that heuristic and this leads to algorithms which typically find the solution very quickly so they are faster in finding the goal location if we have a good heuristic the problem with this algorithm is it's not guaranteed to find actually the optimal solution um because eurostic is most likely not the the optimal solution um and you will not and given that it's not optimal you cannot guarantee that you will actually find the optimal solution so your you get your result quicker but the quality may suffer and what we can then do is we can say okay let's combine a uniform cost search and the ideas of greedy search to come up with a new algorithm which on the one hand side takes into account the the accumulated past costs so far but also takes into account the estimated cost to the goal and this is what the a star algorithm does so a star you can see a star as a combination between uniform cost search and greedy search so uniform cost search in the sense that it considers the accumulated path cost and greedy search in the sense that it takes the heuristic into account and actually computes a sum of accumulated cost plus a heuristic of how expensive it is to reach the goal from the current configuration that i'm considering so again in my example if we are again in our node four what a star would do it would take a sum of two costs into account the cost of reaching for so far so the cost that i know the actual cost that i computed my accumulated path cost g plus heuristic how expensive it is potentially to reach my goal location so my state 12 in this example so through this combination of g and h a star can provide you with an algorithm that combines the advantages of both algorithms you need to have some constraints on it heuristics so you can't take any heuristic but if you choose heuristic in a smart manner then you actually end up with an optimal and also complete algorithm which is called a star and a star is an algorithm that actually has been developed in the robotics community so in the late 60s niels nielsen at the stanford research institute was involved in the project was called shaky the robot so one of the uh this is this this robot over here um which was one of the very early robots that could do navi solve navigation tasks and perception tasks that are let's call it similar to the tasks that robots can do today of course in a simplified setup under more constraints but it's very impressive to see what shaky the robot was able to do in the late 60s given the capabilities that they had at hand at that point in time and a lot of relevant algorithms that are out there today in our standard algorithms in robotics or in ai today have been developed at sri at that time and among them also the a-star algorithm which is today really the gold standard algorithm in ai for informed search and use in the majority of planning systems out there is a star or some variant of the a star algorithm so it's one of the things where the robot robotics community actually came up with a new solution that impacted also other fields so how is a star used for that type of problem so we're using the a-star this algorithm which combines the accumulated past cost so far and the estimate of the cost for reaching the target location in order to plan a path for the robot from a start location in to a goal location and what we typically assume in here is kind of known poses in a map of the environment so we assume to know how the environment looks like we have a geometric representation of the environment most of the time often that's an occupancy grid map or a grid map structure but it could also be another discretization polygons of the environment for example and we assume always to know where the robot is in the environment so we assume to have a working localization system at hand and then what the algorithm does it provides you with a sequence of configurations that the robot needs to follow in order to be guided from its current location to the goal location and if the robot drives along that path it will be the optimal path and it uses a heuristic to do that and the heuristic depends on your cost function because this heuristic always needs to underestimate the actual cost so you're not allowed to overestimate the actual cost if you're overestimating the actual cost you don't have the optimality anymore the property of optimality but if you're underestimating the cost or smaller or equal then you actually have a heuristic which leads to an optimal solution and if we are looking into the shortest path problem not the fastest but the shortest path problem the straight line distance is a very good tool to use assuming we are living in a euclidean world which we do most of the time depending on the planning problem you want to solve but if you are living we were moving to the 2d world for example um we can very well describe it by euclidean space then the straight line distance between let's say two x y locations or two x y that locations is the shortest connection between those two nodes so it's it's a lower bound for the length of a pass going between those two configurations and this lower bound is exactly what i need for my heuristic so the straight line distance is a very good heuristic for planning in the 2d or 3d world of course you may need to take orientations into account which are non-euclidean or if you look to for example shortest time planning algorithms so we want to generate the faster trajectory but then you can still use the straight line distance by saying the fastest possible way from this configuration to another configuration is driving with the maximum speed that the vehicle can actually drive along a straight line distance you can't be faster so you can still use these ideas of a straight line distance in order to come up with an admissible heuristic so an underestimating heuristic for this a star planning problem and so that's what kind of a core description how a star can be used for robotic path planning so let's see how that looks like in our discretized configuration space that i have actually seen before so what we see over here we have this discretization of the environment our system starts over here and wants to go down here and what it does it basically i will expand those notes over here and we'll always expand the nodes based on a combination of the actual cast actual path cost accumulated so far to reach a node and the predicted cost from that state to the goal so what i can do is just kind of run this animation um once and then we can see how that evolves so you see always in nodes which are expanding so for those gray states here we actually know the cost the other costs here are actually called boundary cod or states where not all the neighbors have been explored yet where i know the cost already and i'm basically expanding that always expanding these new nodes which are um here the kind of the boundaries between the already explored space in the unexplored space until i find my goal location and then this gives me the optimal path from my start to my goal so just kind of rerun that video so with these explanations you may be able to kind of better appreciate that again so you see how the notes are expanded and the the yellow note always is always currently the currently expanded note and you can see the search is actually guided towards um towards this state so you're always expanding notes and it prefers to go down here because it's more promising to reach a goal location until it reaches the goal and those up here for example are not explored because the heuristic tells me it's probably not a good idea to go here because you're moving further away from the goal so this is unlikely to be a good path and it's white exploring those nodes and you're still not sure if this is the shortest path because you do not know if there's a doorway for example over here or this is the shortest path so the algorithm basically expends these two possibilities until it finds the optimal solution to the goal and that's kind of how a star works so let's dive a little bit deeper into the a-star algorithm what a-star does it minimizes the cost which is a combination of the actual accumulated path cost for a node so how expensive it is to reach a node n from an initial state so from the initial state to n how expensive is it so this is the g cost that we used in uniform cost search the accumulated path cost to reach a node and then it also takes a heuristic into account h of n and h of n is an estimate starting from n how expensive is it probably or what do i estimate how expensive it is to reach my um goal location so from n to the goal so g is from start to end and ages from end to the goal where g is the actual cost and h is just an estimate and then what a star use uses the so-called f cost and the f cost is the sum of the g cost and the h cost so the sum of what i know already plus my estimate so i'm basically always using a combination of something i already know and i'm estimating the rest and this is what this f cost is sum of the costs i already know and my heuristic cost so it's a combination of the cheapest solution so far and an estimate to the goal and where this overall sum is the smallest sum this is the node a star is expanding so a star will always expand the node which the smallest f cost among the nodes it has already considered or is currently considering so it basically expends this search tree and always expands the node which which has the smallest f cost but the question is how does age how much h look like the heuristic look like so that we actually find a good solution because it's clear if if h is completely arbitrary this is not of help we actually get we actually mess up our cost function so there's some properties that h must have in order to come up with the right solution so the first thing is of course if i'm at the goal but that should be pretty clear if i'm if if n is the goal state then h of n should be zero because if i'm at gold state i'm at the gold state there is no further cost but that's kind of easy to achieve what's harder to achieve is that the cost for all nodes must be the estimated cost must be smaller than the actual cost so i always must underestimate the actual cost i'm not allowed to overestimate the cost so if that means if there's a path from end to the goal and i say my heuristic says oh this cost is whatever 10 units but the extra cost is 8 units that's something which is forbidden i'm not allowed to make an estimate that is more expensive than the actual cost i always need to make sure that my costs are smaller than the actual cost so i need to underestimate my cost so if eight star is kind of the actual cost the thing which i don't know but which assuming we would know it assume we would know the perfect cost to the goal then they call that function h star of n then h of n must always be smaller or equal than h staff and of course we want to push it up to be as close as possible to h staff and because of h staff and i know all the costs and then my planning problem is super easy because i just need to do a gradient descent in this cost function but this is typically not the case i can't know the optimal accumulated cost for all nodes on the way already so i want to be as close as possible to h star but it must always be smaller and if this is the case i have a so-called admissible heuristic and if i have an admissible heuristic an a-star will lead to an optimal solution and again a standard admissible heuristic for planning in euclidean spaces is a straight line distance so it's a very good starting point which already is a quite good heuristic again if you know more about your problem you may take additional information into account but if you don't any if you just know how many cleaning space you don't know anything else straight line distance is a very good choice so what happens just as a side note if h is actually larger than h star so it's not admissible heuristic anymore in this case you lose the property of optimality so the result that you get is not guaranteed to be the optimal solution anymore but you can say a relationship by how much you overestimate you can make another bound on your solution so if you have let's say a heuristic which is up to twice as expensive at as the optimal heuristic so you're overestimating costs but not more than a factor of two then you can show that the result that you get is only up to a factor of two worth in the optimal solution and this is a property that some of the anytime algorithms like anytime real-time a-star rr star is actually exploiting in order to start with the path which may not be the optimal one but it's very very efficient to compute and then kind of optimize it the more computational resources you have available so you as a rule of thumb you must be smaller than h star to be optimal but the larger h n is the faster your search gets so you want to make h of n as large as possible to be fast but still smaller than h star in order to find an optimal solution and depending if you want to give up optimality and optimize for speed or you say speed is important but not that important i still wanted the optimal solution you can try to overestimate or underestimate an h-star so it basically depends on your on on the solution on your application if it is okay to have a path cost which is slightly more expensive than the optimal cost but if you find it therefore in just a fraction of the time you may use your computational resources in a smarter way for your problem so it may be worth to actually make that trade-off so how does a-star work as an algorithm we said we are always kind of basically expanding the search tree and always expend the uh the tree the nodes which has the um the smallest h cost um in practice it's it's it's like that but it's slightly more complicated and if you want to implement this what you need for that you need to have a priority queue so a q way you can put in nodes and the queue orders the nodes and you can always get out the cheapest possible node and what can also happen given that we in the end have a graph structure of our configuration space so can we can reach a node through different ways we need to be able to update nodes if we find kind of a cheaper way for reaching it and this is something that we need to take into account um i don't want to go now through an algorithmic description of a star i just want to go to basically a flowchart describing the different stages of a star and this is a drawing from joseph's book so and we can just go through those individual steps of the aced algorithm kind of as a pseudocode or flowchart based description on how a star works so we start somewhere over here and what we have we have an open set o which are the nodes that i'm currently considering this is my um my priority queue and i have a list or a storage container c which are the closed nodes so those nodes which i have already investigated i know they cost already i'm not not need to take them into account anymore okay so i start and say is my um open list empty is there any nodes that i haven't fully investigated they haven't visited all the neighbors i know the cost now finally um if this is the case then i'm actually done because then there's nothing to be do to be done anymore i that means basically i explore my whole state space um you typically start with oh being the initial configuration of the robot being inserted to that into that to that o list so i'm not starting with an empty list so that's basically the initial configuration okay so that means it's not we're not done yet so what what do we need to do so we need to pick and best from o in the way that it has the smallest f cost that means i'm picking out from my open list the note with the smallest f cost so the smallest accumulated path cost plus heuristic estimate and this is the next node i'm i'm going to expand and this in my search tree the cheapest node in kind of the boundary of nodes to explore and i'm actually picking that one okay so what i do i'm remove it from my open list so from the nodes to inspect because i'm expecting it right now and then adding to my closed list so those which are already done i don't need to investigate them further then i check if this node that i just took out from my priority queue is it actually the um the goal configuration if it is the goal configuration i'm happy i'm done i found the goal configuration everything is good but in the majority of cases this will not be the case because i need to expend a large number of nodes until i find my goal so assume we are known this on this node track over here then what i need to do is that i need to look into all the nodes which are neighbors of that node that i'm inspecting so i'm taking that node and basically now saying okay it's not the goal node so i need to look further let's check all its neighbors so i'm basically expanding all those neighbors and see and want to check for those neighbors which are not in the in the closed list so those which i haven't visited so far it could be that i visited them already because i reached was next to them on another on another path through the environment because we're in a full graph so there may be several routes um to a configuration and then if i'm taking this neighbor out and it's not in the closed list i need to check is it already in the open list so is it already in my notes i actually need to inspect further um if this is the case so if it's not the case if it's not in o i actually need to add it to o so it's a note i actually need to consider so next thing to visit if it is not the case that means it is already a no i need to see if there is actually the new path that i found is actually a cheaper way for reaching it so if it is only in if it would have been in c so all the nodes that i'm in c are already done the pos the cost can't change anymore everything which is still an o may change because i may find a cheaper path to reach that node and and this is this happens over here i need to make the check if the if the cost through the node that i'm expending is actually cheaper than what i was knowing about it before so in this case i need to update the cost and um basically to update the cost in o and then i'm done and i'm checking again checking is there anything else or no potentially i'm now expanding the note that i've just added or one of the neighbors i just added because um if the cost from the previous note to one of those neighbors was small it's likely to be now the cheapest one so basically i'm expanding towards the goal um but it could also be any other node which is cheaper this is what the priority queue does which is realized with oh it always picks out the um the cheapest node out of the open list and so this is the node that i'm expanding and this is basically the algorithm which can be implemented with a couple of lines of code so one or two pages of code you need in order to get that done assuming you have a priority uh queue for your implementation which does be handling automatically for you and that's the way how you would then implement that approach and this gives you an optimal algorithm which is complete and which is comparably fast the only thing you need to do is you specify your path cost and you need to specify a heuristic which is admissible that means that under underestimates the actual accumulated path costs and with this you have your east algorithm being implemented let's see now how we can how that looks like if we apply that to robot navigation we could apply it out of the box and just plan path but this is often not the best thing we can do we have to make a small small few small tweaks in order to do better we first will do this for simple 2d playing problems but then also see how we can actually exploit that in order to combine this high level planning that we discussed and this bit more low level planning this collision avoidance in one go so application to 2d plus playing what we start with so if the robot is in a certain state in this configuration we typically use the n8 neighborhood that means we on a grid map we can go into all all neighboring states um so again very often we do this on our occupancy grid map and plan a path on an occupancy grid map at least in low dimensional spaces if you have very complicated configuration spaces that's not what we do anymore but often we plan in a 2d space and though the robot can basically rotate on the spot and then move into different neighboring configurations so that if i'm starting over here and i want to go down here in this room you can see here the expanded nodes it's kind of this gray area the darker the gray the more expensive the cost and we are expanding those nodes until i actually find the way to the target location that's basically what we have described so far how the planning actually works [Music] and in its basic form nothing needs to be added to that you can plan your path for your robot the sequence of two these grid styles you need to visit in order to find an optimal solution to the path that's something that works however there are a couple of assumptions which underlie which we have made on the way which may not hold in for real-world robotics and we need to always make sure we revisit our assumptions to see are those assumptions actually valid and justified so the first assumption we made is that the robot knows its own location the orbit always knows where it is that's kind of an assumption that we often say ah maybe we can assume that but the question is what the quality of my localization estimate how accurate is my localization actually is it accurate up to 20 centimeters you can say okay let's count something i can typically assume when navigating with a typical sensor setup in an indoor environment um but that may be different if um you're you you go really really close to obstacles let's say just one centimeter next to an obstacle in order to optimize your trajectory in this case your pose estimate which has 20 centimeters accuracy uh may not be good enough anymore um and if you so you need to be aware what means you know your own location so if you need to be really precise millimeter precise that's something which most systems are unlikely to provide to you at least can't guarantee it um what you then assume is that your map is correct so assume the map of the environment is correct so either your pre-given map is correct or you use your sensor data to update the map but you assume at the point in time when you start the planning process you know that your map is correct and that's something which is again an assumption if you have people which walk through the environment that may not be actually the case and so you need to we need to find a way in order to deal with this problem and the other question is is your motion execution actually being correct if you say go meter forward does the system go meter forward or does it maybe even drive a little bit to the left inside just say one degree to the left because there's a small drift if you need to take that into account the planning process it gets much more complicated a star ignored this and so it means basically that you shouldn't drive really really really close to walls if you do that really really driving close to walls and just turn a one degree more to the left hand side than you actually should you would actually collide into the wall so although a star computed a collision free pass who guarantee that is actually collision free in the real world no one and so you need to revisit always your assumptions what are the assumptions that you made and maybe you should change your problem a little bit to at least not completely avoid the problem that we have but being more robust under small variations of those problems so if i say most of the time actually well localized but sometimes maybe not perfectly can i take that into account in the planning process not necessarily in the planning process itself but in the way i formulate the problem that the solution that the planner generates is actually less sensitive of for example leading to a collision um if those assumptions are at least slightly violated so potential problem that i have that the robot is slightly delocalized that one i just said before so you oppose this when it's slightly wrong um the other thing that you can see if you if you watched a few of the a-star algorithms finding a path from the current configuration to goal location that the pass often guides very close to an obstacle because you want to drive as fast as possible and you don't want to make a small detour it's totally fine to go just a millimeter next to a wall because it's collision free right according to planner no overlap between the robot and the wall even if you just pass by with a millimeter of safety distance um and um the algorithm will say yeah perfect it's collision free if you would could guarantee to execute that millimeter precise power that would be the case but that's something we can't do in reality so you may want to increase your safety distance to obstacles a little bit don't make the trajectory substantially longer but try to avoid to go to really close to obstacles that's one way how we can actually deal with this potential problem um something else that we can see if we have a grip map we basically have discretizations in 45 degrees in the orientation of the platform and so if you're if the typical routes that you take or the corridors in your environment for example are not in line with the grid structure that can actually lead to sub-optimal trajectories just because of the discretization of your state space you may get actually weird navigation trajectories that the robot does some weird movements when navigating through the environment because you just did a two-course discretization of your configuration space and that is something you may always also want to take into account so we now want to look in the remaining part of the course on how can we should deal with those two potential problems and see how we can actually tackle that so what to do in order to avoid that the robot is often guided really really close to obstacles because if it is slightly delocalized this would be a sub-optimal behavior so they actually coupled those two problems if the path bending algorithms wouldn't guide the robot that close to obstacles a small delocalization may not be that problematic and that's actually something that we now want to try to achieve how can we change the overall procedure or how can we actually actually not changing the procedure the plan the the ace diagram itself we keep it as it is but how can we chop change the formulation of the problem that we actually get trajectories which keep some safety distance to the um the easiest way you could think of is probably just expend the obstacles so take your map and make all the obstacles 10 centimeters thicker if you do that just for the purpose of planning if the robot was before a guy being guided one millimeter next to the obstacle it would be now guided ten centimeters plus one millimeter close to the obstacle where you say okay that's a reasonable safety distance i'll stick with this i just expand all my obstacles and plan a path in this inflated world and so the robot will keep a certain safety distance to obstacles totally fine you can do that that's a very good approach if you have enough clearance in your environment if you don't have any narrow passages in most environments you have door frames for example and the door frames are not too large maybe 80 centimeters wide and if your robot is 70 centimeters wide expanding everything by 10 centimeters would actually lead to a planning problem where there would be no path no way for the robot to actually pass through the doorway so that's optimal so you don't want to enforce the safety distance but say if possible do it and we can do this with a trick and the trick that we are now applying or to solve this to kind of expand the obstacles but don't expend them either full yes no the heart constraint we can use the convolution of the grip map so remember we can add a gaussian blur so to say to our map to occupancy grip map for example so that means we are basically smoothing the occupancy values in local neighborhoods something that we commonly do in images and image processing so for example you take your favorite image processing software whatever game photoshop or whatever it is and you have a filter it's called gaussian blur what it basically does it takes for every pixel neighboring pixels into account and performs a certain type of smoothing um with a certain kernel basically averaging over the occupancy values that means if you have values which where one value is high and the neighboring value is small you basically take some of the high probability mass of being occupied and kind of move it into the neighboring cell which had a low probability so you're increasing the probability the pro occupancy probability of that of that cell and if you then relate your cost to the probability that the seller's occupied you're basically adding some of the expensive costs of the or infinitely large cost of the of the occupied space into the nearby free space you're basically increasing the cost so it's cheaper for the robot to go to real free space which is far away from an obstacle and just by kind of smearing so to say the cost into the neighboring cells just a little bit the system will actually try to avoid those cells if it can but if there is no other path it says okay there's no other way i have to pass through that door frame so at that position the door frame i actually need to be really precise and go directly to the center of the door frame which is also an effect that that smooth thing can generate for you that you really want the path um through the center of the door frame trying to maximize the distance to the walls um there's a more expensive path the robot may even slow down or use this information to slow down to navigate more precisely but whenever there's enough free space you will actually drive through the free space so what we do we convolve a grid map and perform the a-star planning in this convolved grid map so just as a small example basically a 1d cut through a set of grid cells so if two cells one and zero and one which are occupied occupancy probability of one and those which are next to it have an occupancy probability of zero there are three cells this would be kind of basically a clipped grip map a way of just have zeros and one values and if you kind of perform a gaussian or a binomial kernel which just kind of gives you a gaussian blur basically um and move it over those cells and do a two convolutions then your values your probability values your smooth probability values would actually look like this so those get a little bit lower and those get a little bit higher because you're basically taking probability mass and smearing it into the free space and if you then would actually perform your planning in this convolved grid map or maybe you afterwards actually enlarge those values back to one because you know they are for sure occupied you never want to go through move through those states and then these values over here will actually generate cost which would like keep the robot here at uh at cell number four if it can do if there would be another obstacle over here it would actually go exactly through the middle and that's what the map can convolution does for you if you perform planning in a convolved grid map and this is done through so-called binomial kernel which basically means four cell where you want to perform that smooth thing this cell over here you say i take half of the probability value of that cell and a quarter of the left and the right cell and do this over all rows and all columns for the map and this basically generates a gaussian blur and this is a form of kind of pre-processing that you're doing you basically perform this convolution and then you take the maximum operation from this convolved map and the original grid map and then perform the planning in this modified map which is also often referred to as a star in an convolved grid map and this allows you to kind of avoid that the robot goes too close to obstacles if it has the possibility to avoid it given the possible trajectories to the goal and this is kind of a standard method that is done today in most planning algorithms that you kind of in this way kind of inflate or not not fully inflate but kind of smoothly inflate your obstacles in the environment and plan in this type of map and this leads to navigation behaviors or trajectories which keep a safety distance if possible to obstacles so you still find trajectories which are close to the optimal trajectory so they're just slightly longer if at all but your motion execution is much more reliable because the system is much less likely to bump into an obstacle take into account that you could be delocalized or the system doesn't perfectly execute what you want to execute if you would take uncertain action execution into account properly you would need to go to a mark of decision process which is something that we also discussed we'll discuss later on which is a technique in order to plan under uncertainty but kind of this convolving grid map is kind of a quick fix to get a similar behavior with much less computational effort so it's always valuable to actually take that into account the second thing i want to talk about here is a short remark on that heuristic so we have seen that heuristics should always underestimate the true cost and so the question is can we actually compute the true cost yes we can compute the true cost for a given configuration and that's something that dijkstra's algorithm is doing for us if you remember dijkstra's algorithm computes the uh cost for every node two for each to reach one goal node so we start the x's algorithm from the goal node and expand the whole state space if we can do this we can actually get a cost for every cell that is the optimal heuristic the problem is performing this operation is more expensive than a-star itself or the full or even the uninformed search itself so if you plan only once a star is still the way to go use your euclidean heuristic everything is good if you however plan more often a path to the same goal location then it may be worse to invest a little bit efforts into coming up with a good heuristic and if this is the case so if you say you're re-planning every second still reaching the same goal location you're just re-planing or to avoid if the robot is slightly slightly moved somewhere else you're still on a trajectory or you may change your map a little bit by adding obstacles based on what you see let's say people walking by but the underlying planning problem is still very very similar then it makes sense to run a dijkstra path first in order to compute the optimal heuristic and then plan with that optimal heuristic and that's something which is very efficient and typically done and which is something we will also exploit in the remaining part of that course so with this you get the optimal heuristic for a star so this is an example if this is your goal location you start your dijkstra's algorithm from that location so it expands kind of backwards and increase the cost will increase the further you go away the darker the color until you will be reaching here cos states of high cost and if the environment doesn't change the only thing that you then need to do is basically gradient descent in this map and basically a star will then basically reduce to a gradient descent if you have the optimal heuristic um being taken into account because you always expend the right notes um it's not it's not fully so it's not fully true what i just said um so it's not fully a gradient descent that a star will actually generate but it's still extremely fast if you know that you have the optimal heuristic then you could actually do a gradient descent assuming that the environment did never change if you have dynamics around the people walking by you may need to change your cost map again and let's say if a person buy some of the states will get a higher cost but that's not problematic from the heuristic point of view because heuristics should underestimate so you could stick with this extra heuristic run a star and a star would assume there is no obstacle as soon as it finds an obstacle it will plan a path around it put a little bit more computational effort into this but then continue with the optimal heuristic and so that's the way you go for if you take into account that um you need to replan more of in the remaining part of the lecture i would like to look into high dimensional state spaces so what can we do in order to take into uh i think higher state spaces into account or high dimensional spaces for example a velocities matter and i want to take into account that i cannot artificially increase the acceleration or the the speed of my robot by putting very high acceleration in that this is a smooth process that i need to do what we can do is we can still use acer planning let's say a five-dimensional state-space planning taking velocities and changing velocities into account however would that work so what we can do is we just say we don't only plan in the x and y-space as we did so far if you're planning the grid map we actually plan an x y orientation theta translation velocity and rotation velocity so a five dimensional state space and plan in this five dimensional state space so that means the velocities that we are setting basically in the in the current configuration tell us what our neighboring configuration would actually look like so by taking this information into account we are getting the neighboring states automatically in here and so now the only problem is we probably can't discretize this state space fully because moving from a two dimensional state space to a five-dimensional state space considering that the number of states um would would grow exponentially with the number of dimensions that i had this will lead to abstentions in a substantially larger planning problem something that is larger that what i can typically handle very well and that's a problem that i have so this state space gates actually huge if i would however be able to plan in this configuration would be great because i would have a planning problem which generates trajectories where the the faster trajectories better take into account um because i'm explicitly taking for example the rotations properly into account which is not necessarily the case if i've just planned on a 2d grid map and i can take velocity constraints into account or change some velocities so i can say for example in certain i don't want to change the velocity dramatically i just have a certain acceleration that bound the change in velocities which generates smooth starting and stopping behaviors for the robot or which can say in certain areas i want to drive more slowly and others want to drive faster i can just take that in the planning process already into account or to come up with the optimal trajectory that i'm going to estimate okay so how does it look like that state space is now this five-dimensional state space and in order to go from state number one into state number two and the velocities actually matter so to go from here to here so from one 5d stage another 5d state i may be able to do this if they are nearby and the velocities and the orientation are compatible so computing those neighbors gets more complicated because i need to take the physical constraints of the platform into account but i can for example say i have constraints in the acceleration in the rotate in the rotational part and the translational part how fast i can want to stop or slow down the robot this can be perfectly taken into account and so based on the velocities constraints that i said i actually get out the new configuration of my state the problem that i have is that i typically need to execute that planning system frequently so if i don't have an additional underlying obstacle avoidance system running i need to execute that at high frequencies 10 hertz or maybe 5 hertz for depending how fast my robot actually goes so if you do indoor navigation maybe 5 hertz are fine if forward goes a little bit faster than walking speed you need to increase the 10 hertz if you're driving outdoors on a road you really need to bump it up to whatever 30 hertz 40 hertz maybe so that's the problem you can't run that planner that quickly in this five-dimensional space state space because the number of states that you need to explore is simply too large you need to restrict the search in the state space down so what you can do is you can restrict your your state space in order to just explore a part of the state space and take those states into account which are most likely to be good states and for this we can actually come up with a hierarchical way of planning so the algorithm works the following we start with our occupancy grid map and we use sensor information in order to update our grid map that means if we see an obstacle we just add it to our grid map so we have a grid map with my standard grid map which contains only the static part of the world the one which is given to me and basically have a second map where i'm just adding what i'm seeing and there is a map i use for planning is basically the maximum of those two maps and of course i do the trick with the convolution itself and then i plan an a-star in the standard two-dimensional grid world so i just do a 2d planning using this optimal heuristic and all the tricks that i just explained before so i come up with a trajectory in the 2d world and then i say and then i make the assumption basically the path that i plan in the 2d world and the path that i plan in the 5d world shouldn't be substantially different from the xy position point of view so i might deviate a bit from the path taking the accelerations into count or hard drive on a curve but i shouldn't change the trajectory dramatically and therefore what we can do is we can build up the five-dimensional configuration space only around the trajectory that i plan in this 2d world around this xy space so i kind of restrict the states that i'm actually expanding towards a 5d configuration space so i'm basically building a small channel in this five-dimensional configuration space and only built that five-dimensional configuration space explicitly in this small channel around the past complete in the 2d world and then do the optimal planning in that channel in my in my 5d space but don't need to expand the full search base for this 5d space and that's something that you can do so let's go through these individual steps so i start with using the occupancy grid map this one over here my static world i convolve it so you can see it on this black values here around obstacles this is a result of the convolution and if i detect um i see some obstacles for example from my laser scanner in the environment i can add that to the map and i can add it and what i do in practice i basically have two maps one map which never changes and then a dynamic map which just contains those dynamic obstacles so thing that i right now see and i get that for my send on a high frequency let's say 10 hertz and can with 10 hertz update that map and take the dynamics into the account so that's kind of the first step and then i perform my planning in my um two-dimensional state space so i take that map and plan a path in this two-dimensional map so for example if my goal is over here my current location is over here that's a valid 2d path which guides the robot from the current location to the goal location and here basically these keeping this distance to obstacle is a result of the smoothing and i can do this with using this as optimal heuristic in order to be as fast as possible in planning and don't expend too many states i actually don't want to visit and for that these dijkstra based heuristic is valuable because i'm executing that now whatever five times per second so i'm doing this really really often and i want to do as many pre-computations as possible for my heuristic to be as close as possible to the optimal heuristic in order to be able to run this aced algorithm fast that means i first need to do this computation so once i invest maybe half a second of computational resources in order to come up with the optimal heuristic but then this thing gets 10 20 30 times faster than the original planning algorithm it gets blazingly fast if you have the optimal heuristic okay and then i'm kind of building up my restricted 5d space again i'm making the assumption here that i'm building this 5d space only around the 2d path and this makes an assumption that the shortest path and the fastest path are not too different so if i have one path which is short but makes a lot of stops and turns and one which is slightly longer but a very nice straight line or curved line then this may not lead to a very good solution because the 2d path will actually take this small detoured path where the robot actually will drive very slowly um compared to the slightly longer trajectory and so if those trajectories are far apart in the xy space then you won't find them and take them into account but this happens very very rarely in reality so what we're doing here we're taking this the xy path that i computed and built this 5d space around this 2d path and basically build up a small channel so how did that look like i have again the example i had before so this was my my path my 2d path to the goal and i'm building up that channel and don't even build up that channel fully to the goal only up to a stop goal let's say 5 meters or 10 meter ahead of me i said i want to reach that point 10 meters ahead of me and now plan how i can optimally reach that point knowing that i will re-plan anyhow five times per second and within 200 milliseconds i won't drive this 10 meters to the sub-goal so i have this blue area which which is shown over here and just for this blue area i'm actually building up this five-dimensional state space which i know can't visualize further so you have to stick with this 2d visualization and knowing that for all those blue states we are actually building up this five-dimensional configuration space and then plan a star in this five-dimensional configurations to configuration space so i'm doing the full planning um and what i'm also taking into account in the heuristic basically taking velocities into account how far it is for the distance to the goal but also the cost from dijkstra's algorithm is taking into account in order to guide the surge as fast as possible to that state space or in this case to my sub goal so something that you do in order to be efficient here just the so this is the dijkstra or value iteration is only done in 2d in this 2d space not in the 5d space that's important to note otherwise would be too expensive but then guide the 5d search using at least for the xy states the optimal heuristic so this is an example how that looks like this is a small video animation so um this was just one one screenshot and this shows basically the whole animation how the system starts over here navigates around over here you see the 2d path and then you will also see how the 5d state is expanded you see those dotted white line this is actually the trajectory that the robot actually computes in every time step and this is the actual trajectory that the system execute then so you can see the curve is not it doesn't has this 45 degree rotations it really generates a more smoother trajectory towards going to the target location so that the system then reaches that target location on a trajectory which can actually drive fast taking account the kinematic constraints and we can also not just do that here in simulation we can also do that live and change the environment while the system is navigating in order to come up with an efficient path of the environment and something that you've shown here you see this robot driving through the environment these are unknown obstacles this is a free space also over here and then we can actually walk into and kind of obstruct the robot so the person is standing here the robot sees it and then navigates around you can see how this channel is built up for the robot in order to navigate through the environment and then even path through rather narrow doorways and avoid collisions taking always the optimal controls into account so this was kind of the robot view and this here was the internal state of the planner so i can actually rerun that video again so you can see how the trajectory is actually built how new obstacles pop up here so the garbage bin which is over here will pop up in a second over there and so the planner plans around here until a person occupies that space so the planner plans around and then goes navigates through the doorway um and then in this case is able to take the kinematic constraints into account and drive very smoothly through the obstacles you can also check how much um does the size of the channel actually impacts your solution so the smaller that channel is this kind of blue area the further away from the optimal solution so you can see it here this was the width of 70 centimeters in the length of a meter 50 so the sub coil you're taking into account so something which is very small um then your solution is maybe 10 percent worth in the optimal solution optimal solution means here planning in the full 5d space not something you can only do offline but if you make the channel a little bit bigger like say whatever the 2 meter 50 long and 90 centimeters wide or even more than a meter wide and 5 meters long then there's actually in most indoor environments no difference to the optimal solution so it's a very effective means to build up this restricted configuration space now to come up with trajectories which are optimal or near optimal um although you only have expanded a small fraction of the state space so that you can run that even on by now whatever 18 year old hardware that was a time when this algorithm was developed you can actually run that at several times per second even on very old hardware something that modern robotics systems can actually run easily online so with this i'm coming to the end of today so we have introduced planning as a way for finding optimal or near optimal trajectories from start location to a target location and for that we looked into search techniques uninformed search and informed search techniques where in practice we always use informed search techniques in robotics or most of the time so a-star is the gold standard for performing path planning i may do some tweaks to my maps i may do some pre-computations with the heuristic in order to get that fast and you know to allow for effective re-planning but it's the gold standard algorithm in order to plan a path for robot to go from a to b it is an algorithm this is around since the whatever 1969 approximately when niels nielsen developed a star for shaky the robot so it's one of the standard planning algorithm used in robotics and now but today a key tool in mai in order to perform informed search and there are optimizations for that four different types of problems finding heuristics or learning even heuristics in order to not make it a user-defined task but that's all much more in the field of ai rather than robotics because we're often living in a euclidean world where we can quite easily define good heuristics and therefore this is a standard choice that you want to take into account you can do this in 2d on a grid map but you can also expand this with some approximations to higher dimensional spaces in order to take for example kinematic constraints into account okay that's it for my side i hope you've understood how you plan at least if your system has no serious uncertainty in the motion execution and if this is the case you need different techniques but if you can assume your system behaves reasonable then what we presented here is the way to go so thank you very much for your attention

Channel: Cyrill Stachniss

Views: 6,867

Keywords: robotics, photogrammetry, motion planning, lecture, search, planning

Id: HR1TNa8Lp7w

Length: 98min 20sec (5900 seconds)

Published: Fri Oct 09 2020