Factor graph adaptive weight tuning method and system for underwater robot navigation and positioning

Abstract

The application discloses an underwater robot navigation positioning method and system based on factor graph adaptive weight adjustment, comprising the following steps: collecting multi-source measurement information in real time; performing sliding window sampling on the multi-source information and constructing multi-source observation factors; constructing a residual observation model based on the multi-source observation factors and introducing corresponding soft switch weights to construct a weighted residual model; obtaining an optimal weight vector by minimizing the weighted residual model; updating the state based on the optimal weight vector to obtain a navigation result; and the application establishes unified constraints based on multi-source observation factors to realize joint correction of cumulative drift, local abnormal observation and water flow disturbance, thereby improving the continuous pose estimation and water flow field perception ability of the robot in a complex cave environment, and the application realizes adaptive adjustment of different factors by introducing soft switch weights, which is beneficial to accurate regulation and control for different observation conditions and improves the robustness to sudden interference.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and more specifically to a factor graph adaptive weighting underwater robot navigation and positioning method and system. Background Technology

[0002] Underwater karst cave environments present challenges such as lack of GNSS signals, weak illumination, high turbidity, complex wall reflections, strong water flow disturbances, and susceptibility to DVL lock loss. With the development of multi-source information fusion theory, integrated navigation methods are gradually being introduced into underwater robot systems to compensate for the shortcomings of single sensors in terms of dynamic performance and robustness. For example, inertial / acoustic integrated navigation and inertial / terrain-assisted navigation methods have improved autonomy to some extent. However, due to echo interference and complex flow fields in karst cave environments, these methods still struggle to meet the requirements for convergence, real-time performance, and accuracy in groundwater exploration and pollution control.

[0003] Optical flow (OF) sensors can provide relative displacement estimation under certain lighting conditions, but their performance degrades significantly under low light, turbidity, or obstruction conditions. While direct velocity (DVL) sensors can provide absolute velocity measurements, measurement errors increase significantly under conditions of strong water flow disturbance, narrow pipe spaces, or wall reflection interference, affecting overall navigation accuracy. Injector noise (IMU) sensors can output high-frequency motion information, but their cumulative drift is difficult to suppress. Therefore, how to efficiently fuse information from complementary sensors such as IMU, optical flow, and DVL has become a core research challenge.

[0004] Most existing solutions employ extended Kalman filtering (EKF) or unscented Kalman filtering (UKF) to achieve multi-sensor fusion. However, EKF has a large estimation error in strongly nonlinear systems, and UKF is prone to instability in high-dimensional states, especially in environments such as karst caves with strong flow field interference and high dynamic characteristics. Traditional filtering frameworks are difficult to guarantee the long-term stability and robustness of navigation.

[0005] Therefore, how to achieve accurate navigation and positioning in environments with strong interference and high dynamic characteristics is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0006] In view of the above problems, the present invention is proposed to provide a robot navigation and positioning method and system that overcomes or at least partially solves the above problems.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] A factor graph adaptive weighting method for underwater robot navigation and localization includes the following steps: Real-time acquisition of multi-source measurement information; The information from both sides of the multi-source sources is sampled using a sliding window, and a multi-source observation factor is constructed. Based on the aforementioned multi-source observation factors, a residual observation model is constructed, and corresponding soft-switching weights are introduced to construct a weighted residual model. The optimal weight vector is obtained by minimizing the weighted residual model. The navigation result is obtained by updating the state based on the optimal weight vector.

[0009] Preferably, when the sliding window is updated, prior information is generated by marginalizing the historical window state and constructed as prior factors; The multi-source observation factors include the prior factors.

[0010] Preferably, the multi-source observation factors include one or more of the following: IMU pre-integration factor, optical flow reprojection factor, DVL velocity factor, error correction factor, and newly added hydrodynamic disturbance factor.

[0011] Preferably, the construction of the weighted residual model includes: Based on the residual observation model, the residual observation term is constructed by weighted summation using the soft-switching weights. An observation confidence term is constructed based on the soft-switching weights and the preset residual confidence factor; The weighted residual model is obtained by combining the residual observation term and the observation confidence term.

[0012] Preferably, the observation trust term is:

[0013] in, The number of the observed factor, For the first The soft-switching weights of each observation factor This is the residual confidence factor, used to constrain the soft-switching weights to remain near the reliable observation state. When the... When the residuals of each observed factor are small and the observation health is high, Approaching 1 allows the observed factor to participate normally in optimization; when the first When the residuals of an observed factor increase or the observation health decreases, Decrease it to reduce the impact of the observation factor on the state update.

[0014] Preferably, the construction of residual observation terms includes: The original residuals output by the residual observation model are whitened to obtain the whitened residuals. The whitening residuals corresponding to each factor are weighted and summed to obtain the residual observation term.

[0015] Preferably, a health weight is introduced into the residual observation term to weight the residuals of each factor.

[0016] A robot navigation and positioning system includes a sensor measurement module, a factor modeling and adaptive weighting module, and an optimization output module; The sensor measurement module is used to collect multi-source measurement information; The factor modeling and adaptive weighting module is used to perform sliding window sampling on the information from both sides of the multi-source sources and construct multi-source observation factors; it is used to construct a residual observation model based on the multi-source observation factors and introduce corresponding soft-switching weights to construct a weighted residual model; it is used to obtain the optimal weight vector by minimizing the weighted residual model. The optimized output module is used to update the state based on the optimal weight vector to obtain the navigation result.

[0017] A navigation robot employing the aforementioned navigation and positioning method includes a base and an upper shell and a lower shell connected to both sides of the base; A DVL module and a water pump are installed at the bottom of the lower shell; The first internal space formed by the lower shell and the base is equipped with a drive module and an inertial navigation cabin; the drive module is used to drive the water pump; the inertial navigation cabin is used to install the IMU component. The power module and control module are installed in the second internal space formed by the upper shell and the base station.

[0018] As can be seen from the above technical solution, compared with the prior art, the present invention discloses a factor graph adaptive weighting underwater robot navigation and positioning method and system, which has the following outstanding effects: 1. Based on multi-source observation factors, factor graph modeling was carried out, and a weighted residual model was constructed. A unified constraint relationship was established between the state node and the observation factors. By jointly constraining the state node through the above multi-source factors, the joint correction of cumulative drift, local anomaly observation and water flow disturbance was realized, thereby improving the robot's continuous pose estimation and water flow field perception capabilities in complex karst cave environments.

[0019] 2. By introducing soft-switching weights to achieve adaptive residual weighting, adaptive control of different observation factors can be realized, resulting in better optimization effects.

[0020] Firstly, adaptive control based on observation health is employed. When the observation health of a certain observation factor is high, its weight matrix is ​​increased, allowing it to play a major role in state updates. When the observation health of a certain observation factor decreases or the residual abnormally increases, its weight matrix is ​​decreased, and the influence of the observation factor on the optimization results is reduced by dynamically switching constraint factors, thereby improving the stability and global consistency of navigation state updates.

[0021] Secondly, adaptive control based on observation residuals is employed. A dynamic switching constraint (Memory-SC) strategy is introduced. By calculating the residual magnitude, observation health, and soft-switching weight of each observation factor at the previous optimization time, the soft-switching weight of the current observation factor is continuously adjusted. The soft-switching weight at the previous optimization time characterizes the reliability of the observation factor in historical state updates. When the residual of an observation factor is small and the observation health is high, its soft-switching weight is approached 1, allowing the observation factor to participate normally in state updates. When local observations are abnormal, noise increases, or the observation health decreases, its soft-switching weight is reduced, thereby reducing the impact of the observation factor on the optimization results and ensuring the robustness of the overall graph optimization to sudden disturbances.

[0022] 3. A sliding window edge-out and historical information compression strategy is introduced. By reducing the number of nodes in the whitened IMU pre-integration factor, optical flow constraint factor and DVL velocity factor, the information of edge nodes is passed to the next optimization round as a priori factors, thereby effectively reducing the amount of computation while maintaining global consistency. Attached Figure Description

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

[0024] Figure 1 The factor graph combination navigation process provided in this embodiment of the invention; Figure 2 The underwater working state of the cave exploration underwater robot provided in the embodiments of the present invention; Figure 3 This is a front view of the overall structure of the cave exploration robot provided in an embodiment of the present invention; Figure 4 The left view of the overall structure of the cave exploration robot provided in this embodiment of the invention; Figure 5 This is a bottom structure view of the cave exploration robot provided in an embodiment of the present invention; Figure 6 This is a front view (internal view) of the internal structure of the cave exploration robot after removing the outer shell, as provided in an embodiment of the present invention. Figure 7 This is a side view (inner side view) of the internal structure of the cave exploration robot after removing the outer shell, as provided in an embodiment of the present invention. Detailed Implementation

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

[0026] Example 1 like Figure 1 and Figure 2 This invention discloses a factor graph adaptive weighting underwater robot navigation and localization method, comprising the following steps: S1: Real-time acquisition of multi-source measurement information; S2: Perform sliding window sampling on both sides of the multi-source information and construct multi-source observation factors by calculating residuals; S3: Based on the multi-source observation factors, corresponding soft-switching weights are introduced to construct a weighted residual model; S4: Obtain the optimal weight vector by minimizing the weighted residual model; S5: Update the state based on the optimal weight vector to obtain the navigation result.

[0027] In this embodiment, a factor graph framework is formed based on multi-source observation factors, and a weighted residual model is constructed as the factor graph solution problem. The factor graph is used to characterize the constraint relationship between robot state nodes and multi-source observation factors. The state nodes include the robot's pose, velocity, inertial device zero bias, and water flow disturbance state at different times. The observation factors include IMU pre-integration factor, optical flow reprojection factor, DVL velocity factor, water flow disturbance factor, and prior factors. The multi-source measurements are uniformly transformed into residual constraints through the factor graph, and a nonlinear least squares objective function is constructed to achieve joint optimization estimation of the robot's navigation state.

[0028] The following is a detailed explanation of each step in this embodiment: For S1, in one embodiment, the acquired multi-source measurement information includes optical flow data, IMU inertial navigation data, and DVL velocity data.

[0029] In addition, other data may be included, such as surface GPS or other external auxiliary data.

[0030] For S2, in one embodiment, corresponding observation factors are constructed based on the information collected in S1, for example: IMU pre-integration factor: High-frequency angular velocity and linear acceleration data provided by the inertial measurement unit are used to construct motion priors between adjacent keyframes and compensate for short-term pose changes.

[0031] Optical flow factor: The motion of feature points is captured by an optical flow sensor to correct inertial drift. In low-texture or low-light scenarios, an adaptive feature extraction and filtering strategy is adopted to ensure observation stability.

[0032] DVL speed factor: The absolute speed constraint provided by the Doppler acoustic log is used to reduce accumulated errors and improve the accuracy of long-term operation.

[0033] External observation factors: Where permissible, surface GPS or other external auxiliary data can be introduced as additional constraints in the factor plot.

[0034] Furthermore, it also includes prior factors. When the sliding window is updated, prior information is generated by marginalizing the historical window state and constructed as prior factors.

[0035] In this embodiment, different observation factors impose different constraints on the robot's state nodes. The IMU pre-integration factor provides short-term motion constraints between adjacent state nodes; the optical flow constraint factor constrains the robot's relative motion within the local visible area; the DVL velocity factor provides velocity observation constraints; the error factor characterizes the deviation between various observations and the current predicted state; the dynamic switch constraint factor adjusts the participation level of the corresponding observation factor according to the magnitude of the observation residual; and the water flow disturbance factor describes the influence of water flow velocity on the robot's motion state in the underwater cave environment. By jointly constraining the state nodes through the above factors, joint correction of cumulative drift, local anomaly observations, and water flow disturbances is achieved, thereby improving the robot's continuous pose estimation and water flow field perception capabilities in complex cave environments.

[0036] For S3, the residual observation models for each factor are as follows: 1. Prior factors The prior factor (Prior) comes from the Gaussian information of the previous round of marginalization. This generates prior residuals. Their function is to compress the information of historical state nodes removed from the previous window and their associated observation constraints, and then pass this information to the current optimization window as prior constraints. It's important to note that these prior factors do not directly correspond to any new sensor observation, but rather indirectly contain constraint information from the previous window's IMU pre-integration factor, optical flow constraint factor, DVL velocity factor, and other observation factors on the marginalized states. This maintains the continuity of historical information and the consistency of state estimation during the sliding window optimization process.

[0037]

[0038]

[0039] in, For the current optimized state vector, It is the mean of the prior states passed down from the previous round of marginalization. The prior information matrix (obtained by marginalization) is the matrix of prior information. It is a unit selection matrix.

[0040] 2. IMU pre-integration factor Bias linearization pre-integration is performed on the IMU data at adjacent sampling points in the sampling interval to obtain the pre-integrated rotation increment. Pre-integral velocity increment and pre-integral displacement increment and first-order sensitivity to bias. (This bias term includes gyroscope zero bias and accelerometer zero bias) and covariance .

[0041] The residual is defined as: (1) Attitude residuals:

[0042] in:

[0043] (2) Velocity residual:

[0044] (3) Positional residuals:

[0045] The total residual is The weighted form is . For pre-integration of the first-order sensitivity Jacobian of the gyroscope / accelerator with zero bias, To achieve zero bias in the gyroscope, To achieve zero bias in the accelerometer, The first-order perturbation (small increment) of the zero-bias state at the current linearization point. For the logarithmic mapping from Lie groups to Lie algebras (minimum attitude residual). IMU sampling time interval between adjacent pre-integration segments, Representation of gravitational acceleration in navigation coordinate system n This is the covariance of the total residuals from the IMU pre-integration.

[0046] 3. DVL velocity factor (including installation angle / mismatch angle): Assume that the DVL outputs the relative velocity of the water body in its own coordinate system d. By installing the angle correction matrix and the rotation matrix for DVL coordinate system calibration error The observed velocity under system b can be obtained:

[0047] The actual relative velocity of the water body in the navigation coordinate system n is: Its projection in system b is:

[0048] Therefore, the DVL residual is:

[0049] in: It is the relative water velocity measured by DVL in its own coordinate system d. The DVL residual reflects the difference between the DVL measurement speed and the prediction speed.

[0050] covariance The beamforming condition and solution noise are given. For the Jacobian of each variable (a first-order approximation is given), linearization is achieved by right-multiplying by a small-angle perturbation. Attitude error... Take a small spinor in the b-system, such that

[0051] in, For antisymmetric matrix operators, satisfying , Small perturbations to attitude error are defined in the load system b.

[0052] The Jacobian of velocity versus water flow is: Jacobi, who was a mismatched character in DVL:

[0053] in, For the installation angle (or parameters of the installation matrix); The derivative of the rotation matrix with respect to the installation angle is used to describe the sensitivity under small-angle perturbations.

[0054] 4. Optical flow / reprojection factor (inverse depth): Select anchor frame a Inverse depth parameterization. i The normalized coordinates of each feature point in the anchor frame (The third dimension is 1), its inverse depth is The three-dimensional coordinates in the navigation system are then:

[0055] in For anchor frames a Position in the navigation system For anchor frames a The attitude (rotation matrix from the anchor frame coordinate system to the navigation system). For the first i Inverse depth parameters of each feature point; The normalized coordinates of the iiith feature point in the anchor frame (the third dimension is 1). Projected onto the camera in the kth frame (with known extrinsic parameters relative to the machine body). In the camera coordinate system:

[0056] in For a moment k Rotation matrix from the carrier system to the navigation system. For rotation of the known extrinsic parameters from the carrier system to the camera system, The translational extrinsic parameters from the carrier system to the camera system; This refers to the position of the feature point relative to the vehicle in the navigation system.

[0057] The predicted pixel coordinates are:

[0058] Among them, pinhole The optical flow / reprojection residual is:

[0059] This factor Jacobi, which has standard visual SLAM, These are the actual observed pixel coordinates. These are the pixel coordinates predicted based on the current state. This is the reprojection error (observation value – predicted value). The observation noise covariance matrix is ​​used for weighted residuals.

[0060] 5. Optional GNSS location factor; GNSS data can be obtained if the route encounters exposed water surfaces or ventilation wells during travel.

[0061] Its weighted form is:

[0062] in, In the state variables, the estimated position of the vehicle in navigation frame n is... This is the GNSS observation location (already converted to the navigation system). For positional residuals, It is the covariance matrix of GNSS measurement noise.

[0063] 6. Priors of random walk in water flow: where The local water flow velocity (at time step k) in the navigation system is represented as a slowly varying state model. If the piecewise constant water flow is estimated... Using a first-order Markov model:

[0064] For S3, this embodiment introduces a switch variable for each observed factor to achieve switchable constraint weighting, i.e., SC weighting (Switchable Constraints). The switch variable is the soft-switching weight. When the observations are reliable, This factor is almost entirely involved in the optimization; when the observation is an outlier / unstable point, The factor is "soft-closed" and has minimal impact on the solution. The difference from traditional IRLS is that the weights are not "functions" calculated in each round, but rather are learnable / memorable state variables with prior knowledge, which can retain "historical memory" in the sliding window (persistent anomalies will be suppressed for a long time).

[0065] To further implement the above technical solution, the specific steps for constructing the weighted residual model in this embodiment are as follows: Let the whitening residual of any factor j be:

[0066] Subsequently, the switching factor with SC To jointly construct a weighted optimization model:

[0067] in, For the first j The health weight of each observation factor is calculated by comprehensively considering indicators such as optical flow texture evaluation, DVL echo stability, or water flow disturbance intensity. The worse the observation quality, the lower the health weight. The smaller. Control the intensity of the "belief factor should be activated" statement. The larger the value, unless the residual is very large, otherwise... Inclined towards 1, Let j be the noise covariance matrix of the observed factors. These are the residuals after whitening, used for unified weighted optimization. Finding the first-order optimal solution yields a closed-form solution that takes into account observed health conditions.

[0068] As the optimal switch variable (closed-form solution) for factor j, it can be seen that when the quality of a certain observed factor is poor, its health weight... When significantly reduced, the corresponding This will approach 0, achieving a "soft shutdown" of the observation factor; while for observations with stable quality and high signal-to-noise ratio, If it is close to 1, then The solution is close to that of the original SC model, ensuring that effective observations play a dominant role in the overall optimization.

[0069] In this embodiment, the soft-switching weights are adaptively adjusted by online-identified observation health. When the observation health of a certain observation factor is high, its weight matrix is ​​increased so that it plays a major role in state updates. When the observation health of a certain observation factor decreases or the residual abnormally increases, its weight matrix is ​​decreased, and the influence of the observation factor on the optimization results is reduced by dynamic switching constraint factors, thereby improving the stability and global consistency of navigation state updates.

[0070] Furthermore, the Gauss-Newton optimization algorithm is used to solve the constructed weighted optimization model.

[0071] Linear modeling of the residuals based on the current state:

[0072] Then, the residuals are substituted into the optimization model for whitening and weighting to obtain the weighted method equation:

[0073] in, This indicates the number of the observed factor, not a spatial point; For the first The residual vector of each observed factor, For the first The residuals of each observation factor contribute to the current state increment. Jacobian matrix, For the first The residual covariance matrix of each observation factor This is the corresponding information matrix; The system information matrix is ​​obtained by summing up the observed factors. This is the gradient vector obtained by summing up the observed factors. For the first j Switching variables for each observation factor.

[0074] Finally, the state increment is calculated, and the state is updated.

[0075]

[0076] in, For the system information matrix, Let be the gradient vector. The pose is updated using a Lie group, while other quantities are updated directly using addition.

[0077] Specifically, define a minimal parameterized state for the k-th frame:

[0078] Among them, the For the state at a single moment, The rotation matrix from the vehicle coordinate system b to the navigation coordinate system n. It refers to the position of the carrier under the navigation system. It is the speed of the carrier under the navigation system. To achieve zero bias in the gyroscope, To achieve zero bias in the accelerometer, It is an estimate of the water flow velocity slices under the navigation system.

[0079] Update using right-multiplied perturbation (Lye algebra):

[0080]

[0081] in, δ This represents a small perturbation based on the current estimate, used for iterative correction in Gaussian-Newton or nonlinear optimization.

[0082] Example 2 Based on the same inventive concept, this invention also provides an IMU / OF / DVL cave exploration robot system using a global factor graph. The robot system is composed of the following components: Robotic systems such as Figures 3-7 As shown, the upper shell 1 has an arc-shaped sealing structure with an internal electromagnetic shielding layer to protect electronic equipment. The outer shell is made of pressure-resistant composite material to adapt to high water depth and strong disturbance environments.

[0083] The base 2 serves as the core load-bearing component of the entire machine, connecting the upper shell 1 and the lower shell 3, and acting as the mounting reference plane for all sensors and functional components. The base has standardized threaded interfaces arranged around its circumference for spatiotemporal alignment of sensors such as DVL, optical flow, and IMU.

[0084] The lower shell 3 adopts a fluid-optimized arc shape, with mounting windows on the lower surface for installing modules such as DVL4 and inertial measurement unit.

[0085] The DVL module 4 is installed at the center of the bottom of the lower shell 3, used to output relative velocity information, and together with the optical flow sensor, it forms a velocity and flow field sensing unit. The water pump 5 is powered by the drive module 10 and rigidly connected to the base 2 by the fixing component 11, enabling active attitude fine-tuning and stagnation control. The power supply compartment 6 has a sealed cabin structure with a built-in battery pack, arranged symmetrically to reduce center of gravity shift and inertial interference, providing a power source for the robot. The core computing compartment 7 has an embedded computing unit for executing factor graph optimization, visual processing, and navigation algorithms, serving as the computing center of the entire system. The camera / optical flow mounting position 8 is located in the front area of ​​the shell, housing a camera and optical flow sensor, providing visual observation information for optical flow factor and water flow direction inversion.

[0086] The main control compartment 9 is used for system management, power scheduling, sensor synchronization and communication management, and provides a unified time reference for each sensor factor in the factor map.

[0087] The inertial navigation cabin 12 is used to install the IMU component. Its position is close to the robot's overall center of gravity, which enables the IMU to provide more stable short-term priors in factor graph optimization.

[0088] After being deployed into the water, the robot automatically maintains a horizontal orientation based on the buoyancy of its shell and the fine-tuning effect of the water pump. It then utilizes the natural water flow to enter the cave, achieving a non-powered drifting mode. During this drifting motion, the robot observes the external environment using an IMU, optical flow sensor, DVL (Direct Visualization), and water flow sensor. By fusing multi-source data through a factor graph optimization framework, it achieves continuous and robust positioning and water flow perception.

[0089] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0090] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A factor graph adaptive weighting method for underwater robot navigation and localization, characterized in that, Includes the following steps: Real-time acquisition of multi-source measurement information; The multi-source measurement information is sampled using a sliding window, and multi-source observation factors are constructed by calculating the residuals; Based on the multi-source observation factors, corresponding soft-switching weights are introduced to construct a weighted residual model; The optimal weight vector is obtained by minimizing the weighted residual model. The navigation result is obtained by updating the state based on the optimal weight vector.

2. The underwater robot navigation and positioning method with adaptive weighting of factor graphs according to claim 1, characterized in that, During the update of the sliding window, prior information is generated by marginalizing the historical window state and constructed as prior factors. The multi-source observation factors include the prior factors.

3. A factor graph adaptive weighting underwater robot navigation and positioning method according to claim 1 or 2, characterized in that, The multi-source observation factors include one or more of the following: IMU pre-integration factor, optical flow reprojection factor, DVL velocity factor, error correction factor, and newly added hydrodynamic disturbance factor.

4. The underwater robot navigation and positioning method with adaptive weighting of factor graphs according to claim 1, characterized in that, The construction of the weighted residual model includes: Based on the residual observation model, the residual observation term is constructed by weighted summation using the soft-switching weights. An observation confidence term is constructed based on the soft-switching weights and the preset residual confidence factor; The weighted residual model is obtained by combining the residual observation term and the observation confidence term.

5. The underwater robot navigation and positioning method with adaptive weighting of factor graphs according to claim 4, characterized in that, The observation trust term is: in, The number of the observed factor, For the first The soft-switching weights of each observation factor This is the residual confidence factor, used to constrain the soft-switching weights to remain near the reliable observation state. When the... When the residuals of each observed factor are small and the observation health is high, Approaching 1 allows the observed factor to participate normally in optimization; when the first When the residuals of an observed factor increase or the observation health decreases, Decrease it to reduce the impact of the observation factor on the state update.

6. A factor graph adaptive weighting underwater robot navigation and positioning method according to claim 4 or 5, characterized in that, The construction of residual observation terms includes: The original residuals output by the residual observation model are whitened to obtain the whitened residuals. The whitening residuals corresponding to each factor are weighted and summed to obtain the residual observation term.

7. The underwater robot navigation and positioning method with adaptive weighting of factor graphs according to claim 6, characterized in that, A health weight is introduced into the residual observation term to weight the residuals of each factor.

8. A robot navigation and positioning system, characterized in that, It includes a sensor measurement module, a factor modeling and adaptive weighting module, and an optimization output module; The sensor measurement module is used to collect multi-source measurement information; The factor modeling and adaptive weighting module is used to perform sliding window sampling on the information from both sides of the multi-source sources and construct multi-source observation factors; it is used to construct a residual observation model based on the multi-source observation factors and introduce corresponding soft-switching weights to construct a weighted residual model; it is used to obtain the optimal weight vector by minimizing the weighted residual model. The optimized output module is used to update the state based on the optimal weight vector to obtain the navigation result.

9. A navigation robot, characterized in that, The navigation and positioning method described in any one of claims 1-7 is adopted, including a base and an upper shell and a lower shell connected to both sides of the base; A DVL module and a water pump are installed at the bottom of the lower shell; The first internal space formed by the lower shell and the base is equipped with a drive module and an inertial navigation cabin; the drive module is used to drive the water pump; the inertial navigation cabin is used to install the IMU component. The power module and control module are installed in the second internal space formed by the upper shell and the base station.

Images