An unmanned vehicle path planning decision system and method based on multi-feature map fusion

Abstract

This invention discloses an unmanned vehicle path planning and decision-making system and method based on multi-feature map fusion, relating to the field of autonomous driving road planning technology. It addresses the technical problem that existing single-feature map-based planning and decision-making methods cannot meet the requirements of special scenarios such as construction sites and mining areas where structured roads are defined but the environment is constantly changing and new areas requiring brief exploration may arise. The key technical points of this invention include: acquiring road point data and constructing structured roads based on a positioning and perception module; obtaining a raster map through 3D point cloud map conversion; and invoking the planning program based on the nearest points of the vehicle's current position and the structured roads, as well as the nearest points of the destination position and the structured roads. This invention, through modular encapsulation, can directly perform path planning on structured roads that conforms to vehicle dynamics characteristics, while simultaneously enabling rapid path planning for target points outside the road using A-satellite navigation. This improves the generalization of autonomous navigation under structured maps, solves the planning problem for target points outside structured roads, and achieves highly efficient, generalized navigation decision-making based on structured roads.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle path planning and decision-making technology, specifically to an unmanned vehicle path planning and decision-making system and method based on multi-feature map fusion. Background Technology

[0002] With the introduction and advancement of Industry 4.0, the manufacturing industry has ushered in a new era of digital transformation. Simultaneously, breakthroughs in core technologies such as robotics and artificial intelligence are bringing about significant changes to the automotive industry, making driving increasingly intelligent. Autonomous driving technology, as one of the future development directions of the automotive industry, has received widespread attention and made substantial progress in recent years.

[0003] In autonomous driving technology, vehicle path planning is crucial. Its main function is to plan a conflict-free sequence of movement for the vehicle given a starting point and destination in the environment. In traditional autonomous driving technology, key path planning techniques fall into two main categories: one is selecting the optimal path using pre-recorded trajectories, primarily through methods such as using trajectories in high-precision HDMap maps or stitching together path trajectory points; the other is using pre-created grid maps for path planning. However, in scenarios such as construction sites and outdoor environments where new areas need to be inspected or explored at any time, pre-recorded, single-structured trajectories require frequent changes, impacting work efficiency. Furthermore, these scenarios involve shared human and vehicle traffic, necessitating the fixed nature of autonomous vehicle routes. Therefore, relying solely on grid maps can lead to significant randomness in autonomous vehicle path planning.

[0004] The prior art, document number CN115509223A, discloses a path planning method for unmanned vehicles in mines, addressing the technical problems of low path planning accuracy and path planning failure when positioning signals are poor. The method includes: receiving positioning data and obstacle map data in the mine in real time; determining whether the positioning data is abnormal and generating judgment information; and selecting an algorithm module. The algorithm module includes a main algorithm module and a backup algorithm module. The positioning data and obstacle map data are input into the algorithm module for path planning to obtain a path planning message. The path planning message is smoothed using a Bezier curve and then transmitted to the actuator of the unmanned vehicle. This solves the problem that even when the unmanned vehicle loses positioning information in a mine environment, it can still perform vehicle path planning based on the backup algorithm module, thereby preventing vehicle accidents or loss of control and improving system stability and environmental adaptability. However, this prior art does not consider special scenarios such as construction sites and mining areas where the surrounding environment is constantly changing and new areas need to be explored.

[0005] Therefore, for special scenarios such as construction sites and mining areas where there are defined structured roads but the surrounding environment is constantly changing and there are new areas that need to be explored, existing unmanned vehicle path planning and decision-making methods based on single feature maps cannot meet the requirements of specific scenarios. Summary of the Invention

[0006] To address the issue that existing autonomous vehicle (RV) path planning and decision-making systems based on single-feature maps cannot meet the requirements of specific scenarios such as construction sites and mining areas where structured roads are defined but new areas with rapidly evolving environments require exploration, this invention proposes an RV path planning and decision-making system and method based on multi-feature map fusion to meet the RV path planning requirements for scenarios such as construction sites and mining areas.

[0007] According to one aspect of the present invention, an unmanned vehicle path planning and decision-making system based on multi-feature map fusion is proposed, the system comprising:

[0008] The positioning and perception module is used to achieve rapid mapping and accurate positioning in complex scenarios that blend indoor and outdoor environments, such as construction sites.

[0009] The road point data acquisition and structured road construction module is used to acquire road point data based on the real-time location information of the unmanned vehicle obtained by the positioning and perception module, and to construct structured roads by designing road IDs, preceding and succeeding road numbers, and road width information.

[0010] The 3D point cloud to raster map module is used to perform pass-through filtering and radius filtering on the 3D point cloud map constructed by the positioning and perception module to obtain a downsampled sparse point cloud map, which is then projected to obtain a 2D point cloud map. Finally, the coordinates of the projected 2D point cloud are mapped to a raster map with a fixed resolution, and the status (occupied and idle) of each grid is updated according to the projected point cloud information.

[0011] The coordinate unification module is used to statically unify the coordinates of the created structured roads and the converted raster map. It uses coordinate transformation to ensure that the starting point of the structured roads created based on the positioning and sensing module is consistent with the source point of the raster map. At the start of mapping, the vehicle's heading is... The positive axis direction, perpendicular to the vehicle body and satisfying the right-hand coordinate system, is the orientation. Positive direction of the axis.

[0012] The path planning decision module based on multi-feature map fusion is used to define thresholds for the distance between the vehicle's current position and the nearest point on the structured road, as well as the distance between the destination position and the nearest point on the road. If the threshold is exceeded, the excess part is improved by A-star planning. At the same time, the shortest path planning based on the structured road is performed between the point on the structured road closest to the vehicle and the point on the end point. The road trajectory points obtained by the improved A-star and the planned path on the structured road are stitched together and published.

[0013] The shortest path planning module based on structured roads is used to construct directed edges based on the structured roads created in the structured road construction module, and then apply the shortest path planning based on the dynamic programming optimization algorithm.

[0014] The A-Star path planning module based on grid maps is used to improve A-Star path planning, mainly by improving the overall evaluation cost function, adding obstacle distance cost to avoid the shortest path being close to obstacles, and setting grid neighborhood selection to allow diagonal movement in the grid map.

[0015] The path visualization module is used to highlight the final road trajectory information published in the path planning and decision-making system in the multi-feature map that merges the raster map and structured roads on the output terminal.

[0016] According to another aspect of the present invention, an autonomous vehicle path planning and decision-making method based on multi-feature map fusion is proposed, the method comprising the following steps:

[0017] Step 1: Use tightly coupled iterative Kalman filtering to process the data from the lidar and IMU to achieve real-time mapping and localization;

[0018] Step 2: The road point data collection and structured road construction module collects road point data based on the real-time location information of the unmanned vehicle obtained by the positioning and perception module, and constructs structured roads by designing road IDs, preceding and succeeding road numbers, and road width information.

[0019] Step 3: The 3D point cloud map to raster map module performs pass-through filtering and radius filtering on the 3D point cloud map constructed by the positioning and perception module to obtain a downsampled sparse point cloud map. Then, the sparse point cloud map is projected to obtain a two-dimensional point cloud map. Finally, the coordinates of the projected two-dimensional point cloud are mapped to a raster map with a fixed resolution. At the same time, the status (occupied and idle) of each grid is updated according to the projected point cloud information.

[0020] Step 4: The coordinate unification module performs static coordinate system unification between the created structured road and the converted raster map. Coordinate transformation is used to ensure that the starting point of the structured road created based on the positioning and sensing module is consistent with the source point of the raster map. At the start of mapping, the vehicle's facing direction is... The positive axis direction, perpendicular to the vehicle body and satisfying the right-hand coordinate system, is the orientation. Positive direction of the axis.

[0021] Step 5: The path planning decision module based on multi-feature map fusion defines thresholds for the distance between the vehicle's current position and the nearest point on the structured road, as well as the distance between the destination position and the nearest point on the road. If the thresholds are exceeded, the excess part is subjected to improved A-star path planning based on the grid map. At the same time, the point in the structured road closest to the vehicle and the point closest to the destination are subjected to shortest path planning based on the structured road. The road trajectory points obtained by the improved A-star are stitched together with the planned path in the structured road and published.

[0022] Step 6: Highlight the final road trajectory information published in the path planning decision system in the multi-feature map that merges the grid map and structured road on the output terminal.

[0023] Furthermore, step two includes the following steps:

[0024] S21. The road point data acquisition and structured road construction module reads the real-time position information of the unmanned vehicle obtained by the positioning and perception module, that is, after determining the origin, the current coordinates of the unmanned vehicle relative to the origin. , and quaternion information ;

[0025] S22. The quaternion information of the autonomous vehicle fed back by the positioning and perception module. The following coordinates are converted into the heading angle information required for structured road construction;

[0026] (1)

[0027] S23. The location of the unmanned vehicle at this moment, as initially collected. , and quaternion information The real-time converted heading angle information is recorded in a txt file, and the driving mileage of the unmanned vehicle is calculated in real time. If the cumulative driving mileage exceeds the set threshold, waypoint information is recorded, and the recorded driving mileage is cleared to zero.

[0028] S24. Construct a structured road. After collecting road point data, design the road ID, the preceding and succeeding road numbers, and the road width information in a txt file, as shown in the following formula.

[0029] (2)

[0030] Each road trajectory consists of a series of... waypoints and road connection information Composition, in which It is the coordinate position. These are the left and right boundary widths of the current road, respectively. These are the road connection number at the current road's starting point and the road number at the road's ending point, respectively.

[0031] Furthermore, step three includes the following steps:

[0032] S31, the 3D point cloud map to raster map module first processes the 3D point cloud map constructed based on the positioning and perception module using a pass-through filter, selecting... From 0.05 on the axis Up to 1.2 The point cloud data, this height is consistent with the height information of unmanned transport vehicles in construction scenarios such as construction sites, and only the point cloud data within this range in the coordinate system is retained;

[0033] S32. Apply radius filtering to the filtered point cloud image to remove outliers. Set the radius of the radius filter to 0.5. The number of threshold points is set to 5;

[0034] S33. Perform downsampling voxel filtering on the point cloud image after pass-through filtering and radius filtering to reduce the number of points without destroying the point cloud geometry. Replace the point cloud squares in the three-dimensional space with centroid point clouds.

[0035] S34. Project the sparse point cloud map to obtain a two-dimensional point cloud map. Finally, map the coordinates of the projected two-dimensional point cloud onto a grid map with a fixed resolution. At the same time, update the state (occupied and idle) of each grid according to the projected point cloud information.

[0036] Furthermore, step five includes the following steps:

[0037] S51. Obtain the floor values ​​in the parameter server in real time through timer interrupt, perform structured road reading operations on each floor, and store the position of each point on each road in vector form.

[0038] S52. Obtain task instructions and destination location information in real time through the subscriber. , and heading angle information The endpoint location information is traversed with the path location information in the read structured road to determine the structured road point closest to the endpoint. If the distance between the two points is more than four meters, the structured road point is taken as the starting point, the obtained endpoint location is the endpoint, the improved A-star path planning is called, and this point is set as the marker that the endpoint can receive the A-star planned trajectory.

[0039] S53. The vehicle's real-time location information obtained by the positioning and perception module is traversed with the path location information in the structured road to determine the structured road point closest to the vehicle's real-time location. If the distance between the two points exceeds four meters, the improved A-star path planning is called with the vehicle's current location as the starting point and the structured road point closest to the vehicle's real-time location as the ending point.

[0040] S54. The road point information planned by the obtained A-star path planning is judged by the receiver, so that the receiving optimization process is only optimized once in one planning cycle to avoid repeated receiving and sending, and the received trajectory is subjected to outlier removal and low-pass filtering.

[0041] S55. Using the structured road point closest to the vehicle's real-time location as the starting point and the structured road point closest to the destination information as the destination, call the shortest path planning module based on structured roads, use dynamic programming optimization algorithm to perform shortest path planning on the constructed structured roads, and store the planned path information in the vector of track_road.

[0042] S56. Based on the two segments of A-star planned trajectory and the planned trajectory based on structured roads, the marker position is determined and fused. If neither the A-star planned trajectory at the starting point nor the A-star planned trajectory at the ending point is published, the planned trajectory based on structured roads is published directly. If the A-star planned trajectory at the starting point or the ending point is published, the A-star received trajectory marker position needs to be determined. If the corresponding planned trajectory has been received, the trajectory is spliced. The A-star planned trajectory at the starting point needs to be inserted before the structured trajectory, and the trajectory at the ending point needs to be inserted after the structured trajectory before the complete trajectory is published.

[0043] S57. The final road trajectory information will be highlighted in the multi-feature map fused with the grid map and structured road data on the output terminal.

[0044] Furthermore, when using the improved A-star plan for path planning outside of structured roads, the map on which the planning depends is expanded. This prevents randomly placed obstacles of varying heights from being filtered out during raster map processing, which could cause collisions, especially in scenarios like construction sites. Therefore, an expansion layer is designed to expand obstacles, primarily through morphological expansion. The following formula illustrates this:

[0045] (3)

[0046] The maximum value operation here is performed on structuring elements. Each position in Executed. If For any If the position is 1 (for an obstacle), then It will be set to 1 if it is not 0 otherwise. This is used to perform the expansion operation.

[0047] The present invention has the following beneficial technical effects:

[0048] This invention combines an autonomous vehicle path planning and decision-making algorithm based on structured roads with an improved A* path planning algorithm based on grid maps, developing an autonomous vehicle path planning and decision-making algorithm based on multi-feature map fusion. This addresses the challenges of existing autonomous vehicle applications in construction sites, mining areas, and other scenarios where structured roads are fixed but the surrounding environment is constantly changing, requiring exploration of new areas intermittently. It improves the generalization of path planning decisions based on structured roads while mitigating the uncertainty of autonomous navigation route planning based on grid maps. This invention also features an interactive application interface that allows the positioning and perception module to acquire sensor information in real time for location tracking and feedback, which is then displayed on the interface. Simultaneously, it can perform real-time planning based on a user-specified destination, highlighting the final road trajectory information published in the path planning and decision-making system within the multi-feature map fused from the grid map and structured roads on the interactive interface.

[0049] The key technical points of this invention include: acquiring road point data and constructing structured roads based on a positioning and perception module; obtaining a raster map through 3D point cloud map conversion; and making planning decisions based on the nearest point between the vehicle's current position and the structured road, as well as the nearest point between the destination position and the structured road. This invention, through modular encapsulation, can directly perform path planning on structured roads that conforms to vehicle dynamics characteristics, while simultaneously enabling rapid path planning for target points outside the road using A-satellite navigation. This improves the generalization of autonomous navigation under structured maps, solves the planning problem for target points outside structured roads, and achieves highly efficient, generalized navigation decisions based on structured roads. This invention effectively solves the technical problem that existing planning decisions based on single-feature maps cannot meet the requirements in special scenarios such as construction sites and mining areas where structured roads are defined but the environment is constantly changing and new areas requiring brief exploration may arise at any time. Attached Figure Description

[0050] The present invention can be better understood by referring to the description given below in conjunction with the accompanying drawings, which together with the following detailed description are included in and form part of this specification, and are used to further illustrate preferred embodiments of the invention and explain the principles and advantages of the invention.

[0051] Figure 1 This is a flowchart of an unmanned vehicle path planning and decision-making system based on multi-feature map fusion, as described in an embodiment of the present invention.

[0052] Figure 2 This is a structured road construction diagram in an embodiment of the present invention;

[0053] Figure 3 This is a comparison image of 3D point cloud downsampling before and after in an embodiment of the present invention;

[0054] Figure 4 In this embodiment of the invention, the initial map of the structured road is read using the floor values ​​in the parameter server.

[0055] Figure 5 This is a flowchart of the path planning decision-making process based on multi-feature map fusion in an embodiment of the present invention;

[0056] Figure 6 This is an improved image of satellite A and structured road planning trajectory stitched together in an embodiment of the present invention;

[0057] Figure 7 , Figure 8 , Figure 9 The multi-feature map output by the unmanned material transport vehicle in a real-time planning scenario is an application example diagram of the present invention. Detailed Implementation

[0058] To enable those skilled in the art to better understand the present invention, exemplary embodiments or examples of the present invention will be described below in conjunction with the accompanying drawings. Obviously, the described embodiments or examples are merely some, not all, of the embodiments or examples of the present invention. All other embodiments or examples obtained by those skilled in the art based on the embodiments or examples of the present invention without inventive effort should fall within the scope of protection of the present invention.

[0059] In the description of this invention, it should be noted that the terms used in the various embodiments, such as "upper," "lower," "front," "rear," "left," and "right," which indicate orientation, are only used to simplify the description of the positional relationships based on the accompanying drawings and do not mean that the components and devices referred to must be operated in accordance with the specific orientations and defined operations, methods, and structures in the specification. Such directional terms do not constitute a limitation of this invention.

[0060] In the description of this invention, it should be noted that the terms "first," "second," and "third" mentioned in the embodiments of this invention are for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Therefore, a feature defined with "first," "second," and "third" may explicitly or implicitly include one or more of that feature.

[0061] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0062] Specific Implementation Plan 1: Combining Figure 1 As shown, this invention provides an unmanned vehicle path planning and decision-making system based on multi-feature map fusion, comprising:

[0063] The positioning and perception module 1 is used to achieve rapid mapping and accurate positioning in complex scenarios that integrate indoor and outdoor environments, such as construction sites.

[0064] The Road Point Data Acquisition and Structured Road Construction Module 2 is used to collect road point data based on the real-time location information of the unmanned vehicle obtained by the positioning and perception module, and to construct a structured road by designing the road ID, the preceding and succeeding road numbers, and the road width information.

[0065] The 3D point cloud to raster map module 3 is used to perform pass-through filtering and radius filtering on the 3D point cloud map constructed by the positioning and perception module to obtain a downsampled sparse point cloud map, and then project it to obtain a two-dimensional point cloud map. Finally, the coordinates of the projected two-dimensional point cloud are mapped to a raster map with a fixed resolution, and the status (occupied and idle) of each grid is updated according to the projected point cloud information.

[0066] The coordinate unification module 4 is used to unify the static coordinate system of the created structured roads and the converted raster map. It uses coordinate transformation to ensure that the starting point of the structured roads created based on the positioning and sensing module is consistent with the source point of the raster map. At the start of mapping, the vehicle's heading is... The positive axis direction, perpendicular to the vehicle body and satisfying the right-hand coordinate system, is the orientation. Positive direction of the axis.

[0067] The path planning decision module 5 based on multi-feature map fusion is used to define thresholds for the distance between the vehicle's current position and the nearest point on the structured road, as well as the distance between the destination position and the nearest point on the road. If the threshold is exceeded, the excess part is improved by A-star planning. At the same time, the shortest path planning based on the structured road is performed between the point in the structured road closest to the vehicle and the point closest to the destination. The road trajectory points obtained by the improved A-star are spliced ​​with the planned path in the structured road and published.

[0068] The shortest path planning module 6 based on structured roads is used to construct directed edges based on the structured roads created in the structured road construction module, and to apply shortest path planning based on a combination of greedy strategy and breadth-first search.

[0069] The A-Star path planning module 7, based on a grid map, is used to improve the path planning of A-Star based on the grid map. It mainly improves the overall evaluation cost function by adding obstacle distance cost to avoid the shortest path being close to obstacles. At the same time, it sets the grid neighborhood selection to allow diagonal movement in the grid map.

[0070] The path visualization module 8 is used to highlight the final road trajectory information published in the path planning and decision system in the multi-feature map that merges the raster map and structured road on the output terminal.

[0071] Specific Implementation Plan Two: Combining Figures 1 to 6 As shown, this invention provides a path planning and decision-making method for unmanned vehicles based on multi-feature map fusion, comprising the following steps:

[0072] Step 1: Use tightly coupled iterative Kalman filtering to process the data from the lidar and IMU to achieve real-time mapping and localization;

[0073] Step 2: Road Point Data Acquisition and Structured Road Construction Module. Based on the real-time location information of the autonomous vehicle obtained by the positioning and perception module, the module collects road point data and constructs a structured road by designing road IDs, preceding and succeeding road numbers, and road width information. The specific steps are as follows:

[0074] S21. The road point data acquisition and structured road construction module reads the real-time position information of the unmanned vehicle obtained by the positioning and perception module, that is, after determining the origin, the current coordinates of the unmanned vehicle relative to the origin. , and quaternion information ;

[0075] S22. The quaternion information of the autonomous vehicle fed back by the positioning and perception module. The following coordinates are converted into the heading angle information required for structured road construction;

[0076] (1)

[0077] S23. The location of the unmanned vehicle at this moment, as initially collected. , and quaternion information The real-time converted heading angle information is recorded in a txt file, and the driving mileage of the unmanned vehicle is calculated in real time. If the cumulative driving mileage exceeds the set threshold, waypoint information is recorded, and the recorded driving mileage is cleared to zero.

[0078] S24. Construct a structured road. After collecting road point data, design the road ID, the preceding and succeeding road numbers, and the road width information in a txt file, as shown in the following formula.

[0079] (2)

[0080] Each road trajectory consists of a series of... waypoints and road connection information Composition, in which It is the coordinate position. These are the left and right boundary widths of the current road, respectively. These are the road connection number at the current road's starting point and the road number at the road's ending point, respectively.

[0081] Step 3: The 3D point cloud map to raster map module performs pass-through filtering and radius filtering on the 3D point cloud map constructed by the positioning and perception module to obtain a downsampled sparse point cloud map. Then, the sparse point cloud map is projected to obtain a two-dimensional point cloud map. Finally, the coordinates of the projected two-dimensional point cloud are mapped to a raster map with a fixed resolution. At the same time, the status (occupied and idle) of each grid is updated according to the projected point cloud information.

[0082] The specific steps are as follows:

[0083] Furthermore, step three includes the following steps:

[0084] S31, the 3D point cloud map to raster map module first processes the 3D point cloud map constructed based on the positioning and perception module using a pass-through filter, selecting... From 0.05 on the axis Up to 1.2 The point cloud data, this height is consistent with the height information of unmanned transport vehicles in construction scenarios such as construction sites, and only the point cloud data within this range in the coordinate system is retained;

[0085] S32. Apply radius filtering to the filtered point cloud image to remove outliers. Set the radius of the radius filter to 0.5. The number of threshold points is set to 5;

[0086] S33. The point cloud image after pass-through filtering and radius filtering is subjected to downsampling voxel filtering to reduce the number of points without destroying the point cloud geometry. Point cloud squares in a three-dimensional space with a limited size are replaced with centroid point clouds, such as... Figure 3The image shows a comparison of the point cloud image effects before and after point cloud processing.

[0087] S34. Project the sparse point cloud map to obtain a two-dimensional point cloud map. Finally, map the coordinates of the projected two-dimensional point cloud onto a grid map with a fixed resolution. At the same time, update the state (occupied and idle) of each grid according to the projected point cloud information.

[0088] Step 4: The coordinate unification module performs static coordinate system unification between the created structured road and the converted raster map. Coordinate transformation is used to ensure that the starting point of the structured road created based on the positioning and sensing module is consistent with the source point of the raster map. At the start of mapping, the vehicle's facing direction is... The positive axis direction, perpendicular to the vehicle body and satisfying the right-hand coordinate system, is the orientation. Positive direction of the axis.

[0089] Step 5: The path planning decision module based on multi-feature map fusion defines thresholds based on the distance between the vehicle's current position and the nearest point on the structured road, as well as the distance between the destination position and the nearest point on the road. Figure 5 As shown, if the threshold is exceeded, the excess part will be subjected to improved A-star path planning based on grid map. At the same time, the point in the structured road closest to the vehicle and the point closest to the destination will be subjected to shortest path planning based on structured road. The road trajectory points obtained by improved A-star and the planned path in structured road will be stitched together and published.

[0090] The specific steps are as follows:

[0091] S51. Obtain floor values ​​from the parameter server in real time via timer interrupts, such as... Figure 4 As shown, the structured roads of each floor are read, and the positions of each point on each road are stored in vector form.

[0092] S52. Obtain task instructions and destination location information in real time through the subscriber. , and heading angle information The endpoint location information is traversed with the path location information in the read structured road to determine the structured road point closest to the endpoint. If the distance between the two points is more than four meters, the structured road point is taken as the starting point, the obtained endpoint location is the endpoint, the improved A-star path planning is called, and this point is set as the marker that the endpoint can receive the A-star planned trajectory.

[0093] S53. The vehicle's real-time location information obtained by the positioning and perception module is traversed with the path location information in the structured road to determine the structured road point closest to the vehicle's real-time location. If the distance between the two points exceeds four meters, the improved A-star path planning is called with the vehicle's current location as the starting point and the structured road point closest to the vehicle's real-time location as the ending point.

[0094] Furthermore, in the improved A-satellite path planning invoked in S52 and S53, the planning point information is mainly stored by establishing open and closed lists, with the starting point placed in the open list; at the same time, an overall improvement evaluation cost function is established. ,

[0095] (3)

[0096] Its main value is the cost from the previous point to the current point. Expected cost from the current point to the destination Distance to the nearest obstacle grid cell from the current grid cell Iterate through the nodes of the raster map surrounding the starting point, adding them to the open list. Select the point with the smallest overall evaluation cost function in the open list as the exploration point, and add the starting point to the close list. During the calculation, the open list ignores obstacles and points in the close list. During exploration, if any nodes in the raster map surrounding the new point are not in the open list, update the list and calculate the overall evaluation cost function. Also, if any nodes in the open list exist around the new exploration point... Smaller, then Update and change the parent node of the current node; repeat this process until the target node is found or the open list is empty. That is, starting from the target node, connect the nodes in the closed list in the direction pointed to by the parent node to form the final trajectory.

[0097] S54. The road point information planned by the obtained A-star path planning is judged by the receiver, so that the receiving optimization process is only optimized once in one planning cycle to avoid repeated receiving and sending, and the received trajectory is subjected to outlier removal, low-pass filtering and trajectory optimization.

[0098] Furthermore, in the trajectory optimization process for the planned trajectory of satellite A, the original coordinates are set to... The optimized coordinates are The design cost function is as follows:

[0099] (4)

[0100] In the equation on the right, the first term is the road smoothness cost, the second term is the cost of similarity with the original path points, and the third term is the road compactness cost. , , These are the cost coefficients; after extracting the coefficients of the constant terms in each formula, we obtain the coefficient matrix, which is listed in the quadratic programming formula below:

[0101] (5)

[0102] Combining the above equation, we can obtain the coefficients in the quadratic form as:

[0103] (6)

[0104] in , , , The coefficients are constant terms. The minimum value is obtained through quadratic optimization. Road optimization will be carried out.

[0105] S55. Using the structured road point closest to the vehicle's real-time location as the starting point and the structured road point closest to the destination information as the destination, the shortest path planning module based on structured roads is called. Since the structured road is constructed by collecting positioning information based on the unmanned vehicle's own motion trajectory, and the main motion trajectory conforms to the dynamic characteristics of the unmanned vehicle, the dynamic programming optimization algorithm is used to perform shortest path planning on the constructed structured road, and the path information obtained from the planning is stored in the vector of track_road.

[0106] Furthermore, in the shortest path planning based on dynamic programming optimization applied to the structured road, the start and end points are the two points on the structured road closest to the autonomous vehicle and the end point, respectively. During algorithm initialization, the distance to the start point is set to 0, and the distances of all other vertices to the start point are set to infinity, indicating that no path has yet been found to reach that point. Simultaneously, the topology map is set as a weighted directed graph. ,in Store the vertex information of each structured road. Store the edge information of each structured road, and define the minimum cumulative distance from the explored vertex to the starting point as the weight of each vertex. The algorithm sequentially selects the unvisited vertex with the smallest distance, performs a breadth-first search through that vertex, and updates the distance values ​​from adjacent vertices to the starting point in turn, until all vertices have been visited.

[0107] Therefore, a distance array is defined. Record from the starting point To each vertex The shortest path length, initially, is:

[0108] (7)

[0109] Define an array of preceding path points. Record the preceding vertices of the shortest path from the starting point to each vertex, initially set to null. Based on this, iterate through the list, selecting the vertex with the shortest distance from the current point to the starting point that has not yet been visited in each iteration. traversal and All adjacent vertices And update the distance value based on a greedy strategy, if the condition is met... Then we have:

[0110] (7)

[0111] After traversing all vertices, the preceding path points of the endpoint are returned sequentially according to the position of the endpoint, and the road points are stored to form the global path planned by the topology-based structured road.

[0112] S56. Based on the A-star planned trajectory and the structured road-based planned trajectory, the marker position is determined and fused. If neither the A-star planned trajectory at the starting point nor the A-star planned trajectory at the ending point is published, the structured road-based planned trajectory is published directly. If either the A-star planned trajectory at the starting point or the ending point is published, the A-star received trajectory marker position needs to be determined. If the corresponding planned trajectory has been received, trajectory splicing is performed. Figure 6 As shown, the large area box represents the structured road planning trajectory, and the small area box represents the A-star planning trajectory. The A-star planning trajectory at the starting point needs to be inserted before the structured trajectory, and the trajectory at the ending point needs to be inserted after the structured trajectory before the complete trajectory is published.

[0113] S57. The final road trajectory information will be highlighted in the multi-feature map fused with the grid map and structured road data on the output terminal. For example... Figure 7 As shown in Figures 8 and 9, this demonstrates the practical application of road tracking control for unmanned material transport vehicles in real-time construction scenarios, based on the global path fused from multi-feature maps generated by real-time planning.

[0114] Step 6: Highlight the final road trajectory information published in the path planning decision system in the multi-feature map that merges the grid map and structured road on the output terminal.

[0115] The other combinations and connections in this implementation scheme are the same as in Specific Implementation Scheme 1.

[0116] Furthermore, when using the improved A-star plan for path planning outside of structured roads, the map on which the planning depends is expanded. This prevents randomly placed obstacles of varying heights from being filtered out during raster map processing, which could cause collisions, especially in scenarios like construction sites. Therefore, an expansion layer is designed to expand obstacles, primarily through morphological expansion. The following formula illustrates this:

[0117] (8)

[0118] The maximum value operation here is performed on structuring elements. Each position in Executed. If For any If the position is 1 (for an obstacle), then It will be set to 1 if it is not 0 otherwise. This is used to perform the expansion operation.

[0119] The algorithm (method) proposed in this invention is the underlying technical core of this invention, and various products can be derived based on the algorithm.

[0120] Based on the algorithm (method) proposed in this invention, a path planning and decision-making system for unmanned vehicles based on multi-feature map fusion is developed using a programming language. This system has program modules corresponding to the steps of path planning and decision-making for unmanned vehicles based on multi-feature map fusion, and executes the steps in the above-mentioned path planning and decision-making method for unmanned vehicles based on multi-feature map fusion when running.

[0121] The developed system (software) computer program is stored on a computer-readable storage medium. This computer program is configured to implement the steps of the aforementioned unmanned vehicle path planning and decision-making method based on multi-feature map fusion when invoked by a processor. In other words, the invention is materialized on a carrier, becoming a computer program product.

[0122] An autonomous vehicle path planning and decision-making device based on multi-feature map fusion is disclosed. The device includes at least one processor and a memory communicatively connected to the at least one processor. The memory stores instructions executable by the at least one processor. The instructions are executed by the at least one processor to enable the at least one processor to execute the aforementioned autonomous vehicle path planning and decision-making method based on multi-feature map fusion, thereby realizing autonomous driving road planning.

[0123] Various implementations of the systems and techniques described herein can be implemented in digital electronic circuit systems, integrated circuit systems, application-specific integrated circuits (ASICs), computer hardware, firmware, software, and / or combinations thereof. These various implementations may include: implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0124] The computational programs (also referred to as programs, software, software applications, or code) of this invention include machine instructions of a programmable processor and can be implemented using high-level procedural and / or object-oriented programming languages, and / or assembly / machine languages. As used herein, the terms "machine-readable medium" and "computer-readable medium" refer to any computer program product, device, and / or apparatus (e.g., disk, optical disk, memory, programmable logic device PLD) for providing machine instructions and / or data to a programmable processor, including machine-readable media that receive machine instructions as machine-readable signals. The term "machine-readable signal" refers to any signal for providing machine instructions and / or data to a programmable processor.

[0125] Verification has shown that the method proposed in this invention solves the technical problem raised in this invention, and practical application has verified the technical effects and practicality claimed in this invention.

[0126] The method described in this invention has been verified through simulation experiments and practical applications, demonstrating the technical effects claimed by this invention.

[0127] Although the invention has been described with respect to a limited number of embodiments, those skilled in the art will understand from the foregoing description that other embodiments are conceivable within the scope of the invention described herein. The disclosure of the invention is illustrative and not restrictive, and the scope of the invention is defined by the appended claims.

Claims

1. An unmanned vehicle path planning and decision-making system based on multi-feature map fusion, characterized in that, The system includes: The system includes: The positioning and perception module is used to achieve rapid mapping and accurate positioning in complex scenarios that integrate indoor and outdoor environments at construction sites. The road point data acquisition and structured road construction module is used to acquire road point data based on the real-time location information of the unmanned vehicle obtained by the positioning and perception module, and to construct structured roads by designing road IDs, preceding and succeeding road numbers, and road width information. The 3D point cloud map to raster map module is used to perform pass-through filtering and radius filtering on the 3D point cloud map constructed by the positioning and perception module to obtain a downsampled sparse point cloud map, which is then projected to obtain a 2D point cloud map. Finally, the coordinates of the projected 2D point cloud are mapped to a raster map with a fixed resolution. At the same time, the status of each raster is updated according to the projected point cloud information, including occupied and idle. The coordinate unification module is used to statically unify the coordinates of the created structured roads and the converted raster map. It uses coordinate transformation to ensure that the starting point of the structured roads created based on the positioning and sensing module is consistent with the source point of the raster map. At the start of mapping, the vehicle's heading is... The positive axis direction, perpendicular to the vehicle body and satisfying the right-hand coordinate system, is the orientation. Positive direction of the axis; The path planning decision module based on multi-feature map fusion is used to define thresholds for the distance between the vehicle's current position and the nearest point on the structured road, as well as the distance between the destination position and the nearest point on the road. If the threshold is exceeded, the excess part is improved by A-star planning. At the same time, the shortest path planning based on the structured road is performed between the point on the structured road closest to the vehicle and the point on the end point. The road trajectory points obtained by the improved A-star and the planned path on the structured road are stitched together and published. The shortest path planning module based on structured roads is used to construct directed edges based on the structured roads created in the structured road construction module, and then apply the shortest path planning based on the dynamic programming optimization algorithm. The A-Star path planning module based on grid maps is used to improve A-Star path planning on grid maps. It improves the overall evaluation cost function by adding obstacle distance cost to avoid the shortest path being close to obstacles. At the same time, it sets the grid neighborhood selection to allow diagonal movement in the grid map. The path visualization module is used to highlight the final road trajectory information published in the path planning and decision-making system in the multi-feature map that merges the grid map and structured roads on the output terminal. In the improved path planning for Satellite A, open and closed lists are used to store planning point information, with the starting point placed in the open list; simultaneously, an overall improvement evaluation cost function is established. , Among them, the value from the previous starting point to the current point Expected cost from the current point to the destination Distance to the nearest obstacle grid cell from the current grid cell Iterate through the nodes of the raster map surrounding the starting point, adding them to the open list. Select the point with the smallest overall evaluation cost function in the open list as the exploration point, and add the starting point to the close list. During the calculation, the open list ignores obstacles and points in the close list. During exploration, if any nodes in the raster map surrounding the new point are not in the open list, update the list and calculate the overall evaluation cost function. Also, if any nodes in the open list exist around the new exploration point... Smaller, then Update and change the parent node of the current node; repeat this process until the target node is found or the open list is empty. That is, starting from the target node, connect the nodes in the close list in the direction pointed to by the parent node to form the final trajectory.

2. A method for unmanned vehicle path planning and decision-making based on multi-feature map fusion, characterized in that, The method includes the following steps: Step 1: Use tightly coupled iterative Kalman filtering to process the data from the lidar and IMU to achieve real-time mapping and localization; Step 2: The road point data collection and structured road construction module collects road point data based on the real-time location information of the unmanned vehicle obtained by the positioning and perception module, and constructs structured roads by designing road IDs, preceding and succeeding road numbers, and road width information. Step 3: The 3D point cloud map to raster map module performs pass-through filtering and radius filtering on the 3D point cloud map constructed by the positioning and perception module to obtain a downsampled sparse point cloud map. Then, the sparse point cloud map is projected to obtain a two-dimensional point cloud map. Finally, the coordinates of the projected two-dimensional point cloud are mapped to a raster map with a fixed resolution. At the same time, the status of each raster is updated according to the projected point cloud information, including occupied and idle. Step 4: The coordinate unification module performs static coordinate system unification between the created structured road and the converted raster map. Coordinate transformation is used to ensure that the starting point of the structured road created based on the positioning and sensing module is consistent with the source point of the raster map. At the start of mapping, the vehicle's facing direction is... The positive axis direction, perpendicular to the vehicle body and satisfying the right-hand coordinate system, is the orientation. Positive direction of the axis; Step 5: The path planning decision module based on multi-feature map fusion defines thresholds for the distance between the vehicle's current position and the nearest point on the structured road, as well as the distance between the destination position and the nearest point on the road. If the thresholds are exceeded, the excess part is subjected to improved A-star path planning based on the grid map. At the same time, the point in the structured road closest to the vehicle and the point closest to the destination are subjected to shortest path planning based on the structured road. The road trajectory points obtained by the improved A-star are stitched together with the planned path in the structured road and published. Step 6: Highlight the final road trajectory information published in the path planning decision system in the multi-feature map fused with the grid map and structured road on the output terminal; In the improved path planning for Satellite A, open and closed lists are used to store planning point information, with the starting point placed in the open list; simultaneously, an overall improvement evaluation cost function is established. , Among them, the value from the previous starting point to the current point Expected cost from the current point to the destination Distance to the nearest obstacle grid cell from the current grid cell Iterate through the nodes of the raster map surrounding the starting point, adding them to the open list. Select the point with the smallest overall evaluation cost function in the open list as the exploration point, and add the starting point to the close list. During the calculation, the open list ignores obstacles and points in the close list. During exploration, if any nodes in the raster map surrounding the new point are not in the open list, update the list and calculate the overall evaluation cost function. Also, if any nodes in the open list exist around the new exploration point... Smaller, then Update and change the parent node of the current node; repeat this process until the target node is found or the open list is empty. That is, starting from the target node, connect the nodes in the close list in the direction pointed to by the parent node to form the final trajectory.

3. The unmanned vehicle path planning and decision-making method based on multi-feature map fusion according to claim 2, characterized in that, Step two includes the following steps: S21. The road point data acquisition and structured road construction module reads the real-time position information of the unmanned vehicle obtained by the positioning and perception module, that is, after determining the origin, the current coordinates of the unmanned vehicle relative to the origin. , and quaternion information ; S22. The quaternion information of the autonomous vehicle fed back by the positioning and perception module. The following coordinates are converted into the heading angle information required for structured road construction; （1） S23. The location of the unmanned vehicle at this moment, as initially collected. , and quaternion information The real-time converted heading angle information is recorded in a txt file, and the driving mileage of the unmanned vehicle is calculated in real time. If the cumulative driving mileage exceeds the set threshold, waypoint information is recorded, and the recorded driving mileage is cleared to zero. S24. Construct a structured road. After collecting road point data, design the road ID, the preceding and succeeding road numbers, and the road width information in a txt file, as shown in the following formula. （2） Each road trajectory consists of a series of... waypoints and road connection information Composition, in which It is the coordinate position. These are the left and right boundary widths of the current road, respectively. These are the road connection number at the starting point of the current road and the road connection number at the end of the road.

4. A method for unmanned vehicle path planning and decision-making based on multi-feature map fusion according to claim 2 or 3, characterized in that, Step three includes the following steps: S31, the 3D point cloud map to raster map module first processes the 3D point cloud map built based on the positioning and perception module using a pass-through filter, selecting... From 0.05 on the axis Up to 1.2 The point cloud data, this height is consistent with the height information of unmanned transport vehicles in construction site scenarios, and only the point cloud data within this range in the coordinate system is retained; S32. Apply radius filtering to the filtered point cloud image to remove outliers. Set the radius of the radius filter to 0.

5. The number of threshold points is set to 5; S33. The point cloud image after passing through filtering and radius filtering is subjected to downsampling voxel filtering. By defining a point cloud grid of a limited size in three-dimensional space, the centroid point cloud within the grid is used to replace all the point clouds within the entire grid. This reduces the number of point clouds without destroying the geometric structure of the point cloud, thereby shortening the pre-map loading time of the entire algorithm. S34. Project the sparse point cloud map to obtain a two-dimensional point cloud map. Finally, map the coordinates of the projected two-dimensional point cloud onto a grid map with a fixed resolution. At the same time, update the status of each grid cell, including occupied and idle, according to the projected point cloud information.

5. The unmanned vehicle path planning and decision-making method based on multi-feature map fusion according to claim 4, characterized in that, Step five includes the following steps: S51. Obtain the floor values ​​in the parameter server in real time through timer interrupt, perform structured road reading operations on each floor, and store the position of each point on each road in vector form. S52. Obtain task instructions and destination location information in real time through the subscriber. , and heading angle information The endpoint location information is traversed with the path location information in the read structured road to determine the structured road point closest to the endpoint. If the distance between the two points is more than four meters, the structured road point is taken as the starting point, the obtained endpoint location is the endpoint, the improved A-star path planning is called, and this point is set as the marker that the endpoint can receive the A-star planned trajectory. S53. The vehicle's real-time location information obtained by the positioning and perception module is traversed with the path location information in the structured road to determine the structured road point closest to the vehicle's real-time location. If the distance between the two points exceeds four meters, the improved A-star path planning is called with the vehicle's current location as the starting point and the structured road point closest to the vehicle's real-time location as the ending point. S54. The road point information planned by the obtained A-star path planning is judged by the receiver, so that the receiving optimization process is only optimized once in one planning cycle to avoid repeated receiving and sending, and the received trajectory is subjected to outlier removal and low-pass filtering. S55. Using the structured road point closest to the vehicle's real-time location as the starting point and the structured road point closest to the destination information as the destination, call the shortest path planning module based on structured roads, use dynamic programming optimization algorithm to perform shortest path planning on the constructed structured roads, and store the planned path information in the vector of track_road. S56. Based on the two segments of A-star planned trajectory and the planned trajectory based on structured roads, the marker position is determined and fused. If neither the A-star planned trajectory at the starting point nor the A-star planned trajectory at the ending point is published, the planned trajectory based on structured roads is published directly. If the A-star planned trajectory at the starting point or the ending point is published, the A-star received trajectory marker position needs to be determined. If the corresponding planned trajectory has been received, the trajectory is spliced. The A-star planned trajectory at the starting point needs to be inserted before the structured trajectory, and the trajectory at the ending point needs to be inserted after the structured trajectory before the complete trajectory is published. S57. The final road trajectory information will be highlighted in the multi-feature map fused with the grid map and structured road data on the output terminal.

6. The unmanned vehicle path planning and decision-making method based on multi-feature map fusion according to claim 5, characterized in that, Step five also includes, when calling the improved A-star plan for path planning outside of structured roads, performing dilation processing on the map that the planning depends on. This avoids the phenomenon of randomly placed obstacles of different heights in the environment being filtered out during the raster map processing when facing construction site scenarios, which would cause scraping. Therefore, a dilation layer is designed to dilate obstacles, and the design is carried out through morphological dilation, as follows: （3） The maximum value operation here is performed on structuring elements. Each position in Executed, if For any Position 1 is an obstacle, then It will be set to 1 if necessary, otherwise 0, to perform the expansion operation.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program configured to, when invoked by a processor, implement the steps of the unmanned vehicle path planning and decision-making method based on multi-feature map fusion as described in any one of claims 2-6.

Images