CN114371709A - Path planning method, device, storage medium and program product - Google Patents

Abstract

Embodiments of the present invention relate to the technical field of intelligent driving, and provide a path planning method, device, storage medium and program product. The method includes acquiring a driving situation awareness map, wherein the driving situation awareness map is divided into a plurality of grids, and includes the information of obstacles and a priori planned trajectory, according to the information of the obstacles and the a priori planned trajectory, the cost value is assigned to each grid in the driving situation awareness map, according to the driving situation The cost value of each grid in the cognitive graph is used to obtain the target path of the vehicle through heuristic search. In the embodiment of the present invention, by assigning a cost value to the driving situation cognition map, and performing heuristic search based on the cost value, the operation can be greatly simplified, the calculation efficiency can be improved, and the real-time performance can be improved to meet the needs of unmanned vehicles.

Description

Path planning method, device, storage medium and program product

technical field

The embodiments of the present invention relate to the technical field of intelligent driving, and in particular, to a path planning method, device, storage medium and program product.

Background technique

Robot path planning is based on the distribution of surrounding dynamic and static obstacles, taking into account the overall dimensions and dynamic characteristics of the robot, to find a collision-free motion trajectory from the starting position to the target position. In robotics, path planning problems are often transformed into path search problems in configuration spaces.

In the prior art, a collision-free path search is performed by directly calculating the obstacle area and the free area in the configuration space.

However, in the process of realizing the present invention, the inventor found that there are at least the following problems in the prior art: the above-mentioned direct calculation method has high computational complexity, and the unmanned vehicle is a special wheeled mobile robot, which Only the front wheels can be actively steered, which needs to meet the vehicle dynamics constraints, and there are many dynamic obstacles in the unmanned environment. These factors increase the computational complexity and affect the computational efficiency, so that the real-time requirements cannot be met. .

SUMMARY OF THE INVENTION

The embodiments of the present invention provide a path planning method, device, storage medium and program product, so as to reduce the computational complexity of the unmanned vehicle, improve the computational efficiency, and thereby improve the real-time performance.

In a first aspect, an embodiment of the present invention provides a path planning method, including:

Obtain a driving situation awareness map, wherein the driving situation awareness map is a spatial layout map of the environment where the vehicle is currently located, including information on obstacles and a priori planned trajectory, and the driving situation awareness map is divided into multiple grid;

According to the information of the obstacle and the prior planning trajectory, assign a cost value to each grid in the driving situation awareness map;

According to the cost value of each grid in the driving situation awareness map, the target path of the vehicle is obtained through heuristic search.

In a second aspect, an embodiment of the present invention provides a path planning device, including:

an acquisition module for acquiring a driving situation awareness map; the driving situation awareness map is divided into a plurality of grids and includes obstacle information and a priori planning trajectory;

an assignment module, configured to assign an assignment value to each grid in the driving situation awareness map according to the information of the obstacle and the prior planning trajectory;

The search module is used for obtaining the target path of the vehicle through heuristic search according to the cost value of each grid in the driving situation awareness map.

In a third aspect, an embodiment of the present invention provides a path planning device, including: at least one processor and a memory;

the memory stores computer-executable instructions;

The at least one processor executes computer-implemented instructions stored in the memory to cause the at least one processor to perform the methods described in the first aspect and various possible designs of the first aspect above.

In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium, where computer-executable instructions are stored in the computer-readable storage medium, and when a processor executes the computer-executable instructions, the first aspect and the first Aspects various possible designs of the described method.

In a fifth aspect, embodiments of the present invention provide a computer program product, including a computer program, which, when executed by a processor, implements the method described in the first aspect and various possible designs of the first aspect.

The path planning method, device, storage medium, and program product provided in this embodiment include acquiring a driving situation awareness map, where the driving situation awareness map is divided into multiple grids and includes information about obstacles and prior information. According to the information of the obstacle and the prior planning trajectory, a cost value is assigned to each grid in the driving situation awareness map, according to the driving situation awareness map of each grid. The cost value, the target path of the vehicle is obtained through heuristic search. By assigning the cost value to the driving situation cognitive map, and performing heuristic search based on the cost value, the operation can be greatly simplified, the calculation efficiency can be improved, and the real-time performance can be improved to meet the needs of unmanned vehicles.

Description of drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description These are some embodiments of the present invention, and for those of ordinary skill in the art, other drawings can also be obtained from these drawings without any creative effort.

FIG. 1 is a schematic diagram of a route planning principle based on a driving cognitive situation diagram provided by an embodiment of the present invention;

FIG. 2 is a schematic flowchart 1 of a path planning method provided by an embodiment of the present invention;

3 is a second schematic flowchart of a path planning method provided by an embodiment of the present invention;

FIG. 4 is a schematic diagram of the principle of assigning a cost value to a driving cognitive situation map according to an embodiment of the present invention;

5 is a comparison diagram of a process of assigning a cost value to a driving cognitive situation map according to an embodiment of the present invention;

6 is a third schematic flowchart of a path planning method provided by an embodiment of the present invention;

7 is a schematic diagram of a driving cognitive situation diagram including a target path according to an embodiment of the present invention;

FIG. 8 is a fourth schematic flowchart of a path planning method provided by an embodiment of the present invention;

9 is a comparison diagram before and after target path optimization provided by an embodiment of the present invention;

10 is a schematic flowchart of collision optimization in a path planning method provided by an embodiment of the present invention;

11 is a schematic structural diagram of a path planning device according to an embodiment of the present invention;

FIG. 12 is a block diagram of a terminal device according to an embodiment of the present invention.

Detailed ways

In order to make the purposes, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments These are some embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

与自由区域

来进行。该直接计算的方式，具有很高的计算复杂度。Robot path planning is based on the distribution of surrounding dynamic and static obstacles, taking into account the overall dimensions and dynamic characteristics of the robot, to find a collision-free motion trajectory from the starting position to the target position. In robotics, path planning problems are often transformed into path search problems in configuration spaces. In the indoor artificial environment, the research of robot path planning has been quite mature. However, route planning in a wide-area, complex, and time-varying real traffic environment is still an open problem. In the prior art, in the path planning based on the configuration space, the obstacle area in the configuration space can usually be directly calculated

with free area

to proceed. This direct calculation method has high computational complexity.

The path planning of unmanned vehicles needs to deal with large-scale and dynamically changing environmental maps. There are many types and numbers of obstacles, and the motion trends are also very complex. The motion of unmanned vehicles must also meet the constraints of vehicle dynamics. Autonomous vehicle path planning also needs to consider driving behavior and driving intent. Therefore, compared with the general robot path planning problem, unmanned vehicles need to consider a large number of environmental elements and have a high degree of uncertainty. The increased computational complexity affects the computational efficiency, so that the real-time requirements cannot be met.

In view of the above technical problems, the inventor found through research that the driving cognitive situation map can be rasterized, and each grid in the driving cognitive situation map can be processed according to the information of the prior planned trajectory and obstacles in the driving cognitive situation map. Reasonable cost value assignment, heuristic search based on the assigned driving cognitive situation map to quickly find the target path, can greatly reduce the amount of computation. Based on this, the embodiments of the present invention provide a path planning method, which can reduce the computational complexity, thereby achieving higher real-time performance and meeting the driving requirements of unmanned vehicles.

Exemplarily, as shown in FIG. 1 , the driving cognitive situation graph is divided into multiple grids, and the driving cognitive situation graph includes the position information of obstacle A, obstacle B, obstacle C and a priori planned trajectory. Each obstacle occupies a different grid. When assigning the cost value to the driving cognitive map, the distance from the vehicle to the prior planning trajectory and the distance from the vehicle to each obstacle can be comprehensively considered to assign a value to the drivable area without collision with the obstacle, and then according to the assigned driving value Cognitive situation graphs perform heuristic search of target paths, which can greatly reduce the amount of computation and computational complexity, so as to achieve higher real-time performance and meet the driving needs of unmanned vehicles.

The technical solutions of the present invention will be described in detail below with specific examples. The following specific embodiments may be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments.

FIG. 2 is a schematic flowchart 1 of a path planning method provided by an embodiment of the present invention. As shown in Figure 2, the method includes:

201. Obtain a driving situation awareness map, wherein the driving situation awareness map is a spatial layout diagram of the environment where the vehicle is currently located, including information on obstacles and a priori planned trajectory, and the driving situation awareness map is divided into for multiple grids.

The execution body of this embodiment may be a processing device such as a computer that can perform path planning. The processing device can be provided on the unmanned vehicle, so that the unmanned vehicle travels according to the planned path.

In this embodiment, the driving situation awareness map is a spatial layout map of the environment in which the vehicle is currently located, which can indicate the spatial orientation and traffic state that the vehicle can reach. The driving situation awareness map can include: obstacle height and confidence situation map (CT1), obstacle speed and direction situation map (CT2), drivable area edge situation map (CT3), prior planning trajectory situation map (CT4) ). It can be understood that there are static obstacles and dynamic obstacles in the driving environment of unmanned vehicles, and information such as the plane size and height of obstacles can be obtained from the driving situation awareness map. In this embodiment, the a priori planned trajectory is the historical travel trajectory of the unmanned vehicle.

In this embodiment, the driving situation awareness map is divided into multiple grids, and the size of each grid can be set according to actual needs, for example, it can be set to 1 meter×1 meter.

202. According to the information of the obstacle and the a priori planned trajectory, assign a cost value to each grid in the driving situation awareness map.

Considering that not colliding with obstacles and driving along a priori planned trajectory are the two main influencing factors for the driving process of the unmanned vehicle. Therefore, each grid in the driving situation awareness map can be assigned a cost value based on these two factors.

In practical applications, according to different driving purposes, driving behaviors can be divided into various types, such as driving along a priori planned trajectory, avoiding obstacles, reversing and getting out of trouble, and getting out of trouble in the forward direction. The map adopts different assignment strategies. The basic strategy is to comprehensively consider the surrounding traffic environment information in the driving cognitive CT map, and assign a cost value to each grid in the driving situation cognitive map. The higher the cost value, the higher the cost of choosing the grid, which is farther from the prior planning trajectory and closer to the obstacle.

203. According to the cost value of each grid in the driving situation awareness map, obtain the target path of the vehicle through heuristic search.

After the cost value is assigned to each grid in the driving situation awareness map, a heuristic search can be performed based on the cost value of each grid to obtain the target path of the vehicle.

The path planning method provided by this embodiment includes acquiring a driving situation awareness map, where the driving situation awareness map is divided into multiple grids and includes information on obstacles and a priori planned trajectory. information and the prior planning trajectory, assign a cost value to each grid in the driving situation awareness map, and obtain the vehicle's value through heuristic search according to the cost value of each grid in the driving situation awareness map. Target path. By assigning the cost value to the driving situation cognitive map, and performing heuristic search based on the cost value, the operation can be greatly simplified, the calculation efficiency can be improved, and the real-time performance can be improved to meet the needs of unmanned vehicles.

FIG. 3 is a second schematic flowchart of a path planning method provided by an embodiment of the present invention. As shown in FIG. 3 , on the basis of the above-mentioned embodiment, for example, on the basis of the embodiment shown in FIG. 2 , the present embodiment describes in detail the way of assigning the value of the value, and the method includes:

301. Obtain a driving situation awareness map, wherein the driving situation awareness map is a spatial layout diagram of the environment where the vehicle is currently located, including information on obstacles and a priori planned trajectory, and the driving situation awareness map is divided into for multiple grids.

In this embodiment, step 301 is similar to step 201, and details are not repeated here.

302. Determine a first grid set in which the vehicle cannot travel according to the information of the obstacle, and assign a maximum cost value to each grid in the first grid set.

In some embodiments, the driving situation awareness map includes an obstacle height and confidence level situation map and a drivable area edge situation map; the first grid set for which the vehicle cannot be driven is determined according to the information of the obstacles , including: determining each first target grid occupied by static obstacles in the situation map of the obstacle height and confidence level and each second target grid outside the edge of the drivable area in the drivable area edge situation map; A grid set formed by the first target grid and the second target grid is determined as the first grid set.

In this embodiment, the grid that must collide with the obstacle, that is, the non-driving grid, is assigned the cost value as the maximum value ρ _max =1

As shown in Figure 4, the grid filled with dots represents the non-drivable grid, wherein the non-drivable grid can include all grids outside the edge of the drivable area in CT3, namely the second target grid, the static obstacle in CT1 All grids inside, i.e. the first target grid. The vehicle model can be converted to a single-point model, so that the grids that will collide with obstacles can be easily calculated regardless of the position of the vehicle.

In some embodiments, the determining the first grid set that the vehicle cannot drive according to the information of the obstacle further includes: determining the first radius according to the distance from the edge of the vehicle to the center point of the vehicle; Each point on the edge line of the obstacle and each point on the edge line of the drivable area is the center of the circle, and the first radius is used as the radius to determine a plurality of circles; each third target grid occupied by the plurality of circles is added to all the third target grids. Describe the first grid set.

In this embodiment, in order to expand the range of the non-drivable grid, the actual size of the vehicle may be considered, and in order to simplify the calculation, only the shortest distance from the edge of the vehicle to the center point of the vehicle may be considered. In the specific implementation process, the shortest distance from the edge of the vehicle to the center point of the vehicle may be set as μ, for example, μ may be equal to the half vehicle width. Then, the edge of the road and the edge of the static obstacle can be used as the center of the circle, and the radius r=μ can be used as a circle to obtain a plurality of circles. When the center of the vehicle is on the inner grid of the circle, it will collide with obstacles regardless of the vehicle's attitude. Based on this, the cost value of the third target grid occupied by each circle is also assigned ρ _max =1. So far, all grids that have not been assigned have at least one possible attitude, so that the vehicle does not collide with obstacles.

In some embodiments, before determining the plurality of circles, the method further includes taking each point of the edge line of the static obstacle and each point of the edge line of the drivable area as the center of the circle and the first radius as the radius, before determining the plurality of circles. : Determine the safety margin according to the current speed of the vehicle; determine the second radius according to the first radius and the safety margin; use the points on the edge line of the static obstacle and the edge line of the drivable area respectively Each point is the center of the circle, and a plurality of circles are determined with the second radius as the radius.

In this embodiment, considering that in engineering practice, since the vehicle control is not completely accurate, a safety margin can be set based on this to prevent the vehicle from colliding with obstacles due to insufficient control accuracy. The size of the safety margin is related to the control accuracy and can be a function of the vehicle's own speed. The faster the vehicle is, the worse the control accuracy is, and a larger safety margin can be set for higher vehicle speeds. Therefore, the radius r=μ+S(v) after the safety margin can be considered. Using this radius to make circles in the above-mentioned manner, the third target grid occupied by each circle is obtained.

In some embodiments, the driving situation awareness map includes a situation map of obstacle heights and confidence levels, and a situation map of obstacle speed and direction; the first grid is determined according to the information of the obstacles where the vehicle cannot be driven. The grid set includes: determining each fourth target grid occupied by the dynamic obstacle itself according to the position information of the dynamic obstacle in the obstacle height and the confidence level situation map, and adding the fourth target grid to the the first grid set; according to the speed of the obstacle and the speed of the dynamic obstacle in the directional situation diagram, the expansion operation is performed on each grid occupied by the dynamic obstacle itself, and the newly added grids are expanded after expansion. A fifth target grid is added to the first grid set.

In this embodiment, for the dynamic obstacle, in addition to considering the determination of each fourth target grid occupied by the dynamic obstacle itself in CT2 as a non-drivable grid, the dynamic obstacle can also be determined according to the speed of the dynamic obstacle in CT3. The expansion operation is performed in the direction of velocity. The fifth target grid added by expansion is also determined as a non-drivable grid. The faster the speed, the larger the expansion range, that is, the more grids need to be expanded, and the influence of the possible future states of obstacles on the current path planning is also taken into account.

303. According to the first grid set and the prior planning trajectory, assign a cost value to each grid in the second grid set; the second grid set includes the driving situation awareness map. Each grid except the first grid set.

In some embodiments, the performing cost value assignment to each grid in the second grid set according to the first grid set and the prior planning trajectory includes:

For each sixth target grid in the second grid set, according to the difference between the sixth target grid and the grid closest to the sixth target grid in the first grid set The first distance, and the second distance between the sixth target grid and the a priori planned trajectory, determine the cost value of the sixth target grid.

Optionally, the cost value of the sixth target grid is inversely proportional to the first distance and proportional to the second distance.

In some embodiments, according to the first distance between the sixth target grid and the grid closest to the sixth target grid in the first grid set, and the sixth target The second distance between the grid and the prior planning trajectory, to determine the cost value of the sixth target grid, includes:

The cost value of the sixth target grid is determined according to the first distance, the second distance and the attenuation parameter; the attenuation parameter and the cost value are attenuated as the first distance increases Speed is inversely proportional.

In some embodiments, according to the first distance between the sixth target grid and the grid closest to the sixth target grid in the first grid set, and the sixth target The second distance between the grid and the prior planning trajectory, to determine the cost value of the sixth target grid, includes:

Determine the cost value of the sixth target grid according to the first distance, the second distance and the weight parameter; the weight parameter and the cost value of the sixth target grid from the first distance The influence strength is proportional to and inversely proportional to the influence strength of the second distance on the cost value of the sixth target grid.

Exemplarily, the following comprehensively considers the obstacle position information and the prior planning trajectory, and calculates the cost value of the remaining grids in the driving situation awareness map except the first grid set, that is, each sixth grid in the second grid set is calculated. The cost value of the target raster. When calculating the cost value of the sixth target grid, the angle when the vehicle travels to the sixth target grid may not be considered, but only the relative positions between the sixth target grid and the obstacle and the prior planned trajectory are considered. Based on this, the following potential function can be designed to calculate the cost value:

Among them, d _obs (i, j) is the Euclidean distance from the current grid to the nearest non-drivable grid; d _tra (i, j) is the Euclidean distance from the current grid to the prior planned trajectory. The first multiplier describes the decay of the potential function with the increase of d _obs (i, j). The decay parameter α is a constant, which describes the decay speed of the potential function with a _obs (l, j). The larger the α, the smoother the curve. Explain that the potential function decays slowly with d _obs (i, j) and vice versa. The second multiplier describes that the potential function increases as d _tra (i, j) increases, and the closer to the prior planned trajectory, the smaller the value of this term, and vice versa. The weight parameter P is a constant, which describes the weight relationship between d _tra (i, j) and d _obs (i, j) in the calculation of the potential function. The smaller the β, the higher the weight of the prior planning trajectory in the potential function calculation, the smaller the setting β, the more likely to drive along the prior planning trajectory; the larger the β, the lower the weight of the prior planning trajectory in the potential function calculation, The larger the setting β, the more likely it is to drive away from obstacles, but is insensitive to the deviation from the prior planned trajectory distance. The value range of p(i, j) is (0, 1], which increases with the increase of d _obs (i, j) and decreases with the increase of d _tra (i, j). When d _obs (i, j) →0, ρ(i,j)→1, the potential function is continuous at the boundary between the drivable grid and the non-drivable grid. For a fixed d _obs (i, j), p(i, j) varies with d The decrease of _tra (i, j) decreases, indicating that the closer to the grid of the prior planning trajectory, the smaller the potential function, and the smaller the cost of selecting the grid.

The process of calculating the cost value can refer to Figure 5. As shown in Figure 5, the left figure shows the distribution of obstacles in the driving cognitive situation map, the middle figure shows the pseudo-configuration space after assigning the non-drivable grid, and the right figure shows the potential of the drivable grid. The driving cognitive situation diagram after the function calculates the cost value. The darker grid is assigned a greater cost value, and the lighter color grid is assigned a smaller cost value, and the figure shows that the area farther away from the obstacle is assigned a smaller cost value.

In this embodiment, the potential function comprehensively considers the distance from the grid to the obstacle and the distance from the a priori planned trajectory, which provides a suitable cost value for the path search in the three-dimensional space {x, y, θ}. The relationship between the function value and d _obs (i, j) and d _tra (i, j) can be changed by adjusting the two parameters α and β of the potential function. Due to different driving behaviors, some focus on driving along the prior planned trajectory, and some focus on avoiding obstacles. Therefore, different driving behaviors need to adjust the parameters α, β, and select potential functions with different emphasis.

304. According to the cost value of each grid in the driving situation awareness map, obtain the target path of the vehicle through heuristic search.

In this embodiment, step 304 is similar to step 203, and details are not repeated here.

The path planning method provided in this embodiment, by assigning the maximum cost value to the non-drivable grid, assigns the cost value to the non-drivable grid according to the location information of the a priori planned trajectory and the obstacle, which can simplify the operation on the premise of Next, assign accurate cost values to each grid. In addition, for non-drivable grids, through cleverly designed potential functions, according to the first distance, second distance, weight parameters, and attenuation parameters, the reasonable value of each grid can be easily obtained. The cost value lays a solid foundation for the fast and accurate search of the subsequent path, and the adjustment of the weight parameters and attenuation parameters can be applied to different driving behaviors and has a wide range of application scenarios.

FIG. 6 is a third schematic flowchart of a path planning method provided by an embodiment of the present invention. As shown in FIG. 6 , on the basis of the above embodiment, for example, on the basis of the embodiment shown in FIG. 2 , the present embodiment describes in detail the evaluation function used in the heuristic search, and the method includes:

601. Obtain a driving situation awareness map, wherein the driving situation awareness map is a spatial layout diagram of the environment where the vehicle is currently located, including information on obstacles and a priori planned trajectory, and the driving situation awareness map is divided into for multiple grids.

602. According to the information of the obstacle and the a priori planned trajectory, assign a cost value to each grid in the driving situation awareness map.

Steps 601 to 602 in this embodiment are similar to steps 201 to 202 in the above-mentioned embodiment, and are not repeated here.

603. According to the cost value of each grid in the driving situation awareness map, based on the first evaluation function and the second evaluation function, perform a heuristic search to obtain the target path of the vehicle; the first evaluation function and The steerable angle range of the vehicle is related, and the second evaluation function is related to the position information of the obstacle.

仅以车辆边缘到车辆中心点的最短距离μ为半径，进行碰撞判断，因此势函数ρ(i，j)＝ρ_max＝1只是碰撞发生的充分而非必要条件。然而，在实际搜索中，对于距离不可行驶栅格较近的栅格，需对其对应的车辆位姿进行碰撞检测。此外，路径还需要满足车辆的动力学要求，基于此，下面介绍如何设计启发式搜索算法，保证搜索得到的路径满足既能满足车辆动力学要求，并且能够加快搜索速度。In this embodiment, the vehicle path search can be performed in the three-dimensional space {x, y, θ}, because for the same grid, although the center of the vehicle is located in the grid, there may be various vehicle orientations. The orientation corresponds to a variety of different vehicle poses. Some vehicle poses in the same grid are favorable for the vehicle to drive towards the target pose, and some vehicle poses tend to make the vehicle deviate from the target pose. For grids that are closer to the obstacle, some poses are collision-free, and some poses collide with the obstacle. Since the vehicle is abstracted into a mass point, the configuration space is not strictly calculated

Only the shortest distance μ from the vehicle edge to the center point of the vehicle is used as the radius to judge the collision, so the potential function ρ(i,j)= _ρmax =1 is only a sufficient but not a necessary condition for the collision to occur. However, in the actual search, for grids that are close to the non-drivable grid, collision detection needs to be performed on the corresponding vehicle poses. In addition, the path also needs to meet the dynamic requirements of the vehicle. Based on this, the following describes how to design a heuristic search algorithm to ensure that the searched path can meet the vehicle dynamics requirements and speed up the search.

In the heuristic search algorithm provided in this embodiment, the search space is discretized by grids, each grid corresponds to a node in the posture graph to be searched, and the connectivity between grids forms an edge between nodes. In addition, each grid (i, j) of the driving situation awareness map is associated with a continuous direction angle θ∈[0, 2π), which corresponds to the direction of the vehicle on the grid. The search will be performed on the three-dimensional space {i, j, θ} of the driving situation awareness map.

In the specific implementation process, starting from the starting node, if the target node is included in the child nodes of the current extension node, the search is ended. The expansion node is added to the expansion list, and the total value of the paths of multiple nodes to be expanded is calculated according to the evaluation function, sorted according to the size of the total value, and the node to be expanded with the smallest total path value in the expansion list is selected as the expanded node. That is, the current extension node, repeat the above steps until the target node is extended.

In this embodiment, it should be noted that, after the node to be expanded is selected from the expansion list in the above manner, the child nodes of the current expansion node can be obtained and expanded according to the following rules. First, under the constraints of the vehicle dynamics model, let the maximum turning angle corresponding to the current speed of the vehicle be γ, and select n turns on average between the angle range [-γ, γ], and make the vehicle move forward one step according to the vehicle dynamics model. A small distance δ generates n new states as children of the current extended node. Second, for each new state, the grid into which the new state falls is determined. If the grid has been expanded, the total cost value of the path corresponding to the newly expanded state is calculated according to the evaluation function. If the total cost value of the new path is less than the total cost value of the original path to the grid, the path to the grid and the total cost value are updated, and the sorting of the grid in the expanded list is updated. This process is repeated until the extended new state has an error with the target pose that is less than the threshold.

Since the generation of each new state follows the vehicle dynamics model, the searched path is executable by the vehicle. The direction angle θ changes continuously, even if these new states fall in the same grid, they all belong to different states. Since the total number of states increases exponentially, a heuristic search is performed according to a suitable evaluation function, and the node expansion order is reasonably determined, so that the available paths can be searched under the premise of meeting the real-time requirements. The path planning of the vehicle must not collide with obstacles, but also meet the vehicle dynamics constraints. Based on this, the present embodiment adopts the first evaluation function and the second evaluation function, two evaluation functions, respectively, to consider these two restriction factors, and comprehensively use the two evaluation functions to perform heuristic search.

Specifically, each grid in the driving situation awareness map corresponds to a node, and performing the heuristic search according to the first evaluation function and the second evaluation function may specifically include the following steps:

According to the cost value of each grid in the driving situation awareness map, and based on the first evaluation function, the total value of the first path of the node to be expanded is calculated.

According to the cost value of each grid in the driving situation awareness map, and based on the second evaluation function, the total value of the second path of the node to be expanded is calculated.

The total value of the first path is compared with the total value of the second path, and the total value of the third path of the node to be expanded is determined according to the comparison result.

According to the total value of the third path of the node to be expanded, a heuristic search is performed to obtain the target path of the vehicle.

In some embodiments, calculating the total value of the first path of the node to be expanded based on the first evaluation function according to the cost value of each grid in the driving situation awareness map may include:

The steerable angle range of the node to be expanded is discretized to obtain a plurality of discrete states.

Calculate the total value of the shortest paths corresponding to the plurality of discrete states according to the vehicle motion model.

Through the table lookup method, the shortest path corresponding to the minimum value is selected from the total value of multiple shortest paths as the shortest path of the node to be expanded.

The total value of the shortest path of the node to be expanded is multiplied by the minimum cost value among the cost values of the grids in the driving situation awareness map to obtain the total value of the first path of the node to be expanded.

In this embodiment, the first evaluation function assumes that there are no obstacles in the environment, and only considers the vehicle dynamics constraints, and calculates the shortest path from the three-dimensional space state {i, j, θ} to the target pose that meets the vehicle dynamics requirements. Since the shortest path assumes that there are no obstacles in the environment, after discretizing the direction angle θ, the shortest path from each discrete state to the target pose can be calculated and saved offline. In real-time calculation, the table look-up method can be used to quickly obtain the shortest path. By multiplying the total value of the shortest path by the minimum cost value ρ _min of the grid in the driving situation awareness map, the function value of the first evaluation function, that is, the total value of the first path can be obtained. The first evaluation function penalizes the position and orientation that deviates from the target pose, preventing a large number of useless branches from expanding, and comprehensively considering the vehicle orientation and vehicle dynamics constraints, which greatly improves the effectiveness of the first evaluation function.

It should be noted that the total value of the first path of the first evaluation function can be obtained by multiplying the above-mentioned total value of the shortest path by the minimum cost value ρ _min of the grid in the driving situation awareness map. The total value is obtained by multiplying the average cost value of the grids passed by the shortest path in the driving situation awareness map, and other methods may also be used, which may be specifically set as required, which is not limited in this embodiment.

In some embodiments, calculating the total value of the second path of the node to be expanded based on the second evaluation function according to the cost value of each grid in the driving situation awareness map includes:

Through a dynamic programming algorithm, the total value of the shortest path of the node to be expanded is calculated.

The total value of the shortest path of the node to be expanded is multiplied by the minimum cost value among the cost values of each grid in the driving situation awareness map to obtain the total value of the second path of the node to be expanded.

Specifically, the second evaluation function assumes that the vehicle has no dynamic constraints, that is, it can turn arbitrarily in place, and only considers the distribution of obstacles in the environment. Since the vehicle can turn arbitrarily in situ, the three-dimensional space state {i, j, θ} can be degenerated into a two-dimensional space state [i, j}, as long as the collision-free trajectory from each grid to the target pose is calculated in the two-dimensional space . Since the cost value p(i, j) = ρ _max = 1 corresponds to a grid that collides with any pose, the grid with ρ(i, j) < 1 has at least one pose that does not collide with an obstacle. Under the condition that the vehicle has no dynamic constraints, the grid with ρ(i, j) = 1 is the inaccessible grid, and all the grids with ρ(i, j) < 1 are available grids. Based on this, we can Binarize the grid in the driving situation awareness map according to whether it can be driven. At this time, the non-collision shortest path can be quickly calculated by the dynamic programming algorithm. The function value of the second evaluation function, that is, the total value of the second path, can be obtained by multiplying the total value of the shortest path by the minimum value P _min of the cost value in the driving situation awareness map. The second evaluation function can guide the search direction, preventing the search from falling into a dead end or requiring a detour to reach the direction of the target pose.

It should be noted that the total value of the second path of the second evaluation function can be obtained by multiplying the above-mentioned total value of the shortest path by the minimum cost value ρ _min of the grid in the driving situation awareness graph. The total value is obtained by multiplying the average cost value of the grids passed by the shortest path in the driving situation awareness map, and other methods may also be used, which may be specifically set as required, which is not limited in this embodiment.

Optionally, the comparing the total value of the first path with the total value of the second path, and determining the total value of the third path of the node to be expanded according to the comparison result may include: comparing the total value of the first path. and the larger value of the total value of the second path is determined as the total value of the third path of the node to be expanded.

Specifically, the first evaluation function and the second evaluation function may be used independently in the heuristic algorithm, or may be used simultaneously.

In this embodiment, in order to improve the search speed, and because the vehicle must meet both the dynamic constraints and the collision-free requirements, for each state, the final value of the evaluation function takes the first path of the first evaluation function The greater of the total value and the second path total value of the second evaluation function. A heuristic search is performed on the scene in Figure 5, and the path is obtained as shown in Figure 7. The left image in Figure 7 contains the distribution of obstacles, and the right image contains the distribution of target paths and obstacles.

In a typical scenario, the total number of extended nodes that do not use any evaluation function to search, use one evaluation function to search, and use two evaluation functions to search at the same time, as shown in Table 1.

Table 1 The relationship between the evaluation function and the number of extension nodes

As shown in Table 1, using the evaluation function can effectively reduce the total number of extended nodes. In the case shown in Figure 7, the effect of using two evaluation functions at the same time is better than using only one evaluation function. The estimation function considering the position of the obstacle is better than the estimation function considering the vehicle dynamics. Usually, in an unstructured environment, considering two evaluation functions at the same time is better than using only one evaluation function; there is no obvious advantage or disadvantage between the two evaluation functions, which is related to the specific distribution of obstacles in the environment.

In the path planning method provided in this embodiment, by using the first evaluation function and the second evaluation function for heuristic search at the same time, not only the vehicle orientation and vehicle dynamics constraints are considered, but also the guidance of the search direction is taken into account, preventing search Fall into a dead end and avoid detours, so as to reduce the number of extended nodes, reduce the amount of computation, and quickly search to obtain the target path.

FIG. 8 is a fourth schematic flowchart of a path planning method provided by an embodiment of the present invention. As shown in FIG. 8 , on the basis of the above-mentioned embodiment, for example, on the basis of the embodiment shown in FIG. 2 , this embodiment describes in detail the optimization method of the target path, and the method includes:

801. Obtain a driving situation awareness map, wherein the driving situation awareness map is a spatial layout diagram of the environment where the vehicle is currently located, including information on obstacles and a priori planned trajectory, and the driving situation awareness map is divided into for multiple grids.

802. According to the information of the obstacle and the a priori planned trajectory, assign a cost value to each grid in the driving situation awareness map.

803. According to the cost value of each grid in the driving situation awareness map, obtain the target path of the vehicle through heuristic search.

Steps 801 to 803 in this embodiment are similar to steps 201 to 203 in the foregoing embodiment, and details are not described herein again.

804. Perform smooth optimization on the target path by using the conjugate gradient descent method to obtain an optimized target path.

In this embodiment, the target path of the vehicle obtained through the heuristic search satisfies the dynamic properties of the vehicle and does not collide with obstacles. In general, the vehicle can directly track the path and travel to the target pose. However, this target path will often contain some unnatural bends, resulting in additional steering wheel turns, affecting ride comfort. Based on this, the embodiment of the present invention performs smooth optimization on the target path of the vehicle obtained by the heuristic search.

In some embodiments, the target path may be smoothly optimized by conjugate gradient descent. Optionally, the smooth optimization step may specifically include: calculating the gradient of the target path at each vertex according to an objective function; calculating and obtaining an optimization path that minimizes the objective function according to the gradient; the objective function It includes the penalty item of the safe distance from the vertex to the obstacle, the penalty item of the maximum curvature of the vertex, and the measurement item of the smoothness of the path; the optimized path is used as the optimized target path. Wherein, optionally, the maximum curvature in the maximum curvature penalty item of the vertex is determined according to the speed of the vehicle and the minimum turning radius. Optionally, the measure of the path smoothness is a quadratic function of the difference between the displacements of two consecutive adjacent vertices.

Exemplarily, since the vertices of the optimized target path change continuously, it is no longer suitable to use the grid granularity description of the driving situation awareness graph. In this embodiment, plane rectangular coordinates are used to describe the path vertices.

定义共轭梯度下降法的目标函数如下：Let the vertex of the target path obtained by heuristic search before smoothing be p _i =( _xi , y _i ), i∈[1,N], the periphery of vertex p _i , the nearest obstacle point to vertex p _i is o _i , The displacement of two adjacent vertices in the target path is Δp _i = p _i -p _i-1 , and the change of the angle between the two adjacent vertices and the origin is

The objective function of the conjugate gradient descent method is defined as follows:

Among them, the first item is the penalty item of the safe distance from the vertex to the obstacle, which is used to punish the vertices that are closer to the obstacle, d _m is the distance safety threshold, and the vertex p _i whose distance to the obstacle is greater than d _m is safe without The second term is the maximum curvature penalty term of the vertex, which gives the upper bound of the curvature at each vertex. To ensure that the path conforms to the dynamic characteristics, κ _m is the maximum curvature, and κ _m can be determined according to the vehicle speed and the minimum turning radius of the vehicle, Vertices pi exceeding the maximum curvature will be penalized, usually _{w κ} takes a large value. σ _o and σ _κ are penalty functions; the third item is a measure of path smoothness, which can be implemented by multiple functions, such as quadratic functions. w _o , w _κ , and _ws are the respective weights of the three items.

In this embodiment, the objective function is differentiable at each vertex, and the gradient of the objective function at the vertex is calculated below.

The first item, the safe distance penalty item:

When |p _i -o _i |<d _m , the gradient is:

The _curvature at the vertex pi depends not only on the vertex _pi , but also on the two adjacent vertices pi _-1 , pi ₊₁ . The angle change of vertex _pi is:

κ_i相对p_i、p_i-1、p_i+1三个顶点的梯度分别为： _curvature at vertex pi

The gradients of κ _i relative to the three vertices of p _i , p _i-1 , and p _i+1 are:

in,

在p_i处的梯度可用正交补进行表达与计算，向量a，b的正交补为：

The gradient at p _i can be expressed and calculated by the orthogonal complement. The orthogonal complement of the vectors a and b is:

The normalized orthogonal complement is defined as follows:

but:

Therefore, the gradient is calculated according to the above formula, and the optimized target path that minimizes the objective function can be obtained by using the gradient descent method. Generally, the optimized target path is smoother than the optimized path obtained by heuristic search before smooth optimization, which can provide higher ride comfort. As shown in Figure 9, the left graph contains the target path and obstacle distribution, and the right graph contains the target path, the optimized target path, and the obstacle distribution. By comparison, it can be found that the optimized target path is smoother than the target path.

Although the above objective function includes a penalty term for collision with obstacles, it tends to guide the optimized target path to an area far away from the obstacle, but due to the comprehensive consideration of vehicle dynamics constraints and path smoothness in the optimization process This factor makes the optimized target path likely to have colliding vertices. In order to ensure that the final target path can be safe and collision-free on the premise of ensuring a certain smoothness, in some embodiments, the target path is smoothly optimized by the conjugate gradient descent method, and the optimized target path is obtained. , further collision-free optimization processing is performed, as shown in Figure 10, the specific steps of the method may include:

1001. Perform collision detection on each vertex of the optimized target path.

1002. Obtain a final target path according to the collision detection result, so that the vehicle travels according to the final target path without collision.

Optionally, in some embodiments 1002 may specifically include:

10021. Determine whether there is a colliding vertex in the collision detection result. If no, go to step 10022, if yes, go to step 10023.

10022. Determine the optimized target path as the final target path.

10023. Divide the optimized target path into multiple sub-paths according to the collision detection result.

Optionally, dividing the optimized target path into a plurality of sub-paths according to the collision detection result may include: adding each vertex that collided to a collision set in sequence; replacing each vertex in the collision set respectively Obtaining a new collision set for the original vertices in the target path; dividing the optimized path into multiple sub-paths according to each original vertex in the new collision set.

10024. For each sub-path, perform a smooth optimization on the sub-path by using the conjugate gradient descent method to obtain a new optimized target path.

10025. Perform collision detection on each vertex of the new optimized target path.

10026. Determine whether there is a colliding vertex in the collision detection result. If yes, repeat step 10023 to step 10025, if not, execute step 10027.

10027. Determine the current new optimized target path as the final target path.

Specifically, collision detection is performed on each vertex p'i on the optimized target path, and the collided vertices are added to the set S' in sequence. If S′=φ, it means the path is safe, the optimization ends, and the optimized target path is determined as the final target path; if the set S′ is not empty, then all the vertices in S′ are sequentially replaced with the ones obtained by heuristic search. The corresponding original vertex in the target path. The original vertices after replacement, as fixed "wrong points", are denoted as p _a1 , p _a2 , ....p _ak in sequence. Divide the path into k+1 segments based on the current path's starting point, ending point and k "wrong points". The k+1 segments of the path are optimized by the gradient descent method, and a new optimized target path is obtained.

The new optimized target path still cannot ensure safety, and it is necessary to perform collision detection on each vertex on the new optimized target path. , and sequentially replaced with the corresponding original vertices in the target path obtained by heuristic search. The original vertices after replacement, as fixed "wrong points", are sequentially recorded as p _a1 , p _a2 , ....p _ak . Based on The starting point, end point and k "wrong points" of the current path divide the path into k+1 segments. The k+1 segments of the path are optimized by the gradient descent method, and a new optimized target path is obtained). In the most extreme case, the path iteratively degenerates into the target path of the vehicle obtained by the heuristic search. Since the target path ensures collision-free, after a certain number of iterations, an optimized path that conforms to vehicle dynamics and collision-free can be finally obtained, realizing safety and collision-free on the premise of ensuring a certain smoothness.

In the path planning method provided in this embodiment, the target path is smoothly optimized by the conjugate gradient descent method, so that the optimized target path is smoother, unnecessary turns can be reduced, additional steering wheel rotations during driving are avoided, and the ride comfort. In addition, on this basis, by performing collision-free optimization on the optimized path, safety and collision-free can be achieved on the premise of ensuring a certain smoothness.

FIG. 11 is a schematic structural diagram of a path planning device according to an embodiment of the present invention. As shown in FIG. 11 , the path planning device 110 includes: an acquisition module 1101 , an assignment module 1102 and a search module 1103 .

The acquiring module 1101 is used to acquire a driving situation awareness map, wherein the driving situation awareness map is a spatial layout diagram of the environment where the vehicle is currently located, including information on obstacles and a priori planned trajectory, and the driving situation awareness map is The knowledge map is divided into multiple grids.

The assignment module 1102 is configured to assign a cost value to each grid in the driving situation awareness map according to the information of the obstacle and the prior planning trajectory.

The search module 1103 is configured to obtain the target path of the vehicle through heuristic search according to the cost value of each grid in the driving situation awareness map.

In the path planning device provided by the embodiment of the present invention, the driving situation awareness map is obtained through the acquisition module 1101, and the driving situation awareness map is divided into multiple grids and includes obstacle information and a priori planning trajectory. The assignment module 1102 According to the information of the obstacle and the a priori planned trajectory, assign a cost value to each grid in the driving situation awareness map, and the search module 1103 assigns a value to each grid according to the driving situation awareness map. The cost value, the target path of the vehicle is obtained through heuristic search. By assigning the cost value to the driving situation cognitive map, and performing heuristic search based on the cost value, the operation can be greatly simplified, the calculation efficiency can be improved, and the real-time performance can be improved to meet the needs of unmanned vehicles.

The path planning device provided in the embodiment of the present invention can be used to execute the above-mentioned method embodiments, and its implementation principle and technical effect are similar, and details are not described herein again in this embodiment.

FIG. 12 is a block diagram of an electronic device provided by an embodiment of the present invention. The device may be a computer, a messaging device, a tablet device, a medical device, etc., and the device may be installed on an unmanned vehicle.

Device 120 may include one or more of the following components: processing component 1201 , memory 1202 , power supply component 1203 , multimedia component 1204 , audio component 1205 , input/output (I/O) interface 1206 , sensor component 1207 , and communication component 1208 .

The processing component 1201 generally controls the overall operation of the device 120, such as operations associated with display, phone calls, data communications, camera operations, and recording operations. The processing component 1201 may include one or more processors 1209 to execute instructions to perform all or part of the steps of the methods described above. Additionally, processing component 1201 may include one or more modules to facilitate interaction between processing component 1201 and other components. For example, processing component 1201 may include a multimedia module to facilitate interaction between multimedia component 1204 and processing component 1201.

Memory 1202 is configured to store various types of data to support operations at device 120 . Examples of such data include instructions for any application or method operating on device 120, contact data, phonebook data, messages, pictures, videos, and the like. Memory 1202 may be implemented by any type of volatile or non-volatile storage device or combination thereof, such as static random access memory (SRAM), electrically erasable programmable read only memory (EEPROM), erasable Programmable Read Only Memory (EPROM), Programmable Read Only Memory (PROM), Read Only Memory (ROM), Magnetic Memory, Flash Memory, Magnetic or Optical Disk.

Power supply assembly 1203 provides power to various components of device 120 . Power supply components 1203 may include a power management system, one or more power supplies, and other components associated with generating, managing, and distributing power to device 120 .

Multimedia component 1204 includes screens that provide an output interface between the device 120 and the user. In some embodiments, the screen may include a liquid crystal display (LCD) and a touch panel (TP). If the screen includes a touch panel, the screen may be implemented as a touch screen to receive input signals from a user. The touch panel includes one or more touch sensors to sense touch, swipe, and gestures on the touch panel. The touch sensor may not only sense the boundaries of a touch or swipe action, but also detect the duration and pressure associated with the touch or swipe action. In some embodiments, the multimedia component 1204 includes a front-facing camera and/or a rear-facing camera. When the device 120 is in an operation mode, such as a shooting mode or a video mode, the front camera and/or the rear camera may receive external multimedia data. Each of the front and rear cameras can be a fixed optical lens system or have focal length and optical zoom capability.

Audio component 1205 is configured to output and/or input audio signals. For example, audio component 1205 includes a microphone (MIC) that is configured to receive external audio signals when device 120 is in operating modes, such as call mode, recording mode, and voice recognition mode. The received audio signal may be further stored in memory 1202 or transmitted via communication component 1208. In some embodiments, audio component 1205 also includes a speaker for outputting audio signals.

The I/O interface 1206 provides an interface between the processing component 1201 and a peripheral interface module, and the above-mentioned peripheral interface module may be a keyboard, a click wheel, a button, and the like. These buttons may include, but are not limited to: home button, volume buttons, start button, and lock button.

Sensor assembly 1207 includes one or more sensors for providing status assessment of various aspects of device 120 . For example, the sensor assembly 1207 can detect the open/closed state of the device 120, the relative positioning of components, such as the display and keypad of the device 120, and the sensor assembly 1207 can also detect changes in the position of the device 120 or a component of the device 120 , the presence or absence of user contact with the device 120 , the orientation or acceleration/deceleration of the device 120 and the temperature change of the device 120 . Sensor assembly 1207 may include a proximity sensor configured to detect the presence of nearby objects in the absence of any physical contact. Sensor assembly 1207 may also include a light sensor, such as a CMOS or CCD image sensor, for use in imaging applications. In some embodiments, the sensor component 1207 may also include an acceleration sensor, a gyroscope sensor, a magnetic sensor, a pressure sensor, or a temperature sensor.

Communication component 1208 is configured to facilitate wired or wireless communication between apparatus 120 and other devices. Device 120 may access wireless networks based on communication standards, such as WiFi, 2G or 3G, or a combination thereof. In one exemplary embodiment, the communication component 1208 receives broadcast signals or broadcast related information from an external broadcast management system via a broadcast channel. In an exemplary embodiment, the communication component 1208 also includes a near field communication (NFC) module to facilitate short-range communication. For example, the NFC module may be implemented based on radio frequency identification (RFID) technology, infrared data association (IrDA) technology, ultra-wideband (UWB) technology, Bluetooth (BT) technology and other technologies.

In an exemplary embodiment, apparatus 120 may be implemented by one or more application specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field programmable A gate array (FPGA), controller, microcontroller, microprocessor or other electronic component implementation is used to perform the above method.

In an exemplary embodiment, a non-transitory computer-readable storage medium including instructions, such as a memory 1202 including instructions, executable by the processor 1209 of the apparatus 120 to perform the method described above is also provided. For example, the non-transitory computer-readable storage medium may be ROM, random access memory (RAM), CD-ROM, magnetic tape, floppy disk, optical data storage device, and the like.

A non-transitory computer-readable storage medium, when an instruction in the storage medium is executed by a processor of a terminal device, enables the terminal device to execute the above-mentioned method for split-screen processing of the terminal device.

The present application further provides a computer-readable storage medium, where computer-executable instructions are stored in the computer-readable storage medium, and when the processor executes the computer-executable instructions, the above path planning method executed by the path planning device is implemented.

The above-mentioned computer-readable storage medium, the above-mentioned readable storage medium can be realized by any type of volatile or non-volatile storage device or their combination, such as static random access memory (SRAM), electrically erasable Programmable Read Only Memory (EEPROM), Erasable Programmable Read Only Memory (EPROM), Programmable Read Only Memory (PROM), Read Only Memory (ROM), Magnetic Memory, Flash Memory, Magnetic or Optical Disk. A readable storage medium can be any available medium that can be accessed by a general purpose or special purpose computer.

An exemplary readable storage medium is coupled to the processor such that the processor can read information from, and write information to, the readable storage medium. Of course, the readable storage medium can also be an integral part of the processor. The processor and the readable storage medium may be located in application specific integrated circuits (Application Specific Integrated Circuits, ASIC for short). Of course, the processor and the readable storage medium may also exist in the device as discrete components.

Those of ordinary skill in the art can understand that all or part of the steps of implementing the above method embodiments may be completed by program instructions related to hardware. The aforementioned program can be stored in a computer-readable storage medium. When the program is executed, the steps including the above method embodiments are executed; and the foregoing storage medium includes: ROM, RAM, magnetic disk or optical disk and other media that can store program codes.

Embodiments of the present invention further provide a computer program product, including a computer program, which, when executed by a processor, implements the path planning method executed by the above path planning device.

Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, but not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: The technical solutions described in the foregoing embodiments can still be modified, or some or all of the technical features thereof can be equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the technical solutions of the embodiments of the present invention. scope.

Claims (26)

1. A method of path planning, comprising:

the method comprises the steps of obtaining a driving situation cognitive map, wherein the driving situation cognitive map is a space layout map of the current environment of a vehicle, the driving situation cognitive map comprises barrier information and a priori planning track, and the driving situation cognitive map is divided into a plurality of grids;

according to the information of the obstacles and the prior planning track, assigning a cost value to each grid in the driving situation cognitive map;

and obtaining a target path of the vehicle through heuristic search according to the cost value of each grid in the driving situation cognitive map.

2. The method of claim 1, wherein assigning a cost value to each grid in the driving situation cognitive map based on the information of the obstacle and the a priori planned trajectory comprises:

determining a first grid set which cannot be driven by a vehicle according to the information of the obstacles, and assigning a maximum cost value to each grid in the first grid set;

according to the first grid set and the prior planning track, carrying out cost value assignment on each grid in a second grid set; the second grid set comprises grids in the driving situation cognitive map except the first grid set.

3. The method of claim 2, wherein the driving situation recognition map comprises an obstacle height and confidence map and a travelable region edge map; the determining a first grid set which can not be driven by the vehicle according to the information of the obstacles comprises the following steps:

determining each first target grid occupied by a static obstacle in the obstacle height and confidence map and each second target grid outside the travelable region edge in the travelable region edge map;

determining a grid set formed by the first target grid and the second target grid as the first grid set.

4. The method of claim 3, wherein determining the first grid set that the vehicle is not drivable based on the information of the obstacle further comprises:

determining a first radius according to the distance from the edge of the vehicle to the center point of the vehicle;

determining a plurality of circles by respectively taking each point of the edge line of the static obstacle and each point of the edge line of the travelable area as the circle center and taking the first radius as the radius;

adding each third target grid occupied by the plurality of circles to the first grid set.

5. The method according to claim 4, wherein before determining a plurality of circles using the respective points of the edge line of the static obstacle and the points of the edge line of the travelable area as centers of circles and the first radius as radii, further comprising:

determining a safety margin according to the current speed of the vehicle;

determining a second radius according to the first radius and the safety margin;

the determining a plurality of circles respectively by taking each point of the edge line of the static obstacle and each point of the edge line of the travelable area as a circle center and taking the first radius as a radius comprises the following steps:

and determining a plurality of circles by respectively taking each point of the edge line of the static obstacle and each point of the edge line of the travelable area as the circle center and taking the second radius as the radius.

6. The method of claim 2, wherein the driving situation awareness map comprises an obstacle height and confidence map and an obstacle speed and direction map; the determining a first grid set which can not be driven by the vehicle according to the information of the obstacles comprises the following steps:

determining fourth target grids occupied by the dynamic barrier according to the height of the barrier and position information of the dynamic barrier in the confidence coefficient situation diagram, and adding the fourth target grids into the first grid set;

and according to the speed of the obstacle and the speed of the dynamic obstacle in the direction situation diagram, performing expansion operation on each grid occupied by the dynamic obstacle, and adding each newly added fifth target grid after expansion into the first grid set.

7. The method of claim 2, wherein assigning a cost value to each grid in the second grid set according to the first grid set and the a priori planning trajectory comprises:

for each sixth target grid in the second grid set, determining a cost value of the sixth target grid according to a first distance between the sixth target grid and a grid in the first grid set closest to the sixth target grid and a second distance between the sixth target grid and the prior planning trajectory.

8. The method of claim 7, wherein the cost value of the sixth target grid is inversely proportional to the first distance and directly proportional to the second distance.

9. The method of claim 8, wherein determining the cost value for the sixth target grid based on a first distance between the sixth target grid and a grid of the first grid set that is closest to the sixth target grid and a second distance between the sixth target grid and the a priori planned trajectory comprises:

determining a cost value of the sixth target grid according to the first distance, the second distance and a decay parameter; the decay parameter is inversely proportional to a decay rate at which the cost value decays as the first distance increases.

10. The method of claim 8, wherein determining the cost value for the sixth target grid based on a first distance between the sixth target grid and a grid of the first grid set that is closest to the sixth target grid and a second distance between the sixth target grid and the a priori planned trajectory comprises:

determining a cost value of the sixth target grid according to the first distance, the second distance and a weight parameter; the weight parameter is proportional to a strength of the first distance's impact on the cost value of the sixth target grid and inversely proportional to a strength of the second distance's impact on the cost value of the sixth target grid.

11. The method according to claim 1, wherein a grid corresponding to an end point of the prior planned trajectory is a target node, and obtaining a target path of the vehicle by heuristic search according to a cost value of each grid in the driving situation cognitive map comprises:

performing heuristic search based on a first evaluation function and a second evaluation function according to the cost value of each grid in the driving situation cognitive map to obtain a target path of the vehicle; the first estimation function is associated with a vehicle steerable angle range, and the second estimation function is associated with position information of an obstacle.

12. The method of claim 11, wherein each grid in the driving situation awareness graph corresponds to a node, and wherein performing a heuristic search based on the first valuation function and the second valuation function comprises:

calculating the total value of a first path of a node to be expanded based on the first valuation function according to the cost value of each grid in the driving situation cognitive map;

calculating a second path total value of the node to be expanded based on the second evaluation function according to the cost value of each grid in the driving situation cognitive map;

comparing the first path total value with the second path total value, and determining a third path total value of the node to be expanded according to a comparison result;

and carrying out heuristic search according to the total value of the third path of the node to be expanded to obtain a target path of the vehicle.

13. The method according to claim 12, wherein the calculating a first path total value of the node to be expanded based on the first estimation function according to the cost value of each grid in the driving situation cognitive map comprises:

discretizing the steerable angle range of the node to be expanded to obtain a plurality of discrete states;

calculating the total value of the shortest paths corresponding to the discrete states respectively according to a vehicle motion model;

selecting the shortest path corresponding to the minimum value from the total values of the shortest paths as the shortest path of the node to be expanded through a table lookup method;

and multiplying the total value of the shortest path of the node to be expanded by the minimum cost value in the cost values of the grids in the driving situation cognitive map to obtain the total value of the first path of the node to be expanded.

14. The method according to claim 12, wherein the calculating a second path total value of the node to be expanded based on the second estimation function according to the cost value of each grid in the driving situation cognitive map comprises:

calculating the total value of the shortest path of the node to be expanded through a dynamic programming algorithm;

and multiplying the total value of the shortest path of the node to be expanded by the minimum cost value in the cost values of the grids in the driving situation cognitive map to obtain the total value of the second path of the node to be expanded.

15. The method according to any one of claims 1 to 14, wherein after obtaining the target path of the vehicle by a heuristic search according to the cost value of each grid in the driving situation cognitive map, the method further comprises:

and carrying out smooth optimization on the target path by a conjugate gradient descent method to obtain an optimized target path.

16. The method of claim 15, wherein the smoothly optimizing the target path by conjugate gradient descent comprises:

calculating the gradient of the target path at each vertex according to an objective function;

calculating to obtain an optimized path which minimizes the objective function according to the gradient; the objective function comprises a safety distance penalty term from a vertex to an obstacle, a vertex maximum curvature penalty term and a metric term of path smoothness;

and taking the optimized path as the optimized target path.

17. The method of claim 16, wherein the maximum curvature in the vertex maximum curvature penalty term is determined based on a vehicle speed and a minimum turn radius of the vehicle.

18. The method of claim 16, wherein the metric term for path smoothness is a quadratic function of the difference between two consecutive adjacent vertex displacements.

19. The method of claim 15, wherein after the smoothly optimizing the target path by conjugate gradient descent to obtain the optimized target path, further comprising:

performing collision detection on each vertex of the optimized target path;

and obtaining a final target path according to the collision detection result so that the vehicle can run according to the final target path without collision.

20. The method of claim 19, wherein the deriving a final target path based on the collision detection result comprises:

if the collision detection result has a collision vertex, dividing the optimized target path into a plurality of sub-paths according to the collision detection result;

carrying out smooth optimization on each sub-path by a conjugate gradient descent method to obtain a new optimized target path;

performing collision detection on each vertex of the new optimized target path;

if the vertex with the collision exists, repeatedly executing the step of dividing the optimized target path into a plurality of sub-paths according to the collision detection result, performing smooth optimization on the sub-paths by a conjugate gradient descent method aiming at each sub-path to obtain a new optimized target path, performing collision detection on each vertex of the new optimized target path until no vertex with the collision exists, and determining the current new optimized target path as a final target path.

21. The method of claim 20, wherein the dividing the optimized target path into a plurality of sub-paths according to collision detection results comprises

Sequentially adding each vertex with collision into a collision set;

replacing each vertex in the collision set with an original vertex in the target path respectively to obtain a new collision set;

and dividing the optimized path into a plurality of sub-paths according to each original vertex in the new collision set.

22. The method of claim 19, wherein the deriving a final target path based on the collision detection result comprises:

and if no collision vertex exists in the collision detection result, determining the optimized target path as a final target path.

23. A path planning apparatus, comprising:

the system comprises an acquisition module, a judgment module and a display module, wherein the acquisition module is used for acquiring a driving situation cognitive map, the driving situation cognitive map is a spatial layout map of the current environment of a vehicle, the spatial layout map comprises barrier information and a priori planning track, and the driving situation cognitive map is divided into a plurality of grids;

the assignment module is used for assigning a cost value to each grid in the driving situation cognitive map according to the information of the obstacles and the prior planning track;

and the searching module is used for obtaining a target path of the vehicle through heuristic search according to the cost value of each grid in the driving situation cognitive map.

24. An electronic device, comprising: at least one processor and memory;

the memory stores computer-executable instructions;

the at least one processor executing the computer-executable instructions stored by the memory causes the at least one processor to perform the path planning method of any of claims 1 to 22.

25. A computer-readable storage medium having computer-executable instructions stored thereon which, when executed by a processor, implement a path planning method according to any one of claims 1 to 22.

26. A computer program product comprising a computer program, characterized in that the computer program, when being executed by a processor, implements the path planning method according to any one of claims 1 to 22.

Images