Tracking trajectory analysis and selection method and terminal

Abstract

The invention discloses a tracking trajectory analysis and selection method and a terminal. The method comprises the following steps: acquiring trajectory point information of a real-time planned path; for each real-time planning path, calculating the curvature radius of each track point, and further obtaining the average curvature radius of the real-time planning path; obtaining the path cost of each real-time planned path in a cost map containing obstacles; obtaining the path length, the straight path number and the curve path number of each real-time planned path; the optimal tracking path is determined by considering the average curvature radius of the planned path, the path cost and the path length in the cost map, the number of straight roads and the number of bent roads, and the real-time planned path is selected more reasonably, so that tracking driving is more stably carried out, and the tracking driving efficiency is improved. And the safety of the tracking track is ensured by considering the cost value of the path, and the driving efficiency of the path is ensured by considering the average curvature radius, so that the method is more excellent.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to a tracking trajectory analysis and selection method and terminal. Background Art

[0002] Unmanned driving has various applications, one of which is autonomous tracking. Unmanned tracking driving is usually done by pre-recording the track path, then using the recorded track as the target track, and controlling the vehicle through the tracking algorithm to track along the recorded track.

[0003] With the development of technology, the methods of obtaining trajectory paths have also diversified. One of the methods of obtaining paths is real-time path planning. The vehicle obtains road obstacle information through perception sensors while driving, and updates the old path based on the information. Existing technologies usually directly follow the path obtained by the sensor. Sometimes multiple real-time paths may be generated. If no processing is done, abnormal switching will occur. Therefore, it is necessary to analyze and judge the real-time path to distinguish and select a better path. How to select the path is the current problem to be solved. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a method for analyzing and selecting a tracking trajectory, which can more reasonably select the implementation planning path, thereby performing tracking driving more stably.

[0005] In order to solve the above technical problems, the technical solution adopted by the present invention is: A method for analyzing and selecting a tracking trajectory comprises the steps of: S1. Obtain the trajectory point information of the real-time planning path; S2. For each of the real-time planned paths, calculate the radius of curvature of each trajectory point, and then obtain the average radius of curvature of the real-time planned path; S3, obtaining the path cost of each of the real-time planned paths in the cost map containing obstacles; S4, obtaining the distance length, number of straight roads and number of curves of each real-time planned path; S5. Select a path according to the average curvature radius, the path cost, the distance length, the number of straight roads, and the number of curves.

[0006] In order to solve the above technical problems, another technical solution adopted by the present invention is: A tracking trajectory analysis and selection terminal includes a processor, a memory, and a computer program stored in the memory and executable on the processor, wherein the processor implements the following steps when executing the computer program: S1. Obtain the trajectory point information of the real-time planning path; S2. For each of the real-time planned paths, calculate the radius of curvature of each trajectory point, and then obtain the average radius of curvature of the real-time planned path; S3, obtaining the path cost of each of the real-time planned paths in the cost map containing obstacles; S4, obtaining the distance length, number of straight roads and number of curves of each real-time planned path; S5. Select a path according to the average curvature radius, the path cost, the distance length, the number of straight roads, and the number of curves.

[0007] The beneficial effects of the present invention are as follows: a tracking trajectory analysis and selection method and terminal of the present invention determine the optimal tracking path by considering the average curvature radius of the planned path, the path cost in the cost map, the path length, the number of straight roads and the number of curves, and more reasonably select the real-time planned path, thereby more stably performing tracking driving. The consideration of the path cost value ensures the safety of the tracking trajectory, and the consideration of the average curvature radius ensures the driving efficiency of the path, which is more superior. BRIEF DESCRIPTION OF THE DRAWINGS

[0008] Figure 1 A flowchart of a method for analyzing and selecting a tracking trajectory according to an embodiment of the present invention; Figure 2 A structural diagram of a tracking trajectory analysis and selection terminal according to an embodiment of the present invention; Description of labels: 1. A tracking trajectory analysis and selection terminal; 2. A processor; 3. A memory. DETAILED DESCRIPTION

[0009] In order to explain the technical content, achieved objectives and effects of the present invention in detail, the following is an explanation in combination with the implementation modes and the accompanying drawings.

[0010] Please refer to Figure 1 , a method for analyzing and selecting a tracking trajectory, comprising the steps of: S1. Obtain the trajectory point information of the real-time planning path; S2. For each of the real-time planned paths, calculate the radius of curvature of each trajectory point, and then obtain the average radius of curvature of the real-time planned path; S3, obtaining the path cost of each of the real-time planned paths in the cost map containing obstacles; S4, obtaining the distance length, number of straight roads and number of curves of each real-time planned path; S5. Select a path according to the average curvature radius, the path cost, the distance length, the number of straight roads, and the number of curves.

[0011] From the above description, it can be seen that the beneficial effects of the present invention are: a tracking trajectory analysis and selection method and terminal of the present invention determine the optimal tracking path by considering the average curvature radius of the planned path, the path cost in the cost map, the path length, the number of straight roads and the number of curves, and more reasonably select the real-time planned path, so as to perform tracking driving more stably, and the consideration of the path cost value ensures the safety of the tracking trajectory, and the consideration of the average curvature radius ensures the driving efficiency of the path, which is more superior.

[0012] Furthermore, the generation of the cost map in step S3 is specifically as follows: Create a raster map; Obtaining laser radar point cloud data, identifying various obstacles based on the laser radar point cloud data, and projecting the obstacles onto a grid map; According to the distance between each grid point and the obstacle in the grid map, a cost value is set for each grid, and the cost value is negatively correlated with the distance between the grid and the obstacle. A cost gradient is generated to obtain a cost map.

[0013] From the above description, we can see that by identifying the obstacle position through lidar, marking the obstacle area on the grid map, and generating a cost map containing obstacles and impassable areas, it can effectively reflect the collision risk of the real-time planned path and reflect safety.

[0014] Furthermore, the path cost is the sum of the cost values ​​of each trajectory point in the cost map in the implementation of the planned path.

[0015] From the above description, it can be seen that the risk level of the real-time planning path is determined according to the sum of the cost values ​​of each trajectory point in the cost map.

[0016] Furthermore, the step S5 is specifically as follows: The real-time planned paths are ranked according to the average curvature radius, the path cost, the distance length, the number of straight roads and the number of curves, and a comprehensive weight calculation is performed based on the ranking values ​​to select the real-time planned path with the smallest total value as the optimal path.

[0017] From the above description, it can be seen that weighted calculation is performed according to the ranking data of various aspects, and the real-time planning path with the smallest total value is selected as the optimal path, that is, the optimal path is selected with comprehensive consideration, which is more reasonable.

[0018] Furthermore, the weight values ​​associated with each ranking value are determined in advance through the AHP hierarchical analysis method.

[0019] From the above description, it can be seen that the weight values ​​of each ranking value are determined in advance through the AHP hierarchical analysis method to ensure the effectiveness of the weight value determination.

[0020] Please refer to Figure 2 , a tracking trajectory analysis and selection terminal, comprising a processor, a memory, and a computer program stored in the memory and executable on the processor, wherein the processor implements the following steps when executing the computer program: S1. Obtain the trajectory point information of the real-time planning path; S2. For each of the real-time planned paths, calculate the radius of curvature of each trajectory point, and then obtain the average radius of curvature of the real-time planned path; S3, obtaining the path cost of each of the real-time planned paths in the cost map containing obstacles; S4, obtaining the distance length, number of straight roads and number of curves of each real-time planned path; S5. Select a path according to the average curvature radius, the path cost, the distance length, the number of straight roads, and the number of curves.

[0021] From the above description, it can be seen that the beneficial effects of the present invention are: a tracking trajectory analysis and selection method and terminal of the present invention determine the optimal tracking path by considering the average curvature radius of the planned path, the path cost in the cost map, the path length, the number of straight roads and the number of curves, and more reasonably select the real-time planned path, so as to perform tracking driving more stably, and the consideration of the path cost value ensures the safety of the tracking trajectory, and the consideration of the average curvature radius ensures the driving efficiency of the path, which is more superior.

[0022] Furthermore, the generation of the cost map in step S3 is specifically as follows: Create a raster map; Obtaining laser radar point cloud data, identifying various obstacles based on the laser radar point cloud data, and projecting the obstacles onto a grid map; According to the distance between each grid point and the obstacle in the grid map, a cost value is set for each grid, and the cost value is negatively correlated with the distance between the grid and the obstacle. A cost gradient is generated to obtain a cost map.

[0023] From the above description, we can see that by identifying the obstacle position through lidar, marking the obstacle area on the grid map, and generating a cost map containing obstacles and impassable areas, it can effectively reflect the collision risk of the real-time planned path and reflect safety.

[0024] Furthermore, the path cost is the sum of the cost values ​​of each trajectory point in the cost map in the implementation of the planned path.

[0025] From the above description, it can be seen that the risk level of the real-time planning path is determined according to the sum of the cost values ​​of each trajectory point in the cost map.

[0026] Furthermore, the step S5 is specifically as follows: The real-time planned paths are ranked according to the average curvature radius, the path cost, the distance length, the number of straight roads and the number of curves, and a comprehensive weight calculation is performed based on the ranking values ​​to select the real-time planned path with the smallest total value as the optimal path.

[0027] From the above description, it can be seen that weighted calculation is performed according to the ranking data of various aspects, and the real-time planning path with the smallest total value is selected as the optimal path, that is, the optimal path is selected with comprehensive consideration, which is more reasonable.

[0028] Furthermore, the weight values ​​associated with each ranking value are determined in advance through the AHP hierarchical analysis method.

[0029] From the above description, it can be seen that the weight values ​​of each ranking value are determined in advance through the AHP hierarchical analysis method to ensure the effectiveness of the weight value determination.

[0030] A tracking trajectory analysis and selection method and terminal of the present invention are suitable for scenarios where multiple tracking trajectories generated in real time need to be selected during unmanned tracking driving.

[0031] Please refer to Figure 1 , Embodiment 1 of the present invention is: A method for analyzing and selecting a tracking trajectory comprises the steps of: S1. Obtain the trajectory point information of the real-time planning path; S2. For each of the real-time planned paths, calculate the radius of curvature of each trajectory point, and then obtain the average radius of curvature of the real-time planned path.

[0032] In this embodiment, for each trajectory point, the trajectory points 5 meters before and after are taken to form a set of calculation points (x i ,y i ), i is 1, 2 or 3, and a triangle can be formed by three points. Calculate the area of ​​the triangle: ; The radius of the circumscribed circle of the triangle is the radius of curvature: ; in, d 21is the distance between point (x1, y1) and point (x2, y2), d 23 and d 13 Same reason.

[0033] S3, obtaining the path cost of each of the real-time planned paths in the cost map containing obstacles; The generation of the cost map in step S3 is specifically as follows: Create a raster map; In this embodiment, for the establishment of a grid map, initially, the grid map is a large blank map, and the map size can be configured according to actual needs, and the size of the grid can also be configured according to actual needs.

[0034] In this embodiment, a grid size of 0.1 m×0.1 m is used, that is, the track resolution will not be reduced due to the grid being too large, and the amount of calculation will not be increased due to the grid being too small.

[0035] The creation of a raster map consists of a map image file and a map description file. The map image file is a pixel image, one pixel for one raster; the map description file describes the resolution of each raster and the position of the starting coordinate origin.

[0036] Obtaining laser radar point cloud data, identifying various obstacles based on the laser radar point cloud data, and projecting the obstacles onto a grid map; In this embodiment, the laser radar point cloud data is obtained, and the ground points and above-ground points are distinguished according to a preset algorithm, and the ground points are removed; the coordinates of the above-ground points are transformed and converted to the grid map coordinate system, and the Z-axis is compressed and projected to the grid map, and the grids where the above-ground points are projected are marked as obstacle grids.

[0037] In this embodiment, the laser radar point cloud data of the front of the vehicle is obtained for processing, including point cloud segmentation, distinguishing ground points and ground points. Ground points are ground reflection points, ground points are obstacle reflection points, and ground points of point cloud data are removed. In this embodiment, each point cloud contains the (x, y, z) coordinates relative to the radar. If the installation height of the laser radar is known, the height can be corresponded to the Z axis to distinguish between ground points and non-ground points (ground points). In other equivalent embodiments, bottom points and obstacle reflection points can also be distinguished by other methods.

[0038] After distinguishing between ground points and above-ground points, the coordinates of the above-ground points are transformed, and the coordinates of each relative laser radar of the vehicle body are transformed into the coordinate system of the grid map, and the Z axis of the coordinates is compressed and projected onto the grid map. The grids where the ground points are projected are obstacle grids, and those that are not projected are blank grids, thus obtaining an obstacle map.

[0039] The vehicle starts to travel from the origin of the map. Any position and posture can be represented by position translation and posture rotation of the origin position and posture. The origin of the laser radar point cloud data coordinate is the laser radar. The point cloud data needs to be converted according to the position translation and heading to correspond to the grid map.

[0040] According to the distance between each grid point and the obstacle in the grid map, a cost value is set for each grid, and the cost value is negatively correlated with the distance between the grid and the obstacle. A cost gradient is generated to obtain a cost map.

[0041] In this embodiment, the cost values ​​of the grid points within the first preset distance from the obstacle grid point in the grid map are all set to the highest value, and the cost is reduced by a gradient unit every time the second preset distance is increased to obtain a cost map.

[0042] In this embodiment, the first preset distance is half of the vehicle body width, the cost value range is [254,0], and according to the cost value of each grid, the cost value 254 is black and 0 is white, and each grid is filled with grayscale pixels to obtain a cost map.

[0043] According to the superposition of the grid map and the obstacle, the cost gradient is generated for the map area. The area occupied by the obstacle extends outward to half the width of the vehicle body, which is the fatal cost area. If the vehicle moves to this area, it will send a collision risk. Extending outward from the fatal area, the cost gradually weakens, and the weakening gradient is affected by the distance between the grid and the obstacle area. The greater the distance, the greater the weakening. In this embodiment, on the basis of the first preset distance, each increase in the second preset distance is a gradient. The second preset distance can be configured according to actual needs. In this embodiment, it is preferably the width of the grid, that is, 0.1m, and it can be adjusted separately in other equivalent embodiments. In this embodiment, the cost value is [254,0], and the grid is filled with grayscale pixels according to the cost value, the fatal area (cost value 254) is set to black, the grid with a cost value of 0 is set to white, and the cost value decreases (254~0) is gray. The color gradually becomes lighter as the cost value decreases, thereby obtaining a cost map containing obstacles. In addition, it should be noted that when the cost value of a grid is affected by two obstacles, the higher cost value is taken as the cost value of the grid. For example, if a grid needs to set a cost value of 254 when considering one obstacle and a cost value of 101 when considering another obstacle, the cost value of the grid should finally be set to 254.

[0044] The path cost is the sum of the cost values ​​of each trajectory point in the cost map in the implementation of the planned path.

[0045] In this embodiment, each trajectory point is associated with its cost value, and the path cost of the real-time planned path can be obtained by accumulating the cost point by point.

[0046] S4. Obtain the distance length, number of straight roads, and number of curves of each of the real-time planned paths.

[0047] In this embodiment, the distance between two links of trajectory points is recursively calculated and summed up to obtain the trajectory distance.

[0048] S5. Select a path according to the average curvature radius, the path cost, the distance length, the number of straight roads, and the number of curves.

[0049] The step S5 is specifically as follows: The real-time planned paths are ranked according to the average radius of curvature, the path cost, the distance length, the number of straight roads and the number of curves, and a comprehensive weight calculation is performed according to the ranking values, and the real-time planned path with the smallest total value is selected as the optimal path. The weight value associated with each ranking value is determined in advance by the AHP hierarchical analysis method.

[0050] In this embodiment, before the comprehensive evaluation of step S5 is performed, the implementation planning path needs to be filtered once. The highest cost value of each track point in each implementation planning path is obtained, and it is determined whether there is a track point with a fatal cost value among the highest cost values. If there is, the real-time planning route is discarded and does not participate in the subsequent steps.

[0051] Please refer to Figure 2 , Embodiment 2 of the present invention is: A tracking trajectory analysis and selection terminal 1 includes a processor 2, a memory 3, and a computer program stored in the memory 3 and executable on the processor 2. When the processor 2 executes the computer program, the steps in the above embodiment 1 are implemented.

[0052] In summary, the present invention provides a tracking trajectory analysis and selection method and terminal, which determine the optimal tracking path by considering the average curvature radius of the planned path, the path cost in the cost map, the path length, the number of straight roads, and the number of curves, so as to more reasonably select the real-time planned path, thereby performing tracking driving more stably. The consideration of the path cost value ensures the safety of the tracking trajectory, and the consideration of the average curvature radius ensures the driving efficiency of the path, which is more superior.

[0053] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent transformations made using the contents of the present invention's specification and drawings, or directly or indirectly applied in related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for analyzing and selecting a tracking trajectory, characterized in that: Includes steps: S1. Obtain the trajectory point information of the real-time planning path; S2. For each of the real-time planned paths, calculate the radius of curvature of each trajectory point, and then obtain the average radius of curvature of the real-time planned path; S3, obtaining the path cost of each of the real-time planned paths in the cost map containing obstacles; The generation of the cost map in step S3 is specifically as follows: Create a raster map; Obtaining laser radar point cloud data, identifying various obstacles based on the laser radar point cloud data, and projecting the obstacles onto a grid map; According to the distance between each grid point and the obstacle in the grid map, a cost value is set for each grid, wherein the cost value is negatively correlated with the distance between the grid and the obstacle, a cost gradient is generated, and a cost map is obtained; Among them, the area extending outward from the obstacle area to half the width of the vehicle body is assigned the highest cost value and is set as the fatal cost area; S4, obtaining the distance length, number of straight roads and number of curves of each real-time planned path; Before step S5, the method further includes the following steps: Obtaining the highest cost value of the grid points in each real-time planning path, and judging whether there is a fatal cost area according to the highest cost value, and the real-time planning path with the fatal cost area does not participate in the path selection of step S5; S5. Select a path according to the average curvature radius, the path cost, the distance length, the number of straight roads, and the number of curves.

2. The method for analyzing and selecting a tracking trajectory according to claim 1, characterized in that: The path cost is the sum of the cost values ​​of each trajectory point in the cost map in the implementation of the planned path.

3. The method for analyzing and selecting a tracking trajectory according to claim 1, characterized in that: The step S5 is specifically as follows: The real-time planned paths are ranked according to the average curvature radius, the path cost, the distance length, the number of straight roads and the number of curves, and a comprehensive weight calculation is performed based on the ranking values ​​to select the real-time planned path with the smallest total value as the optimal path.

4. The method for analyzing and selecting a tracking trajectory according to claim 3, characterized in that: The weight values ​​associated with each ranking value are determined in advance through the AHP hierarchical analysis method.

5. A tracking trajectory analysis and selection terminal, comprising a processor, a memory, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the following steps are implemented: S1. Obtain the trajectory point information of the real-time planning path; S2. For each of the real-time planned paths, calculate the radius of curvature of each trajectory point, and then obtain the average radius of curvature of the real-time planned path; S3, obtaining the path cost of each of the real-time planned paths in the cost map containing obstacles; The generation of the cost map in step S3 is specifically as follows: Create a raster map; Obtaining laser radar point cloud data, identifying various obstacles based on the laser radar point cloud data, and projecting the obstacles onto a grid map; According to the distance between each grid point and the obstacle in the grid map, a cost value is set for each grid, wherein the cost value is negatively correlated with the distance between the grid and the obstacle, a cost gradient is generated, and a cost map is obtained; Among them, the area extending outward from the obstacle area to half the width of the vehicle body is assigned the highest cost value and is set as the fatal cost area; S4, obtaining the distance length, number of straight roads and number of curves of each real-time planned path; Before step S5, the method further includes the following steps: Obtaining the highest cost value of the grid points in each real-time planning path, and judging whether there is a fatal cost area according to the highest cost value, and the real-time planning path with the fatal cost area does not participate in the path selection of step S5; S5. Select a path according to the average curvature radius, the path cost, the distance length, the number of straight roads, and the number of curves.

6. A tracking trajectory analysis and selection terminal according to claim 5, characterized in that: The path cost is the sum of the cost values ​​of each trajectory point in the cost map in the implementation of the planned path.

7. The tracking trajectory analysis and selection terminal according to claim 5, characterized in that: The step S5 is specifically as follows: The real-time planned paths are ranked according to the average curvature radius, the path cost, the distance length, the number of straight roads and the number of curves, and a comprehensive weight calculation is performed based on the ranking values ​​to select the real-time planned path with the smallest total value as the optimal path.

8. The tracking trajectory analysis and selection terminal according to claim 7, characterized in that: The weight values ​​associated with each ranking value are determined in advance through the AHP hierarchical analysis method.

Images