Pose resolving method and system for sea-land fusion navigation

Abstract

The invention belongs to the technical field of navigation and positioning, particularly discloses a sea-land fusion navigation pose resolving method and system, and aims to solve the problems of positioning failure, model mismatch and the like of traditional navigation in a cross-medium environment. The method comprises the following steps: acquiring multi-source sensor data of an inertial measurement unit, a satellite system and the like; extracting surface texture and underwater topographic information from the surface texture and underwater topographic information, and generating perception features containing land, water areas or transition areas; determining a dynamic fusion strategy to generate adaptive parameters, and selecting a matched dynamic error model; and weighting fusion data to obtain an initial pose, and outputting a final result through error model compensation. The system comprises a data acquisition module, an environment characteristic generation module and the like, realizes environment adaptive switching, improves navigation continuity, robustness and precision, and is suitable for complex operation scenes of amphibious robots.

Description

Technical Field

[0001] This invention belongs to the field of navigation and positioning technology, and relates to a pose calculation method and system for integrated land and sea navigation. Background Technology

[0002] With the widespread application of amphibious robots in complex scenarios such as marine exploration, disaster relief, border patrol, and special operations, unprecedented challenges have been posed to their ability to perform high-precision pose calculations across various media environments. Traditional navigation systems are mostly designed based on a single media environment, relying on inertial measurement units and global navigation satellite systems to build a basic positioning framework. Their core principle is based on a static environment model and fixed sensor weight allocation.

[0003] However, robots face dramatic dynamic transitions in real-world operations: from hard land surfaces to buoyancy-supported water, from visually textured surfaces to acoustically reflective underwater terrain. Their kinematic characteristics and perceived environment exhibit nonlinear and discontinuous transitions. Fixed fusion strategies, unable to perceive real-time changes in environmental media, are prone to pose drift, model mismatch, and error accumulation, exacerbating localization failures, attitude instability, and trajectory oscillations, significantly reducing operational safety and task completion rates. Furthermore, users' demands for navigation accuracy, environmental adaptability, and dynamic response speed are becoming increasingly stringent, and the rigid algorithm architecture of traditional methods struggles to meet the adaptive optimization requirements of cross-domain scenarios.

[0004] Existing technologies for cross-medium navigation generally suffer from three major drawbacks: fixed sensor weights, lack of environmental perception, and a single dynamic error model. On the one hand, most systems adopt independent navigation architectures for land-based mobile robots or underwater vehicles, without establishing mechanisms for medium recognition and model switching. This leads to fatal blind spots in shallow water or tidal flats where visual perception fails while sonar is not activated, or where accumulated inertial errors are not corrected by the fluid model. Some improvement schemes attempt to introduce terrain classification modules, but these only reach the level of outputting environmental labels and fail to translate the classification results into linkage control commands for fusion parameters and error models, causing strategy adjustments to lag behind environmental changes.

[0005] On the other hand, existing methods generally neglect the hybrid dynamics characteristics of the transition region and fail to construct a ground-fluid coupling error compensation mechanism, causing sudden pose jumps and control instability during robot wading, climbing, or landing. These problems are particularly prominent in high-speed maneuvering, strong interference, or low signal-to-noise ratio scenarios, and have become a core bottleneck restricting the all-domain operation capability of amphibious robots. Therefore, there is an urgent need for a sea-land fusion navigation pose calculation method and system with environmental self-awareness, policy self-adaptation, and model self-switching capabilities. Summary of the Invention

[0006] In view of this, in order to solve the problems mentioned in the background technology, a pose calculation method and system for integrated land and sea navigation is proposed.

[0007] The objective of this invention can be achieved through the following technical solution: The first aspect of this invention provides a pose calculation method for integrated land and sea navigation, comprising: acquiring multi-source sensor data collected by an inertial measurement unit, a global navigation satellite system, a visual odometry system, and a multibeam sonar depth sounding system.

[0008] Based on the multi-source sensor data, environmental perception features are generated.

[0009] Based on the environmental perception characteristics, a dynamic fusion strategy is determined to generate adaptive fusion parameters, and a dynamic error model that matches the current environment is selected.

[0010] By combining the multi-source sensor data with the adaptive fusion parameters, a weighted fusion calculation is performed to output a preliminary pose result.

[0011] The preliminary pose result is input into the selected dynamic error model for calculation, an error compensation value is generated, the error compensation value is applied to the preliminary pose result, and the final pose data is output.

[0012] A second aspect of the present invention provides a pose calculation system for integrated land and sea navigation, comprising: a data acquisition module for acquiring multi-source sensor data collected by an inertial measurement unit, a global navigation satellite system, a visual odometry system, and a multibeam sonar depth sounding system.

[0013] An environmental feature generation module is used to generate environmental perception features based on the multi-source sensor data. These environmental perception features reflect the state of the land, water, or transitional area where the robot is located.

[0014] The dynamic strategy decision module is used to determine a dynamic fusion strategy based on the environmental perception characteristics to generate adaptive fusion parameters and select a dynamic error model that matches the current environment.

[0015] The weighted fusion calculation module is used to combine the multi-source sensor data with the adaptive fusion parameters to perform weighted fusion calculation and output preliminary pose results.

[0016] The error compensation module is used to input the preliminary pose result into the selected dynamic error model for calculation to generate an error compensation value, apply the error compensation value to the preliminary pose result, and output the final pose data.

[0017] Compared to existing technologies, the beneficial effects of this invention are as follows: 1. This invention, through the coordinated selection of dynamic fusion strategies and dynamic error models based on environmental perception features, can intelligently adjust the fusion weights and error compensation methods of multi-source sensor data. This environmental adaptability enables the navigation system to achieve seamless and smooth switching between land and water operating modes, effectively avoiding jumps or interruptions in positioning results caused by drastic environmental changes, and ensuring the continuity of the navigation trajectory when the robot operates across domains.

[0018] This invention introduces a real-time evaluation mechanism for sensor data quality and dynamically correlates it with the fusion strategy, forming a dual adaptive closed loop. When the signal quality of any sensor deteriorates due to external interference or its own malfunction, the system can quickly reduce its weight in the fusion calculation and strengthen the role of other reliable data sources. This significantly improves the robustness and reliability of the entire navigation system under unexpected conditions such as complex electromagnetic environments, severe weather, or partial sensor failure.

[0019] This invention combines a physics-based dynamic error model with a data-driven fusion algorithm. By verifying the consistency of historical trajectory sequences, it achieves dual suppression of systematic errors and random outliers. The dynamic error model can actively compensate for the cumulative drift of inertial navigation caused by environmental media, such as water flow resistance or ground friction. Meanwhile, trajectory consistency verification can effectively identify and eliminate abrupt changes that do not conform to kinematic laws. This combination significantly improves the absolute accuracy and trajectory smoothness of the navigation system during long-term, long-distance operation. Attached Figure Description

[0020] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of 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.

[0021] Figure 1 This is a schematic diagram illustrating the implementation steps of the method of the present invention.

[0022] Figure 2 This is a schematic diagram of the system module connections of the present invention.

[0023] Figure 3 This is a flowchart illustrating the generation of environmental perception features in an embodiment of the present invention.

[0024] Figure 4 This is a schematic diagram illustrating the dynamic fusion strategy and adaptive fusion parameter generation in an embodiment of the present invention.

[0025] Figure 5 This is a schematic block diagram of the error compensation analysis steps in an embodiment of the present invention. Detailed Implementation

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

[0027] Please see Figure 1 As shown, the first aspect of the present invention provides a method and system for attitude calculation in integrated land and sea navigation, the specific steps of which are as follows: acquiring multi-source sensor data collected by an inertial measurement unit, a global navigation satellite system, a visual odometry system, and a multibeam sonar depth sounding system.

[0028] It should be noted that the inertial measurement unit (IMU) is installed in a core location inside the robot's main body, used to output real-time three-axis acceleration and three-axis angular velocity data. The global navigation satellite system (GNSS) receiving antenna is fixed in an open area on the top of the robot to ensure signal reception quality, providing latitude, longitude, and altitude information. The visual odometry camera is positioned at the front or bottom of the robot to capture the distribution of ground feature points and local geometric structure information. The multibeam sonar bathymetry system's sonar probe is installed on the bottom of the robot near the water body to collect water depth point cloud data and seabed surface morphology information. These sensors are connected to the data acquisition unit via a high-speed data bus to ensure synchronous data acquisition and transmission.

[0029] Based on the multi-source sensor data, environmental perception features are generated.

[0030] In a preferred embodiment of the present invention, the specific method for generating environmental perception features includes: extracting surface texture information corresponding to the visual mileage data and underwater topographic information corresponding to the sonar depth sounding data from the multi-source sensor data. The surface texture information includes the distribution of ground feature points and local geometric structure data, and the underwater topographic information includes depth measurement point cloud data and seabed surface morphology features.

[0031] By jointly analyzing the surface texture information and the underwater topography information, and combining feature matching and spatial registration techniques, a comprehensive environmental feature vector reflecting the current working environment is constructed.

[0032] It should be noted that after the above joint analysis and technical processing, the extracted quantitative environmental indicators are integrated into a structured comprehensive environmental feature vector. Preferably, this vector typically includes the following dimensions: surface texture indicators: density of surface feature points, clarity of geometric structure; underwater topography indicators: density of water depth point clouds, complexity of seabed morphology; spatial correlation indicators: spatial overlap rate of the two types of information, feature matching success rate; effectiveness indicators: effective proportion of surface texture information, effective proportion of underwater topography information.

[0033] It should be noted that the joint analysis verifies the effectiveness of the two types of information through comparison and complementarity. Furthermore, for surface texture information, it is necessary to determine whether the feature point density meets the standard and whether the geometric structure is clear; for underwater topography information, it is necessary to determine whether the depth point cloud density is sufficient and whether the seabed morphology is identifiable. The effectiveness of the two types of information is then used to infer environmental attributes: if the surface texture is effective and the underwater topography is ineffective, it is determined to be land; if the surface texture is ineffective and the underwater topography is effective, it is determined to be water; if both types of information are partially effective, it is determined to be a transitional area.

[0034] Preferably, taking an amphibious robot's operation scenario on a mudflat as an example, a comprehensive environmental feature vector is constructed: first, the surface feature points from the visual odometry and the sonar depth point cloud are unified to the robot's body coordinate system through spatial registration, and then the two types of information are jointly analyzed. The vector dimension contains 6 quantitative indicators: surface feature point density 0.35, surface geometric structure clarity 0.42, depth point cloud density 0.58, seabed morphology complexity 0.31, spatial overlap rate of the two types of information 0.62, and average effective data ratio 0.49, ultimately forming a comprehensive environmental feature vector of [0.35, 0.42, 0.58, 0.31, 0.62, 0.49], which accurately characterizes the environmental attributes of the transition area.

[0035] The environmental feature vectors are classified, and the confidence scores corresponding to each environmental type are calculated based on a pre-trained classifier. The environmental type with the highest confidence score is compared and selected, and the environmental type and its related parameters are marked as the environmental perception features.

[0036] It should be explained that the pre-trained classifier is a machine learning model pre-trained based on a large number of labeled samples. These labeled samples contain comprehensive environmental feature vectors under different environments and their corresponding real-world labels. Preferably, land sample vectors are labeled 1, water sample vectors are labeled 2, and transitional region vectors are labeled 3. This allows the classifier to learn the feature vector patterns of various environments.

[0037] The environmental types mentioned above include land, water, and transitional zones.

[0038] It should be noted that after inputting the feature vector to be classified constructed in the current scene into the pre-trained classifier, the classifier will calculate the degree of matching between the vector and the feature patterns of three environmental types: land, water, and transition areas, based on the learned rules, and output three values ​​between 0 and 1, which are the confidence scores for each environmental type. Preferably, after the input of the vector to be classified, the output confidence scores are: land: 0.21, water: 0.18, and transition area: 0.61, indicating that the vector is more consistent with the feature patterns of the transition area.

[0039] It should be explained that by comparing the confidence scores of the three environment types, the environment type with the highest score is selected as the final judgment result for the current working environment. The higher the confidence score, the stronger the fit between the vector to be classified and the characteristic patterns of that type of environment, and the higher the reliability of the judgment result.

[0040] Based on the environmental perception characteristics, a dynamic fusion strategy is determined to generate adaptive fusion parameters, and a dynamic error model that matches the current environment is selected.

[0041] It should be noted that the robot's specific environmental state is determined by analyzing the comprehensive environmental feature vector. When the robot is in a land environment, the module is configured with a sensor weight set dominated by the Global Navigation Satellite System (GNSS). When in a water environment, the module switches to a weight set dominated by the multibeam sonar depth sounding system. In transitional areas, a hybrid weight set of GNSS and multibeam sonar depth sounding systems is used. This dynamic adjustment mechanism optimizes the performance of subsequent modules by updating adaptive fusion parameters in real time, thereby improving the system's adaptability in different scenarios.

[0042] In a preferred embodiment of the present invention, the specific method for determining the dynamic fusion strategy to generate adaptive fusion parameters is as follows: analyze the environmental perception features to determine the state of the land, water or transition area where the robot is located.

[0043] It should be noted that the accuracy of the state judgment is ensured by combining the environment type and its related parameters for dual verification, as follows: 1. Initial label positioning: The label with the highest confidence score output by the classifier is used as the initial reference. 2. Parameter threshold verification: The system's preset environment type parameter threshold library is called, and the quantized parameters in the perceived features are compared with the thresholds. 3. If the label is land: Verify whether the density of surface feature points is greater than the corresponding threshold, and whether the effective proportion of underwater terrain information is less than the corresponding threshold, etc. If they meet the requirements, the land state is confirmed. If the label is water: Verify whether the density of water depth point clouds is greater than the corresponding threshold, and whether the effective proportion of surface texture is less than the corresponding threshold, etc. If they meet the requirements, the water state is confirmed. If the label is a transition area: Verify whether the effective proportions of surface and underwater information are both within the threshold range, and whether the spatial overlap rate is greater than the preset threshold, etc. If they meet the requirements, the transition area state is confirmed. 4. Final state locking: When the label and parameter threshold verification results are consistent, the current environment state is officially determined; if they are inconsistent, the analysis is repeated to avoid misjudgment due to occasional errors in the classifier.

[0044] A dominant sensor weight set is configured for each of the land, water, or transition zone states. The dominant sensor weight set is pre-configured based on the historical performance and theoretical reliability of each sensor in the specific environment.

[0045] It is important to note that the performance of multi-source sensors in navigation systems varies greatly in different environments: a certain type of sensor may be accurate and reliable on land, but completely ineffective in water. Therefore, it is necessary to pre-define a set of dominant sensor weights for each of the three environments: land, water, and transitional zones. By determining the weight values, it is clear which sensor data should be prioritized in each environment. Essentially, this allows the system to utilize the advantages of sensors in a way that is appropriate for the specific conditions.

[0046] It should be noted that the weight set is a combination of values, each summing to 1, assigned to one of the four types of sensors. A higher value indicates a larger proportion and stronger influence of that sensor in the fusion calculation. A dominant sensor weight set indicates that one or two types of sensors have significantly higher weights than others, becoming the core data source in the current environment. It should also be noted that the dominant sensor weight set is based on a quantitative analysis of both historical performance and theoretical reliability: through extensive field experiments, the data effectiveness and average positioning error of various sensors in different environments were statistically analyzed. Sensors with lower positioning errors and higher data effectiveness were assigned higher weights. Furthermore, the environmental adaptability of the sensors was analyzed based on their principles. Sonar relies on underwater sound wave propagation, and theoretically, its reliability in water is far greater than on land; vision relies on visible light imaging, and theoretically, its reliability on land is better than in turbid water.

[0047] The corresponding dominant sensor weight set is selected based on the current state and used as the adaptive fusion parameter.

[0048] In a preferred embodiment of the present invention, the specific method for generating adaptive fusion parameters further includes: evaluating the signal quality of the multi-source sensor data in real time and generating a data confidence index.

[0049] It should be noted that the preset dominant sensor weight set is a static rule based on historical performance and theoretical reliability. However, in actual operation, sensor performance may temporarily decline due to sudden interference, such as land vision being obscured by sandstorms, water sonar being interfered with by water currents, or satellite signals being blocked by buildings. If the preset weights are still used in this case, data from high-weight sensors that have failed in real time may be mistakenly used as the core basis, leading to fusion errors. Therefore, the confidence index generated by real-time evaluation of signal quality aims to dynamically correct the preset weights, allowing the adaptive fusion parameters to adapt to both the environment type and the real-time state of the sensors, achieving dual adaptation.

[0050] Based on the data confidence index, the dominant sensor weight set is dynamically fine-tuned to generate a fine-tuned weight set.

[0051] It should be noted that the specific method for dynamically fine-tuning the dominant sensor weight set is as follows: First, the comprehensive data confidence index is decomposed, and the real-time confidence scores of the inertial measurement unit, satellite system, visual odometry, and sonar depth sounding system are extracted. Using the pre-set dominant sensor weight set for the current environment as a benchmark, the confidence scores of each sensor are normalized. A correction intensity factor is set, and the difference between the normalized confidence value and the benchmark weight is multiplied by the correction intensity factor. This is then summed with the corresponding benchmark weight to obtain the fine-tuning coefficients. Finally, all coefficients are normalized a second time to generate a fine-tuned weight set that fits the real-time quality of the sensors, and the fine-tuning does not overturn the logic of the dominant sensors in the environment.

[0052] The fine-tuned weight set is used as the adaptive fusion parameter.

[0053] The data confidence index is compared with a preset threshold. If the data confidence index is less than the preset threshold, it is determined that there is a data quality problem. The specific data source affecting the data quality problem is further identified and relevant processing is carried out.

[0054] It should be noted that the threshold setting adopts a multi-dimensional comprehensive calibration method. First, high-precision operations require strict control over data reliability, so the initial threshold is set relatively high; in ordinary scenarios, it can be appropriately relaxed. Second, combining sensor characteristics and environmental patterns, the confidence distribution of sensors operating normally under different environments is statistically analyzed, and the lowest value of the majority of qualified samples is taken as the basic threshold. Third, historical fault data is introduced for calibration; past cases of positioning failure due to data quality are analyzed, and the critical confidence value before the failure is extracted, allowing for fine-tuning of the basic threshold. Finally, considering the system redundancy capability, the threshold is relaxed when there are sufficient redundant sensors and tightened when relying on a single key sensor. Ultimately, through multiple rounds of experimental verification, the optimal threshold that balances the false positive rate and the false negative rate is determined.

[0055] In a preferred embodiment of the present invention, the specific method for generating the data confidence index is as follows: calculating the positioning accuracy factor of the satellite positioning data.

[0056] Preferably, the positioning accuracy factor of satellite positioning data is calculated as follows: First, the spatial coordinates of at least four visible satellites are acquired through a Global Navigation Satellite System (GNSS) receiving antenna, and a satellite position matrix is ​​constructed. Simultaneously, the approximate position of the receiver is obtained, and the geometric distance vector between the satellites and the receiver is calculated. Next, based on the satellite positions and the approximate receiver position, a geometric coefficient matrix is ​​constructed, reflecting the spatial geometric configuration relationship between the satellites and the receiver. Finally, the inverse of the coefficient matrix is ​​solved through matrix operations, and the components corresponding to the position errors are extracted. The positioning accuracy factor is obtained by square root calculation; the smaller the value, the better the geometric configuration of the satellite positioning data.

[0057] The number of valid feature points in the visual odometry data is counted.

[0058] Preferably, the effective feature point count for visual odometry data is calculated as follows: First, two consecutive frames of images acquired by the visual odometry system are preprocessed by grayscale conversion and Gaussian filtering to eliminate noise. Initial feature points are extracted from the two frames using ORB or SIFT algorithms, retaining candidate points with response values ​​higher than a preset threshold. Next, the RANSAC algorithm is used to match and verify the candidate points, calculating the fundamental matrix or essential matrix to eliminate mismatched points. Then, combined with image boundary constraints, feature points exceeding the effective image area are filtered out. Finally, the number of remaining feature points is counted, which represents the effective feature point count for the visual odometry data.

[0059] Analyze the echo signal-to-noise ratio of the sonar depth sounding data.

[0060] Preferably, a method for calculating the echo signal-to-noise ratio (SNR) of sonar bathymetry data is as follows: First, the target echo region and background noise are separated from the raw echo signal acquired by the sonar bathymetry system. The power of the signals in the two regions is calculated separately: the signal power of the target echo region signal is obtained by averaging the squares, and the noise power of the background noise region signal is obtained by averaging the squares. Finally, the echo SNR of the sonar bathymetry data is calculated based on the SNR calculation formula. The higher this value, the less noise interference the seabed topography data is subjected to, and the stronger the data reliability.

[0061] The positioning accuracy factor, the number of effective feature points, and the echo signal-to-noise ratio are normalized to generate the data confidence index.

[0062] It should be noted that, based on the current dominant sensor weight set, corresponding weights are assigned to the three types of normalized indicators. Finally, the data confidence index is obtained by weighted summation. The closer the value is to 1, the better the overall quality of the multi-source sensor data.

[0063] By combining the multi-source sensor data with the adaptive fusion parameters, a weighted fusion calculation is performed to output a preliminary pose result.

[0064] In a preferred embodiment of the present invention, the specific method for outputting the preliminary pose result is as follows: a state space model is constructed, and the multi-source sensor data is used as the observation input.

[0065] The process noise covariance and observation noise covariance in the state-space model are adjusted in real time using the adaptive fusion parameters.

[0066] The state estimate is updated through filtering and iteration, and the preliminary pose result containing position, attitude, and velocity information is output.

[0067] The preliminary pose result is input into the selected dynamic error model for calculation, an error compensation value is generated, the error compensation value is applied to the preliminary pose result, and the final pose data is output.

[0068] In a preferred embodiment of the present invention, the specific method for generating error compensation values ​​is as follows: establishing a model library that includes ground dynamics error models and fluid dynamics error models.

[0069] It should be noted that, taking the construction of a ground dynamics error model as an example: First, the core objective of the model is clearly defined, namely, to analyze the pose errors caused by ground action and mechanical characteristics in the land environment, such as acceleration perception deviations caused by frictional resistance and motion lag caused by mechanical clearance. Second, key input parameters are selected, including the ground friction coefficient, slope angle, tire adhesion coefficient, and robot joint parameters, and motion data and error samples under different parameter combinations are collected through field tests. Then, based on dynamic principles such as Newton's second law and rigid body rotation equations, a mathematical mapping relationship between input parameters and error quantities is established, quantifying the effects of frictional resistance, terrain impact, etc., into error expressions in the dimensions of position, attitude, and velocity. Finally, the model coefficients are calibrated using the collected sample data, and the model prediction error is reduced through iterative optimization, forming a ground dynamics error model that can accurately output error compensation values.

[0070] Based on the environmental perception characteristics, the corresponding dynamic error model is called from the model library.

[0071] The motion information in the preliminary pose result is used as input, and the error compensation value is calculated through the dynamic error model.

[0072] In a preferred embodiment of the present invention, after outputting the preliminary pose result, it is necessary to recalibrate the dynamic error model. The specific method is as follows: store the final pose data into the historical trajectory sequence.

[0073] Based on the motion trend of the historical trajectory sequence, the theoretical pose at the current moment is predicted and compared with the final pose data to generate the trajectory deviation.

[0074] It's important to note that a robot's motion is continuous and predictable; its current pose is necessarily strongly correlated with the motion trend of the previous stage. Historical trajectory sequences record this motion trend. Predicting the theoretical pose using this sequence essentially involves constructing an expected value based on historical patterns and comparing it with the final pose data. The difference reflects the degree of deviation between the current pose and the historical motion trend. The smaller the deviation, the more reliable the current pose; a large deviation suggests potential problems such as sensor failure or insufficient error compensation.

[0075] When the trajectory deviation exceeds a pre-calibrated threshold for a continuous period of time, the adaptive fusion parameters and the dynamic error model are recalibrated.

[0076] It should be noted that exceeding the threshold for a continuous period indicates a systemic problem, suggesting that the adaptive fusion parameters are no longer suitable for the long-term performance degradation of the current sensor, or that the dynamic error model is no longer able to adapt to continuous environmental changes. In this case, the possibility of occasional interference is ruled out, and recalibration is needed to address the fundamental problem of mismatch between parameters / model and actual conditions.

[0077] In a preferred embodiment of the present invention, when the environmental perception feature is a transition region, the specific analysis method for the corresponding error compensation value is as follows: calling the ground dynamics error model and the fluid dynamics error model.

[0078] Based on the water depth information in the sonar bathymetry data, the contribution weights of the ground dynamics error model and the fluid dynamics error model to the pose error compensation are calculated.

[0079] The errors output by the two models are weighted and summed to generate an error compensation value for the transition region.

[0080] It is important to reveal that the robot's motion environment in the transition zone exhibits hybrid and dynamically changing characteristics: it is affected by both ground friction and terrain impact, as well as water flow resistance and fluid impact, and the weights of these two environmental influences dynamically change with the robot's position. Using only a ground dynamics error model would overlook errors such as attitude deviation and position drift caused by water flow; using only a fluid dynamics error model would ignore the frictional resistance error caused by ground support. Therefore, a weighted summation is needed to fuse the error outputs of the two models, allowing the compensation value to dynamically adjust according to the environmental proportions.

[0081] Please see Figure 2 As shown, the second aspect of the present invention provides a pose calculation system for integrated land and sea navigation, comprising a data acquisition module, an environmental feature generation module, a dynamic strategy decision module, a weighted fusion calculation module, and an error compensation module, wherein the data acquisition module is connected to the environmental feature generation module, the environmental feature generation module is connected to the dynamic strategy decision module, the dynamic strategy decision module is connected to the weighted fusion calculation module, and the weighted fusion calculation module is connected to the error compensation module.

[0082] The data acquisition module is used to acquire multi-source sensor data collected by the inertial measurement unit, global navigation satellite system, visual odometry and multibeam sonar depth sounding system.

[0083] An environmental feature generation module is used to generate environmental perception features based on the multi-source sensor data. These environmental perception features reflect the state of the land, water, or transitional area where the robot is located.

[0084] The dynamic strategy decision module is used to determine a dynamic fusion strategy based on the environmental perception characteristics to generate adaptive fusion parameters and select a dynamic error model that matches the current environment.

[0085] The weighted fusion calculation module is used to combine the multi-source sensor data with the adaptive fusion parameters to perform weighted fusion calculation and output preliminary pose results.

[0086] The error compensation module is used to input the preliminary pose result into the selected dynamic error model for calculation to generate an error compensation value, apply the error compensation value to the preliminary pose result, and output the final pose data.

[0087] Preferably, as the robot moves from land into shallow water, the signal from the Global Navigation Satellite System gradually weakens, while the importance of the multibeam sonar bathymetry data gradually increases. The dynamic strategy decision-making module adjusts the sensor weight set in real time based on the comprehensive environmental feature vector to ensure stable operation of the system in different environments. The weighted fusion calculation module performs weighted fusion of multi-source data with the updated weight set, outputting preliminary pose results. The error compensation module calls the hybrid dynamics model to calculate error compensation values ​​and corrects the preliminary pose results, ultimately outputting high-precision pose data. This process not only solves the pose drift problem that occurs during complex environment switching in traditional methods, but also significantly improves the robustness and adaptability of the system.

[0088] The above content is merely an example and illustration of the concept of the present invention. Those skilled in the art can make various modifications or additions to the specific embodiments described, or use similar methods to replace them, as long as they do not deviate from the concept of the invention or exceed the scope defined by the present invention, and all such modifications and additions should fall within the protection scope of the present invention.

Claims

1. A pose calculation method for integrated land and sea navigation, characterized in that: include: Acquire multi-source sensor data collected by inertial measurement unit, global navigation satellite system, visual odometry and multibeam sonar depth sounding system; Based on the multi-source sensor data, environmental perception features are generated; Based on the environmental perception characteristics, a dynamic fusion strategy is determined to generate adaptive fusion parameters, and a dynamic error model that matches the current environment is selected. By combining the multi-source sensor data with the adaptive fusion parameters, a weighted fusion calculation is performed to output a preliminary pose result; The preliminary pose result is input into the selected dynamic error model for calculation, an error compensation value is generated, the error compensation value is applied to the preliminary pose result, and the final pose data is output.

2. The pose calculation method for integrated land and sea navigation according to claim 1, characterized in that: The specific methods for generating environmental awareness features include: The surface texture information corresponding to the visual mileage data and the underwater topography information corresponding to the sonar bathymetry data are extracted from the multi-source sensor data. The surface texture information includes the distribution of ground feature points and local geometric structure data, and the underwater topography information includes water depth measurement point cloud data and seabed surface morphology features. By jointly analyzing the surface texture information and the underwater topography information, and combining feature matching and spatial registration techniques, a comprehensive environmental feature vector reflecting the current working environment is constructed. The comprehensive environmental feature vector is classified, and the confidence level corresponding to each environmental type is calculated based on the pre-trained classifier. The environmental type corresponding to the highest confidence level is compared and selected, and the environmental type and its related parameters are marked as the environmental perception feature. The environmental types mentioned above include land, water, and transitional zones.

3. The pose calculation method for integrated land and sea navigation according to claim 2, characterized in that: The specific method for determining the dynamic fusion strategy to generate adaptive fusion parameters is as follows: Analyze the environmental perception features to determine the state of the land, water, or transitional area where the robot is located; A dominant sensor weight set is configured for the land, water or transition zone state, and the dominant sensor weight set is pre-configured based on the historical performance and theoretical reliability of each sensor in the environment. The corresponding dominant sensor weight set is selected based on the current state and used as the adaptive fusion parameter.

4. The pose calculation method for integrated land and sea navigation according to claim 1, characterized in that: The specific method for outputting the preliminary pose result is as follows: A state-space model is constructed, and the multi-source sensor data is used as the observation input; The process noise covariance and observation noise covariance in the state-space model are adjusted in real time using the adaptive fusion parameters. The state estimate is updated through filtering and iteration, and the preliminary pose result containing position, attitude, and velocity information is output.

5. The pose calculation method for integrated land and sea navigation according to claim 2, characterized in that: The specific method for generating the error compensation value is as follows: Establish a model library that includes ground dynamics error models and fluid dynamics error models; Based on the environmental perception characteristics, the corresponding dynamic error model is called from the model library; The motion information in the preliminary pose result is used as input, and the error compensation value is calculated through the dynamic error model.

6. The pose calculation method for integrated land and sea navigation according to claim 3, characterized in that: The specific methods for generating adaptive fusion parameters also include: Real-time evaluation of the signal quality of the multi-source sensor data to generate a data confidence index; Based on the data confidence index, the dominant sensor weight set is dynamically fine-tuned to generate a fine-tuned weight set; The fine-tuned weight set is used as the adaptive fusion parameter; The data confidence index is compared with a preset threshold. If the data confidence index is less than the preset threshold, it is determined that there is a data quality problem. The specific data source affecting the data quality problem is further identified and relevant processing is carried out.

7. The pose calculation method for integrated land and sea navigation according to claim 6, characterized in that: The specific method for generating the data confidence index is as follows: Calculate the positioning accuracy factor of satellite positioning data; Count the number of valid feature points in the visual odometry data; Analyze the echo signal-to-noise ratio of sonar depth sounding data; The positioning accuracy factor, the number of effective feature points, and the echo signal-to-noise ratio are normalized to generate the data confidence index.

8. The pose calculation method for integrated land and sea navigation according to claim 5, characterized in that: After outputting the preliminary pose results, the dynamic error model needs to be recalibrated, and the specific method is as follows: The final pose data is stored in a historical trajectory sequence; Based on the motion trend of the historical trajectory sequence, the theoretical pose at the current moment is predicted and compared with the final pose data to generate the trajectory deviation. When the trajectory deviation exceeds a pre-calibrated threshold for a continuous period of time, the adaptive fusion parameters and the dynamic error model are recalibrated.

9. The pose calculation method for integrated land and sea navigation according to claim 5, characterized in that: When the environmental perception feature is a transition region, the specific analysis method for the corresponding error compensation value is as follows: Invoke the ground dynamics error model and the fluid dynamics error model; Based on the water depth information in the sonar bathymetry data, calculate the contribution weights of the ground dynamics error model and the hydrodynamics error model to the pose error compensation; The errors output by the two models are weighted and summed to generate an error compensation value for the transition region.

10. A pose calculation system for integrated land and sea navigation, characterized in that: include: The data acquisition module is used to acquire multi-source sensor data collected by the inertial measurement unit, global navigation satellite system, visual odometry and multibeam sonar depth sounding system; An environmental feature generation module is used to generate environmental perception features based on the multi-source sensor data. The environmental perception features reflect the state of the land, water or transitional area where the robot is located. The dynamic strategy decision module is used to determine the dynamic fusion strategy based on the environmental perception characteristics to generate adaptive fusion parameters and select a dynamic error model that matches the current environment. The weighted fusion calculation module is used to combine the multi-source sensor data with the adaptive fusion parameters to perform weighted fusion calculation and output preliminary pose results; The error compensation module is used to input the preliminary pose result into the selected dynamic error model for calculation to generate an error compensation value, apply the error compensation value to the preliminary pose result, and output the final pose data.

Images