Algorithms for building a path for an unmanned vehicle. Yandex lecture

    Yandex has been developing an unmanned vehicle for some time . Here is one of the first technical lectures on this topic. Yandex employees work in unmanned vehicles in various cities, including Minsk. The author of the lecture, Roman Udovichenko, is just from Minsk - he leads the road situation handling group. At the September J. Subbotnik, Roman spoke about one of the great tasks facing his group.


    We just take the current position of the car, look at the path that we would like to go, and smoothly turn onto this path, taxi out onto it. It turns out quite simple. But moving in the city is due to the fact that you need to follow the rules of the road.

    - My name is Roma Udovichenko, I work in Yandex in Minsk as the head of the traffic control group in the direction of unmanned vehicles. Today I’ll tell you about a very small but important part of our work - the path construction algorithms for an unmanned vehicle.

    First, I want to understand what needs to be done to teach the car to drive on its own - provided that we hung it with sensors, hung cameras, radars and anything on it. We can do everything, it remains to write a code of assumptions.



    We can use the classic approach from robotics. We divide our task of independent movement into four modules. The localization module is responsible for making the machine understand where it is. Recognition module - for what is around the car. The planning module has information about what is around, and knowing where you want to come, it builds a route. The control module says how to go along the route, to arrive, to follow this trajectory.

    And we can apply the technology of neural networks now fashionable and say that we will not program anything explicitly. Let's just submit the picture from the cameras to the neural network, having previously trained it on some human movements. We will teach you in what situations where you need to turn the steering wheel, brake, accelerate, take a lot of data, make a big neural network, and as planned, it should behave well after that.

    A lot of work is being done in this direction, but in practice it seems that you still need too much data, you need too big a neural network to successfully repeat everything in different situations. Therefore, today we will talk about the classical approach, in particular - about planning a path, building a trajectory along which we will go.

    Why is path planning a difficult task that cannot be done in a week and to go on doing something interesting next? The car generally has a number of fairly significant limitations. This is not a ball in space, which can roll in any direction and conditionally instantly stop and slow down. The car has the current direction, the angle of rotation of the wheels, and it cannot just be two meters to the left of the current location, it is very difficult. He can go approximately forward, turning at some angle, but nevertheless, movement is very limited. And the trajectory we are building is subject to the constraints that follow from kinematics. For example, we cannot instantly accelerate and even instantly increase our acceleration.



    Today we will consider such varieties of algorithms for constructing a path along which a car can drive well. In particular, more or less classical graph algorithms, optimization methods that use continuous mathematical optimizations, stochastic algorithms that consider random behavior, and specialized methods that solve a very particular problem, but solve it very well - better than in the general case.

    The first thing to start with is graph algorithms. The first question we need to answer in order to understand which algorithms can be applied is how to construct a graph in general? Here is a car, we can understand where it stands, but in reality there is no graph, the tops of the ribs on the road are not drawn. We need to somehow come up with this graph ourselves. The first thing we can do: we divide the entire space into cells, consider small cells and say that there is our entire earth's surface, divided into cells 25 by 25 or 50 by 50 cm. Then we connect adjacent cells with edges and look for a path to them. It will be quite far from the path that the car can drive, but it will give some approximation. And we will have such peaks in two-dimensional space.

    We can complicate our space by adding there the current angle of rotation of the machine. We will already have cells not just x and y, but also the current orientation of the machine in the directions north, south, west and east, also somehow discretized. In addition to the direction, we can take into account a lot of things: the current speed of the machine, the current acceleration, the current tangential acceleration, normal. All this is important to consider. But the more we complicate our space, the more complex our graph becomes.



    In addition to dividing space into cells, we can say that we are able to move in small steps in the form of primitives. For example, drive 50 cm forward or drive 50 cm forward and turn left or drive 50 cm forward and turn right. And with such primitives we can bridge our entire space. We do not have explicit cells, but if these small steps are reasonably well coordinated with each other, then we get a regular grid that covers the space well and neatly.

    Suppose we consider primitives that are not so well connected with each other - for example, let's say that a left turn occurs at an angle of 89 degrees, and a turn to the right at an angle of 90 degrees. Then we already have the same number of vertices as in the previous picture, which will occupy a significantly smaller area of ​​space, since they are badly joined together and the density of points will be very high, and we will not be able to cover the space far.

    We can combine these two approaches and say that we consider the primitives of movement at any angle, drive forward, drive forward and turn 10 degrees, 15 degrees, whatever. At the same time, we divide the space into cells anyway and say that in one cell there can be no more than 1, 2, ..., k vertices.

    When we consider the next vertex in the graph in the process of using some algorithm, we say that there is a new vertex in the cell, we process it accordingly. If then it turns out that we want to get to the indicated cell at another vertex, then we will not consider it. This makes it impossible for us to build some more optimal route than if we used all-all the peaks. But this allows you to significantly increase the speed and efficiency.

    We have a graph, we want to apply some kind of algorithm on it. There is an algorithm A *.



    There is Dijkstra's algorithm, which is a little more known than A * algorithm, and which bypasses all the vertices in the graph in increasing distance from the starting vertex. He takes them out of a priority queue in order of increasing distance. In the upper figure, we see that if the starting pink vertex is at the beginning, then we will gradually consider all the vertices at a radius increasing from the starting one, and in the end we will come to the vertex, which is our goal.

    To search more efficiently, we will consider the peaks not only by increasing the distance from the start, but also add to this distance some heuristic estimate of how much we have to go to the end. The logic is very clear: we do not just say that we are now standing at some peak and therefore we are considering the next peak in turn at some distance from the start. We add to this parameter some function that allows us to evaluate more promising peaks, from which, most likely, it’s not far to go to the finish line. We don’t know how long to actually go from each peak to the finish line, because if we knew, we would not have to look for a way. But we can somehow appreciate it and get the picture in the bottom image. We will evaluate the peaks that somehow go towards our goal, and consider a significantly smaller number of vertices than in Dijkstra's algorithm. You can talk a lot about this, but in general the idea is that for the algorithm to work correctly, it is necessary that our heuristic, the function of estimating the distance to the final vertex, never exceed the real distance that we have to travel. Only in this case is the correct operation of the algorithm guaranteed.



    As heuristic functions, different approaches can be used. This is the difficulty in applying the A * algorithm. The better heuristics you can pick up, the better it will go towards the target, the fewer peaks it will consider and the faster it will work.

    The simplest thing we can do is to look at the distance to the target directly. Obviously, we cannot arrive at the final point faster than if we just go straight to it like a tank.

    A more complex and more efficient heuristic is distance, taking into account our kinematic limitations, but without taking into account obstacles. Suppose there is nothing in the world, only we are the target, but the car cannot instantly move left or right, travels according to its own laws, according to the physical model of the machine. Since there are no obstacles, we can pre-calculate the distance to the target from various places in which we can be, and use this as a heuristic function.

    We can do the opposite: say that there is an obstacle, but, let's say, the car can move forward, backward, left, right, as you like, quickly accelerate and brake quickly. We will build our distance taking into account the obstacles that we see. Again we get an estimate that is lower than the actual distance that we have to drive.

    And finally, of all the heuristics, we can consider the maximum. Since each of them does not exceed our actual distance, the maximum of them will also not exceed the actual distance and the algorithm will work well.



    What do we get? Algorithm A * allows you to find a known optimal path that leads us to the goal, going around obstacles. But if we have a space of small dimension, we take into account only x, y, the orientation of the machine and the fact that it cannot be accelerated instantly, which means that all these restrictions will be taken into account. If we add a large number of parameters to our graph, then it will be in a space of very high dimension. We will have a 7-, 8-, 10-dimensional space, we will have time to consider small pieces of this space and we will not be able to build a sufficiently distant route due to the very high variability of the parameters. In some situations, A * is difficult to use, but somewhere it shows itself quite well.

    The second option is optimization methods based not on a discrete, but on a more continuous approach.




    The problem statement can be this: consider the trajectory of our position in time, x and y, depending on time t, that is, we will understand at what point we want to be at time t. We can determine the angle of the tangent through the arctangent of the derivatives, we can say that in this case the optimal trajectory will be, which minimizes the functional, which is the integral over time forward of some function of the trajectory. The function of the trajectory here somehow fines us for sharp turns, sharp accelerations, being close to obstacles, etc. Then, if we sum along all our fines along our trajectory, and try to minimize it by standard mathematical apparatus that is not connected in any way with cars in general and unmanned vehicles in particular, we will solve the problem in some general form.



    What can we consider examples of fines? For example, we can say that we do not want to drive close to obstacles, take this into account with some weight, we do not want our speed to be much higher or lower than the desired speed, which we determined for ourselves. We can penalize ourselves for the second derivative, which is acceleration, because we do not want the car to sink sharply to the floor and brake sharply.

    We can consider the third derivative, which is a jerk, that is, we do not want the acceleration to change sharply enough either. By the way, it is from a sharp change in the acceleration of people that cradles during the ride. If the acceleration is fixed and the car just accelerates all the time, then, as studies show, people do not get sick. The human vestibular apparatus captures and does not always respond well, if it accelerates, it slows down. We may not want to make sharp turns, we limit our angle. And there are additional restrictions that say that a car physically cannot accelerate faster than some kind of acceleration. If we all take this into account in some way, then we can solve the problem using the abstract function minimization algorithm and get some result.

    I would also like to talk about distance calculations, because in most optimization methods they like to work with smooth functions that are well differentiable, smooth. Then everything works well there. And our car is an object of rather complex shape, and the obstacles that we go around are also objects of cunning forms. Therefore, some simplification is necessary here. For example, we can say that the car is not something complicated, but just five circles that go back and forth.



    It will be like the truth, although not completely, but the approximation is good. And from circles it is very easy to count distances to anything and it is very easy to check a circle for intersections with other geometric primitives. If the distance to the center is less than the radius, then here is the intersection, otherwise everything is fine.

    What is needed to smoothly change the distance? Our Euclidean distance to non-convex polygons does not possess the properties we need and is poorly differentiable in places where this non-convex occurs. Therefore, we can construct such a pseudo-distance along a gradient field to our polyline, it is marked in red here and represents an obstacle. We can quite easily introduce the distance field from each point to this polyline, which is directed towards the polyline and has the necessary differentiability properties - albeit not being strictly shortest. If we do all this, we can build a smooth, beautiful and neat trajectory.



    Among the advantages of such methods, we attribute the fact that a good trajectory and control space are obtained continuously. We can go along it, all restrictions are to one degree or another respected. But unfortunately, most of the optimization methods, or even almost all, one way or another suffer from local minima, where their attempts to optimize something get stuck and do not find a good enough solution. It is very difficult to formulate everything in the form of mathematical differentiable functions; this also does not always work.

    However, there is a solution. We can apply algorithms that work in some random way, but allow us to build some kind of approximate route quickly and conveniently. For example, we will construct our graph, which is a tree, iteratively. At first, we won’t divide anything into cells, we won’t build any primitives. We just take the simulator of our car, which in general is able to simulate the movement, as close as possible to how the car goes for real. And take the starting point. After that, iteratively select a random point in space. And - we find the node closest to it in the current constructed tree. We construct an edge towards this point using the simulation of driving in this direction.

    It turns out that each time we go from our constructed tree to a random side and we can make vertices in this tree in space of any dimension, that is, at each vertex we take into account the current direction of the machine, current speed, acceleration, all angles that are important to us. And then, when we take a new point, we will also take the peak closest to this point. We’ll drive through some kind of simulation, for example, 5 meters towards this point. Then we take another point and pass in its direction.

    What does this give us? Each time we explore space, but very aggressively. We are not looking for optimal ways to go around an obstacle. We just travel in different directions, but each time we do it from the most explored section of our space to that unknown side.




    Due to this, we quickly get a picture where "tentacles" grow in different directions, which somehow cover our space. Perhaps not optimal, but pretty fast. A small square in the corner is our starting position. Then we can get some way to the goal, which, perhaps, does not look quite optimally and elegantly, but is obtained in a rather quick way.



    And it turns out, the advantages are that we work very quickly in multidimensional spaces. Since we get the paths using honest simulation, we can almost directly transfer them to the control unit. Disadvantages? We have no guarantee that our points will be selected well enough. The solutions can be quite crooked and not always optimal.

    Specialized methods. When the car drives in the city, we do not have abstract points A and B and an unstructured environment with random obstacles. In our city, everything is more or less clear: there are specific lanes and the movement of our car almost always lies in the fact that we drive approximately in the center of the lane, sometimes we move left or right to go around an obstacle, sometimes we change lanes to turn according to the rules of the road to where you need it. At the intersections from one lane to another, we are rebuilding.




    We do not always need these cunning trees to park or do complex maneuvers. They are needed, but when we are driving in a lane, it’s enough for us to build a more or less smooth path that goes to the center of this lane or with some kind of left-right shift. This is much simpler than looking for an abstract path in a graph. We just take the current position of the car, look at the path that we would like to go, and smoothly turn onto this path, taxi out onto it. It turns out quite simple. But moving in the city is due to the fact that you need to follow the rules of the road.



    This is not entirely connected with the rules for constructing the path, but it is not far from them. When we approach the intersection, we need to stop, let the cars moving there. We cannot drive randomly, we can ride one or the other, change lanes, have to stop at a red traffic light, let pedestrians pass, etc. And we need to follow traffic rules, information about which the car can receive either in advance from some very detailed maps, or recognize on the go by marking, signs, etc.

    And in order to recognize all this on the go, there is some problem. An unmanned vehicle is fairly easy to trap.



    From it, following the rules, he will not be able to get out. It is a joke that in life there are many situations in which a person understands what to do. Here is a continuous strip, and ahead is road repair. The workers did not put all the necessary signs about the detour. They just set up their cement truck, breaking the asphalt. A person will understand that nothing terrible will happen from crossing a solid line - they will probably not even be fined. But with unmanned vehicles, everything is more complicated.


    In general, in the process of solving there are a lot of problems of building a path - in all related areas that were first. If you want to help us deal with these difficulties and implement everything in algorithms, be sure to come to us to do good. We will be glad to see you in the Minsk development group. The construction of various paths, taking into account the rules of the road, is just one of our topics. Many thanks.

    Also popular now: