Driving test scene navigation method based on vector map and intelligent network connection system

Abstract

The invention relates to a driving test scene navigation method based on a vector map and an intelligent network connection system. The method comprises the following steps: S1, constructing a dynamic global map; s2, subject data structural processing is carried out; s3, path planning optimization is carried out; s4, driving test scene navigation is realized; the invention 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 the vector map and vehicle data of intelligent network connection, data structuring is performed on subjects of the vector map, and navigation is realized by using a hybisdA * algorithm. The method has the advantages of point-point and point-subject path planning and navigation, high calculation efficiency and low requirement on calculation power.

Description

Technical Field

[0001] The present invention belongs to the field of map and planning technology, and specifically relates to a driving test scene navigation method based on vector map and intelligent network connection system. Background Art

[0002] In the driving test scenario, there are some shortcomings when the navigation system is used for scientific examination route planning. First, when dealing with complex road conditions and traffic rules, the navigation system may not update information in a timely manner or select inaccurate routes. This may cause candidates to encounter unexpected traffic conditions during the test, thus affecting the test results. Secondly, when planning the route, the navigation system may not fully consider various emergencies in actual driving, such as temporary traffic control, road construction, etc., which will make candidates lack the ability to deal with emergencies in actual driving. In addition, when planning the route, the navigation system may rely too much on the preset route and ignore the flexible adaptability that candidates need to have in actual driving. Therefore, in order to improve the accuracy and practicality of scientific examination route planning, it is necessary to further optimize the algorithm of the navigation system so that it can better adapt to complex driving test scenarios.

[0003] In view of the particularity and complexity of the driving test environment, there is an urgent need to provide an innovative driving test scene navigation solution. This solution aims to optimize the students' learning experience by providing students with clear destination navigation and significantly improving the ease of operation of the driving test robot. Based on the accuracy of the site vector map and the dynamics of real-time traffic information in the site, it is necessary to carefully plan the driving routes of the training subjects. Scientific and reasonable route arrangements are of vital importance to improving the overall operational efficiency of driving schools. It can not only reduce unnecessary waiting and congestion, but also ensure the smoothness and safety of the training process, laying a solid foundation for the long-term development of driving schools. Summary of the invention

[0004] In view of the limitations of the prior art, the present invention proposes a driving test scene navigation method based on vector maps and intelligent network systems. This method provides students with clear destination navigation, significantly improves the operational convenience of the driving test robot, and thus optimizes the students' learning experience. Considering the accuracy of the site-based vector map and the dynamics of real-time traffic information in the site, it is particularly important to carefully plan the driving routes of the training subjects. A scientific and reasonable route arrangement is crucial to improving the overall operational efficiency of the driving school.

[0005] To achieve the above object, the present invention 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: Build a dynamic global map; this process integrates the vector map and the data transmitted by the intelligent networked vehicles. First, use the vector map data to build a global static map to present fixed road and terrain information; then, based on the intelligent networked V2X real-time data, dynamically generate a global dynamic map to reflect the real-time road and traffic conditions;

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

[0008] Step S3: Path planning optimization: using the hybridA* 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;

[0009] Step S4: Implementation of driving test scenario navigation; based on the current vehicle position, the 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, discretization of boundary information; the vector map of the driving school scene mainly includes the site boundary information and subject information, and the site boundary information is mainly used to generate the map; the boundary information is composed of a series of polylines, each polyline contains a number of points, so 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, grid projection is performed to generate a global static grid map;

[0012] S12. Generate a grid map from boundary information: The grid map is in the message format of nav_mgs / OccupancgGrid of ROS, which mainly includes the length and width of the map, the map resolution, the position of the map origin, and the value of each grid point. The indexes of the grid points are numbered from left to right and from bottom to top. The value of each index point is 0, indicating that there is no obstacle at the grid point, and 100, indicating that there is an obstacle at the grid point.

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

[0014] (1) Traverse all point cloud points and obtain the row index and column index of each point in the grid 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, which is generally the minimum x and y values ​​(xmin, ymin) of the electronic fence interpolation point; map_resolution is the resolution of the map, which can be set by yourself;

[0018] (2) Calculate the map index

[0019] Index = my * map_width + mx;

[0020] Where map_width is the width of the map, and its value is the interpolation point (xmax-xmin) / map_resolution.

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

[0022] Furthermore, in step S1, the method for generating a global dynamic map using intelligent networked V2X data is as follows:

[0023] The vehicle v2x device can receive vehicle information sent by other vehicles, including position, size, speed and other information. This part uses the vehicle's position, size and other information to project surrounding vehicles onto a global static grid map, generating a dynamic global map of associated Internet of Vehicles data. The implementation steps are as follows:

[0024] Step S13: Discretization profile of the bicycle at the origin (0,0) and the heading angle yaw = 0.0

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

[0026] Step S14: Update the contour according to the vehicle position and posture

[0027] For any point p(px,py) of the discrete contour, its new coordinates at any position (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 update 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] Furthermore, in step S2, the subject data structuring processing is implemented as follows: for the subject information of the vector map, relevant data is extracted, and the subjects are structured according to the customized ROS message format; the customized structured message of the subject mainly includes the subject type, subject code, subject point list, identification area code, identification area point list and index pose, and there will be fine-tuning for different subject types.

[0033] Furthermore, it includes data structuring of storage locations, data structuring of sides, data structuring of S-curves, data structuring of right-angle bends, and data structuring of ramps;

[0034] The structured data of storage locations mainly include:

[0035] sub_type (fixed to 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 point 0 and point 7, its heading is the normal of the line from point 0 to point 7 inward); search_pose_ex (the midpoint of point 1 and point 2, its heading is the normal of the line from point 1 to point 2 inward);

[0038] The side structured data mainly includes:

[0039] sub_type (fixed to 0);

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

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

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

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

[0044] The data of S-bend structure mainly include:

[0045] sub_type (fixed to 4);

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

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

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

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

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

[0051] search_pose (the midpoint between point 0 and point M, with its heading pointing inward from the normal of the line from point 0 to point M);

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

[0053] sub_type (fixed to 2);

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

[0055] sub_points (extracted from QuarterTurn->Point information of 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 QuarterTurnIdentZone->Point information, a total of 4 points); search_pose (on the straight line between point 0 and point 5, half the car width + 0.2 from point 5, its heading is the normal of the straight line from point 0 to point 5 pointing inward);

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

[0059] sub_type (fixed to 3);

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

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

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

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

[0064] Furthermore, in step S3, the path planning optimization is implemented as follows: This method combines the traditional A* algorithm and the sampling-optimization technique to efficiently search for paths in complex environments; the method includes the following:

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

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

[0067] S33, A* algorithm search: Based on the discretized map and heuristic function, use A* to search for 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: generating a vehicle trajectory according to the optimized path.

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

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

[0072] S41, point-to-point navigation

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

[0074] S42, Subject Navigation

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

[0076] The technical solution of the present invention brings at least the following significant advantages:

[0077] 1. This patent innovatively proposes a driving test scenario navigation system that combines vector maps and intelligent network systems. The system successfully constructs a dynamic global map by integrating vector maps and intelligent network vehicle data, and implements refined data structuring processing for subjects in the vector map. On this basis, the system adopts the hybridA* algorithm to achieve efficient and accurate path planning and navigation functions, supporting diversified navigation needs from point to point and from point to subject. This solution is not only computationally efficient, but also has low demand for computing resources, demonstrating its superiority in practical applications.

[0078] 2. The present invention is committed to improving the operational convenience of the driving test robot and providing students with an intuitive and clear destination navigation service. This improvement significantly optimizes the students' learning experience, allowing them to focus more on mastering driving skills. At the same time, given the high precision of the site vector map and the dynamic changes in real-time traffic information in the site, the present invention emphasizes the careful planning and optimization of the driving routes of the training subjects. A scientific and reasonable route arrangement is of vital importance to improving the overall operational efficiency of the driving school and ensuring the smooth progress of the training process. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

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

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

[0082] Figure 3 It is a schematic diagram of generating a raster map from a point cloud according to the present invention;

[0083] Figure 4 It is a schematic diagram describing the storage point of the present invention;

[0084] Figure 5 It is a schematic diagram for describing the side points of the present invention;

[0085] Figure 6 It is a schematic diagram describing the S-bend point position of the present invention;

[0086] Figure 7 It is a schematic diagram describing the right-angle bend point of the present invention;

[0087] Figure 8 It is a schematic diagram describing the ramp point information of the present invention;

[0088] Fig. 9 It is a schematic diagram of point-to-point path planning of the present invention;

[0089] Fig.10 It is a schematic diagram of subject path planning of the present invention. DETAILED DESCRIPTION

[0090] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. Instead, they are merely examples of devices and methods consistent with some aspects of the present invention as detailed in the appended claims.

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

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

[0093] Intelligent network V2X data refers to the data collected and transmitted through the communication technology between vehicles and everything (Vehicle to Everything, referred to as V2X). V2X includes communication between vehicles and vehicles (V2V), vehicles and pedestrians (V2P), vehicles and infrastructure (V2I) and vehicles and networks (V2N). This data can be used to enhance road safety, optimize traffic flow and provide in-vehicle information services. In short, intelligent network V2X data is the data that vehicles exchange information with the outside world.

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

[0095] Step S3: Path planning optimization: using the hybridA* 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: Implementation of driving test scenario navigation; based on the current vehicle position, the global grid map and the path planning results calculated in step S3, point-to-point and point-to-subject path planning is implemented.

[0097] 1: Generation of global map

[0098] 1.1: Generate global static map from vector map data

[0099] 1.1.1 Discretization of Boundary Information

[0100] The vector map of the driving school scene mainly includes site boundary information and subject information. The site boundary information is mainly used here to generate the map.

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

[0102] The interpolation method is:

[0103] (1) For each point on a line, in the order (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) The calculation needs to generate several points n = 1 + dis / resolution (for example 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, raster projection is performed to generate a global static raster map.

[0111] 1.1.2 Generate raster map from boundary information

[0112] like Figure 2 As shown in the figure, the grid map is in the message format of nav_mgs / OccupancgGrid of ros, which mainly includes the length and width of the map, the map resolution, the map origin position and the value of each grid point. The index of the grid points is numbered from left to right and from bottom to top. The value of each index point is 0, which means there is no obstacle at the grid point, and 100, which means there is an obstacle at the grid point.

[0113] like Figure 3 As shown in the figure, the process of generating raster maps from point clouds includes:

[0114] (1) Traverse all point cloud points and obtain the row index and column index of each point in the grid 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, which is generally the minimum x and y values ​​(xmin, ymin) of the electronic fence interpolation point; map_resolution is the resolution of the map, which can be set by yourself.

[0118] (2) Calculate the map index

[0119] Index = my * map_width + mx;

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

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

[0122] 1.2: Intelligent connected V2X data generates global dynamic map

[0123] The vehicle v2x device can receive vehicle information sent by other vehicles, including position, size, speed and other information. Here, the vehicle's position, size and other information are used to project surrounding vehicles onto a global static grid map to generate a dynamic global map of associated Internet of Vehicles data.

[0124] 1.2.1: Discretized outline of a bicycle at the origin (0,0) and with yaw = 0.0

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

[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:

[0133]

[0134] pointX=pointX+resolution

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

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

[0137] For any point p(px,py) of the discrete contour, its new coordinates at any position (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 Discrete Contour Update Grid Map

[0141] According to the method in 1.1.2, all vehicle outlines are projected onto the static global map to generate a dynamic global map.

[0142] 2: Data structuring of subjects

[0143] For the subject information of the vector map, relevant data is extracted and the subject is structured according to the custom ROS message format. The custom structured message of the subject mainly includes the subject type, subject code, subject point list, identification area code, identification area point list, index pose, etc., and there will be fine-tuning for different subject types.

[0144] 2.1: Data structuring of storage locations

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

[0146] sub_type (fixed to 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, its heading is the normal of the line from point 0 to point 7 inward); search_pose_ex (the midpoint of point 1 and point 2, its heading is the normal of the line from point 1 to point 2 inward);

[0149] 2.2: Lateral data structuring

[0150] like Figure 5 As shown, the side structured data mainly includes:

[0151] sub_type (fixed to 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, its heading is the normal of the straight line from point 0 to point 7 pointing inward).

[0154] 2.3: Data structuring of S-bend

[0155] like Figure 6 As shown, the S-bend structured data mainly includes:

[0156] sub_type (fixed to 4);

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

[0158] sub_iz_code (extracted from CurveDrivingIdentZone "CODE" information of vector map information); sub_iz_points (extracted from CurveDrivingIdentZone->Point information, 4 points in total);

[0159] left_line_points (extracted from PointType Type = "LeftLine"->Point, several);

[0160] right_line_points (extracted from PointType Type = "RightLine"->Point, several); search_pose (the midpoint between point 0 and point M, whose heading is the normal of the straight line from point 0 to point M pointing inward).

[0161] 2.4: Right-angle bend data structuring

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

[0163] sub_type (fixed to 2);

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

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

[0166] sub_iz_code (extracted from QuarterTurnIdentZone "CODE" information in vector map information); sub_iz_points (extracted from QuarterTurnIdentZone->Point information, 4 points in total); search_pose (on the straight line between point 0 and point 5, half the car width + 0.2 away from point 5, its heading is the normal of the straight line from point 0 to point 5 pointing inward).

[0167] 2.5: Data Structuring of Ramp

[0168] like Figure 8 As shown, the ramp structured data mainly includes:

[0169] sub_type (fixed to 3);

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

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

[0172] 3: Introduction to hybirdA* path planning

[0173] Hybrid A* is an efficient path planning method widely used in autonomous driving and intelligent navigation systems. Its goal is to calculate a shortest path or optimal path based on the current position of the vehicle and the position of the target point. This method combines the traditional A* algorithm and sampling-optimization technology to efficiently search for paths in complex environments. This method consists of 5 parts:

[0174] (1) Map modeling: Based on the characteristics of the vehicle’s environment, the continuous space is discretized into a network and a map is constructed for search.

[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: Based on the discretized map and heuristic function, use A* to search for the initial path;

[0177] (4) Sampling and optimization: By sampling discrete points on the path and performing path optimization, a better path can be obtained;

[0178] (5) Trajectory generation: Generate the vehicle trajectory based on the optimized path.

[0179] 4: Navigation function:

[0180] The system can realize point-to-point path planning and point-to-subject path planning based on the current posture, global grid map and hybrid A* algorithm.

[0181] 4.1: Point-to-point navigation

[0182] like Fig. 9As shown in the figure, the HybirdA* algorithm can perform path planning by subscribing to the current vehicle posture (geometry_msgs / Pose format), the global grid map (nav_mags / OccupancyGrid message format), and the target point posture (geometry_msgs / Pose format), and generate a planned path (nav_msgs / Path message format), which can avoid obstacles on the map.

[0183] 4.2: Subject Navigation

[0184] like Fig.10 As shown, the HybridA* algorithm subscribes to the vehicle's current pose (geometry_msgs / Pose format), the global grid map (nav_mags / OccupancyGrid message format), and the subject number and subject type, and searches for the search_pose (geometry_msgs / Pose format) of the subject from the above structured subject information. It can also generate a path to the corresponding subject, which can be used for subject practice and parking, etc.

[0185] This patent provides a driving test scenario navigation system based on vector maps and intelligent network systems, which uses vector maps and intelligent network vehicle data to build a dynamic global map, structures the data of the subjects in the vector map, and uses the hybridA* algorithm to achieve navigation. This method has point-to-point and point-to-subject path planning and navigation, with high computational efficiency and low computing power requirements.

[0186] Although the embodiments of the present invention have been shown and described above, it will be appreciated that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, substitute and vary the above embodiments within the scope of the present invention.

Claims

1. A driving test scene navigation method based on vector map and intelligent network system, characterized by: It at least includes the following steps: 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 the real-time data of intelligent connected V2X to reflect real-time road and traffic conditions; Step S2: Structural processing of subject data: extract relevant subject information for the driving test subjects defined in the vector map, and organize the subject data in a structured manner according to the standard of the ROS message format; Step S3: Path planning optimization: using the hybridA* 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; 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, point-to-point and point-to-target path planning is implemented.

2. The method according to claim 1, characterized in that: In step S1, constructing a global static map using vector map data includes at least the following steps: S11, discretization of boundary information; the vector map of the driving school scene mainly includes the site boundary information and subject information, and the site boundary information is mainly used to generate the map; the boundary information is composed of a series of polylines, each polyline contains a number of points, so 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, grid projection is performed to generate a global static grid map; S12. Generate a grid map from boundary information: The grid map is in the message format of nav_mgs / OccupancgGrid of ROS, which mainly includes the length and width of the map, the map resolution, the position of the map origin, and the value of each grid point. The indexes of the grid points are numbered from left to right and from bottom to top. The value of each index point is 0, indicating that there is no obstacle at the grid point, and 100, indicating that there is an obstacle at the grid point. The process of generating a raster map from a point cloud includes: (1) Traverse all point cloud points and 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 x and y values ​​(xmin, ymin) of the electronic fence interpolation point; map_resolution is the resolution of the map, which can be set by yourself; (2) Calculate the map index Index = my * map_width + mx; Where map_width is the width of the map, and its value is the interpolation point (xmax-xmin) / map_resolution. (3) The index of the map is assigned a value of 100, indicating that there is an obstacle.

3. The method according to claim 2, characterized in that: In step S1, the method for generating a global dynamic map using intelligent networked V2X data is also included as follows: The vehicle v2x device can receive vehicle information sent by other vehicles, including position, size, speed and other information. We use the vehicle's position, size and other information to project surrounding vehicles onto a global static grid map, generating a dynamic global map of associated Internet of Vehicles data. The implementation steps are as follows: Step S13: Discretization profile of the bicycle at the origin (0,0) and the heading angle yaw = 0.0 The base_link coordinate system of the vehicle is at the center of the rear axle. The plane dimensions of the vehicle can be described by three dimensions: vehicleLength (vehicle length), vehicleWidth (vehicle width), and baseLink2Back (from rear axle to rear of vehicle). The discretization process is as follows: (1) define the left, right, top, and bottom boundaries of the vehicle; (2) discretize the horizontal and vertical directions; Step S14: Update the contour according to the vehicle position and posture For any point p(px,py) of the discrete contour, its new coordinates at any position (x,y,yaw) are: new_px=x+px*cos(yaw)-py*sin(yaw) new_py=y+px*cos(yaw)-py*sin(yaw) Step S15: Discrete contour update grid map According to the method of step S12, all vehicle contours are projected onto the static global map to generate a dynamic global map.

4. The method according to claim 3, characterized in that: In step S2, the subject data structuring 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 customized ROS message format; the customized structured message of the subject mainly includes the subject type, subject code, subject point list, identification area code, identification area point list and index pose, and there will be fine-tuning for different subject types.

5. The method according to claim 4, characterized in that: Including data structuring of storage locations, data structuring of sides, data structuring of S-curves, data structuring of right-angle bends and data structuring of ramps; The structured data of storage locations mainly include: sub_type (fixed to 1); sub_code (extracted from the ReverseParking "CODE" information of the vector map information); 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, its heading is the normal of the line from point 0 to point 7 inward); search_pose_ex (the midpoint of point 1 and point 2, its heading is the normal of the line from point 1 to point 2 inward); The side structured data mainly includes: sub_type (fixed to 0); sub_code (extracted from the ParallelParking "CODE" information of the vector map information); sub_points (extracted from ParallelParking->Point information of vector map information, a total of 8 points); sub_iz_code (extracted from ParallelParkingIdentZone "CODE" information of vector map information); sub_iz_points (ParallelParkingIdentZone->Point information extracted, a total of 4 points); search_pose (the midpoint between point 0 and point 7, its heading is the normal of the straight line from point 0 to point 7 pointing inward); The data of S-bend structure mainly include: sub_type (fixed to 4); sub_code (extracted from CurveDriving "CODE" information of vector map information); sub_iz_code (extracted from CurveDrivingIdentZone "CODE" information of vector map information); sub_iz_points CurveDrivingIdentZone->Point information is extracted, a total of 4 points); left_line_points (extracted from PointType Type = "LeftLine"->Point, several); right_line_points (extracted from PointType Type = "RightLine"->Point, several); search_pose (the midpoint between point 0 and point M, with its heading pointing inward from the normal of the line from point 0 to point M); The data of right-angle bend structure mainly includes: sub_type (fixed to 2); sub_code (extracted from the QuarterTurn "CODE" information of the vector map information); sub_points (extracted from QuarterTurn->Point information of vector map information, a total of 6 points); sub_iz_code (extracted from the QuarterTurnIdentZone "CODE" information of the vector map information); sub_iz_points (extracted from QuarterTurnIdentZone->Point information, a total of 4 points); search_pose (on the straight line between point 0 and point 5, half the car width + 0.2 from point 5, its heading is the normal of the straight line from point 0 to point 5 pointing inward); The structured data of the ramp mainly includes: sub_type (fixed to 3); sub_code (extracted from SlopeParking "CODE" information of vector map information); sub_points (extracted from SlopeParking->Point information of vector map information, a total of 10 points); sub_iz_code (extracted from SlopeParkingIdentZone "CODE" information of vector map information); sub_iz_points (extracted from SlopeParkingIdentZone->Point information, a total of 4 points); search_pose (on the straight line between point 0 and point 9, half the car width + 0.5 away from point 5, and its heading is the normal of the straight line from point 0 to point 9 pointing inward).

6. The method according to claim 1, characterized in that: In step S3, the path planning optimization is implemented as follows: This method combines the traditional A* algorithm and sampling-optimization technology to efficiently search for paths in complex environments; the method includes the following: S31, map modeling: according to the characteristics of the vehicle's environment, the continuous space is discretized into a network and a map for search is constructed; S32, heuristic function design: according to the actual situation, select the appropriate heuristic function to estimate the distance from each node to the target; S33, A* algorithm search: Based on the discretized map and heuristic function, use A* to search for the initial path; S34, sampling and optimization: by sampling discrete points on the path and performing path optimization, a better path is obtained; S35, trajectory generation: generating a vehicle trajectory according to the optimized path.

7. The method according to any one of claims 1 to 6, characterized in that: In step S4, the driving test scene navigation is implemented as follows: According to the current position, global grid map and hybrid A* algorithm, point-to-point path planning and point-to-subject path planning can be realized; S41, point-to-point navigation The HybirdA* algorithm can perform path planning by subscribing to the vehicle's current pose in geometry_msgs / Pose format, the global grid map in nav_mags / OccupancyGrid message format, and the target point pose in geometry_msgs / Pose format, and generate a planned path in nav_msgs / Path message format, which can avoid obstacles on the map; S42, Subject Navigation The HybirdA* algorithm subscribes to the vehicle's current pose, which is in the geometry_msgs / Pose format, the global grid map, which is in the nav_mags / OccupancyGrid message format, and the subject number and subject type, and searches for the search_pose of the subject from the above structured subject information, which is in the geometry_msgs / Pose format. It can also generate a path to the corresponding subject, which can be used for subject practice and parking.

Images