Three-dimensional terrain modeling analysis and adaptive navigation system and method

Abstract

The invention discloses a three-dimensional terrain modeling analysis and adaptive navigation system and method, and relates to the technical field of navigation. A laser radar in the system is used for acquiring point cloud data of an area where a to-be-navigated device is located in real time; the IMU is used for measuring the acceleration and the angular velocity of a to-be-navigated device in real time; the data fusion sub-module is used for preprocessing the point cloud data, the acceleration and the angular velocity in real time and fusing the preprocessed data; the three-dimensional map modeling sub-module is used for constructing a three-dimensional map model according to the point cloud data by adopting an SLAM algorithm in real time; the three-dimensional terrain analysis module is used for determining terrain features of all points in the three-dimensional map model in real time; and the adaptive navigation module is used for performing path planning on the to-be-navigated device according to the three-dimensional map model and the topographic features of each point in real time to realize navigation. According to the invention, the precision of original data can be improved, the positioning precision is improved, and accurate navigation is realized.

Description

Technical Field

[0001] The present 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 Art

[0002] The status of high-precision 3D terrain modeling and analysis and adaptive navigation systems is a key focus in today's technology. Through the use of advanced technologies such as satellite data, LiDAR and photogrammetry, remarkable progress has been made to achieve accurate 3D simulation of the earth's surface. This provides accurate 3D map models for adaptive navigation systems, allowing them to perceive the environment and plan paths more accurately.

[0003] However, the 3D terrain modeling analysis and adaptive navigation system still faces some challenges. The 3D modeling in the 3D terrain modeling analysis and adaptive navigation system usually requires multiple sensors to obtain environmental information to obtain raw data. Especially in the presence of dynamic targets, the acquired raw data may have inconsistencies and noise, resulting in instability in the accuracy of subsequent positioning based on the raw data, which leads to the problem of large errors in posture calculation. Therefore, a high-precision 3D terrain modeling analysis and adaptive navigation system is needed to process the raw data to improve the accuracy of the raw data, thereby improving the positioning accuracy and avoiding the problem of large errors in posture calculation. Summary of the invention

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

[0005] To achieve the above object, the present invention provides the following solutions:

[0006] A three-dimensional terrain modeling analysis and adaptive navigation system is provided on a navigation device, and the three-dimensional terrain modeling analysis and adaptive navigation system comprises:

[0007] SLAM module, 3D terrain analysis module and adaptive navigation module;

[0008] The SLAM module includes: a laser radar, an IMU, a data fusion submodule and a three-dimensional map modeling submodule;

[0009] The laser radar is used to obtain point cloud data of the area where the navigation device is located in real time; the IMU is used to measure the acceleration of the navigation device and the angular velocity of the navigation device in real time; the data fusion submodule is used to pre-process 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 fuse the pre-processed point cloud data of the area where the navigation device is located, the pre-processed acceleration of the navigation device and the pre-processed angular velocity of the navigation device to obtain the real-time position of the navigation device; the three-dimensional map modeling submodule is used to construct a three-dimensional map model of the area where the navigation device is located according to the point cloud data of the area where the navigation device is located by using the SLAM algorithm in real time;

[0010] The three-dimensional terrain analysis module is used to determine in real time the terrain features of each point in the three-dimensional map model of the area where the navigation device is located;

[0011] The adaptive navigation module is used to plan the path of the navigation device and realize navigation according to the real-time position of the navigation device, the three-dimensional map model of the area where the navigation device is located, and the terrain characteristics of each point in the three-dimensional map model of the area where the navigation device is located obtained by the three-dimensional terrain analysis module.

[0012] A three-dimensional terrain modeling analysis and adaptive navigation method, comprising:

[0013] Acquire the original data at the current moment; the original data includes: point cloud data of the area where the navigation device is located, acceleration of the navigation device, and angular velocity of the navigation device;

[0014] Preprocessing the original data at the current moment to obtain the preprocessed original data at the current moment;

[0015] The pre-processed raw data at the current moment is fused to obtain the position of the navigation device at the current moment;

[0016] Constructing a three-dimensional map model of the area where the navigation device is currently located according to the point cloud data of the area where the navigation device is currently located;

[0017] Determine the terrain features of each point in the three-dimensional map model of the area where the navigation device is currently located;

[0018] According to the current position of the navigation device, the three-dimensional map model of the area where the navigation device is located at the current moment, and the terrain features of each point in the three-dimensional map model of the area where the navigation device is located at the current moment, the path of the navigation device is planned, and then the current moment is updated and the original data of the current moment is returned.

[0019] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0020] The three-dimensional terrain modeling analysis and adaptive navigation system provided by the present invention comprises: a SLAM module, a three-dimensional terrain analysis module and an adaptive navigation module; the SLAM module comprises: a laser radar, an IMU, a data fusion submodule and a three-dimensional map modeling submodule; the laser radar is used to obtain point cloud data of the area where the navigation device is located in real time; the IMU is used to measure the acceleration of the navigation device and the angular velocity of the navigation device in real time; the data fusion submodule is used to pre-process 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 the pre-processed point cloud data of the area where the navigation device is located, the pre-processed acceleration of the navigation device and the pre-processed angular velocity of the navigation device are stored in the data fusion submodule; The angular velocity is fused to obtain the real-time position of the device to be navigated; the three-dimensional map modeling submodule is used to construct a three-dimensional map model of the area where the device to be navigated is located according to the point cloud data of the area where the device to be navigated is located using the SLAM algorithm in real time; the three-dimensional terrain analysis module is used to determine the terrain characteristics of each point in the three-dimensional map model of the area where the device to be navigated is located in real time; the adaptive navigation module is used to plan the path of the device to be navigated to achieve navigation according to the three-dimensional map model of the area where the device to be navigated is located and the terrain characteristics of each point in the three-dimensional map model of the area where the device to be navigated is located obtained by the three-dimensional terrain analysis module. The present invention preprocesses and fuses the original data to improve the accuracy of the original data, thereby improving the positioning accuracy and achieving precise navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0022] Figure 1 A three-dimensional map model provided by an embodiment of the present invention;

[0023] Figure 2 is a voxel grid map;

[0024] Figure 3 This is a schematic diagram of the neural network perception principle;

[0025] Figure 4 To explore the terrain on the ground;

[0026] Figure 5 It is a schematic diagram of simulation test;

[0027] Figure 6 It is a schematic diagram of the bidirectional A* algorithm;

[0028] Figure 7 This is a schematic diagram of the merged path;

[0029] Figure 8 This is the updated path diagram;

[0030] Fig. 9 A block diagram of a three-dimensional terrain modeling analysis and adaptive navigation system provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0031] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0032] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0033] The embodiment of the present invention provides a three-dimensional terrain modeling analysis and adaptive navigation system, which is arranged on a navigation device, such as Fig. 9 As shown, the three-dimensional terrain modeling analysis and adaptive navigation system includes:

[0034] SLAM module, 3D terrain analysis module and adaptive navigation module.

[0035] The SLAM module includes: a laser radar, an IMU, a data fusion submodule and a three-dimensional map modeling submodule; the laser radar is used to obtain point cloud data of the area where the navigation device is located in real time; the IMU is used to measure the acceleration of the navigation device and the angular velocity of the navigation device in real time; the data fusion submodule is used to pre-process 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 fuse the pre-processed point cloud data of the area where the navigation device is located, the pre-processed acceleration of the navigation device and the pre-processed angular velocity of the navigation device to obtain the real-time position of the navigation device; the three-dimensional map modeling submodule is used to construct a three-dimensional map model of the area where the navigation device is located according to the point cloud data of the area where the navigation device is located by using the SLAM algorithm in real time.

[0036] The three-dimensional terrain analysis module is used to determine in real time the terrain features of each point in the three-dimensional map model of the area where the navigation device is located.

[0037] The adaptive navigation module is used to plan the path of the navigation device and realize navigation according to the real-time position of the navigation device, the three-dimensional map model of the area where the navigation device is located, and the terrain characteristics of each point in the three-dimensional map model of the area where the navigation device is located obtained by the three-dimensional terrain analysis module.

[0038] In practical applications, the laser radar in the SLAM module emits invisible laser beams to measure the beam information reflected by the object (the area where the navigation device is located), and generates point cloud data through data processing, which can be used for three-dimensional map modeling and provide the accurate terrain model required for environmental perception and automatic navigation.

[0039] In practical applications, the IMU in the SLAM module can be model GY-95T. IMU is usually installed in a moving system. By measuring the inertia of the moving carrier (device to be navigated), the state of the object itself is inferred. These physical quantities related to inertia are usually not directly obtained positions and rotations, but physical quantities after integration. From the outside, it is only necessary to care about whether their measurements of angular velocity and acceleration are accurate, and the relationship between these quantities and vehicle position and posture. In the present invention, the three-axis accelerometer and three-axis gyroscope in the IMU can be used for measurement. The accelerometer measures linear acceleration, and the gyroscope measures angular velocity. Through integral calculation, the accelerometer data is integrated to estimate the velocity, and then the velocity is integrated to estimate the position, as shown in the following formula.

[0040]

[0041] They are the position, velocity and rotation matrix of imu at time t, is the coordinate system transformation matrix of imu at time t, g w is the acceleration due to gravity, a t is the acceleration at time t, is the angular acceleration, is the value at time t+1 obtained by continuous time integration, and Δt is the time change.

[0042] 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 is used to perform denoising 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 in turn to ensure the quality and consistency of the data; then the sensor data fusion unit is used to use an extended Kalman filter to fuse the data of different sensors (lidar and IMU) obtained by the data preprocessing unit, and use the complementary information between the sensors to improve the accuracy and robustness of environmental perception. Data fusion is to solve the situation where the positioning speed and acceleration error fluctuate greatly, and then a more accurate data can be obtained. These data are used for positioning. For example, when an object moves, the acceleration is forward, and the current position on the corresponding map will move forward.

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

[0044] Denoising: The data acquired by the sensor may be affected by various noises, such as the noise of the sensor itself, interference in the environment, etc. Denoising operations can use digital filtering techniques, such as mean filtering, median filtering, Gaussian filtering, etc., to smooth the data and reduce the impact of noise.

[0045] Time synchronization: Time synchronization is the process of ensuring that the data obtained by multiple sensors are consistent in time. Because in the SLAM system, the data from different sensors need to be collected and fused at the same time to ensure that the timing relationship between the data is accurate, so as to ensure the accuracy and reliability of subsequent data fusion and state estimation.

[0046] (2) The sensor data fusion unit is the core part that fuses the 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.

[0047] The extended Kalman filter is a filter commonly used for state estimation, especially for nonlinear systems. In SLAM systems, the extended Kalman filter is widely used in sensor data fusion to estimate the position, posture and other state variables of the robot or vehicle and build a map. Its working principle is as follows:

[0048] State space modeling: The system 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 posture. The observation equations represent the relationship between the data obtained by the sensor and the system state. Through this formal description, state estimation algorithms such as the Kalman filter can be used to predict and update the system state.

[0049] Prediction step: Based on the nonlinear dynamic model of the system, the state estimate and control input at the previous moment are used to predict the state at the current moment through the nonlinear dynamic equation. This process involves modeling the kinematics and control input of the system, and using the system's state transfer equation to convert the state estimate at the previous moment into the state prediction at the current moment, so as to realize the prediction and update of the dynamic evolution of the system state.

[0050] Kalman gain calculation: In the extended Kalman filter, calculating the Kalman gain involves linearizing the state equation and observation equation of the system to obtain the Jacobian matrix. These Jacobian matrices are used to quantify the relationship between the uncertainty of the state estimate and the observation noise, and then calculate the Kalman gain. The Kalman gain represents the trade-off between the system state prediction and the sensor observation, and by fusing the two, a more accurate estimate of the system state is achieved.

[0051] Update step: The observation model is used to convert the observations obtained by the sensor into updates in the state space, and then the predicted state is fused with the sensor observations through the Kalman gain. This process takes into account the uncertainty of the state prediction and the noise of the observation to obtain the final state estimate. The calculation of the Kalman gain adjusts the weights of the predicted and observed values ​​by weighing the credibility of the prediction and observation, thereby maximizing the accuracy of the estimate.

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

[0053] The data fusion submodule also includes (3) fusion result evaluation and optimization: the fusion result needs to be evaluated and optimized to ensure that the fused data meets the system requirements. Evaluation indicators can include positioning error, Figure 1 Consistency, environmental perception accuracy, etc. According to the evaluation results, the algorithm can be adjusted and optimized to be the existing technology.

[0054] In practical applications, the 3D map modeling submodule in the SLAM module uses existing technologies to perform a series of processing such as data registration, map stitching, filtering and reconstruction on the collected multiple point cloud data, and finally obtains a complete 3D map model, such as Figure 1 shown.

[0055] Data registration: Extract feature points or feature descriptors from each point cloud data, match the features between different point cloud data, find the corresponding feature point pairs, and calculate the rigid body transformation relationship between the point cloud data through transformation model estimation based on the result of feature matching. Common transformation models include translation, rotation, scaling, affine transformation, etc. You can use methods such as RANSAC algorithm or least squares method to estimate the transformation model and filter out matching point pairs that meet the requirements. After completing the initial transformation estimation, the transformation parameters are iteratively adjusted to minimize the residual of the matching point pairs, and further optimize the point cloud registration results.

[0056] Map stitching: Align the registered point cloud data to a reference coordinate system. You can select one frame of point cloud as a reference, transform the coordinates of other point cloud data so that they are in the same coordinate system as the reference point cloud, and fuse the aligned point cloud data to generate an overall map model. After map stitching, some discontinuities or gaps may appear. The quality of the map can be further optimized through point cloud optimization methods.

[0057] Filtering: Eliminate noise and invalid data, divide the point cloud data into a regular 3D voxel grid, and use the average or median value of the points in each voxel to represent the point cloud information in the voxel. By adjusting the size of the voxel, the resolution of the filtered point cloud can be controlled, and the point cloud data can be filtered.

[0058] Reconstruction: After map stitching and filtering, the point cloud data obtained is still discrete. Therefore, the discrete point cloud data needs to be reconstructed to generate a continuous 3D map model.

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

[0060] The information obtained through SLAM technology provides a raw map, but in order to achieve effective navigation, the data needs to be further interpreted. This is the three main functions of the 3D terrain analysis module introduced here: terrain feature recognition, terrain classification and real-time update.

[0061] 1. Terrain feature identification:

[0062] Terrain feature recognition is the cornerstone of navigation decisions. A simple 3D map model may contain thousands of data points, but not every data point is useful for navigation decisions. It is crucial to identify key features in the map, such as obstacles, slopes, and gullies. Figure 4 As shown, green is the ground and red is the obstacle. In order to realize terrain feature recognition, a convolutional neural network (CNN) can be used.

[0063] (1) Data preprocessing and voxelization:

[0064] Get point cloud data from LiDAR, define the size of voxels, and voxelize continuous point cloud data to get a regular voxel grid, such as Figure 2 As shown, the speed-up grid is then denoised using statistical filtering, which is a prior art process.

[0065] (2) Data enhancement:

[0066] In order to improve the generalization ability of the model, various transformations can be performed on the point cloud data, such as rotation, scaling and flipping, adding noise, and simulating possible data corruption in the real environment. This process is an existing technology.

[0067] (3) CNN model design:

[0068] like Figure 3 As shown, including:

[0069] Input layer: The voxelized point cloud data is input into the network.

[0070] Convolutional layer: Use a three-dimensional convolution kernel to process the input data and extract primary spatial features from it.

[0071] Activation layer: Use ReLU for feature activation.

[0072] Pooling layer: Uses three-dimensional pooling technology to reduce the spatial size of the data while retaining important features.

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

[0074] Fully connected layer: connects high-level features to the output layer to classify different terrain features. Figure 3 (a) shows the fully connected mode diagram of the CNN model. Figure 3 (b) shows the local connection pattern diagram of the CNN model.

[0075] (4) Model training:

[0076] The model is trained using voxel data (voxelized point cloud data) with labels ("obstacle", "flat", "slope", etc.), where each voxel may be labeled as "obstacle", "flat", "slope", etc. Then, an appropriate loss function, such as cross entropy loss, is selected to optimize the accuracy of the model prediction. The model parameters are updated using an optimization algorithm such as SGD, and the training process is monitored by the validation set to avoid overfitting. This training process is an existing process.

[0077] (5) Model evaluation and testing:

[0078] Evaluate model performance using a test dataset that was not used in training. Check the model's predictive power through various evaluation metrics, such as accuracy, precision, recall, etc. This process is a prior art.

[0079] (6) Deployment and real-time application:

[0080] The trained model is integrated into the navigation system. During the actual navigation process, the point cloud data in the 3D map model is passed to the CNN model in real time to identify terrain features and provide input for navigation decisions.

[0081] 2. Real-time updates

[0082] Because the environment is dynamic, especially in cities or other complex scenes. In order to adapt to these changes, the system must be able to update its map and terrain analysis in real time. Therefore, it is necessary to integrate SLAM technology so that the system can update in real time when new data arrives, and the map can be updated in real time during the movement. Test results are as follows Figure 5 shown.

[0083] (1) Data stream: LiDAR continuously acquires point cloud data.

[0084] (2) Real-time processing: The 3D map modeling submodule uses the currently acquired point cloud data to compare with the previously acquired point cloud data to detect new or disappeared objects, moving objects, or any other forms of changes. If there are changes, a new 3D map model is constructed based on the newly acquired point cloud data. The new 3D map model is then sent to the CNN model described above for feature recognition and classification. If there are no changes, there is no need to update the map model.

[0085] (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 unmanned 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 is changed, the map as a whole will move in the same direction (for example, forward). However, in fact, the map should not move, but its own coordinate system should move, so the old 3D map model is merged with the new 3D map model.

[0086] In actual application, the adaptive navigation module uses the bidirectional A* algorithm in real time to process the three-dimensional map model of the area where the navigation device is located according to the real-time position of the navigation device and the terrain characteristics of each point in the three-dimensional map model of the area where the navigation device is located obtained by the three-dimensional terrain analysis module, and performs path planning for the navigation device. The specific steps are:

[0087] At this moment, the bidirectional A* algorithm is used to implement three-dimensional path planning on the three-dimensional map. The bidirectional A* algorithm is an efficient algorithm for path planning. It searches from the starting point and the end point at the same time, such as Figure 6 to improve the search efficiency.

[0088] The steps of the bidirectional A* algorithm include:

[0089] (1) Preparation:

[0090] 3D map representation: Typically, a 3D map can be represented as a voxel grid. Each voxel represents a small cube in 3D space and can be labeled as an obstacle or free space.

[0091] Start and target position: define the starting point and target point in three-dimensional space, mainly taking the navigation device as the coordinate origin.

[0092] (2) Initialization:

[0093] Set up two open lists: one starting from the starting point (forward search) and one starting from the target point (reverse search). Create two empty closed lists, one for the forward search and one for the reverse search. Add the starting point to the open list of the forward search and the target point to the open list of the reverse search. Set the actual cost of the starting point of the forward search to 0 and the actual cost of the target point of the reverse search to 0. Calculate the heuristic estimated cost (h value) from the starting point to the target point of the forward search and the heuristic estimated cost from the target point to the starting point of the reverse search.

[0094] (3) Use heuristic function:

[0095] When the A* algorithm selects the next node to consider from the open list, it calculates the total cost of each node using the following formula: f(n) = g(n) + h(n).

[0096] in:

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

[0098] h(n) is the heuristic estimated cost from the current node n to the target node.

[0099] This total cost f(n) determines the priority of the nodes to be considered. The nodes with the lowest total cost are 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 goal.

[0100] The forward search uses a heuristic estimate from the starting point to the target point, and the reverse search uses a heuristic estimate from the target point to the starting point.

[0101] (4) Search process:

[0102] In each iteration, select the node with the minimum total cost from the two open lists, one from the forward search and one from the backward search. Check if the two sides meet. If so, the path has been found and the final path can be obtained by merging the paths from the forward and backward searches. Otherwise, continue the iteration, expanding the neighbors of the selected node and calculating their actual and estimated costs. Add the expanded nodes to the respective open and closed lists. Repeat this process until they meet or one or both open lists are empty.

[0103] (5) Final path:

[0104] If the bidirectional A* algorithm successfully finds a path, the final path can be obtained by merging the forward and reverse search paths. Usually, when merging paths, find the node closest to the meeting point in the forward search path, and then find the node closest to the meeting point in the reverse search path to ensure that the paths are connected. The merged path is the robot's three-dimensional path on the grid map. The result simulation is as follows Figure 7 shown.

[0105] When entering the next moment, perform the following steps:

[0106] (6) Re-evaluate the current path:

[0107] Use the existing technology to evaluate the current path using the existing path and the updated 3D map model. 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, some points on the path are now inaccessible 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).

[0108] (7) Re-execute the bidirectional A* algorithm:

[0109] Use the updated 3D map to re-execute the bidirectional A* search to obtain a new path, such as Figure 8 Considering efficiency, the search can be performed in a local range between the current position and the target, rather than the entire map.

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

[0111] (8) Execute the new path:

[0112] Once the new path is obtained, the robot or unmanned vehicle begins to follow it.

[0113] During execution, the environment continues to be monitored for changes so that further dynamic updates can be made when necessary.

[0114] (9) Safety mechanism:

[0115] In addition to the above-mentioned strategic adjustments, in order to ensure safety, robots or unmanned vehicles should be equipped with emergency stop or escape mechanisms. When an impending collision or other emergency is detected, the robot or unmanned vehicle will immediately take action, such as emergency stop or rapid change of direction, to avoid potential danger.

[0116] The technical principle of the present invention is as follows:

[0117] (I) SLAM module

[0118] 1. LiDAR: It provides more accurate point cloud data for 3D maps through high-precision distance measurement. In addition, it can capture information in low-light or low-contrast scenes, making up for the shortcomings of binocular cameras.

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

[0120] 3. Data fusion submodule: Comprehensive application of multi-sensor information integration and processing to improve system perception accuracy and robustness.

[0121] 4. 3D map modeling submodule: collects information about the surrounding environment through sensor data, and then uses the SLAM algorithm to convert this information into a point cloud or grid structure in 3D space, thus forming a digital representation of the physical world.

[0122] (2) 3D terrain analysis module

[0123] 1. Terrain feature recognition: Convolutional neural network (CNN) is used to identify various terrain features (such as obstacles, slopes, gullies, stairs, etc.) in 3D maps.

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

[0125] (III) Adaptive Navigation Module

[0126] 1. Path planning: Use classic path planning algorithms (such as A*) to find the optimal path in the identified and classified terrain.

[0127] 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 collision or other dangers.

[0128] The embodiment of the present invention further provides a three-dimensional terrain modeling analysis and adaptive navigation method, including:

[0129] The original data at the current moment is obtained; the original data includes: point cloud data of the area where the navigation device is located, acceleration of the navigation device and angular velocity of the navigation device.

[0130] The original data at the current moment is preprocessed to obtain the preprocessed original data at the current moment.

[0131] The pre-processed raw data at the current moment is fused to obtain the position of the navigation device at the current moment.

[0132] A three-dimensional map model of the area where the device to be navigated is currently located is constructed according to the point cloud data of the area where the device to be navigated is currently located.

[0133] Determine the terrain features of each point in the three-dimensional map model of the area where the navigation device is currently located.

[0134] According to the current position of the navigation device, the three-dimensional map model of the area where the navigation device is located at the current moment, and the terrain features of each point in the three-dimensional map model of the area where the navigation device is located at the current moment, the path of the navigation device is planned, and then the current moment is updated and the original data of the current moment is returned.

[0135] In practical applications, path planning is performed for the navigation device according to the current position of the navigation device, the three-dimensional map model of the area where the navigation device is located at the current moment, and the terrain features of each point in the three-dimensional map model of the area where the navigation device is located at the current moment, specifically including:

[0136] A bidirectional A* algorithm is used to process the three-dimensional map model of the area where the navigation device is currently located according to the position of the navigation device at the current moment and the terrain features of each point in the three-dimensional map model of the area where the navigation device is currently located, and to plan the path for the navigation device.

[0137] In practical applications, determining the terrain features of each point in the three-dimensional map model of the area where the navigation device is currently located specifically includes:

[0138] The CNN model is used to process the three-dimensional map model of the area where the navigation device is currently located to obtain the terrain features of each point in the three-dimensional map model of the area where the navigation device is currently located.

[0139] 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.

[0140] The present invention can be applied in the following fields:

[0141] 1. Self-driving cars: SLAM technology can be used to build high-precision three-dimensional terrain models in real time, helping self-driving cars to perceive roads, obstacles and traffic signs more accurately, thereby achieving safer and more efficient automatic navigation.

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

[0143] 3. Military applications: The military field can use SLAM integrated terrain models to conduct precise military deployment, reconnaissance and target tracking, and improve battlefield decision-making and combat efficiency.

[0144] 4. UAV and Aviation: SLAM can be used for navigation of UAVs and aircraft, supporting accurate geographic information collection, mapping and environmental monitoring, such as for resource management, environmental protection and disaster response.

[0145] 5. Search and rescue: In emergency situations, the SLAM-integrated adaptive navigation system can help search and rescue teams quickly locate trapped people in complex terrain and improve rescue efficiency.

[0146] 6. Geological Exploration and Mining: High-precision terrain modeling can be used in geological exploration and mining activities to help determine underground resource distribution, mineral exploration, and resource mining planning.

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

[0148] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, 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.

[0149] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A three-dimensional terrain modeling analysis and adaptive navigation system, arranged on a navigation device, 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: a laser radar, an IMU, a data fusion submodule and a three-dimensional map modeling submodule; The laser radar is used to obtain point cloud data of the area where the navigation device is located in real time; the IMU is used to measure the acceleration of the navigation device and the angular velocity of the navigation device in real time; the data fusion submodule is used to pre-process 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 fuse the pre-processed point cloud data of the area where the navigation device is located, the pre-processed acceleration of the navigation device and the pre-processed angular velocity of the navigation device to obtain the real-time position of the navigation device; the three-dimensional map modeling submodule is used to construct a three-dimensional map model of the area where the navigation device is located according to the point cloud data of the area where the navigation device is located by using the SLAM algorithm in real time; The three-dimensional terrain analysis module is used to determine in real time the terrain features of each point in the three-dimensional map model of the area where the navigation device is located; The adaptive navigation module is used to plan the path of the navigation device to achieve navigation based on the real-time position of the navigation device, the three-dimensional map model of the area where the navigation device is located, and the terrain characteristics of each point in the three-dimensional map model of the area where the navigation device is located obtained by the three-dimensional terrain analysis module.

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 1, characterized in that: The adaptive navigation module uses a bidirectional A* algorithm in real time to process the three-dimensional map model of the area where the navigation device is located according to the real-time position of the navigation device and the terrain characteristics of each point in the three-dimensional map model of the area where the navigation device is located obtained by the three-dimensional terrain analysis module, and plans the path for the navigation device.

4. The three-dimensional terrain modeling analysis and adaptive navigation system according to claim 1, characterized in that: The data fusion submodule includes: a data preprocessing unit and a sensor data fusion unit; The data preprocessing unit is used to sequentially perform denoising 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.

5. A three-dimensional terrain modeling analysis and adaptive navigation method, characterized in that: include: Get the raw data at the current moment; The original data includes: point cloud data of the area where the navigation device is located, acceleration of the navigation device and angular velocity of the navigation device; Preprocessing the original data at the current moment to obtain the preprocessed original data at the current moment; The pre-processed raw data at the current moment is fused to obtain the position of the navigation device at the current moment; Constructing a three-dimensional map model of the area where the navigation device is currently located according to the point cloud data of the area where the navigation device is currently located; Determine the terrain features of each point in the three-dimensional map model of the area where the navigation device is currently located; According to the current position of the navigation device, the three-dimensional map model of the area where the navigation device is located at the current moment, and the terrain features of each point in the three-dimensional map model of the area where the navigation device is located at the current moment, the path of the navigation device is planned, and then the current moment is updated and the original data of the current moment is returned.

6. The three-dimensional terrain modeling analysis and adaptive navigation method according to claim 5, characterized in that: According to the current position of the navigation device, the three-dimensional map model of the area where the navigation device is located, and the terrain features of each point in the three-dimensional map model of the area where the navigation device is located, the path planning of the navigation device includes: A bidirectional A* algorithm is used to process the three-dimensional map model of the area where the navigation device is currently located according to the position of the navigation device at the current moment and the terrain features of each point in the three-dimensional map model of the area where the navigation device is currently located, and to plan the path for the navigation device.

7. 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 three-dimensional map model of the area where the navigation device is currently located, specifically including: The CNN model is used to process the three-dimensional map model of the area where the navigation device is currently located to obtain the terrain features of each point in the three-dimensional map model of the area where the navigation device is currently located.

8. 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 are 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.

Images