A three-dimensional terrain modeling analysis and adaptive navigation system and method

Abstract

The application discloses a three-dimensional terrain modeling analysis and adaptive navigation system and method, and relates to the technical field of navigation.The laser radar is used for acquiring point cloud data of the region where the device to be navigated is located in real time; the IMU is used for measuring the acceleration and angular velocity of the device to be navigated in real time; the data fusion sub-module is used for pre-processing the point cloud data, acceleration and angular velocity in real time, and fusing the pre-processed data; the three-dimensional map modeling sub-module is used for constructing a three-dimensional map model according to the point cloud data in real time by using a SLAM algorithm; the three-dimensional terrain analysis module is used for determining the terrain features of each point in the three-dimensional map model in real time; and the adaptive navigation module is used for planning a path for the device to be navigated according to the three-dimensional map model and the terrain features of each point, so that navigation is realized.The application can improve the precision of original data, and then improve the positioning precision, and realize accurate navigation.

Description

[0001] This application is a divisional application of the invention patent application with application number CN202411642627.7, entitled "A Three-Dimensional Terrain Modeling Analysis and Adaptive Navigation System and Method", the parent application being filed on November 15, 2024. Technical Field

[0002] This invention relates to the field of navigation technology, and in particular to a three-dimensional terrain modeling analysis and adaptive navigation system and method. Background Technology

[0003] High-precision 3D terrain modeling and analysis, along with adaptive navigation systems, are key focuses in today's technological field. Significant progress has been made through the use of advanced technologies such as satellite data, lidar, and photogrammetry, enabling accurate 3D simulations of the Earth's surface. This provides adaptive navigation systems with precise 3D map models, allowing them to more accurately perceive their environment and plan routes.

[0004] However, 3D terrain modeling and adaptive navigation systems still face some challenges. 3D modeling in these systems typically requires multiple sensors to acquire environmental information and obtain raw data. Especially in the presence of dynamic targets, the acquired raw data may contain inconsistencies and noise, leading to instability in the accuracy of subsequent positioning based on this data. This results in significant pose calculation errors. Therefore, a high-precision 3D terrain modeling and adaptive navigation system is needed to process the raw data to improve its accuracy, thereby enhancing positioning accuracy and avoiding large pose calculation errors. Summary of the Invention

[0005] The purpose of this invention is to provide a three-dimensional terrain modeling analysis and adaptive navigation system and method, which can improve the accuracy of raw data, thereby improving positioning accuracy and achieving precise navigation.

[0006] To achieve the above objectives, the present invention provides the following solution: A three-dimensional terrain modeling analysis and adaptive navigation system is installed on a device to be navigated. The three-dimensional terrain modeling analysis and adaptive navigation system includes: SLAM module, 3D terrain analysis module, and adaptive navigation module; The SLAM module includes: The lidar is used to acquire point cloud data of the area where the navigation device is located in real time. An IMU (Integrated Measurement Unit) is used to measure the acceleration and angular velocity of the device under navigation in real time. The measurement is performed using a three-axis accelerometer and a three-axis gyroscope within the IMU. The accelerometer measures linear acceleration, and the gyroscope measures angular velocity. The accelerometer data is integrated to estimate velocity, and then the velocity is integrated to estimate position, as shown in the following formula: ; ; ; , , These are the position, velocity, and rotation matrix of IMU at time t. Let IMU be the coordinate transformation matrix at time t. It is the acceleration due to gravity. Let be the acceleration at time t. Angular acceleration, , , It is the value at time t+1 obtained by continuous time integration, where Δt is the change over time; The data fusion submodule is used to preprocess the point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device in real time, and then fuse the preprocessed point cloud data of the area where the navigation device is located, the preprocessed acceleration of the navigation device, and the preprocessed angular velocity of the navigation device to obtain the real-time position of the navigation device; the data fusion submodule includes: a data preprocessing unit and a sensor data fusion unit; The data preprocessing unit is used to sequentially perform noise reduction and time synchronization processing on the point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device. The sensor data fusion unit is used to fuse the data obtained by the data preprocessing unit using an extended Kalman filter to obtain the real-time position of the navigation device.

[0007] The extended Kalman filter is used to perform: State-space modeling: The system's state and sensor observations are modeled as state equations and observation equations. The state equations describe the dynamic evolution of the system, including the changes in variables such as position, velocity, and attitude. The observation equations represent the relationship between the data acquired by the sensors and the system state. State estimation algorithms are used to predict and update the system state. Prediction steps: Based on the nonlinear dynamic model of the system, the state estimate and control input of the previous time step are used to predict the state of the current time step through the nonlinear dynamic equation. This process involves modeling the kinematics and control input of the system, and using the state transition equation of the system to transform the state estimate of the previous time step into the state prediction of the current time step, so as to realize the prediction and update of the dynamic evolution of the system state. Kalman gain calculation: In the extended Kalman filter, calculating the Kalman gain involves linearizing the system's state equation and observation equation to obtain the Jacobian matrix; the Jacobian matrix is ​​used to quantify the relationship between the uncertainty of the state estimation and the observation noise, and then calculate the Kalman gain; the Kalman gain represents the trade-off between system state prediction and sensor observation, and by fusing the two, a more accurate estimate of the system state can be achieved. Update steps: The observations acquired by the sensor are converted into updates in the state space using the observation model, and then the predicted state is fused with the sensor observations using Kalman gain; the calculation of Kalman gain adjusts the weights of the predicted and observed values ​​by balancing the confidence of the prediction and the observation, thereby maximizing the accuracy of the estimation. A 3D map modeling submodule is used to construct a 3D map model of the area where the navigation device is located in real time using the SLAM algorithm based on the point cloud data of the area. The 3D map modeling submodule is used to execute: Data registration: Extract feature points or feature descriptors from each point cloud data, match the features between different point cloud data to find corresponding feature point pairs, and calculate the rigid body transformation relationship between point cloud data through transformation model estimation based on the feature matching results. After completing the initial transformation estimation, the transformation parameters are iteratively adjusted to minimize the residual of the matched point pairs, thereby further optimizing the point cloud registration results. Map stitching: Aligning the registered point cloud data to a reference coordinate system, merging the aligned point cloud data to generate a unified map model, and further optimizing the map quality through point cloud optimization methods; Filtering: Eliminates noise and invalid data, divides point cloud data into a regular three-dimensional voxel grid, and uses the average or median value of points in each voxel to represent the point cloud information within that voxel; Reconstruction: Reconstructing discrete point cloud data to generate a continuous 3D map model; The 3D terrain analysis module is used to determine the terrain features of each point in the 3D map model of the area where the navigation device is located in real time; The adaptive navigation module is used to plan a path for the device to be navigated in real time based on the real-time location of the device, the three-dimensional map model of the area where the device is located, and the terrain features of each point in the three-dimensional map model of the area where the device is located obtained by the three-dimensional terrain analysis module, so as to realize navigation.

[0008] Preferably, the three-dimensional terrain analysis module is a CNN model.

[0009] Preferably, the three-dimensional terrain analysis module is used for terrain feature recognition, terrain classification, and real-time updates.

[0010] Preferably, the adaptive navigation module uses bidirectional A in real time. The algorithm processes the 3D map model of the area where the navigation device is located based on the real-time location of the device and the terrain features of each point in the 3D map model of the area where the device is located, obtained by the 3D terrain analysis module, and performs path planning for the navigation device.

[0011] This invention also provides a three-dimensional terrain modeling analysis and adaptive navigation method, comprising: Obtain the raw data at the current moment; the raw data includes: point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device; Preprocess the raw data at the current time to obtain the preprocessed raw data at the current time; The position of the navigation device at the current moment is obtained by fusing the preprocessed raw data at the current moment; Construct a 3D map model of the area where the navigation device is located at the current moment based on the point cloud data of the area where the navigation device is located at the current moment; Determine the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment; Based on the current location of the device to be navigated, the 3D map model of the area where the device is located, and the terrain features of each point in the 3D map model of the area where the device is located, a path is planned for the device to be navigated, the current time is updated, and the original data of the current time is returned.

[0012] Preferably, based on the current location of the device to be navigated, the 3D map model of the area where the device is located at the current time, and the terrain features of each point in the 3D map model of the area where the device is located at the current time, path planning is performed for the device to be navigated, specifically including: Using bidirectional A The algorithm processes the 3D map model of the area where the device to be navigated is located at the current time, based on the current location of the device and the terrain features of each point in the 3D map model of the area where the device to be navigated is located, and performs path planning for the device to be navigated.

[0013] Preferably, the bidirectional A The algorithm's steps include: (1) Preparations: 3D map representation: Typically, a 3D map can be represented as a voxel grid, where each voxel represents a small cube in 3D space and can be labeled as an obstacle or free space; Starting and target positions: Define the starting point and target point in three-dimensional space, mainly using the device to be navigated as the origin of the coordinate system; (2) Initialization: Define two open lists: one starting from the starting point and one starting from the target point; create two empty closed lists, one for forward search and one for backward search; add the starting point to the open list for forward search and the target point to the open list for backward search; set the actual cost of the starting point for forward search to 0 and the actual cost of the target point for backward search to 0; calculate the heuristic estimated cost from the starting point to the target point for forward search, and also calculate the heuristic estimated cost from the target point to the starting point for backward search. (3) Using heuristic functions: When A When the algorithm selects the next node to consider from the open list, it uses the following formula to calculate the total cost of each node: f(n) = g(n) + h(n); in: g(n) is the actual cost from the starting node to the current node n; h(n) is a heuristic cost estimate from the current node n to the target node; The total cost f(n) determines the priority of node consideration; the node with the lowest total cost is taken out and considered first; the heuristic value h(n) plays a key role here: it provides direction for the search, making the algorithm prioritize those nodes that seem closer to the target; Forward search uses a heuristic estimate from the starting point to the target point, while backward search uses a heuristic estimate from the target point to the starting point. (4) Search process: In each iteration, select the node with the minimum total cost from two open lists, one from the forward search and one from the reverse search; check if the two meet. If they do, the path has been found, and the final path can be obtained by merging the paths from the forward and reverse searches; otherwise, continue iterating, expand the neighboring nodes of the selected node, and calculate their actual and estimated costs. Add the expanded nodes to their respective open and closed lists, and repeat this process until they meet or one or both open lists are empty. (5) Final path: If bidirectional A The algorithm successfully found a path, and the final path can be obtained by merging the forward and reverse search paths. Typically, when merging paths, the node closest to the meeting point in the forward search path is found, and then the node closest to the meeting point in the reverse search path is found to ensure that the paths are connected. The merged path is the robot's 3D path on the grid map. Upon entering the next moment, proceed with the following steps: (6) Reassess the current path: Using existing technology, the current path is evaluated using the existing path and the updated 3D map model to determine whether the path is still feasible or whether there is a better path. If the original path is still feasible but no longer optimal, and some points on the path are now impassable or at risk, the feasibility of the path may be affected. You can choose to continue along the original path or execute (7) to find a new optimal path. If it is not feasible, execute (7). (7) Re-execute bidirectional A algorithm: Re-execute bidirectional A using the updated 3D map. The search yields a new path. Considering efficiency, the search can be conducted within a local area between the current location and the target, rather than across the entire map. To avoid frequent global searches, this step is only performed when detected obstacles or terrain changes render the current path completely infeasible. (8) Execute the new path: Once a new path is obtained, the robot or driverless vehicle begins to follow that path. During execution, continue to monitor changes in the environment so that further dynamic updates can be made as necessary; (9) Security Mechanism: In addition to the aforementioned strategy adjustments, to ensure safety, robots or autonomous vehicles should be equipped with emergency stopping or escape mechanisms. When an impending collision or other emergency is detected, the robot or autonomous vehicle will take immediate action, such as stopping abruptly or changing direction quickly, to avoid potential dangers.

[0014] Preferably, determining the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment specifically includes: A CNN model is used to process the 3D map model of the area where the navigation device is located at the current moment to obtain the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment.

[0015] Preferably, the preprocessed raw data at the current moment is fused to obtain fused data. Specifically, an extended Kalman filter is used to fuse the preprocessed raw data at the current moment to obtain the position of the navigation device at the current moment.

[0016] This invention also provides an application of a three-dimensional terrain modeling analysis and adaptive navigation system for use in autonomous vehicles, robot navigation, military applications, drones and aviation, search and rescue, geological exploration and mining, and architecture and urban planning.

[0017] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects: The present invention provides a three-dimensional terrain modeling analysis and adaptive navigation system, comprising: a SLAM module, a three-dimensional terrain analysis module, and an adaptive navigation module; the SLAM module includes: a lidar, an IMU, a data fusion submodule, and a three-dimensional map modeling submodule; the lidar is used to acquire point cloud data of the area where the navigation device is located in real time; the IMU is used to measure the acceleration and angular velocity of the navigation device in real time; the data fusion submodule is used to preprocess the point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device in real time, and to combine the preprocessed point cloud data of the area where the navigation device is located, the preprocessed acceleration of the navigation device, and the preprocessed angular velocity of the navigation device. The angular velocities are fused to obtain the real-time position of the device to be navigated; the 3D map modeling submodule is used to construct a 3D map model of the area where the device is located in real time using the SLAM algorithm based on the point cloud data of the area where the device is located; the 3D terrain analysis module is used to determine the terrain features of each point in the 3D map model of the area where the device is located in real time; the adaptive navigation module is used to perform path planning for the device to be navigated in real time based on the 3D map model of the area where the device is located and the terrain features of each point in the 3D map model of the area where the device is located obtained by the 3D terrain analysis module, thereby realizing navigation. This invention preprocesses and fuses the original data to improve the accuracy of the original data, thereby improving the positioning accuracy and achieving precise navigation. Attached Figure Description

[0018] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 The three-dimensional map model provided in the embodiments of the present invention; Figure 2 Voxel mesh; Figure 3 This is a schematic diagram of the neural network sensing principle. Figure 4 A map showing the actual terrain. Figure 5 This is a schematic diagram of the simulation test; Figure 6 Bidirectional A Algorithm diagram; Figure 7 This is a schematic diagram of the merged path; Figure 8 This is a diagram showing the updated path; Figure 9 A block diagram of a three-dimensional terrain modeling analysis and adaptive navigation system provided in an embodiment of the present invention. Detailed Implementation

[0020] 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 embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

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

[0022] This invention provides a three-dimensional terrain modeling analysis and adaptive navigation system, which is installed on the device to be navigated, such as... Figure 9 As shown, the three-dimensional terrain modeling analysis and adaptive navigation system includes: The system includes a SLAM module, a 3D terrain analysis module, and an adaptive navigation module.

[0023] The SLAM module includes: a lidar, an IMU, a data fusion submodule, and a 3D map modeling submodule. The lidar is used to acquire point cloud data of the area where the device to be navigated is located in real time. The IMU is used to measure the acceleration and angular velocity of the device to be navigated in real time. The data fusion submodule is used to preprocess the point cloud data, acceleration, and angular velocity of the area where the device to be navigated is located in real time, and then fuses the preprocessed point cloud data, acceleration, and angular velocity to obtain the real-time position of the device to be navigated. The 3D map modeling submodule is used to construct a 3D map model of the area where the device to be navigated is located in real time using the SLAM algorithm based on the point cloud data.

[0024] The 3D terrain analysis module is used to determine the terrain features of each point in the 3D map model of the area where the navigation device is located in real time.

[0025] The adaptive navigation module is used to plan a path for the device to be navigated in real time based on the real-time location of the device, the three-dimensional map model of the area where the device is located, and the terrain features of each point in the three-dimensional map model of the area where the device is located obtained by the three-dimensional terrain analysis module, so as to realize navigation.

[0026] In practical applications, the lidar in the SLAM module emits a non-visible laser beam to measure the beam information reflected by an object (the area where the navigation device is located). After data processing, it generates point cloud data, which can be used for 3D map modeling to provide an accurate shape model required for environmental perception and automatic navigation.

[0027] In practical applications, the IMU in the SLAM module, such as the GY-95T, is typically installed in a moving system. By measuring the inertia of the moving carrier (the device to be navigated), the state of the object is inferred. These inertia-related physical quantities are usually not directly obtained positions and rotations, but rather integrated physical quantities. From an external perspective, only the accuracy of their measurements of angular velocity and acceleration, and the relationship between these quantities and the vehicle's position and attitude, are of concern. In this invention, a three-axis accelerometer and a three-axis gyroscope within the IMU can be used for measurement. The accelerometer measures linear acceleration, while the gyroscope measures angular velocity. Through integration calculation, the accelerometer data is integrated to estimate velocity, and then the velocity is integrated to estimate position, as shown in the following formula.

[0028] ; ; ; , , These are the position, velocity, and rotation matrix of IMU at time t. Let IMU be the coordinate transformation matrix at time t. It is the acceleration due to gravity. Let be the acceleration at time t. Angular acceleration, , , It is the value at time t+1 obtained by continuous time integration, where Δt is the change in time.

[0029] In practical applications, the data fusion submodule in the SLAM module includes a data preprocessing unit and a sensor data fusion unit. The data preprocessing unit performs noise reduction and time synchronization processing on the point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device to ensure data quality and consistency. Then, the sensor data fusion unit uses an extended Kalman filter to fuse the data from different sensors (LiDAR and IMU) obtained by the data preprocessing unit. By using the complementary information between the sensors, the accuracy and robustness of environmental perception are improved. Data fusion is used to solve the problem of large fluctuations in positioning speed and acceleration errors, and then a more accurate data can be obtained. This data is used for positioning. For example, when an object moves, its acceleration is forward, and the current position on the map will be forward.

[0030] (1) Calibration: The sensor needs to be calibrated before use to eliminate errors caused by sensor hardware or environmental factors. Before using the equipment, the lidar needs to be scanned and calibrated to correct angle deviations, etc.

[0031] Noise reduction: Data acquired by sensors may be affected by various noises, such as sensor noise itself and environmental interference. Noise reduction can be achieved using digital filtering techniques, such as mean filtering, median filtering, and Gaussian filtering, to smooth the data and reduce the impact of noise.

[0032] Time synchronization: Time synchronization is the process of ensuring that the data acquired by multiple sensors are consistent in time. In SLAM systems, data from different sensors need to be collected and fused simultaneously to ensure the accuracy of the temporal relationship between the data, so as to guarantee the accuracy and reliability of subsequent data fusion and state estimation.

[0033] (2) The sensor data fusion unit is the core component that fuses data from different sensors. The main algorithm used is the extended Kalman filter, which can utilize the complementary information between sensors to improve the accuracy and robustness of environmental perception.

[0034] The Extended Kalman Filter (EKF) is a commonly used filter for state estimation, particularly suitable for nonlinear systems. In SLAM systems, the EKF is widely applied to sensor data fusion to estimate state variables such as the position and orientation of a robot or vehicle, and to construct a map. Its working principle is as follows: State-space modeling: The state of the system and sensor observations are modeled as state equations and observation equations. The state equations describe the dynamic evolution of the system, including the changes in variables such as position, velocity, and attitude. The observation equations represent the relationship between the data acquired by the sensors and the system state. Through this formal description, state estimation algorithms such as Kalman filters can be used to predict and update the system state.

[0035] Prediction steps: Based on the nonlinear dynamic model of the system, the state estimate and control input of the previous time step are used to predict the state of the current time step through the nonlinear dynamic equation. This process involves modeling the kinematics and control input of the system, and using the state transition equation of the system to transform the state estimate of the previous time step into the state prediction of the current time step, so as to realize the prediction and update of the dynamic evolution of the system state.

[0036] Kalman gain calculation: In the extended Kalman filter, calculating the Kalman gain involves linearizing the system's state equations and observation equations to obtain the Jacobian matrix. This Jacobian matrix is ​​used to quantify the relationship between the uncertainty of the state estimate and the observation noise, thus calculating the Kalman gain. The Kalman gain represents the trade-off between system state prediction and sensor observations; by fusing these two, a more accurate estimate of the system state is achieved.

[0037] Update steps: The observations acquired by the sensor are transformed into updates in the state space using an observation model. Then, the predicted state is fused with the sensor observations using Kalman gain. This process considers the uncertainty of the state prediction and the noise of the observations to obtain the final state estimate. The calculation of Kalman gain adjusts the weights of the predicted and observed values ​​by balancing the confidence of the prediction and the observation, thereby maximizing the accuracy of the estimate.

[0038] In SLAM systems, extended Kalman filters are typically used to fuse data from different sensors. By fusing information from multiple sensors, the accuracy and robustness of state estimation can be improved, thereby enabling more accurate map building and autonomous navigation.

[0039] The data fusion submodule also includes (3) fusion result evaluation and optimization: the fused results need to be evaluated and optimized to ensure that the fused data meets the system requirements. Evaluation indicators may include positioning error, ground Figure 1Consistency, accuracy of environmental perception, etc. Based on the evaluation results, the algorithm can be adjusted and optimized, making it a viable technology.

[0040] In practical applications, the 3D map modeling submodule in the SLAM module uses existing technologies to perform a series of processes on the collected point cloud data, including data registration, map stitching, filtering, and reconstruction, ultimately obtaining a complete 3D map model, such as... Figure 1 As shown.

[0041] Data registration involves extracting feature points or feature descriptors from each point cloud dataset, matching features between different point cloud datasets to find corresponding feature point pairs, and calculating rigid transformation relationships between point cloud datasets based on the feature matching results through transformation model estimation. Common transformation models include translation, rotation, scaling, and affine transformations. RANSAC algorithms or least squares methods can be used to estimate the transformation model and select matching point pairs that meet the requirements. After initial transformation estimation, transformation parameters are iteratively adjusted to minimize the residuals of the matching point pairs, further optimizing the point cloud registration results.

[0042] Map stitching: Aligning registered point cloud data into a reference coordinate system. One frame of point cloud data can be selected as a reference, and the coordinates of other point cloud data are transformed so that they are in the same coordinate system as the reference point cloud. The aligned point cloud data are then merged to generate a unified map model. After map stitching, some discontinuities or gaps may appear. Point cloud optimization methods can be used to further improve the map quality.

[0043] Filtering: This process eliminates noise and invalid data by dividing the point cloud data into a regular 3D voxel grid. The average or median value of the points within each voxel represents the point cloud information within that voxel. By adjusting the size of the voxels, the resolution of the filtered point cloud can be controlled, thus performing filtering processing on the point cloud data.

[0044] Reconstruction: After map stitching and filtering, the resulting point cloud data is still discrete. Therefore, it is necessary to reconstruct the discrete point cloud data to generate a continuous 3D map model.

[0045] In practical applications, the three-dimensional terrain analysis module is a CNN model.

[0046] The information acquired through SLAM technology provides a raw map, but further interpretation of this data is needed for effective navigation. This is where the three main functions of the 3D terrain analysis module introduced here come in: terrain feature recognition, terrain classification, and real-time updates.

[0047] 1. Terrain Feature Recognition: Terrain feature recognition is the cornerstone of navigation decision-making. A simple 3D map model may contain thousands of data points, but not every data point is useful for navigation decisions. Identifying key features in the map, such as obstacles, slopes, and gullies, is crucial. Figure 4 As shown, green represents the ground and red represents obstacles. Convolutional neural networks (CNNs) can be used to achieve terrain feature recognition.

[0048] (1) Data preprocessing and voxelization: Point cloud data is acquired from LiDAR, voxel sizes are defined, and continuous point cloud data is voxelized to obtain a regular voxel mesh, such as... Figure 2 As shown, the speed-up mesh is then denoised using statistical filtering, a process that is already in use.

[0049] (2) Data augmentation: To improve the generalization ability of the model, various transformations can be performed on the point cloud data, such as rotation, scaling and flipping, and noise can be added to simulate possible data corruption in real environment. This process is an existing technology.

[0050] (3) CNN model design: like Figure 3 As shown, it includes: Input layer: Voxelized point cloud data is input into the network.

[0051] Convolutional layer: Uses 3D convolutional kernels to process input data and extract primary spatial features from it.

[0052] Activation layer: Feature activation is performed using ReLU.

[0053] Pooling layer: Uses 3D pooling technology to reduce the spatial size of data while retaining important features.

[0054] Iteratively add more convolutional and pooling layers to gradually extract high-level spatial features.

[0055] Fully connected layer: Connects high-level features to the output layer to classify different terrain features. Figure 3 In the middle (a), the diagram represents the fully connected mode of the CNN model. Figure 3 In the diagram (b), the local connectivity pattern of the CNN model is shown.

[0056] (4) Model training: The model is trained using voxel data (voxed point cloud data) with labels ("obstacles", "flat ground", "slope", etc.), where each voxel may be labeled as "obstacle", "flat ground", "slope", etc. Then, an appropriate loss function, such as cross-entropy loss, is chosen to optimize the model's prediction accuracy. An optimization algorithm such as SGD is used to update the model parameters. The training process is monitored using a validation set to avoid overfitting; this is the existing training process.

[0057] (5) Model evaluation and testing: The model's performance is evaluated using a test dataset that was not used for training. The model's predictive ability is checked using various evaluation metrics, such as accuracy, precision, and recall; this process is a current technique.

[0058] (6) Deployment and real-time application: The trained model is integrated into the navigation system. During actual navigation, point cloud data from the 3D map model is passed to the CNN model in real time to identify terrain features and provide input for navigation decisions.

[0059] 2. Real-time updates Because the environment is dynamic, especially in urban or other complex scenarios, the system must be able to update its map and terrain analysis in real time to adapt to these changes. Therefore, it is necessary to integrate SLAM technology, enabling the system to update in real time as new data arrives, and to perform real-time map updates during movement. Test results are as follows: Figure 5 As shown.

[0060] (1) Data flow: The lidar continuously acquires point cloud data.

[0061] (2) Real-time processing: The 3D map modeling submodule compares the currently acquired point cloud data with the previously acquired point cloud data to detect new or missing objects, moving objects or any other form of change. If there is a change, a new 3D map model is constructed based on the newly acquired point cloud data. Then, the new 3D map model is fed into the CNN model described above for feature recognition and classification. If there is no change, there is no need to update the map model.

[0062] (3) Map Merging: The 3D map modeling submodule merges the old 3D map model with the new 3D map model to ensure that navigation decisions are always based on the latest and most accurate information. When the robot or autonomous vehicle detects a sudden obstacle or terrain change, it can adjust its navigation strategy in real time to avoid collisions or other dangers. If only the new map is used, it is based on its own coordinate system. As long as the direction of movement changes, the map as a whole will move in that direction (taking forward as an example). However, in reality, the map should not move, but its own coordinate system should move. Therefore, the old 3D map model is merged with the new 3D map model.

[0063] In practical applications, the adaptive navigation module uses bidirectional A in real time. The algorithm processes the 3D map model of the area where the navigation device is located based on the real-time location of the device and the terrain features of each point obtained from the 3D terrain analysis module, and then performs path planning for the device. The specific steps are as follows: Use bidirectional A at the current moment The algorithm implements 3D path planning on a 3D map, bidirectional A The algorithm is an efficient algorithm for path planning that searches simultaneously from both the starting point and the destination, such as... Figure 6 As shown, this is to improve search efficiency.

[0064] Two-way A The algorithm's steps include: (1) Preparations: 3D map representation: Typically, a 3D map can be represented as a voxel mesh. Each voxel represents a small cube in 3D space and can be labeled as an obstacle or free space.

[0065] Starting and target positions: Define the starting point and target point in three-dimensional space, mainly using the device to be navigated as the origin of the coordinate system.

[0066] (2) Initialization: Define two open lists: one starting from the starting point (forward search) and one starting from the target point (backward search). Create two empty closed lists, one for the forward search and one for the backward search. Add the starting point to the open list for the forward search and the target point to the open list for the backward search. Set the actual cost of the starting point for the forward search to 0, and the actual cost of the target point for the backward search to 0. Calculate the heuristic estimated cost (h-value) from the starting point to the target point for the forward search, and also calculate the heuristic estimated cost from the target point to the starting point for the backward search.

[0067] (3) Using heuristic functions: When A When the algorithm selects the next node to consider from the open list, it uses the following formula to calculate the total cost of each node: f(n) = g(n) + h(n).

[0068] in: g(n) is the actual cost from the starting node to the current node n.

[0069] h(n) is a heuristic estimate of the cost from the current node n to the target node.

[0070] The total cost f(n) determines the priority of node consideration. The node with the lowest total cost is taken out and considered first. The heuristic value h(n) plays a key role here: it provides direction for the search, causing the algorithm to prioritize nodes that seem closer to the target.

[0071] Forward search uses a heuristic estimate from the starting point to the target point, while backward search uses a heuristic estimate from the target point to the starting point.

[0072] (4) Search process: In each iteration, select the node with the minimum total cost from two open lists, one from the forward search and one from the reverse search. Check if they meet. If they do, a path has been found, and the final path can be obtained by merging the paths from the forward and reverse searches. Otherwise, continue iterating, expanding the neighboring nodes of the selected node and calculating their actual and estimated costs. Add the expanded node to its respective open and closed lists. Repeat this process until they meet or one or both open lists are empty.

[0073] (5) Final path: If bidirectional A The algorithm successfully found a path, and the final path can be obtained by merging the forward and reverse search paths. Typically, when merging paths, the node closest to the meeting point is found in both the forward and reverse search paths to ensure the paths are connected. The merged path is the robot's 3D path on the grid map. The simulation results are as follows... Figure 7 As shown.

[0074] Upon entering the next moment, proceed with the following steps: (6) Reassess the current path: The current path is evaluated using existing technologies, using the existing path and the updated 3D map model. It is determined whether the path is still feasible or whether there is a better path. If the original path is still feasible but no longer optimal, some points on the path are now impassable or risky, and the feasibility of the path may be affected, you can choose to continue along the original path or execute (7) to find a new optimal path. If it is not feasible, execute (7).

[0075] (7) Re-execute bidirectional A algorithm: Re-execute bidirectional A using the updated 3D map. The search yields a new path, such as Figure 8 As shown. For efficiency, the search can be performed within a local area between the current location and the target, rather than the entire map.

[0076] To avoid frequent global searches, this step is only performed when detected obstacles or terrain changes render the current path completely infeasible.

[0077] (8) Execute the new path: Once a new path is obtained, the robot or driverless vehicle begins to follow that path.

[0078] During execution, continue to monitor changes in the environment so that further dynamic updates can be made as necessary.

[0079] (9) Security Mechanism: In addition to the aforementioned strategic adjustments, robots or autonomous vehicles should be equipped with emergency stopping or escape mechanisms to ensure safety. Upon detecting an impending collision or other emergency, the robot or autonomous vehicle should take immediate action, such as a sudden stop or rapid change of direction, to avoid potential danger.

[0080] The technical principle of this invention is as follows: (I) SLAM module 1. LiDAR: Through high-precision distance measurement, it provides more accurate point cloud data for 3D maps. Furthermore, it can capture information in low-light or low-contrast scenes, compensating for the limitations of binocular cameras.

[0081] 2. IMU: Provides the device's orientation, speed, and gravity information to help the SLAM module perform more accurate positioning and mapping.

[0082] 3. Data Fusion Submodule: This module integrates and processes information from multiple sensors to improve the system's perception accuracy and robustness.

[0083] 4. 3D Map Modeling Submodule: Collects information about the surrounding environment through sensor data, and then uses the SLAM algorithm to transform this information into point clouds or mesh structures in 3D space, thereby forming a digital representation of the physical world.

[0084] (II) Three-dimensional terrain analysis module 1. Terrain Feature Recognition: Convolutional Neural Network (CNN) is used to identify various terrain features (such as obstacles, slopes, gullies, stairs, etc.) in the 3D map.

[0085] 2. Real-time updates: Based on the data continuously collected by the SLAM module, the 3D terrain analysis module can update the terrain information in real time, ensuring that navigation decisions are based on the latest environmental data.

[0086] (III) Adaptive Navigation Module 1. Path planning: Utilizing classic path planning algorithms (such as A*). The system finds the optimal path within the identified and classified terrain.

[0087] 2. Navigation strategy adjustment: When a robot or unmanned vehicle detects a sudden obstacle or terrain change, it can adjust its navigation strategy in real time to avoid collisions or other dangers.

[0088] This invention also provides a three-dimensional terrain modeling analysis and adaptive navigation method, including: Obtain the raw data at the current moment; the raw data includes: point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device.

[0089] Preprocessing the raw data at the current time yields the preprocessed raw data at the current time.

[0090] The position of the navigation device at the current moment is obtained by fusing the preprocessed raw data at the current moment.

[0091] A 3D map model of the area where the navigation device is located at the current moment is constructed based on the point cloud data of the area where the navigation device is located at the current moment.

[0092] Determine the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment.

[0093] Based on the current location of the device to be navigated, the 3D map model of the area where the device is located, and the terrain features of each point in the 3D map model of the area where the device is located, a path is planned for the device to be navigated, the current time is updated, and the original data of the current time is returned.

[0094] In practical applications, based on the current location of the device to be navigated, the 3D map model of the area where the device is located, and the terrain features of each point in the 3D map model of the area where the device is located, path planning is performed for the device to be navigated. This specifically includes: Using bidirectional A The algorithm processes the 3D map model of the area where the device to be navigated is located at the current time, based on the current location of the device and the terrain features of each point in the 3D map model of the area where the device to be navigated is located, and performs path planning for the device to be navigated.

[0095] In practical applications, determining the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment specifically includes: A CNN model is used to process the 3D map model of the area where the navigation device is located at the current moment to obtain the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment.

[0096] In practical applications, the preprocessed raw data at the current moment is fused to obtain fused data. Specifically, an extended Kalman filter is used to fuse the preprocessed raw data at the current moment to obtain the position of the navigation device at the current moment.

[0097] This invention can be applied in the following fields: I. Autonomous Vehicles: SLAM technology can be used to build high-precision 3D terrain models in real time, helping autonomous vehicles to perceive roads, obstacles and traffic signs more accurately, thereby achieving safer and more efficient automatic navigation.

[0098] II. Robot Navigation: Robots can use SLAM and high-precision 3D terrain models to plan paths, avoid obstacles, and perform tasks, such as autonomous navigation in environments like warehouses, factories, hospitals, and farms.

[0099] III. Military Applications: In the military field, SLAM-integrated terrain models can be used for precise military deployment, reconnaissance, and target tracking, thereby improving battlefield decision-making and operational efficiency.

[0100] IV. Drones and Aviation: SLAM can be used for navigation of drones and aircraft, supporting precise geographic information collection, mapping, and environmental monitoring, such as for resource management, environmental protection, and disaster response.

[0101] V. Search and Rescue: In emergency situations, the adaptive navigation system integrated with SLAM can help search and rescue teams quickly locate trapped individuals in complex terrain, improving rescue efficiency.

[0102] VI. Geological Exploration and Mining: High-precision terrain modeling can be used in geological exploration and mining activities to help determine the distribution of underground resources, mineral exploration, and resource extraction planning.

[0103] VII. Architecture and Urban Planning: SLAM and terrain modeling can be used in architecture and urban planning to help designers and urban planners better understand terrain features to support construction projects and infrastructure planning.

[0104] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0105] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A three-dimensional terrain modeling analysis and adaptive navigation system, installed on a device to be navigated, characterized in that, The three-dimensional terrain modeling analysis and adaptive navigation system includes: SLAM module, 3D terrain analysis module, and adaptive navigation module; The SLAM module includes: The lidar is used to acquire point cloud data of the area where the navigation device is located in real time. An IMU (Integrated Measurement Unit) is used to measure the acceleration and angular velocity of the device under navigation in real time. The measurement is performed using a three-axis accelerometer and a three-axis gyroscope within the IMU. The accelerometer measures linear acceleration, and the gyroscope measures angular velocity. The accelerometer data is integrated to estimate velocity, and then the velocity is integrated to estimate position, as shown in the following formula: ； ； ； , , These are the position, velocity, and rotation matrix of IMU at time t. Let IMU be the coordinate transformation matrix at time t. It is the acceleration due to gravity. Let be the acceleration at time t. Angular acceleration, , , It is the value at time t+1 obtained by continuous time integration, where Δt is the change over time; The data fusion submodule is used to preprocess the point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device in real time, and then fuse the preprocessed point cloud data of the area where the navigation device is located, the preprocessed acceleration of the navigation device, and the preprocessed angular velocity of the navigation device to obtain the real-time position of the navigation device; the data fusion submodule includes: a data preprocessing unit and a sensor data fusion unit; The data preprocessing unit is used to sequentially perform noise reduction and time synchronization processing on the point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device. The sensor data fusion unit is used to fuse the data obtained by the data preprocessing unit using an extended Kalman filter to obtain the real-time position of the navigation device. The extended Kalman filter is used to perform: State-space modeling: The system's state and sensor observations are modeled as state equations and observation equations. The state equations describe the dynamic evolution of the system, including the changes in variables such as position, velocity, and attitude. The observation equations represent the relationship between the data acquired by the sensors and the system state. State estimation algorithms are used to predict and update the system state. Prediction steps: Based on the nonlinear dynamic model of the system, the state estimate and control input of the previous time step are used to predict the state of the current time step through the nonlinear dynamic equation. This process involves modeling the kinematics and control input of the system, and using the state transition equation of the system to transform the state estimate of the previous time step into the state prediction of the current time step, so as to realize the prediction and update of the dynamic evolution of the system state. Kalman gain calculation: In the extended Kalman filter, calculating the Kalman gain involves linearizing the system's state equation and observation equation to obtain the Jacobian matrix; the Jacobian matrix is ​​used to quantify the relationship between the uncertainty of the state estimation and the observation noise, and then calculate the Kalman gain; the Kalman gain represents the trade-off between system state prediction and sensor observation, and by fusing the two, a more accurate estimate of the system state can be achieved. Update steps: The observations acquired by the sensor are converted into updates in the state space using the observation model, and then the predicted state is fused with the sensor observations using Kalman gain; the calculation of Kalman gain adjusts the weights of the predicted and observed values ​​by balancing the confidence of the prediction and the observation, thereby maximizing the accuracy of the estimation. A 3D map modeling submodule is used to construct a 3D map model of the area where the navigation device is located in real time using the SLAM algorithm based on the point cloud data of the area. The 3D map modeling submodule is used to execute: Data registration: Extract feature points or feature descriptors from each point cloud data, match the features between different point cloud data to find corresponding feature point pairs, and calculate the rigid body transformation relationship between point cloud data through transformation model estimation based on the feature matching results. After completing the initial transformation estimation, the transformation parameters are iteratively adjusted to minimize the residual of the matched point pairs, thereby further optimizing the point cloud registration results. Map stitching: Aligning the registered point cloud data to a reference coordinate system, merging the aligned point cloud data to generate a unified map model, and further optimizing the map quality through point cloud optimization methods; Filtering: Eliminates noise and invalid data, divides point cloud data into a regular three-dimensional voxel grid, and uses the average or median value of points in each voxel to represent the point cloud information within that voxel; Reconstruction: Reconstructing discrete point cloud data to generate a continuous 3D map model; The 3D terrain analysis module is used to determine the terrain features of each point in the 3D map model of the area where the navigation device is located in real time; The adaptive navigation module is used to plan a path for the device to be navigated in real time based on the real-time location of the device, the three-dimensional map model of the area where the device is located, and the terrain features of each point in the three-dimensional map model of the area where the device is located obtained by the three-dimensional terrain analysis module, so as to realize navigation.

2. The three-dimensional terrain modeling analysis and adaptive navigation system according to claim 1, characterized in that, The three-dimensional terrain analysis module is a CNN model.

3. The three-dimensional terrain modeling analysis and adaptive navigation system according to claim 2, characterized in that, The three-dimensional terrain analysis module is used for terrain feature recognition, terrain classification, and real-time updates.

4. The three-dimensional terrain modeling analysis and adaptive navigation system according to claim 1, characterized in that, The adaptive navigation module uses bidirectional A in real time. The algorithm processes the 3D map model of the area where the navigation device is located based on the real-time location of the device and the terrain features of each point in the 3D map model of the area where the device is located, obtained by the 3D terrain analysis module, and performs path planning for the navigation device.

5. A three-dimensional terrain modeling analysis and adaptive navigation method, characterized in that, include: Get the raw data at the current moment; The raw data includes: point cloud data of the area where the navigation device is located, the acceleration of the navigation device, and the angular velocity of the navigation device; Preprocess the raw data at the current time to obtain the preprocessed raw data at the current time; The position of the navigation device at the current moment is obtained by fusing the preprocessed raw data at the current moment; Construct a 3D map model of the area where the navigation device is located at the current moment based on the point cloud data of the area where the navigation device is located at the current moment; Determine the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment; Based on the current location of the device to be navigated, the 3D map model of the area where the device is located at the current time, and the terrain features of each point in the 3D map model of the area where the device is located at the current time, a path is planned for the device to be navigated, then the current time is updated, and the original data of the current time is returned.

6. The three-dimensional terrain modeling analysis and adaptive navigation method according to claim 5, characterized in that, Based on the current location of the navigation device, the 3D map model of the area where the navigation device is located, and the terrain features of each point in the 3D map model of the area where the navigation device is located, path planning is performed for the navigation device, specifically including: Using bidirectional A The algorithm processes the 3D map model of the area where the navigation device is located at the current time based on the current location of the device and the terrain features of each point in the 3D map model of the area where the device is located at the current time, and performs path planning for the device.

7. The three-dimensional terrain modeling analysis and adaptive navigation method according to claim 6, characterized in that, The bidirectional A The algorithm's steps include: (1) Preparations: 3D map representation: Typically, a 3D map can be represented as a voxel grid, where each voxel represents a small cube in 3D space and can be labeled as an obstacle or free space; Starting and target positions: Define the starting point and target point in three-dimensional space, mainly using the device to be navigated as the origin of the coordinate system; (2) Initialization: Define two open lists: one starting from the starting point and one starting from the target point; create two empty closed lists, one for forward search and one for backward search; add the starting point to the open list for forward search and the target point to the open list for backward search; set the actual cost of the starting point for forward search to 0 and the actual cost of the target point for backward search to 0; calculate the heuristic estimated cost from the starting point to the target point for forward search, and also calculate the heuristic estimated cost from the target point to the starting point for backward search. (3) Using heuristic functions: When A When the algorithm selects the next node to consider from the open list, it uses the following formula to calculate the total cost of each node: f(n) = g(n) + h(n); in: g(n) is the actual cost from the starting node to the current node n; h(n) is a heuristic cost estimate from the current node n to the target node; The total cost f(n) determines the priority of node consideration; the node with the lowest total cost is taken out and considered first; the heuristic value h(n) plays a key role here: it provides direction for the search, making the algorithm prioritize those nodes that seem closer to the target; Forward search uses a heuristic estimate from the starting point to the target point, while backward search uses a heuristic estimate from the target point to the starting point. (4) Search process: In each iteration, select the node with the minimum total cost from two open lists, one from the forward search and one from the reverse search; check if the two meet. If they do, the path has been found, and the final path can be obtained by merging the paths from the forward and reverse searches; otherwise, continue iterating, expand the neighboring nodes of the selected node, and calculate their actual and estimated costs. Add the expanded nodes to their respective open and closed lists, and repeat this process until they meet or one or both open lists are empty. (5) Final path: If bidirectional A The algorithm successfully found a path, and the final path can be obtained by merging the forward and reverse search paths. Typically, when merging paths, the node closest to the meeting point in the forward search path is found, and then the node closest to the meeting point in the reverse search path is found to ensure that the paths are connected. The merged path is the robot's 3D path on the grid map. Upon entering the next moment, proceed with the following steps: (6) Reassess the current path: Using existing technology, the current path is evaluated using the existing path and the updated 3D map model to determine whether the path is still feasible or whether there is a better path. If the original path is still feasible but no longer optimal, and some points on the path are now impassable or at risk, the feasibility of the path may be affected. You can choose to continue along the original path or execute (7) to find a new optimal path. If it is not feasible, execute (7). (7) Re-execute bidirectional A algorithm: Re-execute bidirectional A using the updated 3D map. The search yields a new path. Considering efficiency, the search can be conducted within a local area between the current location and the target, rather than across the entire map. To avoid frequent global searches, this step is only performed when detected obstacles or terrain changes render the current path completely infeasible. (8) Execute the new path: Once a new path is obtained, the robot or driverless vehicle begins to follow that path. During execution, continue to monitor changes in the environment so that further dynamic updates can be made as necessary; (9) Security Mechanism: In addition to the aforementioned strategy adjustments, to ensure safety, robots or autonomous vehicles should be equipped with emergency stopping or escape mechanisms. When an impending collision or other emergency is detected, the robot or autonomous vehicle will take immediate action, such as stopping abruptly or changing direction quickly, to avoid potential dangers.

8. The three-dimensional terrain modeling analysis and adaptive navigation method according to claim 5, characterized in that, Determine the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment, specifically including: A CNN model is used to process the 3D map model of the area where the navigation device is located at the current moment to obtain the terrain features of each point in the 3D map model of the area where the navigation device is located at the current moment.

9. The three-dimensional terrain modeling analysis and adaptive navigation method according to claim 5, characterized in that, The preprocessed raw data at the current moment is fused to obtain fused data. Specifically, an extended Kalman filter is used to fuse the preprocessed raw data at the current moment to obtain the position of the navigation device at the current moment.

10. The application of the three-dimensional terrain modeling analysis and adaptive navigation system according to any one of claims 1-4, characterized in that, Applications include autonomous vehicles, robot navigation, military applications, drones and aviation, search and rescue, geological exploration and mining, and architecture and urban planning.

Images