A highly reliable positioning and navigation method for an unmanned system with air-ground cooperation

Abstract

The present invention belongs to the technical field of positioning and navigation, and discloses a highly reliable positioning and navigation method for an unmanned system with air-ground cooperation. The present invention uses the RANSAC algorithm and the region growing method to accurately extract negative obstacles, generates a two-dimensional grid map through three-dimensional point cloud reprojection, and marks the negative obstacle area on the map to provide a prior map for the unmanned vehicle. The unmanned vehicle constructs a local map by combining its own sensors, and matches it with the prior map through the ICP algorithm to accurately locate the global pose. According to the prior map and real-time sensor data, a grid cost map including static obstacles, dynamic obstacles and negative obstacles is generated. The TSCNN predicts the trajectories of dynamic obstacles based on time series data and updates the map cost values. The A* algorithm performs optimal path planning based on the updated dynamic cost map to avoid dynamic obstacles and negative obstacles in real time. The present invention can achieve efficient, accurate and safe navigation and obstacle avoidance in a dynamic environment and complex terrain.

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning and navigation, and relates to a highly reliable positioning and navigation method for an unmanned system with air-ground cooperation. Background Art

[0002] With the rapid development of unmanned systems, unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) are two important autonomous devices that play an increasingly important role in fields such as intelligent transportation, disaster rescue, inspection and monitoring. However, achieving efficient cooperation, precise navigation, and safe obstacle avoidance in complex dynamic environments still faces many technical challenges. Therefore, combining the aerial vision advantages of UAVs with the ground operation capabilities of UGVs to develop a technical system for collaborative work and intelligent navigation has become an important direction in the research of unmanned systems. In the current research and application of unmanned systems in the fields of cooperative navigation and intelligent obstacle avoidance, there are still many technical bottlenecks that restrict their actual performance in complex dynamic environments. Traditional obstacle recognition methods have insufficient detection capabilities for negative obstacles (such as potholes, puddles, etc.), and are prone to misrecognition due to environmental noise or terrain complexity.

[0003] In addition, when UAVs and UGVs cooperate, due to the different positioning reference systems and map expression methods of the two, it is difficult to meet the requirements of efficient cooperation in map data sharing and alignment accuracy. At the same time, the UGV positioning technology based on SLAM is easily interfered in dynamic environments, and it is difficult to accurately match the real-time map with the prior map, thus affecting the reliability of global positioning. In addition, in terms of path planning, traditional static algorithms (such as A* or Dijkstra) lack dynamic perception and prediction capabilities and cannot cope with the real-time changes of dynamic obstacles, resulting in the failure or inefficiency of the planned path. Even when introducing a dynamic obstacle avoidance module, existing solutions are often difficult to achieve real-time response in complex environments due to computational resource limitations. In addition, the deficiencies of existing solutions in multi-sensor fusion, time series data analysis, etc. further limit the system's perception ability and prediction performance for dynamic environments. Summary of the Invention

[0004] The purpose of the present invention is to propose a highly reliable positioning and navigation method for an unmanned system with air-ground cooperation, aiming to provide an efficient, robust, and adaptable collaborative solution for unmanned systems to solve the above problems existing in the prior art in dynamic environments and complex terrains, enabling the unmanned system to achieve efficient, precise, and safe navigation and obstacle avoidance in dynamic environments and complex terrains.

[0005] To achieve the above purpose, the present invention adopts the following technical solutions:

[0006] A highly reliable positioning and navigation method for an unmanned system with air-ground cooperation, comprising the following steps:

[0007] Step 1. The drone uses the lidar, IMU sensor, and 4D millimeter-wave radar carried on itself, and adopts LIO-SAM to fuse the 4D millimeter-wave radar for drone mapping to obtain a three-dimensional map.

[0008] Step 2. For the three-dimensional map (i.e., the ground point cloud map) generated by the drone in Step 1, the negative obstacles are extracted and segmented by combining the RANSAC algorithm and the region growing method.

[0009] Step 3. The three-dimensional map generated in Step 1 is converted into a two-dimensional map by reprojection, and the negative obstacles extracted in Step 2 are marked on the two-dimensional map to generate a two-dimensional grid map with negative obstacle information.

[0010] Step 4. The drone transmits the two-dimensional grid map with negative obstacle information generated in Step 3 and the three-dimensional map generated in Step 1 to the unmanned vehicle, and provides a prior map for the positioning and navigation of the unmanned vehicle.

[0011] Step 5. The unmanned vehicle, based on the lidar, IMU sensor, and 4D millimeter-wave radar it carries, scans its own local map and matches it with the three-dimensional map provided by the drone in Step 1 to determine the initial position of the unmanned vehicle itself.

[0012] Step 6. The unmanned vehicle generates a grid cost map based on the two-dimensional grid map with negative obstacle information provided by the drone; sets a navigation target point, and realizes path planning and navigation based on the combination of the time series convolutional neural network TSCNN and the A* algorithm.

[0013] The present invention has the following advantages:

[0014] As described above, the present invention relates to a highly reliable positioning and navigation method for an air-ground collaborative unmanned system. This method has significant improvements in aspects such as dynamic obstacle prediction, accurate identification of negative obstacles, and intelligent path planning. Therefore, it can achieve efficient, accurate, and safe navigation and obstacle avoidance in dynamic environments and complex terrains, significantly enhancing the overall performance and practicality of the unmanned system. Specifically, in terms of adaptability to dynamic environments, the present invention can predict the future positions and movement trajectories of dynamic obstacles by introducing a Time-Series Convolutional Neural Network (TSCNN), significantly improving the obstacle avoidance ability of the unmanned system in dynamic environments. In the aspect of negative obstacle recognition, the present invention combines the RANSAC (Random Sample Consensus) plane fitting method and the region growing method to accurately detect negative obstacles such as potholes and collapses, and maps them to the grid map to ensure stable driving. In terms of path planning, it integrates the A* algorithm and TSCNN to dynamically adjust the path and optimize the driving efficiency, enabling the unmanned vehicle to achieve safer, more efficient, and intelligent autonomous navigation in complex environments. In addition, aiming at the lack of the ability to predict the future movement of dynamic obstacles in the prior art, which leads to insufficient robustness of path planning, the present invention realizes prediction-driven path planning through the prediction ability of TSCNN, upgrading from "current state optimization" to "future state optimization", dynamically adjusting the planned path, enabling the unmanned vehicle to avoid dynamic obstacles that are about to enter dangerous areas in advance, and avoiding path detours or navigation failures caused by sudden avoidance, improving the stability and safety of path planning. In terms of the real-time update of the map, the present invention adopts a rolling window mechanism, uses the dynamic obstacle trajectories predicted by TSCNN to adjust the grid cost map in advance, ensures that path planning is based on the latest environmental information, and at the same time reduces the problem of path planning failure caused by dynamic changes, thereby improving the real-time performance and adaptability of map update. In terms of positioning robustness, to achieve high-precision positioning and map construction, the traditional ICP is vulnerable to the influence of dynamic obstacles and may mistakenly include dynamic obstacles (such as pedestrians and other vehicles) in the point cloud matching, resulting in an increase in positioning error. Therefore, the stability and robustness of positioning are improved, and mismatches and drifts caused by dynamic interference are reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a flowchart of the highly reliable positioning and navigation method for the air-ground collaborative unmanned system according to an embodiment of the present invention;

[0016] Figure 2 It is a processing flowchart of the navigation by combining the TSCNN network and the A* algorithm according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0017] Existing SLAM solutions use sensors such as lidar, cameras, and IMUs, combined with map construction and real-time positioning technologies, to achieve relative positioning of unmanned vehicles and drones during collaborative operations, thereby realizing the positioning and environmental perception of unmanned vehicles and drones. Although SLAM performs well in static environments, in dynamic environments, especially when there are moving obstacles, the SLAM algorithm has problems such as poor adaptability to dynamic environments, high computational complexity, and lagging map updates. In terms of negative obstacle segmentation, traditional SLAM uses a fixed height threshold to segment negative obstacles, but it is prone to misjudgment in undulating terrains, resulting in normal terrains being misidentified or potholes being ignored. In addition, the region growing method is easily affected by the selection of seed points, which may lead to inaccurate boundary expansion and affect navigation. In terms of poor adaptability to dynamic environments, in dynamic environments, SLAM is easily interfered with, resulting in a decrease in the accuracy of positioning and map construction. The movement of dynamic obstacles can cause a "ghosting" phenomenon in the map, affecting the navigation and obstacle avoidance of unmanned systems. In terms of lagging map updates, the maps constructed by SLAM are usually static and difficult to reflect environmental changes (such as newly added obstacles or negative obstacles) in real time. In dynamic environments, the map update frequency of SLAM cannot meet the requirements of real-time obstacle avoidance. In terms of limited negative obstacle recognition ability, SLAM relies on lidar or visual sensors and has weak recognition ability for negative obstacles (such as potholes and puddles), especially in scenes with complex terrains, which may lead to navigation failures. In addition, the A* algorithm is a currently widely used path planning method and is suitable for avoiding static obstacles. Many existing unmanned systems use the A* algorithm for global path planning and combine sensor data such as lidar for dynamic obstacle avoidance. However, when facing dynamic obstacles, the A* algorithm usually requires frequent path replanning and is less efficient when dealing with highly complex or variable environments. For the recognition of negative obstacles (such as potholes and puddles), most existing technical solutions have not effectively solved the problem, resulting in inaccurate navigation in complex terrains. Existing path planning and obstacle avoidance systems based on the A* algorithm have problems such as low efficiency in dealing with dynamic obstacles, insufficient negative obstacle recognition ability, and lack of prediction ability. In highly dynamic environments, the replanning frequency of the A* algorithm may not meet the requirements of real-time obstacle avoidance. In terms of insufficient negative obstacle recognition ability, the A* algorithm relies on a cost map for path planning, but in existing technical solutions, the cost map usually cannot effectively recognize negative obstacles (such as potholes and puddles). In complex terrains, the A* algorithm may not be able to avoid negative obstacles, resulting in navigation failures or unmanned vehicles falling into dangerous areas. In terms of lack of prediction ability, the A* algorithm conducts path planning based on current environmental information and lacks the ability to predict the future movement of dynamic obstacles. In dynamic environments, the A* algorithm cannot plan avoidance paths in advance, resulting in insufficient robustness of path planning.

[0018] In view of the problems of poor adaptability of existing SLAM and A* in dynamic environments, limited negative obstacle recognition ability, lagging map update, and low path planning efficiency, this solution proposes a highly reliable positioning and navigation method for an air-ground collaborative unmanned system. By using UAV global SLAM + unmanned vehicle local SLAM, RANSAC + region growing method, TSCNN prediction + A* optimized path planning, the positioning accuracy, negative obstacle detection ability, and dynamic obstacle avoidance ability in complex environments are improved, enabling the unmanned system to have stronger autonomous navigation ability. The method of the present invention mainly includes four parts: map construction and negative obstacle detection, map transmission and initial positioning, dynamic cost map generation and update, and real-time path planning and obstacle avoidance.

[0019] In terms of map construction and negative obstacle detection, the UAV is equipped with a lidar with a scanning angle to collect ground point clouds, and the RANSAC algorithm and region growing method are used to accurately extract negative obstacles (such as potholes, puddles, depressions). A two-dimensional grid map is generated through three-dimensional point cloud reprojection, and the negative obstacle area is marked on the map to provide a prior map for the unmanned vehicle. In terms of map transmission and initial positioning, the prior map generated by the UAV is transmitted to the unmanned vehicle through the 5G network. The unmanned vehicle constructs a local map by combining its own sensors (lidar, millimeter-wave radar, IMU) and matches it with the prior map through the ICP algorithm to accurately locate the global pose. In terms of dynamic cost map generation and update, according to the prior map and real-time sensor data, a grid cost map containing static obstacles, dynamic obstacles, and negative obstacles is generated. TSCNN predicts the trajectory of dynamic obstacles based on time series data and updates the map cost value. Negative obstacles and dynamic obstacles are given high cost values or non-passable states. In terms of TSCNN + A* real-time path planning and obstacle avoidance, the A* algorithm performs optimal path planning based on the updated dynamic cost map, and dynamically avoids dynamic obstacles and negative obstacles in real time. The system adopts a linear, continuous, fixed-time-step closed-loop feedback mechanism. Each time step includes data acquisition, prediction, map update, path planning, execution, and feedback. Each time re-planning is performed, the end point of the previous cycle is used as the starting point of the next cycle to ensure the continuity and real-time nature of path planning.

[0020] The following further elaborates on the present invention in conjunction with the accompanying drawings and specific embodiments:

[0021] As Figure 1 shown, the highly reliable positioning and navigation method for the air-ground collaborative unmanned system in this embodiment includes the following steps:

[0022] Step 1. The UAV uses LIO-SAM to fuse the 4D millimeter-wave radar based on its own lidar, IMU sensors, and 4D millimeter-wave radar to create a three-dimensional map.

[0023] Among them, the full name of LIO-SAM is Lidar-Inertial Odometry via Smoothing and Mapping, that is, a lidar-inertial odometry algorithm based on smoothing and mapping.

[0024] A lidar with a 75° scanning angle is mounted under the UAV to specifically scan the ground point cloud.

[0025] The mapping method in this embodiment can adopt the simultaneous localization and mapping method based on heterogeneous multi-modal data fusion mentioned in Patent Document 1, which was also originally proposed by the applicant.

[0026] Patent Document 1 is a Chinese invention patent with a publication number of CN119354180A and a publication date of January 24, 2025.

[0027] Step 2. For the three-dimensional map generated by the UAV in Step 1, that is, the ground point cloud map, use a method combining the RANSAC algorithm and the region growing method to extract and segment negative obstacles such as potholes, puddles, and depressions.

[0028] The processing process of the RANSAC algorithm is as follows:

[0029] Step 2.1. Randomly select 3 points from the three-dimensional map constructed in Step 1, that is, the ground point cloud map, and use the 3 points to fit a fitting plane. The formula is as follows:

[0030] ax + by + cz + d = 0.

[0031] Among them, a, b, c, and d are the plane model parameters estimated by the RANSAC algorithm. a, b, and c are the normal vectors of the plane, and d is the plane intercept.

[0032] Step 2.2. For each point p=(x i , y i , z i ) in the point cloud, calculate the distance from the point to the fitting plane. The formula is as follows:

[0033]

[0034] Among them, (x i , y i , z i ) is the coordinate of the i-th point in the point cloud, and d point-to-plane is the distance from the point to the fitting plane.

[0035] Step 2.3. Preset a distance threshold ∈ between the preset points and the fitting plane; if the distance from a certain point to the fitting plane is less than the distance threshold ∈, then this point is regarded as an inlier, that is, it belongs to the ground, otherwise it is regarded as an outlier.

[0036] The inlier judgment formula is: d point-to-plane < ∈, and the outlier judgment formula is: d point-to-plane ≥ ∈.

[0037] Step 2.4. Repeat the process of randomly selecting points and fitting planes multiple times (for example, 1000 times), and select the fitting plane with the most inliers as the ground model; other points that do not conform to the ground model are classified as non-ground points.

[0038] Step 2.5. For the non-ground points p j =(x j , y j , z j ) in the point cloud, calculate the height difference z diff .

[0039] The height difference calculation formula: z diff =|Z point -Z ground |.

[0040] Where Z point is the Z of the non-ground point, and Z ground is set to 0, that is, the height of the ground model is set to 0.

[0041] The reference coordinate system adopts the ground coordinate system, that is, the ground reference height is set to 0, and the Z values of all non-ground points are calculated relative to this ground reference to ensure the consistency and accuracy of the data.

[0042] Step 2.6. Preset a first height threshold ∈ diff (for example, set to 5 cm); if the height difference between the non-ground point and the ground model is greater than the preset first height threshold ∈ diff , then it is considered that this point is part of an obstacle;

[0043] Step 2.7. For each obstacle point cloud region obtained in Step 2.6, calculate the average value of the Z values within this point cloud region, and compare the calculated average value of the Z values with the heights of the surrounding ground points.

[0044] If the average value of the Z values is lower than the average height of the ground reference points by more than a second height threshold ∈ diff2 (for example, set to 5 cm), then this region may be a negative obstacle (pothole, depression area). If the average value of the Z values is higher than the average height of the ground reference points by more than a second height threshold ∈ diff2 , then this region is considered an ordinary obstacle (stone, curb, etc.).

[0045] Step 2.8. Identify the concave negative obstacles using the region growing method as follows:

[0046] Step 2.8.1. In the identified candidate regions of negative obstacles, select the point with the largest height difference as the seed point (this seed point is the deepest point in the candidate region of the negative obstacle).

[0047] Step 2.8.2. Define the similarity criterion.

[0048] Define the height difference (Z - coordinate difference) as the difference in height between the neighborhood point and the seed point.

[0049] If the Z - coordinate (height) of a certain neighborhood point and the height of the current seed point have a difference less than the set third height threshold ∈ z , then it is considered similar to the seed point and is added to the same region. The formula is as follows:

[0050] |z point -z seed |<∈ z ;

[0051] where z point is the Z - value of the selected point, and z seed is the Z - value of the seed point.

[0052] Define the spatial distance (Euclidean distance) as the distance in space between the neighborhood point and the current point.

[0053] Set a distance threshold ∈ d , and judge whether the neighborhood point is added to the same region as the current seed point according to the distance of the neighborhood point from the current point (less than ∈ d , then it is added to the region where the seed point is located). The formula is as follows:

[0054] distance(p point ,p seed )<∈ d ;

[0055] where distance(p point ,p seed ) represents the distance between the neighborhood point p point and the current point p seed ; when distance(p point ,p seed ) is less than the preset distance threshold ∈ d , it means that the neighborhood point can be added to the same region.

[0056] Step 2.8.3. Neighborhood point expansion.

[0057] For each neighborhood point, calculate its height difference and spatial distance from the seed point, and compare them with the preset thresholds (∈ z , ∈ d ); if the similarity criterion is met, add this point to the same region as the seed point.

[0058] Once a neighborhood point is added to the same region as the seed point, it becomes a new seed point and continues to be compared with its neighborhood points until no more eligible neighborhood points can be added.

[0059] Step 2.8.4. Stop expanding and mark the negative obstacle region. The region growing process continues until no more points that meet the similarity criterion can be found; at this time, the region growing method ends and outputs the point cloud data of the negative obstacle region, which contains all the points connected by the similarity criterion, and these points usually belong to a low-lying area.

[0060] Step 2.8.5. Post-processing optimization of the negative obstacle region; eliminate discrete points and mis-identified points from the point cloud data output in Step 2.8.4 through noise removal, use surface fitting to optimize the smoothness of the boundary of the negative obstacle region, and calculate the edges of the concave region in combination with neighborhood points to ensure the accuracy and rationality of the negative obstacle region recognition.

[0061] Step 2.8.6. Marking and storing the negative obstacle region; mark the negative obstacle region and store it as a raster map or point cloud data for path planning and navigation obstacle avoidance. At the same time, use color coding to distinguish negative obstacles with different depths, such as dark blue indicating deep pits or puddles, and light blue indicating shallow concave regions.

[0062] The present invention is based on point cloud data processing technology, uses the RANSAC algorithm and the region growing method to achieve accurate identification and segmentation of negative obstacles (such as pits, puddles, concave regions), and generates a 2D raster map through raster projection of the 3D map.

[0063] Step 3. Convert the 3D map generated in Step 1 into a 2D map through raster projection, and mark the extracted negative obstacles on the 2D map to generate a 2D raster map with negative obstacle information. Specifically, based on the negative obstacle region extracted in Step 2, rasterize and project it together with the entire 3D point cloud data to construct a 2D map.

[0064] Step 3.1. Project the 3D point cloud data onto a 2D plane through raster projection; for each point (x, y, z), retain the x and y coordinates, and the 2D plane coordinates after projection are (x′, y′), (x′, y′) = (X, Y).

[0065] Step 3.2. Normalize the x' and y' coordinates to map the coordinates to a fixed range, such as [0, 255], for subsequent rasterization. The normalization formula is as follows:

[0066]

[0067] where X min , X max are the minimum and maximum values of the X-axis of the point cloud data. Y min , Y max are the minimum and maximum values of the Y-axis of the point cloud data. X norm , Y norm are the normalized coordinates.

[0068] Step 3.3. Convert the continuous normalized two-dimensional coordinate points into a discrete grid of a fixed size to form a raster map, and map the normalized point cloud coordinates into grid cells of a fixed size; and the following rasterization formula is given:

[0069] x grid = [X norm / res], y grid = [Y norm / res].

[0070] where res is the raster resolution, and each raster represents an area of a preset size in the actual space; y grid and x grid represent the two-dimensional raster indices, and (X norm , Y norm ) are the normalized two-dimensional plane coordinates.

[0071] Step 3.4. Mark the negative obstacles extracted in Step 2 on the two-dimensional map.

[0072] Use color coding to mark the negative obstacle areas, and negative obstacles with different depths are marked with different colors. For example, dark blue represents negative obstacles with a greater depth (such as deep pits, accumulated water), and light blue represents shallower depression areas (slight depressions).

[0073] Step 3.5. Visualize the two-dimensional raster map with negative obstacle information and save it as an occupancy grid map.

[0074] Step 4. The drone transmits the two-dimensional raster map with negative obstacle information generated in Step 3 and the three-dimensional map generated in Step 1 to the unmanned vehicle, and provides a prior map for the positioning and navigation of the unmanned vehicle.

[0075] Step 5. The driverless vehicle scans its local map based on the lidar, IMU sensor, and 4D millimeter-wave radar it carries, and matches it with the 3D map provided by the drone to determine the initial position of the driverless vehicle itself.

[0076] Step 5.1. The driverless vehicle receives the 3D map generated by the drone in Step 1 and performs coordinate transformation to convert it from the drone coordinate system to the local coordinate system of the driverless vehicle. The resulting map is a consistent map in the global coordinate system, i.e., the global map, which is convenient for subsequent matching.

[0077] Step 5.2. According to the mapping method described in Step 1, local SLAM map construction is carried out to construct a local map.

[0078] Step 5.3. In the local SLAM map, determine the pose of the driverless vehicle relative to its initial position.

[0079] The initial position refers to the reference coordinate when the driverless vehicle starts.

[0080] Step 5.4. The driverless vehicle obtains the point cloud data of the local environment and uses the ICP algorithm to match it with the global map. Calculate the pose of the driverless vehicle in the global map through ICP.

[0081] Step 5.4.1. Selection of the matching target area.

[0082] Step 5.4.1.1. Extract the area overlapping with the local map from the global map as the matching target. The matching target area refers to the part selected from the global map that overlaps with the current local map as the matching target of ICP.

[0083] The determination method of the matching target area is as follows:

[0084] Based on the initial position estimation of the driverless vehicle, find the area with a large spatial intersection with the local map in the global map, and accurately select the target area through the bounding box or coordinate range to ensure the matching effectiveness and calculation efficiency.

[0085] Among them, the bounding box is the range box using the local point cloud of the driverless vehicle, and the corresponding area is extracted from the global map. The coordinate range is to find the point cloud data covering this range in the global map according to the approximate initial position of the driverless vehicle.

[0086] Step 5.4.2. Nearest point matching.

[0087] Use the nearest point matching method to find the nearest point for each point in the local map in the global map target area. For each point in the local map, find the point in the global map that is closest to it by calculating the Euclidean distance.

[0088]

[0089] Among them, x1, y1, and z1 are the coordinates of the points in the local map, and x2, y2, and z2 are the coordinates of the points in the global map.

[0090] For each pair of matching points, that is, the local points and global points in the local map and the global map, the error E between them is calculated by minimizing the sum of the squared distances, and the formula is as follows:

[0091]

[0092] Among them, and are the coordinates of the corresponding points in the local map and the global map, and n represents the number of pairs of matching points.

[0093] The purpose of the summation is to calculate the total error for subsequent optimization of the rotation and translation parameters. If the error is large, it may indicate that the matching target area is inaccurate, and the matching area needs to be adjusted (for example, expanding or shrinking the search range).

[0094] Step 5.4.3. Iteratively optimize the translation and rotation matrices to find the transformation that minimizes the error between the two.

[0095] According to the principle of error minimization, the translation vector and rotation matrix are adjusted through an iterative optimization process. The optimization goal is to find a translation vector and rotation matrix such that the coincidence degree between the transformed local point cloud and the global point cloud is the largest and the error is the smallest.

[0096] Step 5.4.3.1. Calculate the initial transformation.

[0097] Solve for the initial rotation matrix R0 and the initial translation vector t0 to align the local map with the global map as much as possible. In this embodiment, the SVD (Singular Value Decomposition) method is used for calculation.

[0098] Calculate the centroid of the matching point pairs:

[0099]

[0100] Among them, C local represents the centroid of the matching points of the local point cloud; C global represents the centroid of the matching points of the global point cloud. P local,i is the coordinate of the i-th matching point in the local map, which is the local point cloud data.

[0101] P global,iare the coordinates of the points matching in the global map, which are global point cloud data. n is the number of matching point pairs, that is, the number of matching points participating in the ICP calculation in the local map and the global map.

[0102] Calculate the covariance matrix:

[0103]

[0104] where: H is the covariance matrix, used to measure the directional relationship between the local point cloud and the global point cloud. (·) T represents the transpose operation, ensuring that the matrix dimensions match and making the subsequent SVD decomposition feasible.

[0105] Perform SVD decomposition on H to calculate the initial rotation matrix R0 and translation vector t0:

[0106] R0 = UV T , t0 = C global -R0C local .

[0107] where R0 is the initially calculated rotation matrix, used to rotate the local point cloud to align it with the global point cloud as much as possible. U and V are the optimal rotation transformations obtained through SVD decomposition and are two orthogonal matrices.

[0108] t0 is the initially calculated translation vector, used to translate the local point cloud so that its center point aligns with the center point of the global point cloud. R0C local is the centroid coordinate of the rotated local point cloud.

[0109] Step 5.4.3.2. Apply the transformation.

[0110] Apply the current rotation matrix and translation vector transformation to all points of the local map to obtain new positions. Apply the rotation matrix and translation vector to all points of the local map:

[0111]

[0112] where, are the coordinates of the local point cloud after rotation and translation. R0P local,i are the coordinates of the rotated local point cloud. t0 is the translation adjustment amount to align the point cloud. After the transformation, the position of the local map point cloud is updated to make it closer to the global map.

[0113] Step 5.4.3.3. Calculate the new least squares error:

[0114]

[0115] where, E newDenote the new least - squares matching error (Error), which measures the matching degree between the local point cloud and the global point cloud after the current iteration. ||·|| 2 Denote the square of the Euclidean distance, which is used to measure the error between two matching points.

[0116] The smaller the error, the better the matching effect.

[0117] Step 5.4.3.4. Iterative optimization.

[0118] If the error does not converge, return to Step 5.4.3.1 to continue optimizing the rotation matrix R and the translation vector t. Continuously update R and t to minimize the matching error between the local point cloud and the global point cloud, and calculate the new least - squares error E new .

[0119] Judge whether the convergence condition is satisfied, and repeat the iterative optimization until the error converges (E new is lower than the preset threshold ∈) or reaches the maximum number of iterations to avoid infinite loops.

[0120] Step 5.5. Determine the position of the unmanned vehicle in the global map by calculating its pose in the global coordinate system; after completing the iterative optimization, that is, the optimization of the translation and rotation matrices, the local map of the unmanned vehicle is aligned to the global map through transformation.

[0121] By applying the transformation parameters of the local map to the global map, calculate the exact position and pose of the unmanned vehicle in the global coordinate system; the current pose of the unmanned vehicle in the local coordinate system is (x slam , y slam , z slam ).

[0122] After translation and rotation transformations, the pose of the unmanned vehicle in the global coordinate system is:

[0123] T global = R·T local + t.

[0124] where R is the rotation matrix optimized by the ICP algorithm, t is the corresponding translation vector, T global is the pose of the unmanned vehicle in the global coordinate system, and T local is the pose of the unmanned vehicle in the local coordinate system. This process effectively maps the position of the unmanned vehicle in the local map accurately to the global map, ensuring the accuracy and consistency of global positioning.

[0125] The unmanned vehicle fuses sensors such as lidar, millimeter - wave radar, and IMU to construct a local environmental map in real - time, and combines it with the prior map for positioning and navigation. The ICP algorithm is used to achieve the accurate matching of the local and global maps, so as to determine its exact pose in the global coordinate system.

[0126] Step 6. The driverless vehicle generates a grid cost map based on the 2D grid map with negative obstacle information provided by the drone; sets a navigation target point, and realizes path planning and navigation based on the combination of the time series convolutional neural network TSCNN and the A* algorithm.

[0127] Step 6.1. Read and preprocess the 2D grid map with negative obstacle information generated in Step 3.

[0128] Step 6.2. Generate a grid cost map;

[0129] Step 6.2.1 Define the grid cost value. The grid cost value is assigned according to the attributes of each grid cell (such as static obstacles, dynamic obstacles, negative obstacles, etc.).

[0130] The attribute of a grid cell refers to the environmental features contained in the grid, namely static obstacles, dynamic obstacles, negative obstacles, and passable areas. Among them, static obstacles include immovable obstacles such as buildings and walls; dynamic obstacles such as moving vehicles and pedestrians change over time; negative obstacles such as potholes and water accumulation are low-lying areas; the passable area is the area where the driverless vehicle can pass, and the cost value is relatively low.

[0131] For fixed obstacles (such as walls, buildings, etc.): Assign a high cost value to indicate an impassable area. The cost value of the static obstacle C static (i,j) is as follows;

[0132] C static (x i ,y i )=w static .

[0133] Among them, w static is the fixed weight of the static obstacle.

[0134] For negative obstacles (such as potholes and sunken areas), special cost value processing is carried out: Assign an avoidance weight, and the weight value represents the priority of avoidance by the path algorithm. The avoidance cost value of the negative obstacle C negative (x i ,y i ) is as follows:

[0135]

[0136] Among them: w negative is the weight factor of the negative obstacle, d is the distance between the grid and the negative obstacle; σ is a scale parameter that controls the influence range (the larger σ is, the wider the avoidance influence range is). The value range of σ usually takes [1,5] meters (depending on specific applications, such as driverless vehicles, robots, etc.).

[0137] An exponential decay function is used to gradually decay the cost value as the distance increases, and the influence of negative obstacles farther away is smaller.

[0138] Define the cost value C of the dynamic obstacle dynamic (x i ,y i ).

[0139] The cost value of the dynamic obstacle should change with time and be adjusted in combination with its movement direction and speed. By predicting the future trajectory of the obstacle, a higher cost value is assigned to the area where it may pass, so as to improve the obstacle avoidance ability of path planning.

[0140]

[0141] Where C dynamic (x i ,y i ) is the cost value of the dynamic obstacle.

[0142] w dynamic is the basic weight of the dynamic obstacle, indicating its influence degree; d is the distance between the current grid and the current position of the dynamic obstacle; σ controls the influence range (the influence range is wider when it is larger), and the value range of σ usually takes [1, 5] meters (depending on the specific application, such as unmanned vehicles, robots, etc.).

[0143] θ is the angle between the movement direction of the dynamic obstacle and the current path direction of the unmanned vehicle, v represents the current speed of the dynamic obstacle, v max The set maximum speed (used to normalize the speed influence).

[0144] Step 6.3. Output the grid cost map and visualize it; use Matplotlib to draw a heat map or OpenCV pseudo-color mapping to visualize the cost map, and represent different cost value areas through colors.

[0145] Step 6.4. Select the navigation target point; use the cost map generated in Step 6.3 to determine the navigation target point, which is specified by the user or the system. If there are negative obstacles near the navigation target point, the navigation target point is dynamically adjusted to a feasible area.

[0146] Where the feasible area refers to the area with a lower cost value where the unmanned vehicle can safely pass.

[0147] Step 6.4.1. Use the grid cost map to check the cost value C(x goal ,y goal ) of the navigation target point, and preset the passable threshold C threshold ; where, (x goal ,y goal ) represents the navigation target point, C(xgoal , y goal ) represents the cost value of the navigation target point.

[0148] If C(x goal , y goal ) > C threshold , then the navigation target point is impassable, and go to step 6.4.2.

[0149] If C(x goal , y goal ) ≤ C threshold The navigation target point is passable, directly perform path planning, and jump to step 6.5.

[0150] Step 6.4.2. If the navigation target point is not selected correctly, set a search area with a search radius of R search at the navigation target point. The search area R search is a circular or rectangular area centered on the target point.

[0151] Neighborhood check traverses the grids near the target point.

[0152] Define (x i , y i ) to represent a grid cell within the search range, and check the cost value C(x i , y i ) of the grid cell.

[0153] If C(x i , y i ) ≤ C threshold , then consider this point as a feasible area; among all feasible areas, select the point closest to the initial target point (x goal , y goal ) as the new navigation target point and place it in the grid cost map.

[0154] The expression form of the new navigation target point is as follows:

[0155]

[0156] Among them, is the new navigation target point, represents finding the point that minimizes the Euclidean distance among all candidate points (x i , y i ).

[0157] Step 6.3. Output the grid cost map and visualize it; use Matplotlib to draw a heat map or OpenCV pseudo-color mapping to visualize the cost map, and represent different cost value areas through colors;

[0158] Step 6.4. Select the navigation target point; use the cost map generated in Step 6.3 to determine the navigation target point, which is specified by the user or the system. If there are negative obstacles near the navigation target point, dynamically adjust the navigation target point to the feasible area.

[0159] The feasible area refers to the area with a lower cost value where the unmanned vehicle can safely pass.

[0160] Step 6.5. Use the Time Series Convolutional Neural Network (TSCNN) combined with the A* algorithm to achieve path planning.

[0161] Specifically, within each time period, based on the unmanned vehicle, collect environmental data using the lidar, IMU sensor, and 4D millimeter-wave radar multi-modal sensors carried by the vehicle, and form the collected sensor data into an input time series.

[0162] Input the time series into the TSCNN model, predict the trajectory information of dynamic obstacles in the future for a period of time based on the TSCNN network, and update the grid cost map using the predicted trajectory of dynamic obstacles by TSCNN.

[0163] Based on the updated grid cost map, use A * Calculate the optimal path and perform path navigation within this time period.

[0164] Step 6.6. In the next time period, repeat the process of Step 6.5 until reaching the navigation target point.

[0165] The present invention innovatively introduces the TSCNN model. By analyzing the sensor time series data, it predicts the position changes of dynamic obstacles and updates the environmental information in real time. Combining with the A* algorithm, it plans the optimal path for dynamic obstacles, and at the same time ensures that the path planning adapts to the dynamic characteristics of the unmanned vehicle, improving the environmental adaptability and navigation accuracy of the system.

[0166] Step 6.5.1. System initialization. When the system starts, initialize all key components, including map initialization, defining the initial pose of the unmanned vehicle, path planning initialization, and initializing the TSCNN model to set the time window T and the future prediction step K.

[0167] Step 6.5.1.1. Initialize the grid cost map generated in Step 6.2.

[0168] Step 6.5.1.2. Define the initial pose X0 of the unmanned vehicle:

[0169] X0 = (x0, y0, θ0).

[0170] Among them, x0 and y0 represent the initial position coordinates of the driverless vehicle. The initial coordinate position of the driverless vehicle can be obtained from Step 3, and θ0 represents the initial heading angle of the driverless vehicle.

[0171] Step 6.5.1.3. Path planning initialization.

[0172] Set the navigation target point (x goal , y goal ), and set A * Calculate the initial path as:

[0173] P0 = {(x0, y0), (x1, y1), …, (x goal , y goal )}.

[0174] Step 6.5.1.4. Initialize the TSCNN prediction model. Set the time window T and the future prediction step K. In this embodiment, the time window T is set to 2 seconds, for example, and K takes values between 1 - 3 seconds, for example.

[0175] Obtain the initial states of the environment and dynamic obstacles during initialization to provide input for subsequent trajectory prediction and path planning.

[0176] Step 6.5.2. At the current moment, the driverless vehicle uses the multi-modal sensors of the lidar, IMU sensor, and 4D millimeter-wave radar to collect environmental data, and forms the collected sensor data into an input time series X.

[0177] X = {X t-T+1 , X t-T+2 , …, X t}.

[0178] Among them, X t-T+1 , X t-T+2 , X t represent the latest sensor data collected at each time step t.

[0179] The sensor input format is: X t = {L t , R t , I t}.

[0180] Among them, L t is the obstacle position, obtained from the LiDAR point cloud data (X, Y, Z); R t is the obstacle speed and distance, obtained from the millimeter-wave radar data (V x , V y , d); I t is the acceleration and angular velocity of the driverless vehicle, obtained from the IMU data (a x , a y, ω) is obtained.

[0181] Obtain the initial states of the environment and dynamic obstacles to provide inputs for subsequent trajectory prediction and path planning.

[0182] Step 6.5.3. Input the input time series X into the TSCNN model to predict the trajectories of dynamic obstacles in the next K seconds based on the TSCNN network. Its expression form is as follows:

[0183]

[0184] Among them, represents the prediction result of the TSCNN network at time t + k. is the predicted future position of the dynamic obstacle, is the predicted speed of the dynamic obstacle, M is the total number of dynamic obstacles predicted at the current time step; i is the obstacle number.

[0185] Assume that at time step t + 1, the TSCNN predicts 3 dynamic obstacles (M = 3), then i = 1, 2, 3, and the output is as follows:

[0186]

[0187] The physical meaning of the above information is:

[0188] The 1st obstacle (i = 1): position speed

[0189] The 2nd obstacle (i = 2): position speed

[0190] The 3rd obstacle (i = 3): position speed

[0191] The TSCNN combines LiDAR and millimeter-wave radar data and processes them through a spatio-temporal convolutional network, which can effectively capture the motion trajectories of dynamic obstacles and predict the future positions of pedestrians or vehicles. This can not only achieve accurate tracking of obstacles but also provide a more accurate cost map for path planning based on the A* algorithm.

[0192] Step 6.5.4. Update the grid cost map G(i, j) using the trajectories of dynamic obstacles predicted by the TSCNN. Specifically, update the positions of dynamic obstacles using the trajectories of dynamic obstacles predicted by the TSCNN. The formula is as follows:

[0193]

[0194] Among them, The future position of the i-th obstacle predicted by TSCNN; G(i,j) represents the grid cost value in the grid cost map, indicating whether a certain position is passable; C dynamic is the cost of the dynamic obstacle area.

[0195] C dynamic is relatively high, indicating that this area is difficult to pass.

[0196] For the positions that the dynamic obstacles predicted by TSCNN may pass through, all possible passing positions in the grid cost map are assigned the value C dynamic , indicating that this position is temporarily occupied by an obstacle and needs to be avoided during path planning.

[0197] In step 6.5.4, the grid cost map is updated based on the prediction results of TSCNN, converting the movement trajectory of the dynamic obstacle into a high-cost area to avoid collisions between the unmanned vehicle and pedestrians.

[0198] Step 6.5.5. Based on the updated grid cost map, use A * to calculate the optimal path. The unmanned vehicle moves forward towards the navigation target point according to the planned path, records the current position, and completes the displacement within the current time step.

[0199] This function is mainly to update the position information of the vehicle in real time, ensuring that the subsequent path planning can be adjusted according to the actual situation of each step during the movement. The record of each displacement provides a new starting point for the next time step, ensuring that the system has a real-time feedback mechanism during execution to cope with the dynamically changing environment. Within a fixed time step (2 seconds), move according to the A* planned path and record the trajectory, providing a starting point and status input for subsequent time steps.

[0200] The specific content of step 6.5.5 is as follows:

[0201] Step 6.5.5.1. Use A * algorithm to calculate the optimal path from the current position of the unmanned vehicle to the navigation target point:

[0202] f(n) = g(n) + h(n);

[0203] Among them, f(n) is the total path cost, indicating the total cost of passing through the current point n from the starting point to the navigation target point, and g(n) is the path cost from the starting point to the current point, indicating the actual cost required for the unmanned vehicle to travel from the starting point to the current point n.

[0204] h(n) is the heuristic cost of the target point, indicating the estimated cost from the current point n to the navigation target point.

[0205]

[0206] Define G(x i , y i ) as the cost value of each grid in the grid cost map.

[0207] If the cost of all grids passed through in the query planning path, i.e., G(x i , y i ) = C negative (negative obstacle), then the cost is very high, and the path planning will avoid passing through this area; if G(x i , y i ) = C dynamic (dynamic obstacle), the path planning may consider detouring.

[0208] If G(x i , y i ) = 0 (passable area), then the path preferentially selects this area for passage.

[0209] Step 6.5.5.2. The A* algorithm selects the path with the minimum cost by searching all current feasible paths.

[0210] n best = argmin f(n).

[0211] Where n best represents the node with the minimum total cost among the current expanded nodes, and argmin f(n) finds the point with the minimum f(n) among all candidate nodes n. The algorithm continuously expands new candidate nodes. Among them, candidate nodes refer to all neighboring nodes that can be expanded from the current node and can be used as the next search target during the current search process.

[0212] Repeat this process until the navigation punctuation (x goal , y goal ) is found and the optimal path P t is finally formed, ensuring that the unmanned vehicle avoids dynamic obstacles and negative obstacles and reaches the target position efficiently.

[0213] Step 6.5.5.3. Finally, the optimal path calculated by A* is:

[0214] P t = {(x0, y0), (x1, y1), …, (x goal , y goal )}.

[0215] Where P t is the set of optimal paths for the unmanned vehicle from the current starting point to the target point, (x0, y0) is the starting point coordinate, (x goal , y goal ) is the target point coordinate, (x i , y i)The coordinate points passed through in the middle of the path.

[0216] Finally, A* calculates the optimal path P based on the dynamically updated map. t , enabling the unmanned vehicle to avoid dynamic obstacles and negative obstacles and achieve precise and efficient autonomous navigation.

[0217] The A* algorithm calculates the optimal path for the current time step based on the cost map, taking into account dynamic obstacles.

[0218] Step 6.5.6. In the next time period, repeat the above steps 6.5.2 to 6.5.5. The unmanned vehicle moves forward according to the re-planned path, records its current position, and completes the displacement within the current time step.

[0219] Step 6.5.7. Repeat the above process, and use the predicted dynamic obstacle trajectory of TSCNN to remove the dynamic obstacle point cloud within the trajectory and use the method in step 5 to perform real-time positioning of the unmanned vehicle until it reaches the navigation target point.

[0220] As Figure 2 shows the specific process of the combination of the TSCNN network and the A* algorithm. Through the combination of TSCNN and the A* algorithm, the system realizes the real-time path planning and obstacle avoidance of the unmanned vehicle in a dynamically complex environment.

[0221] First, the unmanned vehicle starts from the starting point, and the sensor collects environmental information, including the position, speed, and direction of dynamic obstacles. TSCNN predicts the future trajectory of dynamic obstacles based on the sensor data and marks it as an impassable area. Subsequently, the map is updated according to the prediction results of TSCNN, and the paths that dynamic obstacles may pass through are assigned high cost values. The A* algorithm takes the current position of the unmanned vehicle as the starting point and the target point as the predetermined end point, re-plans the optimal path on the updated map, and tries to avoid high-cost areas and impassable areas as much as possible. The unmanned vehicle moves forward according to the planned path, records its current position, and then enters the next time step. The system is driven by a fixed time step, re-plans the path every fixed time, and uses the current position of the unmanned vehicle as the new starting point to ensure the real-time and continuity of path planning. Through linear and continuous time step rolling, all states, data, and path planning are seamlessly connected between time steps, and finally dynamic obstacle avoidance and target point navigation are achieved.

[0222] Through the real-time prediction of TSCNN and the dynamic programming of A*, the present invention realizes the real-time path planning and obstacle avoidance of the unmanned vehicle in a dynamically complex environment. A linear, continuous, and dynamic closed-loop feedback mechanism is adopted, and cyclic updates are performed at fixed time steps. Data acquisition, prediction, map update, path planning, execution, and feedback are all carried out within each time step, making each planning start from the current position of the unmanned vehicle to ensure the coherence of the state and the real-time nature of path planning. Thus, the unmanned vehicle can flexibly cope with dynamic obstacles, negative obstacles, and uncertain factors. TSCNN enhances the environmental adaptability of the A* algorithm through dynamic prediction, and the combination of the two can achieve accurate and efficient global path planning and dynamic obstacle avoidance. This method performs particularly well in dynamically complex environments and can significantly improve the navigation performance of the unmanned vehicle. Compared with traditional methods, the combination of TSCNN and A* solves complex problems such as dynamic obstacles and negative obstacles, making path planning no longer rely solely on static environmental information. The introduction of time series modeling capabilities endows the navigation algorithm with predictive capabilities, which is a leap from "optimizing the current state" to "optimizing the future state".

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

[0224] 1. Dynamic obstacle prediction and real-time map update. By introducing a time series convolutional neural network (TSCNN), and processing the time series data of sensors to predict the future positions and motion trajectories of dynamic obstacles, it can enhance the obstacle avoidance ability of the unmanned system in a dynamic environment, plan avoidance paths in advance, and avoid frequent path replanning.

[0225] 2. Identification of negative obstacles through multi-sensor fusion. Combining lidar, using multi-sensor fusion technology to improve the identification accuracy of negative obstacles (such as potholes and puddles), and combining the RANSAC algorithm and region growing method to accurately identify negative obstacles in complex terrains, avoiding the unmanned vehicle from falling into dangerous areas and enhancing the safety of navigation.

[0226] 3. Combining with an improved A* algorithm to optimize the global optimality and robustness of path planning, which can avoid the local optimality problem of path planning in a dynamic environment and improve the efficiency and adaptability of path planning.

[0227] 4. Real-time map update and dynamic cost map. By designing a dynamic cost map update mechanism, the dynamic obstacle information in the map is updated in real time according to the prediction results of TSCNN, ensuring that path planning is based on the latest environmental information and avoiding navigation errors caused by map lag.

[0228] 5. Prediction-driven path planning. By combining the prediction results of TSCNN with the A* algorithm, prediction-driven path planning is realized. Function: Upgrading from "optimizing the current state" to "optimizing the future state", and enhancing the navigation performance of the unmanned system in a dynamic environment.

[0229] The present invention integrates a variety of advanced algorithms and technologies, has efficient negative obstacle recognition ability, precise high-precision positioning ability, intelligent path planning ability and excellent dynamic obstacle avoidance performance, and is applicable to multiple fields such as intelligent transportation, disaster rescue, inspection of complex terrains, and logistics distribution, providing important technical support for the research and application of cooperative navigation of unmanned systems.

[0230] Certainly, the above description is only a preferred embodiment of the present invention. The present invention is not limited to listing the above embodiments. It should be noted that all equivalent substitutions and obvious deformation forms made by any person skilled in the art under the teaching of this specification fall within the substantial scope of this specification and should be protected by the present invention.

Claims

1. A highly reliable positioning and navigation method for an air-ground coordinated unmanned system, characterized in that: The steps include: Step 1. The drone uses LIO-SAM fused with 4D millimeter-wave radar to map the drone and obtain a three-dimensional map based on its own laser radar, IMU sensor and 4D millimeter-wave radar; Step 2. The three-dimensional map generated by the drone in step 1, i.e., the ground point cloud map, is extracted and segmented using a method based on a combination of the RANSAC algorithm and the region growing method; Step 3. Convert the three-dimensional map generated in step 1 into a two-dimensional map by reprojection, and mark the negative obstacles extracted in step 2 on the two-dimensional map to generate a two-dimensional grid map with negative obstacle information; Step 4. The UAV transmits the two-dimensional grid map with negative obstacle information generated in step 3 and the three-dimensional map generated in step 1 to the unmanned vehicle, and provides a priori maps for the positioning and navigation of the unmanned vehicle; Step 5. The unmanned vehicle scans its own local map based on its own laser radar, IMU sensor and 4D millimeter wave radar, and matches it with the three-dimensional map provided by the drone in step 1 to determine the initial position of the unmanned vehicle; Step 6. The unmanned vehicle generates a grid cost map based on the two-dimensional grid map with negative obstacle information provided by the drone; sets the navigation target point, and implements path planning and navigation based on the combination of the time series convolutional neural network TSCNN and the A* algorithm; The unmanned vehicle uses the laser radar, IMU sensor and 4D millimeter wave radar multimodal sensor to collect environmental data, and forms the input time series X = {X t-T+1 ,X t-T+2 ,…,X t }; Among them, X t-T+1 , X t-T+2 , X t Represents the latest sensor data collected at each time step t; The sensor input format is: X t ={L t ,R t ,I t }; Among them, L t is the obstacle position, obtained from the lidar point cloud data (X, Y, Z); R t is the obstacle speed and distance, obtained from the millimeter wave radar data (V x ,V y ,d) obtain,I t is the acceleration and angular velocity of the unmanned vehicle, obtained from IMU data; Input the input time series X into TSCNN, and predict the dynamic obstacle trajectory in the next K seconds based on TSCNN. The expression is as follows: in, Represents the prediction result of TSCNN at time t+k, is the predicted future position of the dynamic obstacle, is the predicted dynamic obstacle speed, M is the total number of dynamic obstacles predicted in the current time step; i is the obstacle number.

2. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 1 is characterized in that: In step 2, the processing process of the RANSAC algorithm is as follows: Step 2.

1. From the three-dimensional map constructed in step 1, i.e., the ground point cloud map, three points are randomly selected using the RANSAC algorithm, and a fitting plane is obtained by fitting the three points; Step 2.

2. For each point in the point cloud, calculate the distance from the point to the fitting plane; Step 2.

3. Preset the distance threshold ∈ between the point and the fitting plane; if the distance from a point to the fitting plane is less than the distance threshold ∈, the point is considered an inner point, that is, it belongs to the ground, otherwise it is considered an outer point; Step 2.

4. Repeat the process of randomly selecting points and fitting planes multiple times, and select the fitting plane with the most inliers as the ground model, and classify other points that do not conform to the ground model as non-ground points; Step 2.

5. For non-ground points in the point cloud, calculate the height difference between all non-ground points and the ground model; Step 2.

6. Preset a first height threshold ∈ diff1 ; If the height difference between the non-ground point and the ground model is greater than the preset first height threshold ∈ diff1 , then the non-ground point is considered to be part of the obstacle; Step 2.

7. For each obstacle point cloud area obtained in step 2.6, calculate the average Z value in the point cloud area, and compare the calculated average Z value with the height of the surrounding ground points; If the average Z value is lower than the average height of the ground reference point by more than the second height threshold ∈ diff2 , then the area is a negative obstacle candidate area; if the average Z value is higher than the average height of the ground reference point by more than ∈ diff2 , then the area is a common obstacle; Step 2.

8. Use region growing method to identify concave negative obstacles.

3. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 2 is characterized in that: The step 2.8 is specifically as follows: Step 2.8.

1. In the identified negative obstacle candidate area, select the point with the largest height difference as the seed point; Step 2.8.

2. Define the following similarity criteria; The height difference is defined as the difference in height between the neighboring point and the seed point; if the height difference between a neighboring point and the current seed point is less than the set third height threshold ∈ z , it is considered similar to the seed point, and the two are added to the same area; Define the spatial distance as the distance between the neighborhood point and the seed point in space; set a distance threshold ∈ d , if the distance between the neighborhood point and the seed point is less than the distance threshold ∈ d , then the neighboring points are added to the same area as the seed point; Step 2.8.

3. For each neighborhood point, calculate its height difference and spatial distance with the seed point, and compare it with the preset threshold ∈ z ,∈ d Compare; if the similarity criterion is met, add the point to the same region as the seed point; Once a neighbor point is added to the same area as the seed point, it becomes a new seed point and continues to be compared with other neighbor points, iterating until no more neighbor points that meet the conditions are added; Step 2.8.

4. The region growing process will continue until no points that meet the similarity criteria can be found; at this point, the region growing method ends and outputs a point cloud data of the negative obstacle region; Step 2.8.

5. Remove the discrete points and misidentified points from the point cloud data output in step 2.8.4 by noise removal, optimize the smoothness of the boundary of the negative obstacle area by surface fitting, and calculate the edge of the concave area by combining the neighborhood points; Step 2.8.

6. Mark the negative obstacle area and store it as a raster map or point cloud data, and use color coding in the raster map or point cloud data to distinguish negative obstacles at different depths.

4. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 1 is characterized in that: The step 3 is specifically as follows: Step 3.

1. Project the 3D point cloud data onto a 2D plane through rasterization projection. For each point (x, y, z), the projection formula retains the x and y coordinates, and the 2D plane coordinates after projection are (x′, y′). Step 3.

2. Normalize the x′ and y′ coordinates for rasterization; Step 3.

3. Convert the continuous normalized two-dimensional coordinate points into a discrete grid of fixed size to form a grid map. The normalized point cloud coordinates are mapped to the grid cells of fixed size, and the following gridding formula is given: x grid =[X norm / res],and grid =[And norm / beef]; Where res is the grid resolution, and each grid represents an area of ​​a preset size in the actual space; y grid With x grid Represents a two-dimensional grid index, (X norm ,Y norm ) is the normalized two-dimensional plane coordinate; Step 3.

4. Mark the negative obstacles extracted in step 2 on the two-dimensional map; mark the negative obstacle area, use color coding, and negative obstacles of different depths are represented by different colors; Step 3.

5. Visualize the two-dimensional grid map with negative obstacle information and save it as an occupancy grid map.

5. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 1 is characterized in that: The step 5 is specifically as follows: Step 5.

1. The unmanned vehicle receives the three-dimensional map generated by the drone and performs coordinate transformation from the drone coordinate system to the local coordinate system of the unmanned vehicle. After the transformation, a consistent map in the global coordinate system, i.e., a global map, is obtained; Step 5.

2. Construct a local SLAM map according to the composition method described in step 1 to construct a local map; Step 5.

3. In the local map, determine the position of the unmanned vehicle relative to its initial position; The initial position refers to the reference coordinates when the unmanned vehicle starts; Step 5.

4. The unmanned vehicle obtains the point cloud data of the local environment, and uses the ICP algorithm to match it with the global map obtained in step 5.1, and calculates the position and posture of the unmanned vehicle in the global map through ICP; By calculating the position and posture of the unmanned vehicle in the global coordinate system, its position in the global map is determined; after completing the iterative optimization, that is, the optimization of the translation and rotation matrices, the local map of the unmanned vehicle is aligned to the global map through transformation.

6. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 1 is characterized in that: The step 6 is specifically as follows: Step 6.

1. Read and preprocess the two-dimensional grid map with negative obstacle information generated in step 3; Step 6.

2. Generate raster cost map; Step 6.2.

1. Define the grid cost value, which is assigned according to the attributes of each grid cell; The attributes of the grid cell refer to the environmental features contained in the grid, namely static obstacles, dynamic obstacles, negative obstacles, and traversable areas; static obstacle cost, negative obstacle avoidance cost, and dynamic obstacle cost are calculated; Step 6.

3. Output the raster cost map and visualize it; use Matplotlib to draw a heat map or OpenCV pseudo color map to visualize the cost map, and use colors to represent different cost value areas; Step 6.

4. Use the cost map generated in step 6.3 to determine the navigation target point, which is specified by the user or the system. If there are negative obstacles near the navigation target point, dynamically adjust the navigation target point to the feasible area; The feasible area refers to the area with low cost and safe passage for unmanned vehicles; Step 6.

5. In each time period, the unmanned vehicle uses the laser radar, IMU sensor, and 4D millimeter wave radar multimodal sensor to collect environmental data, and the collected sensor data is formed into an input time series; Input the time series into the TSCNN model, predict the dynamic obstacle trajectory information for a period of time in the future based on the TSCNN network, and use the dynamic obstacle trajectory predicted by TSCNN to update the grid cost map; Based on the updated grid cost map, using A * Calculate the optimal path and perform path navigation within the time period; Step 6.

6. In the next time period, repeat the process of step 6.5 above until the navigation target point is reached.

7. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 6 is characterized in that: The step 6.4 is specifically as follows: Step 6.4.

1. Use the grid cost map to check the cost value C(x goal ,y goal ), and preset the passable threshold C threshold ; Among them, 9x goal ,y goal ) indicates the navigation target point, C9x goal ,y goal ) represents the cost value of the navigation target point; If C(x goal ,y goal )>C threshold , then the navigation target point is not accessible, then go to step 6.4.2; If C(x goal ,y goal )≤C threshold The navigation target point is accessible, and the path planning is performed directly, and the process jumps to step 6.5; Step 6.4.

2. If the navigation target point is not selected correctly, set a search radius of R at the navigation target point. search The search area R search It is a circular or rectangular area centered on the target point; Neighborhood checking traverses the grids near the navigation target point; Definition (x i ,y i ) represents a grid cell within the search range, and checks the cost value C(x i ,y i ); If C(x i ,y i )≤C threshold , then the point is considered to be a feasible area; in all feasible areas, select the point that is closest to the initial navigation target point (x goal ,y goal )The nearest point is used as the new navigation target point and placed in the grid cost map.

8. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 6 is characterized in that: The step 6.5 is specifically as follows: Step 6.5.

1. When the system starts, initialize all key components, including map initialization, definition of the initial position of the unmanned vehicle, path planning initialization, TSCNN model initialization, setting the time window T and the future prediction step length K; Step 6.5.

2. In the current time period, the unmanned vehicle uses the laser radar, IMU sensor and 4D millimeter wave radar multimodal sensor to collect environmental data, and forms the collected sensor data into the input time series X; X={X t-T+1 ,X t-T+2 ,…,X t }; Among them, X t-T+1 , X t-T+2 , X t Represents the latest sensor data collected at each time step t; Step 6.5.

3. Input the input time series X into the TSCNN model, and predict the dynamic obstacle trajectory information for the next K seconds based on the TSCNN network. The expression is as follows: in, Represents the prediction result of the TSCNN network at time t+k; is the predicted future position of the dynamic obstacle, is the predicted dynamic obstacle speed, M is the total number of dynamic obstacles predicted in the current time step, and i is the obstacle number; Step 6.5.

4. Update the grid cost map using the dynamic obstacle trajectory predicted by TSCNN; Step 6.5.

5. Based on the updated grid cost map, use A * Calculate the optimal path, the unmanned vehicle moves toward the navigation target point according to the planned path, records the current position and completes the displacement within the current time step; Record the displacement in the current time step, that is, calculate the displacement of the unmanned vehicle from the current position to the next position, update its position and orientation to track the progress of the unmanned vehicle on the path, and ensure continuous update of the posture; Step 6.5.

6. In the next time period, repeat the above steps 6.5.2 to 6.5.5, and the unmanned vehicle moves along the replanned path, records the current position and completes the displacement in the current time step; Step 6.5.

7. Repeat the above process until the unmanned vehicle reaches the navigation target point.

9. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 8 is characterized in that: In step 6.5.4, the dynamic obstacle trajectory predicted by TSCNN is used to update the dynamic obstacle position; in, is the future position of the ith obstacle predicted by TSCNN; G(i,j) represents the grid cost value in the grid cost map, indicating whether a certain position is passable; C dynamic is the cost of the dynamic obstacle area; TSCNN predicts the possible locations of dynamic obstacles, and assigns C to all possible locations on the grid cost map. dynamic , indicating that the position is temporarily occupied by an obstacle and needs to be avoided during path planning.

10. The high-reliability positioning and navigation method for air-ground coordinated unmanned systems according to claim 8, characterized in that: The step 6.5.5 is specifically as follows: Step 6.5.5.

1. Using A * The algorithm calculates the optimal path from the current unmanned vehicle position to the navigation target point: f(n)=g(n)+h(n); Among them, f(n) is the total path cost, which means the total cost from the starting point to the navigation target point through the current point n, and g(n) is the path cost from the starting point to the current point, which means the actual cost required for the unmanned vehicle to travel from the starting point to the current point n; G(x i ,y i ) is the cost value of each grid in the grid cost map; If the query planning path passes through all grid cost values, if G(x i ,y i )=C negative That is, negative obstacles, the cost is very high, and the path planning will avoid passing through this area; if G(x i ,y i )=C dynamic That is, for dynamic obstacles, the path planning will go around them; h(n) is the heuristic cost of the target point, which represents the estimated cost from the current point n to the navigation target point; Among them, h(n) represents the Euclidean distance, which means the current point (x n ,y n ) to the navigation target point (x goal ,y goal ) straight-line distance; the smaller the heuristic cost h(n), the closer to the navigation target point, and A* prefers to choose the path close to the navigation target point; Step 6.5.5.

2. The A* algorithm searches all currently feasible paths and selects the path with the lowest cost; Step 6.5.5.

3. Finally, the optimal path calculated by A* is: P t ={(x0,y0),(x1,y1),…,(x goal ,y goal )}; Among them, P t is the optimal path set from the current starting point to the target point of the unmanned vehicle, (x0, y0) is the starting point coordinate, (x goal ,y goal ) is the target point coordinate, (x i ,y i ) The coordinate points in the middle of the path; Step 6.5.

6. In the next time period, repeat the above steps 6.5.2 to 6.5.5, the unmanned vehicle moves along the replanned path, records the current position and completes the displacement in the current time step; Step 6.5.

7. Repeat the above process until the navigation target point is reached.

Images