Unmanned vehicle field scene passable area segmentation and navigation method based on air-ground cooperation

Abstract

The invention belongs to the field of navigation of an unmanned vehicle in a field unstructured scene, and particularly relates to an unmanned vehicle field scene passable area segmentation and navigation method based on air-ground cooperation, and the method comprises the steps: respectively generating a global point cloud map and a local point cloud map through employing a double-SLAM algorithm; according to the global point cloud map and the local point cloud map, using a CHSM-based passable pavement segmentation method to construct a global passable point cloud map; taking the global passable point cloud map as the input of UGV navigation, and generating a global safe path by adopting a PLA-RRT algorithm; in combination with real-time environment data, using an NMPC algorithm to locally optimize the global security path so as to realize real-time obstacle avoidance and path smoothing, and obtaining a final planning path; and then the motion instruction is transmitted to the controller to guide the UGV to complete navigation. According to the invention, the application limitation of the existing UGV navigation framework in a field unstructured environment is overcome.

Description

Technical Field

[0001] This invention belongs to the field of navigation for unmanned vehicles in unstructured outdoor scenarios, specifically involving a method for segmenting and navigating passable areas in outdoor scenarios for unmanned vehicles based on air-ground cooperation. Background Technology

[0002] In unstructured wilderness environments, the autonomous navigation capabilities of unmanned ground vehicles (UGVs) directly determine the reliability and efficiency of their mission execution. Because the perception range and accuracy of a single UGV are limited, it struggles to cope with complex and ever-changing wilderness environments. Therefore, air-ground collaborative technology has emerged. This technology complements UGVs by introducing the global perspective and high-precision perception capabilities of unmanned aerial vehicles (UAVs). For example, emergency response systems based on the collaboration of UAVs and ground vehicles can be rapidly deployed during disasters, providing real-time data support and command and dispatch.

[0003] With the rapid development of UGV autonomous navigation technology, its application in structured environments such as indoor spaces and flat roads has become increasingly mature, fully demonstrating the significant progress of current technology in high-precision positioning and real-time control. However, when UGVs need to perform tasks such as field exploration and disaster relief, their navigation capabilities face multiple severe challenges in unstructured outdoor environments, including terrain complexity, environmental uncertainty, and interference from diverse obstacles. Among these challenges, the problem of segmenting traversable areas in such environments has become a major bottleneck restricting the accuracy and robustness of UGV autonomous navigation systems. This challenge is mainly reflected in three interrelated and mutually influential dimensions:

[0004] 1. Due to the high heterogeneity of topographic features and the interference of inherent noise during data acquisition, traditional plane fitting strategies that rely on height difference analysis or random sampling consensus algorithms are easily affected by frequent interference from abnormal data points in rugged terrain. This leads to "undersegmentation" during ground segmentation, which in turn makes it impossible to effectively extract and model the continuous and detailed ground structure required to support navigation path generation.

[0005] 2. In outdoor scenarios, dense vegetation poses a severe challenge to the performance of LiDAR (Light Detection and Ranging) sensors. Its occlusion effect not only significantly reduces the effective reception rate of laser echoes but also severely interferes with the accurate identification of surface geometry, causing path planning strategies that rely on such geometric information to face significant uncertainties and failure risks in actual implementation.

[0006] 3. In areas with dense vegetation and extremely complex terrain, the environmental perception capabilities of the LiDAR system carried by the unmanned vehicle are significantly limited, the perception range is greatly reduced, and the ability to distinguish small terrain features is insufficient, which further exacerbates the difficulty of generating autonomous navigation paths in highly complex scenarios. Summary of the Invention

[0007] The purpose of this invention is to provide a method for segmenting and navigating passable areas in unmanned vehicle (UGV) scenarios based on air-ground collaboration, so as to overcome the application limitations of the existing UGV navigation framework in unstructured outdoor environments.

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] A method for segmenting and navigating passable areas in wilderness scenarios using an air-ground cooperative approach for unmanned vehicles includes the following steps:

[0010] S1, Data Acquisition: The UAV terminal is equipped with a first sensor to collect global data and its own status data, and transmits them to the UGV vehicle terminal. The global data includes global point cloud data, UAV terminal attitude data, and UAV terminal geographic coordinates. The UGV vehicle terminal is equipped with a second sensor to collect local data, which includes local point cloud data, UGV vehicle terminal attitude data, and UGV vehicle terminal geographic coordinates.

[0011] S2, dual SLAM parallel construction of layered map, using the first SLAM algorithm to process the global data output from the UAV terminal to generate a global point cloud map; using the second SLAM algorithm to process the local point cloud and IMU data output from the UGV vehicle terminal to generate a local point cloud map;

[0012] S3. Construct a global accessible point cloud map. Based on the global point cloud map and the local point cloud map, construct a global accessible point cloud map using the CHSM-based accessible road segmentation method.

[0013] S4, hierarchical path planning, uses the global passable point cloud map as input for UGV navigation and employs the PLA-RRT algorithm to generate a global safe path; combined with real-time environmental data, the NMPC algorithm is used to locally optimize the global safe path to achieve real-time obstacle avoidance and path smoothing, thus obtaining the final planned path;

[0014] S5 transmits the final planned path as a motion command to the controller to guide the UGV in completing navigation.

[0015] Furthermore, the specific process of constructing a global accessible point cloud map using the CHSM-based accessible road segmentation method in step S3 includes the following steps:

[0016] S31, Keyframe Point Cloud Extraction and Classification;

[0017] Extract the current local keyframe point cloud from the global point cloud map. ;in A point in the Cartesian coordinate system. Let be the x-coordinate of the k-th point in the coordinate system. Let be the y-coordinate of the k-th point in the coordinate system. Let be the z-coordinate of the k-th point in the coordinate system.

[0018] Based on the historical classification results of the global point cloud map, the keyframe point cloud is... Divided into estimated ground point sets And estimating non-ground point sets And both satisfy ;

[0019] The classification objective is defined as: maximizing the estimated set of ground points. The proportion of real ground points in the middle, and maximizing the estimation of non-ground points. The proportion of real non-ground points in the middle;

[0020] S32, CHSM partitioning, based on the spatial distribution characteristics of the local point cloud map, uses the CHSM model partitioning strategy to partition the point cloud region and aligns the local point cloud with the corresponding region of the global point cloud map.

[0021] S33, RN noise removal addresses the issue of reflection noise that easily occurs in the bottom ring data of local point clouds. Noise monitoring and detection are performed using preset global height and intensity thresholds to eliminate abnormal noise points. The bottom ring data refers to the data closest to the laser in the laser sensor. Circle laser data, among which This is the preset number of bottom ring excitation cycles that need to be processed;

[0022] S34, Region Plane Fitting: The centroid and covariance matrix of the points in the nth region after local filtering are calculated to obtain the eigenvalues. ; minimum eigenvalue The corresponding eigenvectors are used as planar vectors To obtain surface variables This enables regional planar fitting, and the fitting results are synchronously updated to the corresponding region of the global point cloud map.

[0023] S35 allows for feasible ground likelihood estimation, constructing a ground estimation likelihood function based on the regional plane fitting results. ,in, Parameters representing point clouds, Let f represent a random variable that follows a continuous probability distribution; combine prior knowledge with the verticality operator, height operator and flatness operator to calculate the probability density function f, and dynamically adjust the passability probability distribution in the global map;

[0024] S36, Accessible map generation: Based on the probability density function f, the accessibility attributes of local keyframe point clouds are marked, thereby generating keyframe point clouds with accessibility markings, and obtaining a global accessible point cloud map.

[0025] Furthermore, the implementation process of step S4 includes the following steps:

[0026] S41, Point cloud downsampling: The global passable point cloud map obtained in step S3 is processed using point cloud downsampling and 3D rasterization methods to reduce data redundancy.

[0027] S42, Global Path Generation: The PLA-RRT* global safe path generation algorithm is used to quantify the uncertainty of ground accessibility and generate a sparse global trajectory set S, thus obtaining a global path that balances real-time performance and accessibility.

[0028] S43, Dense Path Generation, uses Gaussian process (GPR) regression prediction and linear interpolation to transform the sparse global trajectory set S into dense trajectories.

[0029] S44, based on dense trajectories, establishes a local planner based on NMPC, and introduces environmental perception information and dynamic constraints to construct an NMPC optimization problem. By solving the NMPC optimization problem in real time, the final path output of the UGV is generated, thereby realizing the trajectory tracking and local obstacle avoidance of the UGV.

[0030] Furthermore, the specific process of generating the global path in step S42 includes:

[0031] S421, Algorithm Framework Construction and Node Definition: The PLA-RRT* Global Safe Path Generation Algorithm generates an optimal path that balances real-time performance and accessibility while maintaining the characteristics of the fast exploration random tree algorithm. The node elements in the RRT tree within the PLA-RRT* Global Safe Path Generation Algorithm are represented as follows: ,in, express A local plane centered on the node. Indicates passability; The higher the value, the worse the ground conditions. 0 means absolutely passable, and 1 means the terrain is completely unsuitable for UGV passage.

[0032] S422, Design the cost function between two nodes: Node The passability cost function between them is defined as:

[0033] ;

[0034] Wherein, 𝜔 is a penalty scale factor. For nodes and The Euclidean distance between them and The accessibility index represents the fit plane containing the two nodes;

[0035] If the cost function If the set threshold is exceeded, it is determined that the safe passage of UGVs between the two nodes cannot be supported, even if both nodes are under the globally passable point cloud map; the algorithm's solution goal is not only to shorten the path length, but also to avoid those difficult-to-pass paths;

[0036] S423, Quantification of the accessibility index of a node: Defining the slope Flatness and sparsity Based on slope Flatness sparsity Quantify the accessibility of computing nodes to provide a quantifiable basis for safety judgment in path search;

[0037] slope Flatness sparsity The definition is as follows:

[0038] ;

[0039] ;

[0040] ;

[0041] in and The coefficients are constants. Represents the plane normal vector in the global coordinate system The projection onto the Z-axis; N represents the total number of points. This represents the z-axis unit vector of the j-th point in the global coordinate system. The projection on the Z-axis, This indicates the proportion of the empty space on a plane. This indicates the minimum acceptable porosity (a value less than this indicates that sparsity can be ignored). Indicates the maximum acceptable porosity. Represents the trace of all point sets, when lie in and At that time, the gap points correspond to different situations. When the gap defects are concentrated (the trace value is less than the preset value), This indicates that there may be potholes and depressions in the ground;

[0042] Based on slope Flatness sparsity The comparability of the fitted plane containing the node can be obtained through the exponent. The calculation formula is as follows: ;

[0043] in, , and It is a weight, and satisfies ; Indicates the maximum permissible slope. Indicates the maximum permissible flatness. This represents the maximum allowable sparsity; exceeding these three values ​​will cause the robot to become unable to move or to tip over.

[0044] S424 iteratively expands the RRT tree structure and samples nodes within the globally accessible point cloud map. When the target point is included in the tree structure, the initial global path is obtained. Based on this, the path is progressively optimized by continuously adding random samples, and combined with a timed detection mechanism, the path is dynamically adjusted when the UGV pose is abnormal, thus achieving dual protection of the safety and optimality of the global path.

[0045] Furthermore, the specific process of generating dense paths in step S43 includes the following steps:

[0046] Define the dense path point set after interpolation as:

[0047] ;

[0048] in, By analyzing the global path point set Obtain by linear interpolation This indicates the number of path nodes before interpolation encryption. This indicates the number of path nodes after interpolation and encryption. This represents the coordinates of the path nodes before interpolation and encryption. This represents the path node coordinates after interpolation and encryption. This represents the passability index of the path nodes after interpolation and encryption.

[0049] With tree node set The training set for Gaussian process regression, where Represents the node coordinates. To correspond to the accessibility index; interpolate the path point set. The test set is input into a pre-trained Gaussian process model, and the output is a predicted mobility index. The Gaussian process employs radial basis functions. As a covariance function, where The standard deviation of the signal. This is the length scale parameter.

[0050] Furthermore, the implementation method of step S44 includes the following steps:

[0051] S441, Waypoint Set Construction: Based on the dense paths obtained in step S43, construct the desired waypoint set. ;in, Represents the three-dimensional coordinates of each waypoint on the fitted plane. ; It is a feasibility indicator for Gaussian process regression (GPR) prediction. This represents the uncertainty index for Gaussian process regression (GPR) prediction; simultaneously, the local plane normal vector is used. and obstacle set Characterizing local topographic features, among which Number of obstacles The two-dimensional coordinates of the obstacle in the local plane;

[0052] S442, Establish the motion model: Establish the differential driving model of UGV in the fitted plane coordinate system:

[0053] ;

[0054] in, To control the input, For time step; It is the normal vector of the local plane, e x It is a unit vector in direction, e y It is the unit vector in the y-direction. The direction of motion at time k;

[0055] S443, Objective Function Design: Construct an NMPC multi-objective optimization function to simultaneously minimize path tracking error and control energy consumption.

[0056] ;

[0057] in, To control the input, It is a two-dimensional vector representing the target point, by Obtain; two positive definite matrices and These are coefficients that measure terminal costs and control costs, respectively; weighting coefficients. ,in and These are the current local planes and The average value;

[0058] S444, Set constraints: The constraints that the optimization problem must satisfy include:

[0059] Equations of state ;

[0060] initial state ;

[0061] Safety obstacle avoidance constraints ;

[0062] ;

[0063] in, It is based on the weights of matrix A, for vectors A customized metric for the effective length. The two-dimensional coordinates of the obstacle are represented here. It is the transpose of x;

[0064] S445, Optimization Solution: Solve the optimization problem based on the set optimization objective function and constraints; during the optimization process, the prediction time domain is set to 1 second, containing N nodes, and planning is carried out in a rasterized two-dimensional top view space.

[0065] Compared with the prior art, the present invention has the following beneficial effects:

[0066] 1. For ground segmentation tasks in unstructured outdoor environments, based on the CHSM model partitioning, the verticality operator, height operator, and flatness operator are combined to robustly extract passable areas in different outdoor road environments, solving the undersegmentation problem of traditional algorithms in unstructured environments.

[0067] 2. For the global safe path generation task, a path planning algorithm PLA-RRT* is proposed. By fitting a plane with a custom accessibility index and the plane of the unmanned vehicle itself, a passable "strip" path is formed, which solves the driving safety problem that may occur when the UGV is running due to the obstruction of the supporting ground.

[0068] 3. For smooth path and local obstacle avoidance tasks, the global safe path is optimized locally based on the NMPC local optimization module to ensure the continuity and smoothness of UGV movement, while avoiding obstacles that suddenly appear. Attached Figure Description

[0069] Figure 1 This is a flowchart of the unmanned vehicle field scene passable area segmentation and navigation method based on air-ground cooperation according to the present invention;

[0070] Figure 2 This is a flowchart of the accessible road segmentation method based on CHSM;

[0071] Figure 3 This is a flowchart of a UGV navigation method based on plane fitting and combined with a mobility index;

[0072] Figure 4 A schematic diagram of the fitting plane is provided for the embodiment;

[0073] Figure 5 This example illustrates the process of constructing the RRT tree and initial path search in the PLA-RRT* algorithm.

[0074] Figure 6 This example illustrates the real-time update process for the security path in the PLA-RRT* algorithm. Detailed Implementation

[0075] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings and embodiments.

[0076] like Figure 1 As shown in the figure, this embodiment provides a method for segmenting and navigating passable areas in an unmanned vehicle's field scenario based on air-ground cooperation, which includes the following steps:

[0077] S1, Data Acquisition: The UAV terminal is equipped with a first sensor to collect global data and its own status data, and transmits them to the UGV vehicle terminal. The global data includes global point cloud data, UAV terminal attitude data, and UAV terminal geographic coordinates. The UGV vehicle terminal is equipped with a second sensor to collect local data, which includes local point cloud data, UGV vehicle terminal attitude data, and UGV vehicle terminal geographic coordinates.

[0078] In this embodiment, both the first and second sensors include LiDAR, IMU, and GPS. The radar is used to acquire point cloud data, the IMU is used to acquire attitude data, and the GPS is used to acquire geographic location coordinates. The global data and self-state data collected by the UAV terminal are transmitted to the UGV vehicle terminal via the ROS architecture.

[0079] S2, Parallel construction of layered maps using dual SLAM: The first SLAM algorithm is used to process the global data output from the UAV terminal to generate a global point cloud map; the second SLAM algorithm is used to process the local point cloud and IMU data output from the UGV vehicle terminal to generate a local point cloud map.

[0080] S3. Construct a global accessible point cloud map. Based on the global and local point cloud maps, construct the global accessible point cloud map using a CHSM-based accessible road segmentation method; the implementation process is as follows: Figure 2 As shown, it includes the following steps:

[0081] S31, Keyframe Point Cloud Extraction and Classification;

[0082] Extract the current local keyframe point cloud from the global point cloud map. ;in A point in the Cartesian coordinate system;

[0083] Based on the historical classification results of the global point cloud map, Divided into ground point sets and its supplement Supplement It is a set of non-ground points, and both satisfy: ;

[0084] Because the estimation process inevitably contains errors, some points that actually originate from non-ground sources may be included in the estimated ground point set. And estimating non-ground point sets Therefore, the estimation results can be described using the concept of a confusion matrix as follows:

[0085] Estimating the ground point set It consists of true positives (TP) and false positives (FP), i.e. ;in, This represents a point that truly belongs to T and is correctly classified. Indicates that it truly belongs to However, points that were misclassified as ground points;

[0086] Estimating non-ground point sets From true counterexamples And the false counterexample FN constitutes, that is ; where TN represents the true belonging And the points that are correctly classified, This represents points that truly belong to T but are misclassified as non-ground points. Based on the above description, the classification objective can be determined as: maximizing the estimated set of ground points. The proportion of real ground points in the dataset, and maximizing the estimate of the non-ground point set. The proportion of real non-ground points in the middle, to minimize and The emergence of.

[0087] S32, CHSM partitioning, is based on the spatial distribution characteristics of the local point cloud map. It uses a CHSM model partitioning strategy to partition the point cloud region and aligns the local point cloud with the corresponding region of the global point cloud map. The detailed implementation process includes the following steps:

[0088] S321, using the CHSM model to divide the keyframe point cloud H into four sub-regions according to radial distance, the CHSM model is then expressed as:

[0089] ;

[0090] ;

[0091] in, Indicates all regions. Indicates the first Each region Indicates the total number of regions; express The minimum radial boundary, express Maximum radial boundary, This represents each point in the keyframe point cloud H;

[0092] S322, for each point in the keyframe point cloud H Translate it from Cartesian coordinates ( Convert to polar coordinates ( ), , ,in Let be the polar radius of the point. Let be the polar angle of the point; where, For the four-quadrant inverse tangent function; [the function is then applied to each...] Further divided into Non-uniform cells ,in, and These are the number of regions divided along the polar axis and polar radius, respectively, and are defined as follows:

[0093] ;

[0094] in, sub-region radial length, , ,in For point The polar radius, Called a point polar angle, and These represent the number of regions divided along the polar axis and polar radius, respectively.

[0095] S323, the four sub-regions are arranged in ascending order of radial distance: Central area Divided into four zones For half zone, This is the outer region; the boundaries of each region satisfy a recursive relationship: The specific boundary values ​​are:

[0096] ;

[0097] , , ;

[0098] Among them, the global minimum boundary Used to cover the blind spot around the 3D LiDAR sensor, i.e. The data is invalid at that time; the local CHSM partition is aligned with the corresponding area of ​​the global map through a coordinate transformation matrix to ensure that areas with the same index have the same spatial position in the global coordinate system.

[0099] S33, RN noise removal addresses the issue of reflection noise that easily occurs in the bottom ring data of local point clouds. Noise monitoring and detection are performed using preset global height and intensity thresholds to eliminate abnormal noise points. The bottom ring data refers to the data closest to the laser in the laser sensor. Circle laser data, among which It is the preset number of bottom ring excitation cycles that need to be processed.

[0100] S34, Region Plane Fitting: The centroid and covariance matrix of the points in the nth region after local filtering are calculated to obtain the eigenvalues. ; minimum eigenvalue The corresponding eigenvectors are used as planar vectors To obtain surface variables This achieves regional planar fitting, and the fitting results are synchronously updated to the corresponding region on the global point cloud map. Surface variables. The expression is: ;

[0101] S35 allows for feasible ground likelihood estimation, constructing a ground estimation likelihood function based on the regional plane fitting results. ,in, Parameters representing point clouds, Let f represent a random variable that follows a continuous probability distribution. Combining prior knowledge, the probability density function f is calculated using verticality, height, and flatness operators, dynamically adjusting the walkable probability distribution in the global map. The detailed implementation process includes the following steps:

[0102] Based on the region plane fitting results, the likelihood function is defined. Ground-based parameter estimation Perform modeling;

[0103] Based on the assumption of cell independence, the likelihood function is decomposed into a product form:

[0104] ;

[0105] in, express The parameters, express random variable, subscript correspond , It represents a random variable that follows a continuous probability distribution; This represents a preliminary estimated set of ground points;

[0106] Based on prior knowledge, the perpendicularity operator is used. Height operator Flatness operator calculate f is the probability density function of the ground.

[0107] ;

[0108] in, The average value of the point cloud value, Represents the nth partition The distance from the origin to the center of mass. Represents surface variables;

[0109] The verticality operator is defined as follows: If If it is the real ground, then its normal vector It should be orthogonal to the passable ground, i.e., the normal vector. A perpendicularity index function is introduced, perpendicular to the XY plane of the sensor. As a basis for judgment:

[0110] ;

[0111] in, ; Indicates the vertical boundary, that is The angle with the XY plane is set empirically. This empirical value can ensure sufficient rigor while also having good applicability.

[0112] When objects larger than 50cm in diameter, such as mountains or trees, approach the sensor, the verticality operator alone may not be able to effectively filter out the effects of occlusion, leading to some observation problems. Specifically, point cloud measurements above the occluded space may be mispredicted as passable ground (Ĝ), but these point clouds are not actually the real ground area. Therefore, this embodiment introduces a height operator to distinguish between TP and FP. The core idea of ​​the height operator filtering is to compare the current point height hs with the average height value. Determine whether point K belongs to the ground. Typically, points near the sensor are walkable ground, and their average height... It is also highly likely to be the ground level; if hs at a certain point is lower than... If the height difference between a point and a point near the sensor is too large, then that point is also likely not a passable point. In other words, when the radius parameter r... n When it is small, i.e., predicting the ground In the case of regions Z1 and Z2, the height operator is used. It can distinguish TP from FP while minimizing TP loss; when rn is large, i.e. In the cases located in regions Z3 and Z4, TP and FP are difficult to distinguish using this operator. Therefore, the height operator is defined as follows:

[0113] ;

[0114] in, Indicates average value, It is an adaptive midpoint function, which changes with... The increase is exponential, Represents the nth partition The distance from the origin to the center of mass. A preset maximum distance threshold is used. In this embodiment, the height operator dynamically determines the distance by comparing the actual height of the partition with the ground height standard at that distance. Specifically: when... Less than ,and Functions less than the constant range hour, If the area is 100°, it is considered ground; otherwise, it is considered non-ground. It is important to note that when... hour, If the value is always 1, it is still impossible to determine. Is the source of the substance a non-ground object (trees, rocks, etc.) or a steep slope?

[0115] Based on this, this embodiment also adds a flatness operator to correct some ground points that were mistakenly identified as false negative samples due to height threshold filtering, especially point cloud data that are actually located on flat surfaces. For example, when predicting ground areas... When located in a steep uphill area, its average height It may exceed the set threshold. And then by the height operator Incorrect exclusion. To solve this problem, surface variables are introduced. Further determination is made as to whether points filtered out as non-bottom surfaces by the height operator belong to a flat region; after this processing, even... In the case of, it can still be based on The value of the ground determination point is adjusted, therefore, this embodiment adjusts the flatness operator. The definition is as follows:

[0116] ;

[0117] in, Indicates the gain coefficient. The value is greater than 1. The surface variable representing the current partition; This indicates a region. Surface variable thresholds; in this way, the accuracy of ground estimation for steep uphill terrain is significantly enhanced, even with average surface height. Exceeding the local threshold These points can still be effectively reduced to ground estimates. This method, by adjusting the gain and threshold, enables the model to handle unstructured terrain more accurately, especially showing a significant improvement in performance in steep areas. Therefore, the final set of estimated ground points is expressed as:

[0118] ;

[0119] Here, [·] represents Iverson brackets, which return true if the condition is met, otherwise return false. Represents all estimated ground points. This represents the ground dividing point of the nth partition. This represents the total number of partitions. express The parameters, express A random variable.

[0120] S36, the passable point cloud map is generated by marking the passability attributes of the local keyframe point cloud based on the verticality operator, height operator, flatness operator and probability density function f, thereby generating a keyframe point cloud with passability markings and obtaining a global passable point cloud map.

[0121] S4, hierarchical path planning, uses the globally accessible point cloud map as input for UGV navigation and employs the PLA-RRT algorithm to generate a global safe path. Combined with real-time environmental data, the NMPC algorithm is used for local optimization of the global safe path to achieve real-time obstacle avoidance and path smoothing, resulting in the final planned path. The specific process is as follows: Figure 3 As shown, it includes the following steps:

[0122] S41, Point cloud downsampling: The global passable point cloud map obtained in step S3 is processed using point cloud downsampling and 3D rasterization methods to reduce data redundancy.

[0123] S42, Global Path Generation: The PLA-RRT* global safe path generation algorithm is used to quantify the uncertainty of ground accessibility, generating a sparse global trajectory set S, thus obtaining a global path that balances real-time performance and accessibility. Its implementation process is as follows... Figure 5 and Figure 6 As shown, it includes:

[0124] S421, Algorithm Framework Construction and Node Definition: The PLA-RRT* Global Safe Path Generation Algorithm maintains the characteristics of the fast exploration random tree algorithm, performs plane fitting of the surrounding area for each RRT tree node and determines its passability index. At the same time, it combines cost constraints between nodes and real-time pose monitoring to ensure the safety and executability of the generated path, generating an optimal path that balances real-time performance and passability.

[0125] In the Informed-RRT* algorithm, the definition is... Represents a point in space. To represent a point on a plane, Represents a spatial point located on the terrain surface; Indicates the location of the UGV center on the terrain; path For a feasible fitted plane band, such as Figure 4 As shown, the path consists of multiple nodes. Composition, each node Includes six-dimensional posture and passability index The six-dimensional posture Used to describe UGV at terrain points The three-dimensional position and attitude angle; set constraints: based on the selected set of terrain surface points. The plane is fitted using the singular value decomposition (SVD) method. To reflect the overall trend of the local terrain; the distance between nodes must meet the kinematic constraints of the vehicle to ensure that the vehicle can move smoothly.

[0126] By incorporating the aforementioned defined parameters into the Informed-RRT algorithm, the node definition for the tree structure in the improved algorithm is obtained. In this embodiment, each node of the tree... Defined as ,in, express A local plane centered on a node can be understood as a plane fitted by points on the terrain surface; Indicates passability, with a value ranging from 0 to 1. The higher the value, the more difficult it is to pass through. The minimum value of 0 indicates that it is absolutely passable, and the maximum value of 1 indicates that it is absolutely impossible to pass through.

[0127] S422, Design the cost function between two nodes: The passability cost function between them is defined as follows:

[0128] ;

[0129] Wherein, 𝜔 is a penalty scale factor. For nodes and The Euclidean distance between them and The accessibility index represents the fit plane containing the two nodes;

[0130] If the cost function If the set threshold is exceeded, it is determined that safe passage of UGVs between the two nodes is not possible, even if both nodes are within the globally traversable point cloud map. The algorithm's goal is not only to shorten the path length but also to avoid difficult-to-traverse paths. It should be noted that the algorithms for calculating the traversability τ1 and τ2 of the fitted plane containing the two nodes are consistent.

[0131] S423, Quantification of the accessibility index of a node: Defining the slope Flatness sparsity Based on slope Flatness sparsity Quantify the accessibility of computing nodes to provide a quantifiable basis for safety judgment in path search;

[0132] slope Flatness sparsity The definition is as follows:

[0133] ;

[0134] ;

[0135] ;

[0136] in and The coefficients are constants. Represents the plane normal vector in the global coordinate system The projection onto the Z-axis; N represents the total number of points. This represents the z-axis unit vector of the j-th point in the global coordinate system. The projection on the Z-axis, This indicates the proportion of the empty space on a plane. This indicates the minimum acceptable porosity (a value less than this indicates that sparsity can be ignored). Indicates the maximum acceptable porosity. Represents the trace of all point sets, when lie in and At that time, the gap points correspond to different situations. When the gap defects are concentrated (the trace value is less than the preset value), This indicates that there may be potholes and depressions in the ground;

[0137] Based on slope Flatness sparsity Computing nodes can be accessed through exponential methods. The calculation formula is as follows:

[0138] ;

[0139] in, , and It is a weight, and satisfies ; Indicates the maximum permissible slope. Indicates the maximum permissible flatness. This represents the maximum allowable sparsity; exceeding these three values ​​may cause the robot to become unable to move or tip over.

[0140] S424 iteratively expands the RRT tree structure and samples nodes within the globally accessible point cloud map. When the target point is included in the tree structure, the initial global path is obtained. Based on this, the path is progressively optimized by continuously adding random samples, and combined with a timed detection mechanism, the path is dynamically adjusted when the UGV pose is abnormal, thus achieving dual protection of the safety and optimality of the global path.

[0141] S43, Dense path generation, due to the fitting of each local plane at each node. In the middle, accessibility Given a constant value, and considering that the sampling step size and planning time limit the density of generated trajectory points, this embodiment employs GPR regression prediction and linear interpolation to transform the sparse global trajectory set S into a dense trajectory set in order to obtain a denser path for subsequent local path optimization operations. Specifically:

[0142] Define the dense path point set after interpolation as:

[0143] ;

[0144] in, By analyzing the global path point set Obtain by linear interpolation This indicates the number of path nodes before interpolation encryption. This indicates the number of path nodes after interpolation and encryption. This represents the coordinates of the path nodes before interpolation and encryption. This represents the path node coordinates after interpolation and encryption. This represents the passability index of the path nodes after interpolation and encryption.

[0145] With tree node set The training set for Gaussian process regression, where Represents the node coordinates. To correspond to the accessibility index; interpolate the path point set. The test set is input into a pre-trained Gaussian process model, and the output is a predicted mobility index. The Gaussian process employs radial basis functions. As a covariance function, where Let be the signal standard deviation, and l be the length scale parameter.

[0146] S44, based on dense trajectories, establishes a local planner based on NMPC, and introduces environmental perception information and dynamic constraints to construct an NMPC optimization problem. By solving the NMPC optimization problem in real time, the final path output of the UGV is generated, thereby realizing the trajectory tracking and local obstacle avoidance of the UGV.

[0147] S441, Waypoint Set Construction: Based on the dense paths obtained in step S43, construct the desired waypoint set. ;in, Represents the three-dimensional coordinates of each waypoint on the fitted plane. ; It is a feasibility indicator for Gaussian process regression (GPR) prediction. This represents the uncertainty index for Gaussian process regression (GPR) prediction; simultaneously, the local plane normal vector is used. and obstacle set Characterizing local topographic features, among which Number of obstacles The two-dimensional coordinates of the obstacle in the local plane;

[0148] S442, Establish the motion model: Establish the differential driving model of UGV in the fitted plane coordinate system:

[0149] ;

[0150] in, To control the input, For time step; It is the normal vector of the local plane, e x It is a unit vector in direction, e y It is the unit vector in the y-direction. The direction of motion at time k;

[0151] S443, Objective Function Design: Construct an NMPC multi-objective optimization function to simultaneously minimize path tracking error and control energy consumption.

[0152] ;

[0153] in, To control the input, It is a two-dimensional vector representing the target point, by Obtain; two positive definite matrices and These are coefficients that measure terminal costs and control costs, respectively; weighting coefficients. ,in and These are the current local planes and The average value;

[0154] S444, Set constraints: The constraints that the optimization problem must satisfy include:

[0155] Equations of state ;

[0156] initial state ;

[0157] Safety obstacle avoidance constraints ;

[0158] ;

[0159] in, It is based on the weights of matrix A, for vectors A customized metric for the effective length. The two-dimensional coordinates of the obstacle are represented here. It is the transpose of x;

[0160] S445, Optimization Solution: Solve the optimization problem based on the set optimization objective function and constraints; during the optimization process, the prediction time domain is set to 1 second, containing N nodes, and planning is carried out in a rasterized two-dimensional top view space.

[0161] S5 transmits the final planned path as a motion command to the controller to guide the UGV in completing navigation.

[0162] In summary, the unmanned vehicle field scene passable area segmentation and navigation method provided in this embodiment improves upon the ground segmentation task, global safe path generation task, and smooth path and local obstacle avoidance task in unstructured field environments, overcoming the application limitations of existing UGV navigation frameworks in unstructured field environments.

[0163] The above embodiments are merely illustrative of the principles and effects of the present invention and are not intended to limit the invention. Any person skilled in the art can modify or alter the above embodiments without departing from the spirit and scope of the present invention. Therefore, all equivalent modifications or alterations made by those skilled in the art without departing from the spirit and technical concept disclosed in the present invention should still be covered by the claims of the present invention.

Claims

1. A method for segmenting and navigating passable areas in unmanned vehicle (UAV) wilderness scenarios based on air-ground cooperation, characterized in that, Includes the following steps: S1, Data Acquisition: The UAV terminal is equipped with a first sensor to collect global data and its own status data, and transmits them to the UGV vehicle terminal. The global data includes global point cloud data, UAV terminal attitude data, and UAV terminal geographic coordinates. The UGV vehicle terminal is equipped with a second sensor to collect local data, which includes local point cloud data, UGV vehicle terminal attitude data, and UGV vehicle terminal geographic coordinates. S2, dual SLAM algorithms are used to construct a layered map in parallel. The first SLAM algorithm is used to process the global data output from the UAV terminal to generate a global point cloud map; the second SLAM algorithm is used to process the local point cloud and IMU data output from the UGV vehicle terminal to generate a local point cloud map. S3. Construct a global accessible point cloud map. Based on the global point cloud map and the local point cloud map, construct a global accessible point cloud map using the CHSM-based accessible road segmentation method. S4, hierarchical path planning, uses the global traversable point cloud map as input for UGV navigation and employs the PLA-RRT algorithm to generate a global safe path; By combining real-time environmental data, the NMPC algorithm is used to perform local optimization of the global safe path to achieve real-time obstacle avoidance and path smoothing, and to obtain the final planned path. S5 transmits the final planned path as a motion command to the controller to guide the UGV in completing navigation.

2. The method for segmenting and navigating passable areas in wilderness scenarios based on air-ground cooperation for unmanned vehicles according to claim 1, characterized in that, The specific process of constructing a global accessible point cloud map using the CHSM-based accessible road segmentation method in step S3 includes the following steps: S31, Keyframe Point Cloud Extraction and Classification; Extract the current local keyframe point cloud from the global point cloud map. ;in A point in the Cartesian coordinate system. Let be the x-coordinate of the k-th point in the coordinate system. Let be the y-coordinate of the k-th point in the coordinate system. Let be the z-coordinate of the k-th point in the coordinate system. Based on the historical classification results of the global point cloud map, the keyframe point cloud is... Divided into estimated ground point sets And estimating non-ground point sets And both satisfy ; The classification objective is defined as: maximizing the estimated set of ground points. The proportion of real ground points in the middle, and maximizing the estimation of non-ground points. The proportion of real non-ground points in the middle; S32, CHSM partitioning, based on the spatial distribution characteristics of the local point cloud map, uses the CHSM model partitioning strategy to partition the point cloud region and aligns the local point cloud with the corresponding region of the global point cloud map. S33, RN noise removal, combines preset global height threshold and intensity threshold for noise monitoring and detection to eliminate abnormal noise points; S34, Region Plane Fitting: The centroid and covariance matrix of the points in the nth region after local filtering are calculated to obtain the eigenvalues. ; minimum eigenvalue The corresponding eigenvectors are used as planar vectors To obtain surface variables This enables regional planar fitting, and the fitting results are synchronously updated to the corresponding region of the global point cloud map. S35 allows for the estimation of the ground likelihood function. Based on the regional plane fitting results, a ground-estimated likelihood function is constructed. ,in, Parameters representing point clouds, Let f represent a random variable that follows a continuous probability distribution; combine prior knowledge with the verticality operator, height operator and flatness operator to calculate the probability density function f, and dynamically adjust the passability probability distribution in the global map; S36, Accessible map generation: Based on the probability density function f, the accessibility attributes of local keyframe point clouds are marked, thereby generating keyframe point clouds with accessibility markings, and obtaining a global accessible point cloud map.

3. The method for segmenting and navigating passable areas in wilderness scenarios based on air-ground cooperation for unmanned vehicles, as described in claim 2, is characterized in that... CHSM partitioning, based on the spatial distribution of local point cloud maps, employs a CHSM model partitioning strategy to align corresponding regions of local point clouds with global point cloud maps. The specific process includes: S321, using the CHSM model to divide the keyframe point cloud H into four sub-regions according to radial distance, the CHSM model is then expressed as: ； ； in, Indicates all regions. Indicates the first Each region Indicates the total number of regions; express The minimum radial boundary, express Maximum radial boundary, This represents each point in the keyframe point cloud H; S322, for each point in the keyframe point cloud H Translate it from Cartesian coordinates ( Convert to polar coordinates ( ), , ,in Let be the polar radius of the point. Let be the polar angle of the point; where, For the four-quadrant inverse tangent function; [the function is then applied to each...] Further divided into Non-uniform cells ,in, and These are the number of regions divided along the polar axis and polar radius, respectively, and are defined as follows: ； in, sub-region radial length, , ,in For point The polar radius, Called a point polar angle, and These represent the number of regions divided along the polar axis and polar radius, respectively. S323, the four sub-regions are arranged in ascending order of radial distance: Central area Divided into four zones For half zone, This is the outer region; the boundaries of each region satisfy a recursive relationship: The specific boundary values ​​are: ； ， ， ； Among them, the global minimum boundary Used to cover the blind spot around the 3D LiDAR sensor, i.e. The data is invalid at that time; the local CHSM partition is aligned with the corresponding area of ​​the global map through a coordinate transformation matrix to ensure that areas with the same index have the same spatial position in the global coordinate system.

4. The method for segmenting and navigating passable areas in unmanned vehicle wilderness scenarios based on air-ground cooperation as described in claim 3, characterized in that, The implementation method of step S35 includes the following steps: Based on the region plane fitting results, the likelihood function is defined. Ground-based parameter estimation Perform modeling; Based on the assumption of cell independence, the likelihood function is decomposed into a product form: ； in, express The parameters, express random variable, subscript This indicates that these parameters all come from , Let represent a random variable that follows a continuous probability distribution. This represents a preliminary estimated set of ground points; Based on prior knowledge, the perpendicularity operator is used. Height operator Flatness operator calculate f is the probability density function of the ground. ； in, The average value of the point cloud value, Represents the nth partition The distance from the origin to the center of mass. Represents surface variables; The verticality operator is defined as follows: If If it is the real ground, then its normal vector It should be orthogonal to the passable ground, i.e., the normal vector. A perpendicularity index function is introduced, perpendicular to the XY plane of the sensor. As a basis for judgment: ； in, ; Indicates the vertical boundary, that is Angle with the XY plane; set empirically. ; The height operator is defined as follows: ； in, Indicates average value, It is an adaptive midpoint function, which changes with... The increase is exponential, Represents the nth partition The distance from the origin to the center of mass. The preset maximum distance threshold; The flatness operator is defined as: ； in, Indicates the gain coefficient. The value is greater than 1. The surface variable representing the current partition. This indicates a region. Surface variable threshold; The final set of estimated ground points is expressed as: ； Where [·] represents Iverson brackets, it returns true if the condition is met, otherwise it returns false. Represents all estimated ground points. This represents the ground dividing point of the nth partition. This represents the total number of partitions. express The parameters, express A random variable.

5. The method for segmenting and navigating passable areas in wilderness scenarios based on air-ground cooperation for unmanned vehicles according to claim 1, characterized in that, The implementation process of step S4 includes the following steps: S41, Point cloud downsampling: The global passable point cloud map obtained in step S3 is processed using point cloud downsampling and 3D rasterization methods to reduce data redundancy. S42, Global Path Generation: The PLA-RRT* global safe path generation algorithm is used to quantify the uncertainty of ground accessibility and generate a sparse global trajectory set S, thus obtaining a global path that balances real-time performance and accessibility. S43, Dense Path Generation, uses Gaussian process (GPR) regression prediction and linear interpolation to transform the sparse global trajectory set S into dense trajectories. S44, based on dense trajectories, establishes a local planner based on NMPC, and introduces environmental perception information and dynamic constraints to construct an NMPC optimization problem. By solving the NMPC optimization problem in real time, the final path output of the UGV is generated, thereby realizing the trajectory tracking and local obstacle avoidance of the UGV.

6. The method for segmenting and navigating passable areas in unmanned vehicle wilderness scenarios based on air-ground cooperation as described in claim 5, characterized in that, The specific process of generating the global path in step S42 includes: S421, Algorithm Framework Construction and Node Definition: The PLA-RRT* Global Safe Path Generation Algorithm generates an optimal path that balances real-time performance and accessibility while maintaining the characteristics of the fast exploration random tree algorithm. The node elements in the RRT tree within the PLA-RRT* Global Safe Path Generation Algorithm are represented as follows: ,in, express A local plane centered on the node. Indicates passability; The higher the value, the worse the ground conditions. 0 means absolutely passable, and 1 means the terrain is completely unsuitable for UGV passage. S422, Design the cost function between two nodes: The passability cost function between them is defined as: ； Wherein, 𝜔 is a penalty scale factor. For nodes and The Euclidean distance between them and The accessibility index represents the fit plane containing the two nodes; If the cost function If the set threshold is exceeded, it is determined that the safe passage of UGVs between the two nodes cannot be supported, even if both nodes are under the globally passable point cloud map; the algorithm's solution goal is not only to shorten the path length, but also to avoid those difficult-to-pass paths; S423, Quantification of the accessibility index of a node: Defining the slope Flatness sparsity Based on slope Flatness sparsity Quantify the accessibility of computing nodes to provide a quantifiable basis for safety judgment in path search; slope Flatness sparsity The definition is as follows: ； ； ； in and The coefficients are constants. Represents the plane normal vector in the global coordinate system The projection onto the Z-axis; N represents the total number of points. This represents the z-axis unit vector of the j-th point in the global coordinate system. The projection on the Z-axis, This indicates the proportion of the empty space on the plane. Indicates the minimum acceptable porosity. Indicates the maximum acceptable void ratio; Represents the trace of all point sets, when lie in and At different times, the gap points correspond to different situations; when the gaps are concentrated, the trace value is less than the preset value. This indicates that there may be potholes and depressions in the ground; Based on slope Flatness sparsity The comparability of the fitted plane containing the node can be obtained through the exponent. The calculation formula is as follows: ； in, , and It is a weight, and satisfies ; Indicates the maximum permissible slope. Indicates the maximum permissible flatness. This represents the maximum allowable sparsity; exceeding these three values ​​will cause the robot to become unable to move or to tip over. S424 iteratively expands the RRT tree structure and samples nodes within the globally accessible point cloud map. When the target point is included in the tree structure, the initial global path is obtained. Based on this, the path is progressively optimized by continuously adding random samples, and combined with a timed detection mechanism, the path is dynamically adjusted when the UGV pose is abnormal.

7. A method for segmenting and navigating passable areas in wilderness scenarios for unmanned vehicles based on air-ground cooperation, as described in claim 6, is characterized in that... The specific process of generating dense paths in step S43 includes the following steps: Define the dense path point set after interpolation as: ； in, By analyzing the global path point set Obtain by linear interpolation This indicates the number of path nodes before interpolation encryption. This indicates the number of path nodes after interpolation and encryption. This represents the coordinates of the path nodes before interpolation and encryption. This represents the path node coordinates after interpolation and encryption. This represents the passability index of the path nodes after interpolation and encryption. With tree node set The training set for Gaussian process regression, where Represents the node coordinates. To correspond to the accessibility index; interpolate the path point set. The test set is input into a pre-trained Gaussian process model, and the output is a predicted mobility index. The Gaussian process employs radial basis functions. As a covariance function, where Let be the signal standard deviation, and l be the length scale parameter.

8. The method for segmenting and navigating passable areas in an unmanned vehicle's field scenario based on air-ground cooperation, as described in claim 7, is characterized in that... The implementation method of step S44 includes the following steps: S441, Waypoint Set Construction: Based on the dense paths obtained in step S43, construct the desired waypoint set. ;in, Represents the three-dimensional coordinates of each waypoint on the fitted plane. ; It is a feasibility indicator for Gaussian process regression (GPR) prediction. This represents the uncertainty index for Gaussian process regression (GPR) prediction; simultaneously, the local plane normal vector is used. and obstacle set Characterizing local topographic features, among which The number of obstacles. The two-dimensional coordinates of the obstacle in the local plane; S442, Establish the motion model: Establish the differential driving model of UGV in the fitting plane coordinate system: ； in, To control the input, For time step; It is the normal vector of the local plane, e x It is a unit vector in direction, e y It is the unit vector in the y-direction. The direction of motion at time k; S443, Objective Function Design: Construct an NMPC multi-objective optimization function to simultaneously minimize path tracking error and control energy consumption. ； in, To control the input, It is a two-dimensional vector representing the target point, by Obtain; two positive definite matrices and These are coefficients that measure terminal costs and control costs, respectively; weighting coefficients. ,in and These are the current local planes and The average value; S444, Set constraints: The constraints that the optimization problem must satisfy include: Equations of state ; initial state ; Safety obstacle avoidance constraints ; ； in, It is based on the weights of matrix A, for vectors A customized metric for the effective length. The two-dimensional coordinates of the obstacle are represented here. It is the transpose of x; S445, Optimization Solution: Solve the optimization problem based on the set optimization objective function and constraints; during the optimization process, the prediction time domain is set to 1 second, containing N nodes, and planning is carried out in a rasterized two-dimensional top view space.

9. A method for segmenting and navigating passable areas in wilderness scenarios based on air-ground cooperation for unmanned vehicles, as described in claim 1, is characterized in that... The global data and its own status data collected by S1 are transmitted to the UGV vehicle end through the ROS architecture.

10. A method for segmenting and navigating passable areas in an unmanned vehicle's field scenario based on air-ground cooperation, as described in claim 1, is characterized in that... Both the UAV receiver and the UGV vehicle are equipped with sensors including LiDAR, IMU, and GPS. The radar is used to acquire point cloud data, the IMU is used to acquire attitude data, and the GPS is used to acquire geographic location coordinates.

Images