Ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion

Abstract

This invention discloses a ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion. The method includes: generating sub-map sets from local point cloud data collected by fire trucks and drones; and generating rotation-invariant semantic descriptors using a maximum height value rasterization strategy and rotation group enhancement. By calculating geometric and semantic distances, and combining isomorphic and heterogeneous pattern weighted fusion, candidate sub-map pairs with loop closures are selected and geometrically registered to generate loop closure constraints. Simultaneously, ground point clouds are extracted using the RANSAC algorithm, and the ground plane model is smoothly updated using a sliding window to construct height and pose alignment constraints. The loop closure constraints, ground plane constraints, and pose information are input into the Ceres optimization framework for joint optimization to generate a globally consistent overall map, effectively improving the accuracy and robustness of ground-air collaborative mapping, and is suitable for emergency scenarios such as complex fire rescue.

Description

Technical Field

[0001] This invention belongs to the interdisciplinary fields of localization and mapping and computer vision, specifically involving a ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion. Background Technology

[0002] In the fields of emergency rescue and public safety, especially in large-scale urban fire rescue scenarios, real-time and accurate environmental mapping plays a crucial role in rescue route planning, locating trapped personnel, and dispatching rescue equipment. With increasing urban building density and the rise in the number of high-rise buildings, fire scene environments are becoming increasingly complex. For example, high temperatures and smoke obscure visual image contrast, structural collapses cause abrupt terrain changes, and dynamic flame interference distorts local features. Traditional manual reconnaissance is inefficient and poses safety risks; therefore, intelligent rescue equipment such as unmanned fire trucks (UGVs) and multi-rotor rescue drones (UAVs) are gradually becoming core support for fire rescue.

[0003] In missions such as unmanned aerial vehicle (UGV) reconnaissance, unmanned aerial vehicle (UAV) situational awareness, and material transport in hazardous areas, the aforementioned equipment relies on a ground-air collaborative environmental mapping system to generate 3D scene models to support safe route planning, location of trapped personnel, and equipment dispatch. Typically, firefighting UGVs are equipped with LiDAR and surround-view cameras, while UAVs are equipped with overhead cameras and LiDAR. Their flexible deployment, cost-effectiveness, and ability to quickly cover complex areas make them the primary sensing units for environmental mapping.

[0004] However, the extreme environment and equipment characteristics at fire scenes present numerous challenges to accurate mapping:

[0005] 1. Differences in perspective lead to difficulties in cross-platform matching: The difference between the UGV's head-up perspective (height 1-2 meters) and the UAV's top-down perspective (height 10-50 meters) is significant. Traditional mapping methods that rely on geometric features (such as point cloud matching based on NetVLAD) are sensitive to changes in perspective. The geometric similarity of the same building is low under the two perspectives, resulting in insufficient heterogeneous loop recall.

[0006] 2. Sparse features lead to positioning drift: Fire causes building structures to collapse and the ground to be covered with burned debris. In open burned land or untextured road scenes, the number of LiDAR feature points of UGV is reduced. Traditional pure geometric SLAM is prone to cumulative drift, and the pose error increases with the driving distance.

[0007] 3. Insufficient robustness of ground plane constraints: Existing ground plane constraint methods rely solely on point cloud fitting, without incorporating semantic information such as "burned ground" and "uncollapsed areas," and they do not coordinate with loop closure detection, making it impossible to dynamically correct fitting errors and limiting constraint accuracy.

[0008] In the prior art, Chinese patent application CN114782254A discloses a point cloud loop closure detection method based on NetVLAD, but it has the following drawbacks: it relies solely on geometric features and does not incorporate semantic representation technology, making it difficult to address the perspective differences between UGV and UAV, resulting in low recall for heterogeneous loop closures; it lacks data augmentation and multi-round geometric verification, leading to high false matching rates when faced with feature distortion caused by factors such as smoke in fire scenes. Furthermore, Chinese patent application CN116152234A discloses a rotation-invariant descriptor method based on ScanContext, which improves perspective robustness but still suffers from the limitation of "heavy on geometry, light on semantics": it does not incorporate visual semantic models such as SigLIP, making it unable to filter out false matches that are "geometrically similar but semantically unrelated" through semantic consistency; it does not integrate UGV / UAV multimodal features, resulting in insufficient descriptor discriminative power in occluded scenes and a high false detection rate for isomorphic loop closures.

[0009] Furthermore, existing ground plane constraint techniques are not coordinated with computer vision: they rely solely on point cloud fitting, resulting in low confidence in fitting non-flat regions; constraint weights are fixed and cannot be dynamically adjusted according to different ground types; and coordination with loop closure detection remains at the physical level, without dynamically adjusting constraint priorities based on semantic information, making it easy to introduce errors and biases in scenarios with low semantic confidence.

[0010] Therefore, there is an urgent need for a method that can integrate visual semantic representation, multimodal geometric verification, and dynamic ground plane constraints, overcome the bottleneck of perspective difference through semantic features, suppress positioning drift by combining physical constraints, realize ground-air collaborative mapping of fire UGV and UAV, and generate a globally consistent master map. Summary of the Invention

[0011] The purpose of this invention is to overcome the shortcomings of the existing technology and provide a ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion.

[0012] The objective of this invention can be achieved through the following technical solutions:

[0013] This invention provides a ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion, comprising the following steps:

[0014] Local point cloud data is collected by fire trucks and drones, and sub-map sets are generated according to time windows, and corresponding pose information is recorded; the sub-map sets include multiple sub-maps, and each sub-map includes local point cloud data collected within a certain time window.

[0015] The local point cloud data of the submap is projected into a standardized top-down tile image, and a grayscale image is generated using a maximum height value rasterization strategy.

[0016] The grayscale image is rotated at multiple angles to generate a rotated version image, and rotation-invariant semantic descriptors are extracted using a visual semantic model;

[0017] Based on the semantic descriptor, the geometric distance and semantic distance between each sub-map and all sub-maps in the preceding time window are calculated respectively. The distances are then weighted and fused according to the isomorphic or heterogeneous patterns to generate loop closure candidate scores, and loop closure candidate sub-map pairs are selected.

[0018] Geometric registration is performed on the selected candidate sub-map pairs of loop closures, and loop closure constraints are generated based on the registration results;

[0019] Ground point cloud is extracted from the point cloud of the fire truck, and plane fitting is performed using the RANSAC algorithm. The plane normal vector and offset are smoothly updated by sliding window, and height constraints and attitude alignment constraints are constructed to generate ground plane combined constraints.

[0020] The loop closure constraints, ground plane combination constraints, sub-maps and corresponding pose information are input into the Ceres optimization framework to jointly optimize the pose information and generate a globally consistent master map.

[0021] Furthermore, the process of collecting local point cloud data using fire trucks and drones, and generating sub-map sets based on time windows, specifically includes:

[0022] Firefighting unmanned vehicles and drones simultaneously collect local point cloud data, and divide the continuously collected point cloud data into several subsets according to a preset time window;

[0023] For each subset, a sub-map is generated, and the corresponding acquisition time and pose information are recorded to obtain the sub-map set. The pose information includes the rotation quaternion and three-dimensional translation vector of the fire truck or drone when generating the sub-map.

[0024] Furthermore, the process of projecting the local point cloud data of the sub-map into a standardized top-down tile image and generating a grayscale image using a maximum height value rasterization strategy specifically includes:

[0025] Each point in the local point cloud data of the submap Mapping to image coordinates The formula is:

[0026]

[0027] in, For the local point cloud data, the first i The three-dimensional coordinates of a point in the local coordinate system; It is half the length of the tile; , These represent the pixel resolution of the image in the horizontal and vertical directions, respectively. This represents the rounding function; For the first i The coordinates of each point mapped to the image coordinate system;

[0028] For multiple points mapped to the same pixel, the maximum height value strategy is used to calculate the height value of each pixel, using the formula:

[0029]

[0030] in, Indicates pixel position The height value; Indicates mapping to pixel position All points Coordinate set;

[0031] The maximum height value is linearly normalized to the grayscale value range [0, 255] to generate a grayscale image. The formula is:

[0032]

[0033] in, Represents the pixel position in a grayscale image grayscale value; , These are the minimum and maximum height values ​​for all points in the submap, respectively.

[0034] Furthermore, the step of rotating the grayscale image at multiple angles to generate a rotated version image, and extracting rotation-invariant semantic descriptors using a visual semantic model, specifically includes:

[0035] For each grayscale image Generate four rotated versions of the image, with rotation angles of respectively. , forming a rotation group ;

[0036] Each rotated version of the image is preprocessed, including color space conversion, size adjustment, normalization, standardization, and dimensionality transformation.

[0037] Use a visual semantic model for each preprocessed rotated version image Extract the embedding vector and perform L2 normalization, as shown in the formula:

[0038]

[0039] in, For rotated version image The embedding vector; This represents the inference function of the visual semantic model; Represents grayscale image After rotation The rotated version of the image; Represents the L2 norm;

[0040] The embedding vectors of the four rotated versions of the image are averaged and L2 normalized to generate rotation-invariant semantic descriptors, as shown in the formula:

[0041]

[0042] in, This represents the average of the embedding vectors of the four rotated versions of the image; It is a rotation-invariant semantic descriptor.

[0043] Furthermore, based on the semantic descriptor, the geometric distance and semantic distance are calculated for each sub-map and all sub-maps within the preceding time window, and the distances are weighted and fused according to isomorphic or heterogeneous patterns to generate loop closure candidate scores, and loop closure candidate sub-map pairs are selected, specifically including:

[0044] For each submap in the submap set as the current submap, calculate the geometric distance between the current submap and all submaps within the previous time window, using the following formula:

[0045]

[0046] in, Represents geometric distance; , These represent the NetVLAD descriptors for the current submap and a specific submap within a previous time window, respectively. The L2 norm is represented; the preceding time window refers to the set of submaps within all time windows preceding the current submap's corresponding time window;

[0047] Based on the semantic descriptor, calculate the semantic distance between the current submap and all submaps within the previous time window, using the following formula:

[0048]

[0049] in, Indicates semantic distance; , These represent semantic descriptors for the current submap and a specific submap within a preceding time window, respectively. Represents the dot product of two semantic descriptors;

[0050] The fusion weights of geometric distance and semantic distance are dynamically adjusted based on homogeneous or heterogeneous patterns to generate loop closure candidate scores, specifically including:

[0051] When the detection mode is isomorphic, meaning the two sub-maps are acquired by the same machine type, an equal-weight fusion strategy is used to calculate the loop closure candidate score, using the following formula:

[0052]

[0053] in, For loop candidate scores;

[0054] When the detection mode is heterogeneous, meaning the two sub-maps are acquired by different machine types, a semantic-driven strategy is used to calculate the loop closure candidate score, using the following formula:

[0055]

[0056] The types of machines include fire-fighting unmanned vehicles and drones;

[0057] When the loop closure candidate score is less than the first preset threshold, the submap of the corresponding previous time window and the current submap are used as loop closure candidate submap pairs.

[0058] Each submap in the submap set is used as the current submap for filtering, resulting in all candidate loop-loop submap pairs.

[0059] Furthermore, the step of geometrically registering the filtered candidate loop closure sub-map pairs and generating loop closure constraints based on the registration results specifically includes:

[0060] For the point cloud data of the two submaps in each loopback candidate submap pair, preprocessing is performed, including voxel downsampling and secondary downsampling;

[0061] For the two preprocessed sub-maps, the transformation matrix is ​​calculated using a method based on ground normal vector alignment and centroid calculation; then, coarse registration of the two sub-maps is performed using a normal distribution transformation based on the transformation matrix.

[0062] For the coarsely registered sub-map, fine registration is performed using the generalized iterative nearest point method;

[0063] After fine registration, convergence is checked. If convergence is not achieved, loop closure candidate submap pairs are deleted. If convergence is achieved, the Fitnessscore is checked. When the Fitnessscore is less than a second preset threshold, the loop closure candidate submap pairs are considered valid, and loop closure constraints are constructed. The Fitnessscore is an indicator for measuring the registration effect, representing the matching quality between submaps. The smaller the value, the better the registration effect.

[0064] Furthermore, the closure constraint includes:

[0065] The timestamp indicates the time when the two submaps in the loopback candidate submap pair were collected;

[0066] Robot ID, which identifies the robot corresponding to each submap in the loopback candidate submap pair;

[0067] Submap ID, representing a unique identifier for each submap in the loopback candidate submap pair;

[0068] Relative pose, including rotation quaternions Translation vector The pose information of the two submaps in the loop candidate submap pair is calculated.

[0069] Furthermore, the process of extracting ground point cloud from the fire truck point cloud, performing plane fitting using the RANSAC algorithm, smoothly updating the plane normal vector and offset using a sliding window, and constructing height constraints and attitude alignment constraints to generate ground plane combined constraints specifically includes:

[0070] Acquire point cloud data of fire-fighting unmanned vehicles, and separate the ground point cloud from the fire-fighting unmanned vehicle point cloud data by setting a distance threshold;

[0071] Based on the ground point cloud, a ground plane model is constructed by performing plane fitting using the RANSAC algorithm.

[0072] The plane normals and offsets of the constructed ground plane model are smoothly updated using a sliding window.

[0073] Based on the smoothly updated ground plane model, construct ground plane combination constraints.

[0074] Furthermore, the ground plane combination constraint includes height constraint and attitude alignment constraint;

[0075] The height constraint includes: the height constraint residual formula, expressed as:

[0076]

[0077] in, Indicates highly constrained residuals, This represents the ground plane normal vector of the smoothly updated ground plane model; The translation vector of the fire truck represents the coordinates of its current position, which is calculated by solving the current pose of the fire truck based on the ground plane reference system determined by the smoothly updated ground plane model. Indicates the offset between the ground plane and the origin; This indicates the height of the unmanned fire truck.

[0078] The attitude alignment constraint includes: the attitude alignment constraint residual formula, expressed as:

[0079]

[0080] in, This represents the attitude alignment constraint residual; Rotation matrix for fire-fighting unmanned vehicles z The column vector represents the vertical axis of the unmanned fire truck; This represents the vector cross product operation.

[0081] Furthermore, the step of inputting loop closure constraints, ground plane combination constraints, sub-map sets, and corresponding pose information into the Ceres optimization framework to jointly optimize the pose information and generate a globally consistent overall map specifically includes:

[0082] The pose information of the fire truck is represented as a rotation quaternion and a three-dimensional translation vector, and used as optimization variables in the Ceres optimization framework.

[0083] The loop constraint residuals of the loop constraint, the height constraint residual of the ground plane combination constraint, and the attitude alignment constraint residual are used as optimization terms. The optimization objective is to minimize the sum of the loop constraint residuals, height constraint residuals, and attitude alignment constraint residuals. A Ceres optimization solution strategy is set, and the optimal pose information is obtained through iterative solution. The loop constraint residual is the difference between the relative pose of the loop constraint and the optimal pose calculated in the optimization framework. The optimization objective is expressed as:

[0084]

[0085] in, , The unmanned fire trucks are respectively i The rotation quaternion and 3D translation vector of each submap, i.e., the optimal pose; The total number of sub-maps containing unmanned fire trucks; Indicates highly constrained residuals; This represents the attitude alignment constraint residual; This represents the set of all pose constraints generated from the vehicle's odometer information. This represents the set of candidate submap pairs of all loop closure constraints generated by loop closure detection; This represents the loop closure constraint residual; the formula for the loop closure constraint residual is:

[0086]

[0087] in, The Ceres optimization framework predicts the transformation matrix based on the pose information of the two submaps in a submap pair; This is the transformation matrix corresponding to the relative pose in the closure constraint;

[0088] The optimized pose information is updated to the sub-map set, and the local point cloud data of each sub-map is transformed according to the optimized pose information.

[0089] All transformed sub-map point cloud data are stitched together and merged to generate a globally consistent master map.

[0090] Compared with the prior art, the present invention has the following advantages:

[0091] (1) In existing technologies, there are significant differences between the head-up view of UGVs and the top-down view of UAVs. Traditional methods that rely solely on geometric features (such as NetVLAD) are difficult to achieve cross-platform matching, resulting in low heterogeneous loop closure recall and large loop closure detection errors. This invention integrates geometric features (NetVLAD) and visual semantic features (SigLIP2), and performs rotation group enhancement on the grayscale image of the sub-map to generate rotation-invariant semantic descriptors, thereby achieving the robustness of semantic features to viewpoint changes. Robust loop closure associations are established between different platforms (autonomous vehicles and drones). Semantic features are more robust to viewpoint changes (the concept of "buildings" is not affected by viewpoint), significantly improving the heterogeneous loop closure recall. On a real urban ground-air collaborative dataset, the cross-platform loop closure recall is significantly improved. By introducing SigLIP2 semantic features, semantic features are more robust to viewpoint changes and can maintain high similarity even with large viewpoint differences, thus significantly improving the heterogeneous loop closure recall.

[0092] (2) Existing technologies lack full utilization of scene semantic content, which can easily lead to mismatches in complex urban environments. This invention integrates geometric distance and semantic distance, and dynamically adjusts the fusion weight according to the homogeneous or heterogeneous mode. The homogeneous mode adopts equal weight fusion to make full use of the two features, while the heterogeneous mode adopts a semantic-dominant strategy (0.4×geometric + 0.6×semantic) but retains geometric information as an auxiliary, thus avoiding pure semantic misleading, thereby improving recall while maintaining high accuracy.

[0093] (3) Existing methods lack rotation invariance when extracting semantic features, resulting in large differences in descriptors when entering the same location from different directions. This invention enhances the robustness to changes in direction by averaging and L2 normalizing the embedding vectors of the four rotated versions through rotation group enhancement (C4 group, 0°, 90°, 180°, 270°).

[0094] (4) In the prior art, due to the presence of open burned-out areas and textureless roads at fire scenes, UGVLiDAR feature points are scarce, and pure geometric SLAM is prone to cumulative drift, resulting in low global mapping accuracy. This invention uses geometric registration of loop closure candidate submap pairs (including NDT coarse registration and GICP fine registration), combined with relative pose to generate loop closure constraints, and utilizes dynamic parameter adjustments (NDT resolution, GICP iteration count, and corresponding point distance in isomorphic / heterogeneous modes) to effectively suppress cumulative drift of UGV in feature-sparse scenes and improve pose estimation accuracy.

[0095] (5) In existing technologies, ground plane constraints rely solely on point cloud fitting without incorporating semantic information from the fire scene, resulting in low fitting confidence and unstable constraint effects. This invention extracts ground point clouds from unmanned fire trucks, fits the ground plane using the RANSAC algorithm, and smoothly updates the plane normal vector and offset via a sliding window, constructing a combined ground plane constraint of height and attitude alignment. This feature enables stable operation of ground plane constraints in low-quality point clouds or non-flat regions, dynamically adjusts constraint weights, effectively suppresses positioning drift, and ensures mapping accuracy.

[0096] (6) In existing technologies, loop closure detection and ground plane constraints do not form a synergistic optimization, making it impossible to simultaneously utilize physical constraints to suppress pose drift and loop closure constraints to correct errors. This invention, by inputting loop closure constraints, ground plane combined constraints, and sub-map pose information into the Ceres optimization framework, jointly optimizes the pose parameters of UGV and UAV, achieving globally consistent pose estimation. Simultaneously, ground plane constraints provide an initial transformation matrix based on the ground plane normal vector for NDT coarse registration, improving the convergence speed of coarse registration, while the effective pose constraints confirmed by loop closure detection inversely correct the ground plane fitting error, achieving bidirectional synergistic optimization. This technical feature significantly improves the global consistency and robustness of ground-air collaborative mapping in heterogeneous perspectives and cross-scale scenarios. Attached Figure Description

[0097] Figure 1 This is a flowchart of the ground-air collaborative mapping optimization method according to an embodiment of the present invention;

[0098] Figure 2 This is a flowchart illustrating the construction process of ground plane combination constraints according to an embodiment of the present invention. Detailed Implementation

[0099] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0100] Example 1:

[0101] This embodiment provides a ground-to-air collaborative mapping optimization method based on semantic features and ground plane multi-constraint fusion, including three core components: ground-to-air collaborative loop closure detection based on semantic feature fusion, pose estimation based on ground plane constraints, and collaborative optimization of the two; as follows Figure 1 As shown, the specific steps include:

[0102] Step S1: Collect local point cloud data using fire trucks and drones, generate sub-map sets according to time windows, and record corresponding pose information;

[0103] The sub-map set includes multiple sub-maps, each of which includes local point cloud data collected within a certain time window. Step S1 specifically includes: fire trucks and drones simultaneously collect local point cloud data; a preset time window divides the continuously collected point cloud data into several subsets; a sub-map is generated for each subset, and the corresponding collection time and pose information are recorded to obtain the sub-map set.

[0104] Step S2: Project the local point cloud data of the sub-map into a standardized top-down tile image, and generate a grayscale image using a maximum height value rasterization strategy. Specifically, this includes:

[0105] Each point in the local point cloud data of the submap Mapping to image coordinates The formula is:

[0106]

[0107] in, Meters represent half the length of the tile, defining the coverage area as... ,Right now The area; Image resolution, representing the resolution in the ground coordinate system. The resolution of the grid in the horizontal and vertical directions; Indicating the first point cloud The coordinates of a point in the local coordinate system; This represents the pixel coordinates of the mapped image. This represents a rounding function that rounds a floating-point number to the nearest integer, used to convert continuous coordinate values ​​into discrete pixel coordinates. Since the v-axis of the image coordinate system and the y-axis of the map coordinate system are in opposite directions, therefore... Take the negative number.

[0108] When multiple points are mapped to the same pixel, the maximum height value is used. Strategy: Record the position of each pixel The largest value: ,in, Indicates pixel position Maximum height value at; Represents all mapped pixel locations point Coordinate set. This strategy can preserve information about high-rise structures such as buildings and vegetation, which is crucial for loop closure detection.

[0109] The maximum height value is linearly normalized to the grayscale value range [0, 255] to generate a grayscale image. The formula is:

[0110]

[0111] in, Represents the pixel position in a grayscale image grayscale value; , These are the minimum and maximum height values ​​for all points in the submap, respectively. The generated grayscale image retains geometric information such as buildings and roads.

[0112] Check the number of occupied pixels. If it is less than 50 pixels, the point cloud is considered too sparse, and tile generation is skipped.

[0113] Step S2 is to convert the 3D point cloud into an image representation suitable for processing by the visual semantic model (SigLIP2), adapting it to the input requirements of the deep learning model by fixing the output size (256×256), while preserving the structural information of the scene by encoding the height value.

[0114] Step S3: Rotate the grayscale image at multiple angles to generate a rotated version image, and extract rotation-invariant semantic descriptors using a visual semantic model, specifically including:

[0115] For each grayscale image Generate four rotated versions of the image, with rotation angles of respectively. , forming a rotation group This step allows the robot to find similar features even if it enters the same location from different directions.

[0116] Each rotated version of the image is preprocessed, including: color space conversion: BGR→RGB; size adjustment: resize to 224×224 pixels; normalization: [0,255]→[0,1]; standardization: (img-0.5) / 0.5; dimensional transformation: HWC→CHW, and add batch dimension → [1,3,224,224];

[0117] Use a visual semantic model for each preprocessed rotated version image Extract the embedding vector and perform L2 normalization, as shown in the formula:

[0118]

[0119] in, For rotated version image The embedding vector; This represents the inference function of the visual semantic model; Represents grayscale image After rotation The rotated version of the image; Represents the L2 norm;

[0120] The embedding vectors of the four rotated versions of the image are averaged and L2 normalized to generate rotation-invariant semantic descriptors, as shown in the formula:

[0121]

[0122] in, This represents the average of the embedding vectors of the four rotated versions of the image; This is a rotation-invariant semantic descriptor. The final result is a 768-dimensional L2-normalized semantic descriptor, stored in the sub-map data structure.

[0123] Step S3 is to extract feature descriptors that can understand the semantic content of the scene. Rotation group enhancement is used to improve robustness to changes in direction, so that the descriptors can maintain high similarity when entering the same location from different directions.

[0124] Step S4: Based on the semantic descriptor, calculate the geometric distance and semantic distance between each sub-map and all sub-maps within the preceding time window, and perform weighted fusion of the distances according to isomorphic or heterogeneous patterns to generate loop closure candidate scores. Filter loop closure candidate sub-map pairs, specifically including:

[0125] For each submap in the submap set as the current submap, calculate the geometric distance between the current submap and all submaps within the previous time window, using the following formula:

[0126]

[0127] in, Represents geometric distance; , These represent the NetVLAD descriptors for the current submap and a specific submap within a previous time window, respectively. The L2 norm is represented; the preceding time window refers to the set of sub-maps within all time windows preceding the current sub-map's corresponding time window; the NetVLAD descriptor acquisition process includes: extracting the contours of obstacles from the LiDAR point cloud of each sub-map, estimating the ground plane normal vector using the RANSAC algorithm, filtering out points parallel to the ground, projecting the remaining points onto the estimated ground plane, and mapping the projected points onto the XY plane to generate a pixelated thumbnail with a coverage area of ​​50m×50m, generating a new thumbnail every 40 frames; then, using the NetVLAD algorithm to extract features from the generated thumbnails, encoding the image into a fixed-length compact vector (NetVLAD descriptor) to represent the geometric features of the sub-map.

[0128] Based on the semantic descriptor, calculate the semantic distance between the current submap and all submaps within the previous time window, using the following formula:

[0129]

[0130] in, Indicates semantic distance; , These represent semantic descriptors for the current submap and a specific submap within a preceding time window, respectively. Represents the dot product of two semantic descriptors;

[0131] The fusion weights of geometric distance and semantic distance are dynamically adjusted based on homogeneous or heterogeneous patterns to generate loop closure candidate scores, specifically including:

[0132] When the detection mode is isomorphic, meaning the two sub-maps are acquired by the same machine type, an equal-weight fusion strategy is used to calculate the loop closure candidate scores. ; For loop candidate scores;

[0133] When the detection mode is heterogeneous, meaning the two sub-maps are acquired by different machine types, a semantic-driven strategy is used to calculate loop closure candidate scores. The types of machines include fire-fighting unmanned vehicles and drones.

[0134] Candidate search, using a KD-tree to search for candidate submaps in space:

[0135] Homogeneous mode: Search the nearest k=5 submaps, and only search the same robot;

[0136] Heterogeneous mode: Search the nearest k=200 (initialization phase) or k=8 (normal phase) sub-maps, and search all other robots.

[0137] In the heterogeneous mode, more candidates are searched during the initialization phase to improve recall, while the number of candidates is reduced during the normal phase to improve computational efficiency.

[0138] Candidate ranking and screening, with all candidates scored by fusion. Sort in ascending order (lower is better). Candidate selection criteria: Isomorphic pattern: Candidates with a fusion score < 0.9 and a relative angle < 60° will proceed to geometric verification;

[0139] Heterogeneous mode: Geometric verification can proceed if one of the following conditions is met:

[0140] Semantic distance <0.15 (initialization phase) or <0.10 (normal phase);

[0141] The relative angle is less than 135°, so the robot is currently a drone.

[0142] Step S4 aims to achieve a more reasonable candidate ranking and screening strategy by fusing geometric and semantic features and dynamically adjusting the fusion weights according to the homogeneous / heterogeneous patterns, thereby improving the recall and precision of loop closure detection.

[0143] Step S5: Perform geometric registration on the filtered candidate loop closure sub-map pairs, and generate loop closure constraints based on the registration results, specifically including:

[0144] Step S501: Point cloud preprocessing, preprocessing the point clouds of the candidate sub-maps:

[0145] 1. Voxel downsampling: The first downsampling is performed using a voxel size of 0.8m;

[0146] 2. Secondary downsampling: A second downsampling was performed using a voxel size of 1.8m (for NDT registration).

[0147] Step S502: Initial pose estimation, generating the initial transformation matrix based on ground normal alignment and centroid translation calculation: 1. Ground normal alignment: Calculate the ground plane normals of the two sub-maps and align them using a rotation matrix; 2. Centroid translation calculation: Calculate the centroids of the two point clouds and align them using a translation vector; 3. Initial transformation matrix: Combine rotation and translation to generate the initial transformation matrix `init_guess`;

[0148] For heterogeneous modes, if a relative pose estimate is already available, two initial guesses are tried simultaneously.

[0149] Step S503: Coarse registration using NDT (Normal Distribution Transformation):

[0150] Homogeneous mode: NDT resolution is 2.0-3.0m;

[0151] Heterogeneous mode: NDT resolution is 4.0-5.0m (increase appropriately to improve coarse acquisition capability);

[0152] Step S504: GICP Fine Registration. Fine registration is performed using the Generalized Iterative Closest Point (GICP):

[0153] Isomorphic mode: Maximum number of iterations is 50, and maximum distance between corresponding points is 5.0m;

[0154] Heterogeneous mode: The maximum number of iterations is 60, and the maximum corresponding point distance is 10.0-20.0m (20.0m during initialization and 10.0m during normal operation). Heterogeneous mode allows for a larger corresponding point distance to cope with larger initial pose errors.

[0155] Step S505: Loopback Confirmation. Check if GICP has converged; if not, reject the candidate.

[0156] If GICP converges, check the Fitnessscore:

[0157] Isomorphic mode: When Fitnessscore < 0.5 (UGV) or < 0.3 (UAV), it is confirmed as a valid loopback;

[0158] Heterogeneous mode: When Fitnessscore < threshold, it is considered a valid loopback, where:

[0159] Initialization phase: UAV <0.8-0.9;

[0160] Normal phase: Homogeneous drones <0.65, heterogeneous drones <0.75;

[0161] After confirming the loop closure, extract the rotation quaternion from the final transformation matrix obtained by GICP. Translation vector Create closure constraints, including: timestamp, robot ID, submap ID, and relative pose: rotation quaternion. Translation vector .

[0162] Step S5 is to ensure the accuracy of loop closure detection and avoid false matches by passing geometric consistency verification (NDT+GICP). At the same time, the verification parameters are dynamically adjusted according to the homogeneous / heterogeneous mode to improve the cross-platform matching success rate.

[0163] Step S6: Extract ground point cloud from the fire truck point cloud, perform plane fitting using the RANSAC algorithm, smoothly update the plane normal vector and offset using a sliding window, and construct height constraints and attitude alignment constraints to generate ground plane combined constraints, such as... Figure 2 As shown, it specifically includes:

[0164] Step S601: Extract ground point cloud data from the point cloud data acquired by the UGV lidar and complete point cloud preprocessing. Step S601 specifically includes:

[0165] Step S6011: Format Conversion and Outlier Removal. Convert the PointXYZI format point cloud acquired by UGV to PointXYZ format, traverse the point cloud data and remove invalid points containing NaN values, retaining valid 3D coordinate points (x, y, z).

[0166] Step S6012: Height Filtering. Based on the ground height characteristics of the UGV, points with z-coordinates in the range of [-2.0, 0.5] meters are retained. This range covers the typical ground point distribution area around the UGV, thus filtering out potential ground point clouds.

[0167] Step S6013: Statistical filtering for noise reduction. Using a statistical filtering algorithm with a radius of 0.3 meters, the point cloud density in the neighborhood of each point is calculated. Outliers that deviate from the density mean by more than 1 standard deviation are removed to further purify the ground point cloud data and avoid interference from non-ground points in subsequent fitting.

[0168] Step 601 aims to accurately extract ground point clouds from the original laser point cloud, providing a high-quality data source for ground plane fitting and reducing the impact of non-ground feature points on constraint accuracy.

[0169] Step S602: Use the RANSAC algorithm to perform planar fitting on the ground point cloud and construct a ground plane model. Step S602 specifically includes:

[0170] Step S6021: RANSAC parameter settings. Set the distance threshold of the RANSAC algorithm to 0.1 meters, the maximum number of iterations to 1000, and the confidence level to 0.99. This parameter combination can improve the computational efficiency and robustness of the algorithm while ensuring fitting accuracy.

[0171] Step S6022: Ground plane model fitting. Using the RANSAC algorithm, randomly sample three points from the ground point cloud to construct an initial plane. Calculate the distance from all ground points to this plane, and classify points with distances less than a threshold as inliers. Iterate until the plane with the highest proportion of inliers is found, and output the unit normal vector of the ground plane. and offset from the origin Constructing a ground plane model ( (These are the coordinates of a point in space).

[0172] Step S6023: Confidence Calculation. Calculate the proportion of the number of interior points in the fitted plane to the total number of ground point clouds, and use this proportion as the confidence score of the ground plane model (value range 0-1). If the confidence score is lower than 0.5, it indicates that the quality of the ground point cloud is insufficient, and the fitting result of this time is discarded and the ground plane model of the previous frame is used.

[0173] Step S602 is to fit a ground plane model that conforms to physical characteristics from the ground point cloud, so as to provide a geometric basis for subsequent pose constraints.

[0174] Step S603: Use a sliding window mechanism to smoothly update the ground plane fitting results to ensure the continuity of the model.

[0175] Step S603 specifically includes:

[0176] Step S6031: Sliding window initialization. Set the sliding window size to 5 frames, that is, retain the ground plane fitting results of the most recent 5 frames for subsequent smoothing calculations.

[0177] Step S6032: Exponential moving average update. A smoothing factor is used. For each frame, the fitted ground plane normal vector and offset are subjected to an exponential moving average, and the update formula is as follows:

[0178]

[0179] in, , These are the smoothed normal vector and offset from the previous frame. , These are the current fitted normal vector and offset. This method can reduce the impact of single-frame fitting errors, allowing the ground plane model to change smoothly with the movement of the UGV.

[0180] Step S603 is to avoid sudden changes in the ground plane model caused by noise in a single frame of point cloud, and to ensure the stability and continuity of the ground plane constraints.

[0181] Step S604: Construct the ground plane constraint factor and generate combined constraint residuals.

[0182] Step S604 specifically includes:

[0183] Step S6041: Height Constraint Factor Construction. Based on the physical characteristic of a fixed UGV vehicle height, construct the height constraint residuals: ,in Let UGV be the translation vector. The meter is the preset UGV vehicle height. This constraint forces the vertical distance from the UGV to the ground plane to remain constant, and the weight is set to 0.01 by default.

[0184] Step S6042: Constructing Attitude Alignment Constraint Factors. Based on the physical principle that the UGV vertical axis (z-axis) should be parallel to the ground plane normal vector, construct the attitude alignment constraint residuals: ,in For the UGV rotation matrix The axis column vector, this constraint ensures that the UGV's pose remains aligned with the ground plane normal vector, with a default weight of 0.005.

[0185] Step S6043: Adaptive weight adjustment. The constraint weights are dynamically adjusted based on the confidence level of the ground plane model: when the confidence level is <0.8, the weights of the height constraint and attitude alignment constraint are halved; when the confidence level is <0.6, the weights are reduced to 20% of the original weights to avoid introducing incorrect constraints from a low-quality ground plane model.

[0186] Step S6044: Generate combined constraints. Integrate the 1D residual of the height constraint with the 3D residual of the attitude alignment constraint to form a 4D ground plane combined constraint residual, and input it into the optimization framework for solution.

[0187] Step S604 is to transform the physical characteristics of the ground plane into mathematical constraints, limiting the pose drift of the UGV from the two dimensions of height and attitude.

[0188] Step S7: Input the loop closure constraints, ground plane combination constraints, sub-map atlas, and corresponding pose information into the Ceres optimization framework to jointly optimize the pose information and generate a globally consistent master map. Specifically, this includes:

[0189] Step S701: Problem Construction. In the Ceres optimization framework, the pose parameters of the UGV (rotation + 3D translation vectors represented by quaternions) are used as optimization variables. Ground plane combination constraint residual blocks are added, and loop closure detection residuals obtained from loop closure detection are also incorporated.

[0190] Step S702: Optimize the solution. Set the trust region strategy for Ceres optimization, iterate 8 times to solve for the optimal pose, and minimize the total residual of the ground plane constraint and the loop closure constraint.

[0191] Step S703: Pose Update and Feedback. The optimized UGV pose is used as the optimal pose for the current frame, and this pose is fed back to the ground plane fitting module to correct the fitting benchmark of the subsequent ground plane model and reduce the cumulative error of long-term mapping.

[0192] Step S7 aims to achieve bidirectional coordination between ground plane constraints and loop closure detection through joint optimization. This utilizes physical constraints to suppress pose drift and loop closure constraints to correct ground plane fitting errors, thereby improving overall mapping accuracy.

[0193] The ground plane constraint method in this embodiment can provide stable physical priors for pose estimation when the UGV is traveling in environments with few features such as open parking lots and textureless roads, thereby reducing the absolute pose error of the UGV. At the same time, it provides an initial pose for NDT coarse registration of loop closure detection, thereby improving the registration convergence speed.

[0194] Example 2:

[0195] This experimental example uses the method of this invention and evaluates the difference in loop closure detection performance when using and not using the semantic feature fusion-based ground-to-air collaborative loop closure detection method of this invention. Here, SemGeo-Fusion represents the semantic feature fusion-based ground-to-air collaborative loop closure detection model, NetVLAD represents the baseline model (using only geometric features), and SigLIP2-Only represents the model using only semantic features. This experimental example uses a real-world urban ground-to-air collaborative dataset, containing point cloud data collected by UAVs and unmanned vehicles. The dataset contains different urban environmental scenes, each containing synchronously collected data from UAVs and unmanned vehicles. Each sub-map provides point cloud data, pose information, and ground truth annotations for loop closures. This experimental example uses a 70 / 15 / 15% training / validation / test split to partition the dataset. The loop closure detection task includes two modes: homogeneous loop closures (unmanned vehicle↔ unmanned vehicle, UAV↔ unmanned vehicle) and heterogeneous loop closures (unmanned vehicle↔ unmanned vehicle). SemGeo-Fusion, NetVLAD, and SigLIP2-Only were implemented on a ROS system and tested on a computer equipped with an NVIDIA RTX 4060 GPU. The top-view tile image resolution was 256×256, covering a 50m×50m area. The SigLIP2 model outputs a 768-dimensional semantic descriptor.

[0196] Qualitative results of the experiment:

[0197] Robustness of visual semantic descriptors: NetVLAD relies solely on geometric features. Under the UGV's eye-level and UAV's top-down perspectives, the cosine distance between the geometric descriptors of the same building is >0.6 (low similarity), resulting in a heterogeneous loop recall rate of only 30%. In contrast, SemGeo-Fusion's visual semantic descriptors have a cross-view cosine distance <0.1 (high similarity), a predicted loop overlap with the ground truth of >90%, and a semantic label consistency of 92% (such as "building" and "road").

[0198] Visual reprojection verification results: NetVLAD has no visual semantic feature constraints, and its loop closure false detection rate exceeds 20% in an open square (features are scarce); SemGeo-Fusion filters out mismatches by using ORB feature reprojection error (average 1.5 pixels), reducing the false detection rate to below 8%, and shortening the loop closure confirmation time by 30% (due to the early elimination of invalid candidates by visual constraints).

[0199] Visual feature matching rate: In complex intersection scenarios, SemGeo-Fusion achieved a 65% intrapoint matching rate of ORB features between UGV and UAV, which is significantly higher than SigLIP2-Only (50%) and NetVLAD (40%), demonstrating the promoting effect of "geometry + visual semantics" on cross-platform feature association.

[0200] To quantitatively evaluate the difference between the loop closure detection results obtained using the ground-to-air collaborative loop closure detection method based on semantic feature fusion described in this invention and the ground truth, recall, precision, and F1 score are defined as metrics. Based on the calculation formulas for recall, precision, and F1 score, the results of calculating the recall, precision, and F1 score for both methods using and not using this invention are shown in Table 1.

[0201] Table 1 Recall, Precision, and F1 Score:

[0202]

[0203] NetVLAD represents a baseline model that does not employ the method described in this invention (using only geometric features), with a homogeneous recall of 0.852, a heterogeneous recall of 0.325, a precision of 0.881, and an F1 score of 0.47. SigLIP2-Only represents a model that uses only semantic features, with a homogeneous recall of 0.623, a heterogeneous recall of 0.652, a precision of 0.723, and an F1 score of 0.68. SemGeo-Fusion represents a model that uses the method described in this invention, with a homogeneous recall of 0.876, a heterogeneous recall of 0.783, a precision of 0.862, and an F1 score of 0.82. By comparing the recall, precision, and F1 score of the three, the method of the present invention can significantly improve the recall of heterogeneous loop closure detection by up to 45.8 percentage points (from 0.325 to 0.783), while maintaining a high precision (0.862) and improving the F1 score by 35 percentage points (from 0.47 to 0.82).

[0204] To further evaluate the effectiveness of the method of this invention in detecting isomorphic and heteromorphic loop closures, and to assess the robustness of the method in different scenarios, the classification detection results are shown in Table 2:

[0205] Table 2 Classification Detection Results:

[0206]

[0207] In homogeneous loop closure detection, SemGeo-Fusion shows a slight improvement over NetVLAD (UGV↔UGV improved by 2.6 percentage points, UAV↔UAV improved by 2.2 percentage points), indicating that the fusion strategy can also bring benefits in homogeneous scenarios. In heterogeneous loop closure detection, SemGeo-Fusion significantly improves over NetVLAD (recall improved by 45.8 percentage points, F1 score improved by 35 percentage points), fully demonstrating that our method improves loop closure detection performance in heterogeneous scenarios.

[0208] To further evaluate the effectiveness of each module in the method of this invention, ablation experiments were conducted, and the results are shown in Table 3:

[0209] Table 3 Ablation Experiment Results:

[0210]

[0211] Among them, SemGeo-Fusion with rotation group enhancement represents the method without using the rotation group enhancement module described in this invention, with a heterogeneous recall rate of 0.748, a precision rate of 0.851, and an F1 score of 0.796. SemGeo-Fusion with semantic consistency supervision represents the method without using the semantic consistency supervision module described in this invention (i.e., only using the final output supervision signal), with a heterogeneous recall rate of 0.761, a precision rate of 0.858, and an F1 score of 0.807. SemGeo-Fusion with adaptive fusion weight represents the method without using the adaptive fusion weight strategy described in this invention (i.e., homogeneous and heterogeneous modes use the same fusion weight), with a heterogeneous recall rate of 0.735, a precision rate of 0.845, and an F1 score of 0.786. SemGeo-Fusion (complete) represents the method with all modules described in this invention, with a heterogeneous recall rate of 0.783, a precision rate of 0.862, and an F1 score of 0.82. Compared to SemGeo-Fusion (complete), the heterogeneous recall rates of SemGeo-Fusion without rotation group enhancement, SemGeo-Fusion without semantic consistency supervision, and SemGeo-Fusion without adaptive fusion weights decreased by 0.035, 0.022, and 0.048, respectively, demonstrating the effectiveness of the rotation group enhancement, semantic consistency supervision, and adaptive fusion weight strategies. Therefore, the experimental example proves the effectiveness of the ground-to-air collaborative loop closure detection and mapping method based on visual semantic representation and ground plane constraints described in this invention.

[0212] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0213] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion, characterized in that, Includes the following steps: Local point cloud data is collected by fire trucks and drones, and sub-map sets are generated according to time windows, and corresponding pose information is recorded; the sub-map sets include multiple sub-maps, and each sub-map includes local point cloud data collected within a certain time window. The local point cloud data of the submap is projected into a standardized top-down tile image, and a grayscale image is generated using a maximum height value rasterization strategy. The grayscale image is rotated at multiple angles to generate a rotated version image, and rotation-invariant semantic descriptors are extracted using a visual semantic model; Based on the semantic descriptor, the geometric distance and semantic distance between each sub-map and all sub-maps within the preceding time window are calculated respectively. The distances are then weighted and fused according to the isomorphic or heterogeneous patterns to generate loop closure candidate scores, and loop closure candidate sub-map pairs are selected. Geometric registration is performed on the selected candidate sub-map pairs of loop closures, and loop closure constraints are generated based on the registration results; Ground point cloud is extracted from the point cloud of the fire truck, and plane fitting is performed using the RANSAC algorithm. The plane normal vector and offset are smoothly updated by sliding window, and height constraints and attitude alignment constraints are constructed to generate ground plane combined constraints. The loop closure constraints, ground plane combination constraints, sub-maps and corresponding pose information are input into the Ceres optimization framework to jointly optimize the pose information and generate a globally consistent master map.

2. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion as described in claim 1, characterized in that, The process of collecting local point cloud data using fire trucks and drones, and generating sub-map sets based on time windows, specifically includes: Firefighting unmanned vehicles and drones simultaneously collect local point cloud data, and divide the continuously collected point cloud data into several subsets according to a preset time window; For each subset, a sub-map is generated, and the corresponding acquisition time and pose information are recorded to obtain the sub-map set. The pose information includes the rotation quaternion and three-dimensional translation vector of the fire truck or drone when generating the sub-map.

3. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion as described in claim 1, characterized in that, The process of projecting local point cloud data of the sub-map into a standardized top-down tile image and generating a grayscale image using a maximum height value rasterization strategy specifically includes: Each point in the local point cloud data of the submap Mapping to image coordinates The formula is: in, For the local point cloud data, the first i The three-dimensional coordinates of a point in the local coordinate system; It is half the length of the tile; , These represent the pixel resolution of the image in the horizontal and vertical directions, respectively. This represents the rounding function; For the first i The coordinates of each point mapped to the image coordinate system; For multiple points mapped to the same pixel, the maximum height value strategy is used to calculate the height value of each pixel, using the formula: in, Indicates pixel position The height value; Indicates mapping to pixel position All points Coordinate set; The maximum height value is linearly normalized to the grayscale value range [0, 255] to generate a grayscale image. The formula is: in, Represents the pixel position in a grayscale image grayscale value; , These are the minimum and maximum height values ​​for all points in the submap, respectively.

4. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion as described in claim 1, characterized in that, The step of rotating the grayscale image at multiple angles to generate a rotated version image, and extracting rotation-invariant semantic descriptors using a visual semantic model, specifically includes: For each grayscale image Generate 4 rotated versions of the image, with rotation angles of respectively. , forming a rotation group ; Each rotated version of the image is preprocessed, including color space conversion, size adjustment, normalization, standardization, and dimensionality transformation. Use a visual semantic model for each preprocessed rotated version image Extract the embedding vector and perform L2 normalization, as shown in the formula: in, For rotated version image The embedding vector; This represents the inference function of the visual semantic model; Represents grayscale image After rotation The rotated version of the image; Represents the L2 norm; The embedding vectors of the four rotated versions of the image are averaged and L2 normalized to generate rotation-invariant semantic descriptors, as shown in the formula: in, This represents the average of the embedding vectors of the four rotated versions of the image; It is a rotation-invariant semantic descriptor.

5. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion according to claim 1, characterized in that, The process involves calculating geometric and semantic distances between each sub-map and all sub-maps within the preceding time window, based on semantic descriptors. These distances are then weighted and fused according to isomorphic or heterogeneous patterns to generate loop closure candidate scores. The process also includes filtering loop closure candidate sub-map pairs. For each submap in the submap set as the current submap, calculate the geometric distance between the current submap and all submaps within the previous time window, using the following formula: in, Represents geometric distance; , These represent the NetVLAD descriptors for the current submap and a specific submap within a previous time window, respectively. The L2 norm is represented; the preceding time window refers to the set of submaps within all time windows preceding the current submap's corresponding time window; Based on the semantic descriptor, calculate the semantic distance between the current submap and all submaps within the previous time window, using the following formula: in, Indicates semantic distance; , These represent semantic descriptors for the current submap and a specific submap within a preceding time window, respectively. Represents the dot product of two semantic descriptors; The fusion weights of geometric distance and semantic distance are dynamically adjusted based on homogeneous or heterogeneous patterns to generate loop closure candidate scores, specifically including: When the detection mode is isomorphic, meaning the two sub-maps are acquired by the same machine type, an equal-weight fusion strategy is used to calculate the loop closure candidate score, using the following formula: in, For loop candidate scores; When the detection mode is heterogeneous, meaning the two sub-maps are acquired by different machine types, a semantic-driven strategy is used to calculate the loop closure candidate score, using the following formula: The types of machines include fire-fighting unmanned vehicles and drones; When the loop closure candidate score is less than the first preset threshold, the submap of the corresponding previous time window and the current submap are used as loop closure candidate submap pairs. Each submap in the submap set is used as the current submap for filtering, resulting in all candidate loop-loop submap pairs.

6. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion according to claim 1, characterized in that, The step of geometrically registering the selected candidate loop closure sub-map pairs and generating loop closure constraints based on the registration results specifically includes: For the point cloud data of the two submaps in each loopback candidate submap pair, preprocessing is performed, including voxel downsampling and secondary downsampling; For the two preprocessed sub-maps, the transformation matrix is ​​calculated using a method based on ground normal vector alignment and centroid calculation; then, coarse registration of the two sub-maps is performed using a normal distribution transformation based on the transformation matrix. For the coarsely registered sub-map, fine registration is performed using the generalized iterative nearest point method; After fine registration, convergence is checked. If convergence is not achieved, loop closure candidate submap pairs are deleted. If convergence is achieved, the Fitnessscore is checked. When the Fitnessscore is less than a second preset threshold, the loop closure candidate submap pairs are considered valid, and loop closure constraints are constructed. The Fitnessscore is an indicator for measuring the registration effect, representing the matching quality between submaps. The smaller the value, the better the registration effect.

7. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion as described in claim 6, characterized in that, The closure constraint includes: The timestamp indicates the time when the two submaps in the loopback candidate submap pair were collected; Robot ID, which identifies the robot corresponding to each submap in the loopback candidate submap pair; Submap ID, representing a unique identifier for each submap in the loopback candidate submap pair; Relative pose, including rotation quaternions Translation vector The pose information of the two submaps in the loop candidate submap pair is calculated.

8. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion according to claim 1, characterized in that, The process involves extracting ground point clouds from the point cloud of the unmanned fire truck, performing plane fitting using the RANSAC algorithm, smoothly updating the plane normal vector and offset using a sliding window, and constructing height and attitude alignment constraints to generate combined ground-plane constraints. Specifically, this includes: Acquire point cloud data of fire-fighting unmanned vehicles, and separate the ground point cloud from the fire-fighting unmanned vehicle point cloud data by setting a distance threshold; Based on the ground point cloud, a ground plane model is constructed by performing plane fitting using the RANSAC algorithm. The plane normals and offsets of the constructed ground plane model are smoothly updated using a sliding window. Based on the smoothly updated ground plane model, construct ground plane combination constraints.

9. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion as described in claim 8, characterized in that, The ground plane combination constraints include height constraints and attitude alignment constraints; The height constraint includes: the height constraint residual formula, expressed as: in, Indicates highly constrained residuals, This represents the ground plane normal vector of the smoothly updated ground plane model; The translation vector of the fire truck represents the coordinates of its current position, which is calculated by solving the current pose of the fire truck based on the ground plane reference system determined by the smoothly updated ground plane model. Indicates the offset between the ground plane and the origin; This indicates the height of the unmanned fire truck. The attitude alignment constraint includes: the attitude alignment constraint residual formula, expressed as: in, This represents the attitude alignment constraint residual; Rotation matrix for fire-fighting unmanned vehicles z The column vector represents the vertical axis of the unmanned fire truck; This represents the vector cross product operation.

10. The ground-air collaborative mapping method based on semantic features and ground plane multi-constraint fusion according to claim 1, characterized in that, The step of inputting loop closure constraints, ground plane combination constraints, sub-map sets, and corresponding pose information into the Ceres optimization framework, and jointly optimizing the pose information to generate a globally consistent overall map, specifically includes: The pose information of the fire truck is represented as a rotation quaternion and a three-dimensional translation vector, and used as optimization variables in the Ceres optimization framework. The residuals of the closure constraints, the height constraints of the ground plane combination constraints, and the attitude alignment constraints are used as optimization terms. The optimization objective is to minimize the sum of the residuals of the closure constraints, the height constraints, and the attitude alignment constraints. The Ceres optimization solution strategy is set, and the optimal pose information is obtained through iterative solution. Here, the residual of the closure constraints is the difference between the relative pose of the closure constraints and the optimal pose calculated in the optimization framework. The optimized pose information is updated to the sub-map set, and the local point cloud data of each sub-map is transformed according to the optimized pose information. All transformed sub-map point cloud data are stitched together and merged to generate a globally consistent master map.

Images