A driving test scene navigation method based on a vector map and an intelligent network system

Abstract

The application relates to a driving test scene navigation method based on a vector map and an intelligent network connection system, which comprises the following steps: S1, constructing a dynamic global map; S2, subject data structural processing; S3, path planning optimization; and S4, driving test scene navigation implementation; the application provides a driving test scene navigation system based on a vector map and an intelligent network connection system, a dynamic global map is constructed by using a vector map and intelligent network connection vehicle data, the subjects of the vector map are subjected to data structuralization, and navigation is implemented by using a hybird A* algorithm. The method has point-point and point-subject path planning and navigation, is high in calculation efficiency, and is low in algorithm requirement.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of maps and planning, and particularly relates to a driving test scene navigation method based on a vector map and an intelligent network system. BACKGROUND

[0002] In the driving test scene, when the navigation system is used for test path planning, there are some deficiencies. First, when the navigation system processes complex road conditions and traffic rules, there may be cases of information update not in time or inaccurate path selection. This may cause the examinee to encounter unexpected traffic conditions during the test, thereby affecting the test results. Second, when the navigation system plans the path, it may not fully consider various unexpected situations in actual driving, such as temporary traffic control, road construction, etc., which will make the examinee lack the ability to respond to unexpected situations in actual driving. In addition, when the navigation system plans the path, it may rely too much on the preset route and ignore the flexible response ability that the examinee needs to have in actual driving. Therefore, in order to improve the accuracy and practicality of test path planning, it is necessary to further optimize the algorithm of the navigation system so that it can better adapt to the complex driving test scene.

[0003] In view of the particularity and complexity of the driving test environment, it is necessary to provide an innovative driving test scene navigation scheme. This scheme aims to significantly improve the operation convenience of the driving test robot by providing clear destination navigation for the trainee, thereby optimizing the learning experience of the trainee. Based on the accuracy of the site vector map and the dynamics of the real-time traffic information within the site, it is necessary to carefully plan the driving route of the training subject. Scientific and reasonable route arrangement is of great significance to improving the overall operation efficiency of the driving school. Not only can it reduce unnecessary waiting and congestion, but also can ensure the smoothness and safety of the training process, laying a solid foundation for the long-term development of the driving school. SUMMARY

[0004] In view of the limitations of the prior art, the present application proposes a driving test scene navigation method based on a vector map and an intelligent network system. This method provides clear destination navigation for the trainee, significantly improves the operation convenience of the driving test robot, and further optimizes the learning experience of the trainee. Considering the accuracy of the site vector map and the dynamics of the real-time traffic information within the site, it is particularly important to carefully plan the driving route of the training subject. A scientific and reasonable route arrangement is crucial to improving the overall operation efficiency of the driving school.

[0005] To achieve the above-mentioned purpose, the present application provides a driving test scene navigation method based on a vector map and an intelligent network system, which at least includes the following steps:

[0006] Step S1: Construct a dynamic global map; this process integrates vector maps and data transmitted by intelligent connected vehicles. First, a global static map is constructed using vector map data to present fixed road and terrain information; then, a global dynamic map is dynamically generated based on real-time data from intelligent connected V2X systems to reflect real-time road conditions and traffic situations.

[0007] Step S2: Subject data structuring; For the driving test subjects defined in the vector map, extract relevant subject information and organize the subject data in a structured manner according to the ROS message format standard;

[0008] Step S3: Path planning optimization; Using the hybridA* algorithm, calculate and generate the shortest or optimal path based on the vehicle's current position and the target point's position information;

[0009] Step S4: Driving test scenario navigation implementation; Based on the current vehicle location, global grid map, and the path planning results calculated in step S3, point-to-point and point-to-subject path planning is implemented.

[0010] Furthermore, in step S1, constructing a global static map using vector map data includes at least the following steps:

[0011] S11, Decoupling of Boundary Information; The vector map of the driving school scene mainly includes site boundary information and subject information. The site boundary information is mainly used to generate the map; The boundary information consists of a series of polylines, each polyline containing several points. Therefore, according to a certain resolution, the electronic fence is interpolated to generate a series of points with a high density; For the series of points generated after interpolation, a raster projection is performed to generate a global static raster map.

[0012] S12. Boundary information generates a raster map: The raster map is in the message format of ros's nav_mgs / OccupancgGrid, which mainly includes the map's length and width, map resolution, map origin pose, and the value of each raster point; the raster point indexes are numbered from left to right and from bottom to top, and the value of each index point is 0 to indicate that the grid point has no obstacles, and 100 to indicate that the grid point has obstacles.

[0013] The process of generating a raster map from a point cloud includes:

[0014] (1) Traverse all point cloud points and obtain the row and column indices of each point in the raster map:

[0015] mx=int((x-map_orin_x) / map_resolution);

[0016] my = int((y - map_orin_y) / map_resolution);

[0017] where (map_orin_x, map_orin_y) is the origin of the grid map, generally the minimum value of the x and y of the electronic fence interpolation point (xmin, ymin); map_resolution is the resolution of the map, which can be set by oneself;

[0018] (2) Calculate the index of the map

[0019] Index = my * map_width + mx;

[0020] where map_width is the width of the map, which is the value of (xmax-xmin) / map_resolution.

[0021] (3) Assign the value of 100 to the index of the map, indicating that there is an obstacle.

[0022] Further, the implementation method of the intelligent network V2X data generation global dynamic map in step S1 is:

[0023] The vehicle v2X device can receive vehicle information sent by other vehicles, including pose, vehicle size, speed and other information. In this case, the pose and vehicle size information of the vehicle are used to project the surrounding vehicles to the global static grid map to generate a dynamic global map associated with the vehicle networking data. The implementation steps are as follows:

[0024] Step S13, discretization of the single vehicle at the origin (0, 0) and the heading angle yaw = 0.0

[0025] The vehicle base_link coordinate system is at the center of the rear axle, and the vehicle planar size can be described by vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (rear axle to vehicle tail). The discretization process is as follows: (1) define the left, right, upper and lower boundaries of the vehicle; (2) discretize the horizontal and vertical directions;

[0026] Step S14, update the profile according to the vehicle position and attitude

[0027] For any point p(px, py) of the discretized profile, its new coordinates at any pose (x, y, yaw) are:

[0028] new_px = x + px * cos(yaw) - py * sin(yaw)

[0029] new_py = y + px * cos(yaw) - py * sin(yaw)

[0030] Step S15, discrete contour updates the grid map

[0031] According to the method of step S12, all vehicle contours are projected onto the static global map to generate a dynamic global map.

[0032] Further, in step S2, the subject data structure processing is implemented as follows: for the subject information of the vector map, relevant data is extracted, and the subject is structured according to the self-defined ROS message format; the self-defined structured message of the subject mainly includes subject type, subject code, subject point list, identification area code, identification area point list and index pose, and different subject types will be fine-tuned.

[0033] Further, the data structure of the warehouse site, the data structure of the side, the data structure of the S-bend, the data structure of the right-angle bend and the data structure of the ramp are included.

[0034] The data of the warehouse site structure mainly includes:

[0035] sub_type (fixed as 1);

[0036] sub_code (extracted from the ReverseParking "CODE" information of the vector map information);

[0037] sub_points (extracted from the ReverseParking->Point information of the vector map information, a total of 8 points); sub_iz_code (extracted from the ReverseParkingIdentZone "CODE" information of the vector map information); sub_iz_points (extracted from the ReverseParkingIdentZone->Point information, a total of 4 points); search_pose (the midpoint of points 0 and 7, and the heading is the normal of the straight line of points 0-7 inward); search_pose_ex (the midpoint of points 1 and 2, and the heading is the normal of the straight line of points 1-2 inward);

[0038] The data of the side structure mainly includes:

[0039] sub_type (fixed as 0);

[0040] sub_code (extracted from the ParallelParking "CODE" information of the vector map information);

[0041] sub_points (extracted from the ParallelParking->Point information of the vector map information, a total of 8 points);

[0042] sub_iz_code (extracted from ParallelParkingIdentZone "CODE" information of the vector map information);

[0043] sub_iz_points (extracted from ParallelParkingIdentZone->Point information, a total of 4 points); search_pose (the midpoint of point 0 and point 7, and the heading is the normal of the line of point 0-point 7 inward);

[0044] The data of the S-shaped structure mainly includes:

[0045] sub_type (fixed as 4);

[0046] sub_code (extracted from CurveDriving "CODE" information of the vector map information);

[0047] sub_iz_code (extracted from CurveDrivingIdentZone "CODE" information of the vector map information);

[0048] sub_iz_points (extracted from CurveDrivingIdentZone->Point information, a total of 4 points);

[0049] left_line_points (extracted from PointType Type="LeftLine"->Point, a number of);

[0050] right_line_points (extracted from PointType Type="RightLine"->Point, a number of);

[0051] search_pose (the midpoint of point 0 and point M, and the heading is the normal of the line of point 0-point M inward);

[0052] The data of the right-angle bend structure mainly includes:

[0053] sub_type (fixed as 2);

[0054] sub_code (extracted from QuarterTurn "CODE" information of the vector map information);

[0055] sub_points (extracted from QuarterTurn->Point information of the vector map information, a total of 6 points);

[0056] sub_iz_code (extracted from the QuarterTurnIdentZone "CODE" information of the vector map information) ;

[0057] sub_iz_points (extracted from the QuarterTurnIdentZone -> Point information, a total of 4 points) ; search_pose (on the straight line of points 0 and 5, at a distance of half a car width + 0.2 from point 5, and the heading is the normal of the straight line of point 0-point 5 inward) ;

[0058] The data of the ramp structure mainly includes:

[0059] sub_type (fixed as 3) ;

[0060] sub_code (extracted from the SlopeParking "CODE" information of the vector map information) ;

[0061] sub_points (extracted from the SlopeParking -> Point information of the vector map information, a total of 10 points) ;

[0062] sub_iz_code (extracted from the SlopeParkingIdentZone "CODE" information of the vector map information) ;

[0063] sub_iz_points (extracted from the SlopeParkingIdentZone -> Point information, a total of 4 points) ; search_pose (on the straight line of points 0 and 9, at a distance of half a car width + 0.5 from point 5, and the heading is the normal of the straight line of point 0-point 9 inward).

[0064] Further, in step S3, the path planning optimization implementation is as follows: the method combines the traditional A* algorithm and the sampling-optimization technology, and can efficiently search the path in a complex environment; the method includes the following:

[0065] S31, map modeling: according to the characteristics of the environment where the vehicle is located, the continuous space is discretized into a network, and a map for searching is constructed;

[0066] S32, heuristic function design: according to the actual situation, select an appropriate heuristic function to estimate the distance from each node to the target;

[0067] S33, A* algorithm search: according to the discretized map and the heuristic function, use A* to search the initial path;

[0068] S34, sampling and optimization: by sampling discrete points on the path and performing path optimization, a better path is obtained;

[0069] S35, trajectory generation: according to the optimized path, the trajectory of the vehicle is generated.

[0070] Further, in step S4, the driving test scene navigation implementation method is as follows:

[0071] According to the current pose, the global grid map and the hybrid A* algorithm, the point-point path planning and the point-subject path planning can be realized;

[0072] S41, point-point navigation

[0073] The HybirdA* algorithm can plan a path by subscribing to the current pose of the vehicle in the geometry_msgs / Pose format, the global grid map in the nav_mags / OccupancyGrid message format, and the target point pose in the geometry_msgs / Pose format, and can generate a planning path in the nav_msgs / Path message format, which can avoid obstacles on the map;

[0074] S42, subject navigation

[0075] The HybirdA* algorithm can generate a path to the corresponding subject by subscribing to the current pose of the vehicle in the geometry_msgs / Pose format, the global grid map in the nav_mags / OccupancyGrid message format, and the subject number and subject type, searching for the search_pose of the subject in the above structured subject information in the geometry_msgs / Pose format, which can be used for subject practice and parking.

[0076] The technical scheme of the application brings at least the following significant advantages:

[0077] 1. The patent innovatively proposes a driving test scene navigation system combining a vector map and an intelligent network system. The system successfully constructs a dynamic global map by integrating a vector map and intelligent network vehicle data, and implements fine data structured processing on the subjects in the vector map. On this basis, the system uses the hybirdA* algorithm to realize efficient and accurate path planning and navigation functions, supporting diversified navigation requirements from point to point and point to subject. This scheme not only has high calculation efficiency, but also has low demand for computing resources, showing its superiority in practical application.

[0078] 2.The application is committed to improving the operation convenience of the driving test robot, providing intuitive and clear destination navigation services for students. This improvement significantly optimizes the learning experience of students, enabling them to focus more on mastering driving skills. At the same time, given the high precision of the site vector map and the dynamic characteristics of real-time traffic information within the site, the application emphasizes careful planning and optimization of the driving route for the training subjects. A scientific and reasonable route arrangement is of great significance to improving the overall operational efficiency of the driving school and ensuring the smooth progress of the training process. BRIEF DESCRIPTION OF DRAWINGS

[0079] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, a brief introduction of the drawings needed to be used in the embodiments or prior art description will be given below. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor on the basis of these drawings.

[0080] Figure 1 is a flow chart of the driving test scene navigation method of the present application;

[0081] Figure 2 is a schematic diagram of the grid map of the present application;

[0082] Figure 3 is a schematic diagram of the point cloud generating grid map of the present application;

[0083] Figure 4 is a schematic diagram of the description of the reverse warehouse point;

[0084] Figure 5 is a schematic diagram of the description of the side point;

[0085] Figure 6 is a schematic diagram of the description of the S-bend point;

[0086] Figure 7 is a schematic diagram of the description of the right-angle bend point;

[0087] Figure 8 is a schematic diagram of the description of the slope point information;

[0088] Figure 9 is a schematic diagram of the point-point path planning of the present application;

[0089] Figure 10 is a schematic diagram of the subject path planning of the present application. DETAILED DESCRIPTION

[0090] The exemplary embodiments will be described in detail herein with reference to the attached drawings. In the following description, like reference numerals refer to like elements throughout the description. The following exemplary embodiments are described in the context of implementations described herein. These implementations are not intended to represent all implementations consistent with the present disclosure. Instead, they are merely examples consistent with some aspects of the present disclosure as detailed in the appended claims.

[0091] As shown in the embodiment, a driving test scene navigation method based on a vector map and an intelligent network system is provided, which at least includes the following steps: Figure 1

[0092] Step S1: Construct a dynamic global map; this process integrates vector map data and data transmitted by intelligent network connected vehicles. First, use vector map data to construct a global static map to present fixed road and terrain information; then, based on intelligent network connected V2X real-time data, dynamically generate a global dynamic map to reflect real-time road conditions and traffic conditions;

[0093] Intelligent network connected V2X data refers to data collected and transmitted through communication technology between vehicles and everything (Vehicle to Everything, V2X). V2X includes vehicle-to-vehicle (V2V), vehicle-to-pedestrian (V2P), vehicle-to-infrastructure (V2I), and vehicle-to-network (V2N) communication. These data can be used to enhance road safety, optimize traffic flow, and provide vehicle information services. In short, intelligent network connected V2X data is data exchanged between vehicles and the outside world.

[0094] Step S2: Subject data structured processing; for the driving test subjects defined in the vector map, extract relevant subject information and perform structured data processing according to the ROS message format standard;

[0095] Step S3: Path planning optimization; use the hybirdA* algorithm to calculate and generate a shortest or optimal path based on the current position of the vehicle and the position information of the target point;

[0096] Step S4: Driving test scene navigation implementation; based on the current vehicle position, the global grid map, and the path planning result calculated in step S3, implement point-to-point and point-to-subject path planning.

[0097] 1: Generation of global map

[0098] 1.1: Vector map data generates a global static map

[0099] 1.1.1 Boundary information discretization

[0100] ​The vector map of the driving school scene mainly includes site boundary information and subject information. Here, the site boundary information is mainly used for map generation.

[0101] The boundary information is composed of a series of polylines, each of which contains a plurality of points. Therefore, according to a certain resolution, the electronic fence is interpolated to generate a series of points with high density.

[0102] The interpolation method is as follows:

[0103] (1) For each point of a line, in the order of front and back, (xn, yn), (xn+1, yn+1), a series of points are linearly inserted in the middle according to a fixed length (such as 0.1).

[0104] (2) Calculate the distance between two points dis = sqrt((xn+1-xn)**2+(yn+1-yn)**2)

[0105] (3) Calculate the number of points to be generated n = 1 + dis / resolution (such as 0.1)

[0106] (4) Linear interpolation for(i = 0; i < n; i++)

[0107] x = xn+(xn+1-xn)*i / n-1;

[0108] y = yn+(yn+1-yn)*i / n-1;

[0109] }

[0110] For a series of points generated after interpolation, perform raster projection to generate a global static raster map.

[0111] 1.1.2 Boundary information generates a raster map

[0112] As shown in Figure 2 , the raster map is the message format of ros's nav_mgs / OccupancgGrid, mainly including map length and width, map resolution, map origin pose, and the value of each grid point. The index of the grid point is numbered from left to right and from bottom to top. The value of each index point is 0, indicating that the grid point has no obstacle, and 100, indicating that the grid point has an obstacle.

[0113] As shown in Figure 3 , the process of generating a raster map from point cloud includes:

[0114] (1) Traverse all point cloud points to obtain the row index and column index of each point in the raster map:

[0115] mx = int((x-map_orin_x) / map_resolution);

[0116] my = int((y - map_orin_y) / map_resolution);

[0117] Where (map_orin_x, map_orin_y) is the origin of the grid map, generally the minimum value of x and y of the fence interpolation point (xmin, ymin); map_resolution is the resolution of the map, which can be set by yourself.

[0118] (2) Calculate the index of the map

[0119] Index = my * map_width + mx;

[0120] Where map_width is the width of the map, which is the value of (xmax-xmin) / map_resolution.

[0121] (3) Assign the value of 100 to the index of the map, indicating that there is an obstacle.

[0122] 1.2: Intelligent network connection V2X data generates a global dynamic map

[0123] The vehicle v2x device can receive vehicle information sent by other vehicles, including pose, vehicle size, speed and other information, and in this place, the pose, vehicle size information and other information of the vehicle are used to project the surrounding vehicles to the global static grid map, and generate a dynamic global map associated with the vehicle networking data.

[0124] 1.2.1: Discretization of the single vehicle profile at the origin (0, 0) and heading angle yaw = 0.0

[0125] The vehicle base_link coordinate system is at the center of the rear axle, so the vehicle planar size can be described by vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (rear axle to vehicle tail). Therefore, the discretization process is as follows:

[0126] Define the left, right, top and bottom boundaries of the vehicle:

[0127] Left = -1.0 * baseLink2Back;

[0128] Right = vehicleLength - baseLink2Back;

[0129] Top = vehicleWidth / 2.0;

[0130] Bottom = -1.0 * vehicleWidth / 2.0;

[0131] (1) Discretize the horizontal and vertical directions:

[0132] The pseudo code is as follows:

[0133]

[0134] pointX = pointX + resolution

[0135] Where resolution is generally slightly smaller than the resolution of the map.

[0136] 1.2.2 Update the contour according to the vehicle position and attitude

[0137] For any point p (px, py) of the discrete contour, its new coordinates at any pose (x, y, yaw) are:

[0138] new_px = x + px * cos(yaw) - py * sin(yaw)

[0139] new_py = y + px * cos(yaw) - py * sin(yaw)

[0140] 1.2.3 Update the discrete contour to the grid map

[0141] According to the method of 1.1.2, project all vehicle contours onto the static global map to generate a dynamic global map.

[0142] 2: Data structure of the subject

[0143] For the subject information of the vector map, extract the relevant data and structure the subject according to the self-defined ROS message format. The self-defined structured message of the subject mainly includes subject type, subject code, subject point chain table, identification area code, identification area point chain table, index pose, etc. For different subject types, there will be slight adjustments.

[0144] 2.1: Data structure of the library location

[0145] As shown in Figure 4 , the structured data of the reverse warehouse mainly includes:

[0146] sub_type (fixed as 1);

[0147] sub_code (extracted from the ReverseParking "CODE" information of the vector map information);

[0148] sub_points (extracted from the ReverseParking->Point information of the vector map information, a total of 8 points); sub_iz_code (extracted from the ReverseParkingIdentZone "CODE" information of the vector map information); sub_iz_points (extracted from the ReverseParkingIdentZone->Point information, a total of 4 points); search_pose (the midpoint of point 0 and point 7, and the heading is the normal of the line of point 0-point 7 inward); search_pose_ex (the midpoint of point 1 and point 2, and the heading is the normal of the line of point 1-point 2 inward);

[0149] 2.2: Data structuring of the side

[0150] As shown in Figure 5 , the data of the side structuring mainly includes:

[0151] sub_type (fixed as 0);

[0152] sub_code (extracted from the ParallelParking "CODE" information of the vector map information);

[0153] sub_points (extracted from the ParallelParking->Point information of the vector map information, a total of 8 points); sub_iz_code (extracted from the ParallelParkingIdentZone "CODE" information of the vector map information); sub_iz_points (extracted from the ParallelParkingIdentZone->Point information, a total of 4 points); search_pose (the midpoint of point 0 and point 7, and the heading is the normal of the line of point 0-point 7 inward).

[0154] 2.3: Data structuring of the S-bend

[0155] As shown in Figure 6 , the data of the S-bend structuring mainly includes:

[0156] sub_type (fixed as 4);

[0157] sub_code (extracted from the CurveDriving "CODE" information of the vector map information);

[0158] sub iz code (extracted from the CurveDrivingIdentZone "CODE" information of the vector map information); sub iz points (extracted from the CurveDrivingIdentZone -> Point information, a total of 4 points); search pose (the midpoint of point 0 and point M, with the heading being the normal of the straight line of point 0-point M inward).

[0159] left line points (extracted from PointType Type="LeftLine" -> Point, a number of points); right line points (extracted from PointType Type="RightLine" -> Point, a number of points); search pose (the midpoint of point 0 and point M, with the heading being the normal of the straight line of point 0-point M inward).

[0160] left line points (extracted from PointType Type="LeftLine" -> Point, a number of points); right line points (extracted from PointType Type="RightLine" -> Point, a number of points); search pose (the midpoint of point 0 and point M, with the heading being the normal of the straight line of point 0-point M inward).

[0161] 2.4: Data structuring of right-angle bend

[0162] As shown in the right-angle bend structure, the data mainly includes: Figure 7

[0163] sub type (fixed as 2); sub code (extracted from the QuarterTurn "CODE" information of the vector map information); sub points (extracted from the QuarterTurn -> Point information of the vector map information, a total of 6 points); search pose (on the straight line of point 0 and point 5, at a distance of half a car width + 0.2 from point 5, with the heading being the normal of the straight line of point 0-point 5 inward).

[0164]

[0165] sub iz code (extracted from the QuarterTurnIdentZone "CODE" information of the vector map information); sub iz points (extracted from the QuarterTurnIdentZone -> Point information, a total of 4 points); search pose (on the straight line of point 0 and point 5, at a distance of half a car width + 0.2 from point 5, with the heading being the normal of the straight line of point 0-point 5 inward).

[0166] 2.5: Data structuring of slope

[0167] As shown in the slope structure, the data mainly includes:

[0168] sub type (fixed as 3); sub code (extracted from the SlopeParking "CODE" information of the vector map information). Figure 8

[0169]

[0170] sub type (fixed as 3); sub code (extracted from the SlopeParking "CODE" information of the vector map information). ​​​​

[0171] sub_points (extracted from the SlopeParking->Point information of the vector map information, a total of 10 points); sub_iz_code (extracted from the SlopeParkingIdentZone "CODE" information of the vector map information); sub_iz_points (extracted from the SlopeParkingIdentZone->Point information, a total of 4 points); search_pose (on the straight line of points 0 and 9, half a car width + 0.5 away from point 5, and the heading is the normal of the straight line of points 0-9 inward).

[0172] 3: Introduction of hybrid A* path planning

[0173] Hybrid A* is a widely used efficient path planning method in autonomous driving and intelligent navigation systems, which aims to calculate the shortest path or optimal path according to the current pose of the vehicle and the pose of the target point. This method combines traditional A* algorithm and sampling-optimization technology, which can efficiently search for paths in complex environments. This method includes 5 parts:

[0174] (1) Map modeling: according to the characteristics of the environment where the vehicle is located, the continuous space is discretized into a network, and a map for searching is constructed;

[0175] (2) Heuristic function design: according to the actual situation, select an appropriate heuristic function to estimate the distance from each node to the target;

[0176] (3) A* algorithm search: according to the discretized map and heuristic function, use A* to search the initial path;

[0177] (4) Sampling and optimization: by sampling discrete points on the path and optimizing the path, a better path is obtained;

[0178] (5) Trajectory generation: according to the optimized path, generate the trajectory of the vehicle.

[0179] 4: Navigation function:

[0180] The system can realize point-point path planning and point-goal path planning according to the current pose, global grid map and hybrid A* algorithm.

[0181] 4.1: Point-point navigation

[0182] For example Figure 9As shown, the HybirdA* algorithm can plan a path by subscribing to the current pose of the vehicle (geometry_msgs / Pose format), the global grid map (nav_mags / OccupancyGrid message format), and the target point pose (geometry_msgs / Pose format), and generate a planned path (nav_msgs / Path message format) that can avoid obstacles on the map.

[0183] 4.2: Subject navigation

[0184] As shown, the HybirdA* algorithm can plan a path by subscribing to the current pose of the vehicle (geometry_msgs / Pose format), the global grid map (nav_mags / OccupancyGrid message format), and the target point pose (geometry_msgs / Pose format), and generate a planned path (nav_msgs / Path message format) that can avoid obstacles on the map. Figure 10 As shown, the HybirdA* algorithm can plan a path by subscribing to the current pose of the vehicle (geometry_msgs / Pose format), the global grid map (nav_mags / OccupancyGrid message format), and the target point pose (geometry_msgs / Pose format), and generate a planned path (nav_msgs / Path message format) that can avoid obstacles on the map.

[0185] The present patent provides a driving test scene navigation system based on a vector map and an intelligent network connection system, which constructs a dynamic global map using a vector map and intelligent network connected vehicle data, structures the subject data of the vector map, and realizes navigation using a hybirdA* algorithm. This method has point-point and point-subject path planning and navigation, high calculation efficiency, and low computational power requirements.

[0186] Although the embodiments of the present application have been shown and described above, it should be understood that the above-described embodiments are exemplary and should not be construed as limiting the present application, and those of ordinary skill in the art can make changes, modifications, replacements, and variations to the above-described embodiments within the scope of the present application.

Claims

1. A driving test scene navigation method based on a vector map and an intelligent network system, characterized in that: It at least includes the following steps: Step S1: Constructing a dynamic global map; this process fuses vector maps and data transmitted by intelligent connected vehicles, first, using vector map data to construct a global static map to present fixed road and terrain information; then, based on intelligent connected V2X real-time data, a dynamic global map is dynamically generated to reflect real-time traffic conditions and traffic conditions; Step S2: Subject data structured processing; for the driving test subjects defined in the vector map, relevant subject information is extracted, and the subject is structured and data is arranged according to the standard of ROS message format; Step S3: Path planning optimization; using the hybird A* algorithm, a shortest or optimal path is calculated and generated according to the current position of the vehicle and the position information of the target point; Step S4: Realize the navigation of driving test scene; Based on the current vehicle position, dynamic global map and path planning result calculated in step S3, point-to-point and point-to-subject navigation is realized; In step S1, a global static map is constructed using vector map data, which at least includes the following steps: S11, boundary information discretization; the vector map of the driving school scene mainly includes site boundary information and subject information, and the site boundary information is mainly used for map generation; the boundary information is composed of a series of polylines, and each polyline contains a plurality of points, so a series of points with high density are generated by interpolating the electronic fence according to a certain resolution; for the series of points generated after interpolation, grid projection is performed to generate a global static map; S12, boundary information generates a grid map: the grid map is in the message format of ros's nav_mgs / OccupancgGrid, mainly including map length and width, map resolution, map origin pose and the value of each grid point; the index of the grid point is numbered from left to right and from bottom to top, and the value of each index point is 0, indicating that the grid point has no obstacle, and 100, indicating that the grid point has an obstacle; The process of generating a grid map from point cloud includes: (1) Traverse all the point cloud points to obtain the row index and column index of each point in the grid map: mx=int((x-map_orin_x) / map_resolution); my=int((y-map_orin_y) / map_resolution); Where (map_orin_x, map_orin_y) is the origin of the grid map, which is generally the minimum value (xmin, ymin) of the x and y of the interpolation points of the electronic fence; map_resolution is the resolution of the map, which can be set by yourself; (2) Calculate the index of the map: Index=my*map_width+mx; Where map_width is the width of the map, which is the value of (xmax-xmin) / map_resolution, (3) Assign the value of the index of the map to 100, indicating that there is an obstacle: In step S1, the method for generating a dynamic global map from intelligent connected V2X data is: The vehicle v2x device can receive vehicle information sent by other vehicles, including pose, vehicle size, speed and other information, and the pose and vehicle size information of the vehicle are used to project the surrounding vehicles to the global static map to generate a dynamic global map associated with the vehicle networking data. The implementation steps are as follows: Step S13, discretization of the single vehicle profile at the origin (0, 0) and the heading angle yaw = 0.0 The vehicle base_link coordinate system is at the center of the rear axle, and the vehicle planar size can be described by vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (rear axle to vehicle tail). The discretization process is as follows: (1) define the left, right, upper and lower boundaries of the vehicle; (2) discretize the horizontal and vertical directions; Step S14, profile update according to vehicle position and attitude For any point p (px, py) of the discretized profile, the new coordinates at any pose (x, y, yaw) are as follows: new_px = x + px*cos(yaw) - py*sin(yaw) new_py = y + px*sin(yaw) + py*cos(yaw) Step S15, discretized profile updates the grid map According to the method of step S12, all vehicle profiles are projected onto the static global map to generate a dynamic global map; In step S2, the subject data structure processing implementation is as follows: for the subject information of the vector map, relevant data is extracted, and the subject is structured according to the self-defined ROS message format; the self-defined structured message of the subject mainly includes subject type, subject code, subject point chain table, identification area code, identification area point chain table and index pose, and different subject types will be fine-tuned.

2. The method of claim 1, wherein: In step S3, the path planning optimization implementation is as follows: this method combines traditional A* algorithm and sampling-optimization technology, and can efficiently search the path in complex environment; this method includes the following: S31, map modeling: according to the characteristics of the environment where the vehicle is located, the continuous space is discretized into a network, and a map for searching is constructed; S32, heuristic function design: according to the actual situation, select appropriate heuristic function to estimate the distance from each node to the target; S33, A* algorithm search: according to the discretized map and heuristic function, use A* to search the initial path; S34, sampling and optimization: by sampling discrete points on the path and optimizing the path, a better path is obtained; S35, trajectory generation: according to the optimized path, the trajectory of the vehicle is generated.

3. The method of claim 2, wherein: In step S4, the driving test scene navigation implementation method is as follows: According to the current pose, dynamic global map and hybrid A* algorithm, point-point path planning and point-subject path planning can be realized. S41, point-point navigation The hybrid A* algorithm can plan a path by subscribing to a current pose of the vehicle in geometry_msgs / Pose format, a dynamic global map in nav_mags / OccupancyGrid message format, and a target point pose in geometry_msgs / Pose format, generate a planned path in nav_mags / Path message format, and the path can avoid obstacles on the map.

Images