An underwater robot navigation and positioning method and system

Abstract

The underwater robot navigation positioning method and system, the method comprises the following steps: S1: collecting angular velocity and acceleration signals through an inertial measurement unit; S2: calculating three-dimensional velocity observation values according to the beam radial velocity vector signal combined with the angular velocity signal, and extracting environment feature point cloud data according to the acoustic image signal; S3: performing multi-source data time synchronization to generate a spatiotemporally aligned fusion input signal; S4: constructing an adaptive factor graph optimization model, dynamically adjusting the inertial navigation calculation node based on real-time weight coefficients; inputting the environment feature point cloud data into a closed-loop detection module to generate a loop-back factor node, and adaptively correcting the node weight according to the feature matching degree; S5: solving the adaptive factor graph optimization model through a nonlinear optimization algorithm. The underwater robot navigation positioning method and system can solve the problem of insufficient multi-source heterogeneous sensor fusion positioning precision in the underwater GPS-free environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of autonomous navigation and positioning of underwater robots, and particularly relates to an underwater robot navigation and positioning method and system. BACKGROUND

[0002] Underwater robot operation faces the core challenge of GPS signal isolation, and the existing technology mainly relies on the combination scheme of acoustic positioning system, inertial navigation and Doppler speedometer. Acoustic positioning needs to preset a seabed beacon array, which has high deployment cost and limited coverage; pure inertial navigation causes the error to accumulate with time due to gyro drift, and the positioning accuracy decreases sharply. The traditional solution adopts a loose or tight coupling filtering algorithm to fuse multi-sensor data, but there are three defects: firstly, the fixed sensor weight cannot adapt to the dynamic environment, for example, the Doppler speedometer is distorted in strong water flow, and the magnetometer is disturbed near metal structures, resulting in a sharp decrease in the reliability of the fusion system; secondly, underwater acoustic images are affected by reverberation and multipath effects, and the feature extraction stability is poor, and the closed-loop detection mismatch causes the pose to jump; thirdly, the existing algorithm lacks a flexible response mechanism for sensor failure, and local faults can easily cause the entire system to crash.

[0003] In recent years, although some studies have introduced factor graph optimization to improve accuracy, the problem of static weight distribution has not been solved, and the positioning drift is caused by sensor performance degradation or environmental mutations in long-time tasks. In addition, the flat area of underwater terrain lacks distinctive features, and traditional visual SLAM fails, forcing the robot to frequently correct the position. The above bottlenecks seriously restrict the autonomy and safety of underwater robots in exploration, rescue and other tasks, and there is an urgent need for a navigation and positioning method that can dynamically evaluate sensor reliability, adaptively adjust constraint strength and intelligently process sparse environmental features. SUMMARY

[0004] In view of the above shortcomings of the prior art, the purpose of the present application is to provide an underwater robot navigation and positioning method and system for solving the problem of insufficient positioning accuracy of multi-source heterogeneous sensor fusion in underwater GPS-free environment. The present application generates a fusion input by spatiotemporal alignment of multi-source sensor signals, and constructs an adaptive factor graph optimization model: dynamically evaluates sensor confidence to generate weight coefficient to adjust the constraint strength of inertial navigation node, adaptively corrects the loop factor weight based on acoustic image feature matching degree, and finally solves the real-time pose through nonlinear optimization. Robust positioning under sensor reliability fluctuation and environmental feature change is realized, breaking through the fixed weight limitation of traditional fusion algorithm.

[0005] The present application provides an underwater robot navigation and positioning method, comprising:

[0006] S1: Collecting angular velocity and acceleration signals by an inertial measurement unit, collecting beam radial velocity vector signals by a Doppler velocity log, and collecting underwater acoustic image signals by a sonar imaging device;

[0007] S2: Solving three-dimensional velocity observation values according to the beam radial velocity vector signals combined with the angular velocity signals, and extracting environmental feature point cloud data according to the acoustic image signals;

[0008] S3: Time-synchronizing the angular velocity signals, the acceleration signals, the depth observation values, the heading observation values, and the three-dimensional velocity observation values, and generating a spatio-temporally aligned fusion input signal;

[0009] S4: Constructing an adaptive factor graph optimization model, inputting the fusion input signal into a predefined sensor confidence assessment module to generate real-time weight coefficients of each sensor, and dynamically adjusting an inertial navigation solving node based on the real-time weight coefficients; inputting the environmental feature point cloud data into a loop detection module to generate a loop factor node, and adaptively correcting the weight of the node according to the feature matching degree;

[0010] S5: Solving the adaptive factor graph optimization model by a nonlinear optimization algorithm, outputting real-time pose estimation signals of the underwater robot, and updating a global navigation trajectory according to the pose estimation signals.

[0011] In an embodiment of the present application, in step S1, the beam radial velocity vector signals collected by the Doppler velocity log contain original velocity measurement values of at least four non-coplanar beams, the sonar imaging device synchronously collects forward-looking sonar images and downward-looking sonar images to generate multi-view acoustic image signals, the inertial measurement unit continuously outputs angular velocity and acceleration signals at a sampling rate more than three times higher than those of other sensors, and at the same time, auxiliary positioning signals of external reference beacons are received through a hydroacoustic communication machine, and all original signals are marked with time stamps accurate to microseconds.

[0012] In an embodiment of the present application, when solving the three-dimensional velocity observation values in step S2, a beam vector projection compensation method is used to eliminate measurement deviations introduced by dynamic changes of carrier attitude angles, which specifically includes inputting the beam radial velocity vector signals and real-time angular velocity signals into a kinematic coupling model to solve three-dimensional velocity optimal estimates in a carrier coordinate system, and adaptively selecting a feature extraction algorithm based on a feature stability index of the acoustic image signals, using an angle point detection method based on a gradient operator to generate feature point cloud data when the environment is rich in texture, and switching to a feature extraction mode based on surface curvature analysis when the environment is sparse in texture.

[0013] In an embodiment of the present application, the multi-source data time synchronization in step S3 adopts a bidirectional timestamp interpolation alignment mechanism, a high-frequency inertial navigation solving thread is constructed for the angular velocity signal and the acceleration signal to generate a predicted trajectory, the depth observation value, the heading observation value and the three-dimensional velocity observation value are spatiotemporally matched with the predicted trajectory according to the nearest neighbor principle, the fixed time delay error between sensors is eliminated through a sliding window least square fitting, a spatiotemporally aligned fusion input signal is generated, and the synchronization confidence of each signal is recorded as an input parameter for subsequent weight calculation.

[0014] In an embodiment of the present application, the sensor confidence evaluation module in step S4 is implemented as follows: a zero bias stability evaluation function of the angular velocity signal, a vibration noise spectrum analysis function of the acceleration signal, a beam consistency test function of the three-dimensional velocity observation value, a magnetic interference detection function of the heading observation value and a pressure mutation judgment function of the depth observation value are respectively established, and a multi-dimensional confidence score model is used to output real-time weight coefficients of each sensor, which are negatively exponentially related to the sensor failure probability.

[0015] In an embodiment of the present application, the operation of dynamically adjusting the inertial navigation solving node includes: when the real-time weight coefficient indicates that the confidence of the angular velocity signal is higher than a threshold value, a gyro zero bias online estimation node is added in the factor graph and the constraint strength thereof is enhanced; and when the confidence of the acceleration signal is lower than the threshold value, the strong coupling constraint between the vertical motion of the carrier and the direction of gravity is automatically released, and a vertical position constraint node is constructed using the depth observation value.

[0016] In an embodiment of the present application, the working process of the loop closure detection module includes: multi-resolution hierarchical matching of the current environment feature point cloud data with the historical feature map, first-round fast retrieval based on curvature features to narrow the candidate range, second-round precision matching based on feature descriptors to calculate the similarity score, and finally generating a loop closure factor node according to the similarity score and the spatial distribution consistency, and the feature matching degree is calculated comprehensively according to the number of matching point pairs, the distribution uniformity and the geometric invariance error.

[0017] In an embodiment of the present application, the strategy for adaptively modifying the weight of the loop closure factor node is as follows: when the feature matching degree is in a high confidence interval, the loop closure factor node is constructed into a tightly coupled constraint with the inertial navigation solving node and is given a maximum weight; when the feature matching degree is in a critical interval, the loop closure factor node is retained but its weight is reduced to less than 50% of the reference value; and when the feature matching degree is lower than a failure threshold value, the loop closure factor node is completely disabled and a local map reconstruction process is triggered.

[0018] In one embodiment of the present application, the nonlinear optimization algorithm of step S5 is implemented using an incremental smoothing and mapping framework, which only processes newly added sensor observations and loop closure factors in each iteration, reduces the computational complexity through sparse matrix decomposition techniques, and monitors the eigenvalues of the covariance matrix of the pose estimation signal in real time during the solving process, and automatically switches to a robust kernel function to suppress the influence of outliers if the eigenvalues abnormally increase.

[0019] The present application also includes an underwater robot navigation positioning system, comprising:

[0020] The acquisition module acquires angular velocity and acceleration signals through an inertial measurement unit, acquires beam radial velocity vector signals through a Doppler speedometer, and acquires underwater acoustic image signals through a sonar imaging device.

[0021] The coordination module acquires angular velocity and acceleration signals through an inertial measurement unit, acquires beam radial velocity vector signals through a Doppler speedometer, and acquires underwater acoustic image signals through a sonar imaging device.

[0022] The comparison module performs multi-source data time synchronization on angular velocity signals, acceleration signals, depth observations, heading observations, and three-dimensional velocity observations to generate a spatiotemporally aligned fusion input signal.

[0023] The analysis module constructs an adaptive factor graph optimization model, inputs the fusion input signal into a predefined sensor confidence assessment module to generate real-time weight coefficients of each sensor, and dynamically adjusts the inertial navigation solving node based on the real-time weight coefficients; inputs the environmental feature point cloud data into a loop detection module to generate a loop closure factor node, and adaptively corrects the weight of the node according to the feature matching degree.

[0024] The present application provides an underwater robot navigation positioning method and system, which generates a fusion input through spatiotemporal alignment of multi-source sensor signals, constructs an adaptive factor graph optimization model: dynamically assesses the confidence of the sensor to generate weight coefficients to adjust the constraint strength of the inertial navigation node, adaptively corrects the weight of the loop closure factor based on the acoustic image feature matching degree, and finally solves the real-time pose through nonlinear optimization. Realize robust positioning under the fluctuation of sensor reliability and the change of environmental features, break through the limitation of fixed weight of traditional fusion algorithm. BRIEF DESCRIPTION OF DRAWINGS

[0025] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings needed to be used in the embodiment description. Obviously, the drawings in the following description are only some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0026] Figure 1 A method flow chart of a method for navigation and positioning of an underwater robot;

[0027] Figure 2 A schematic diagram showing steps three and four of a method for navigation and positioning of an underwater robot;

[0028] Figure 3 A system architecture diagram of a system for navigation and positioning of an underwater robot. DETAILED DESCRIPTION

[0029] The forgoing detailed description of the application, for illustrative purposes, describes embodiments of the application in terms of specific embodiments, procedures, techniques, and examples. In this regard, no individual element or

[0030] It is to be understood that the above-referenced arrangements are illustrative of the application described in the present disclosure. Numerous modifications and changes have been made to the embodiments without departing from the spirit and scope of the application as set forth in the following claims. It is to be understood that the following claims are to express the true scope of this application. The exemplary disclosed embodiments are to be considered in a descriptive sense only and not for purposes of limitation. The scope of the present application is defined by the appended claims rather than being circumscribed by the foregoing description, which includes alternative embodiments.

[0031] In the following description, numerous specific details are discussed so as to provide a thorough understanding of embodiments of the application. However, various

[0032] Reference will now be made to the drawings, wherein Figures 1-3The underwater robot navigation positioning method and system are shown, and the underwater robot navigation positioning method comprises the following steps: S1, collecting angular velocity and acceleration signals through an inertial measurement unit, collecting beam radial velocity vector signals through a Doppler speedometer, and collecting underwater acoustic image signals through a sonar imaging device; S2, calculating three-dimensional velocity observation values according to the beam radial velocity vector signals combined with the angular velocity signals, and extracting environmental feature point cloud data according to the acoustic image signals; S3, performing multi-source data time synchronization on the angular velocity signals, the acceleration signals, the depth observation values, the heading observation values and the three-dimensional velocity observation values to generate spatio-temporally aligned fusion input signals; S4, constructing an adaptive factor graph optimization model, inputting the fusion input signals into a predefined sensor confidence assessment module, generating real-time weight coefficients of each sensor, and dynamically adjusting an inertial navigation calculation node based on the real-time weight coefficients; inputting the environmental feature point cloud data into a closed-loop detection module to generate a loop factor node, and adaptively correcting the node weight according to the feature matching degree; S5, solving the adaptive factor graph optimization model through a nonlinear optimization algorithm, outputting real-time pose estimation signals of the underwater robot, and updating a global navigation trajectory according to the pose estimation signals.

[0033] As Figure 1As shown, the three-axis angular velocity signals and three-axis acceleration signals of the carrier are continuously captured by an inertial measurement unit fixed to the robot body, the measurement reference of which is strictly aligned with the carrier coordinate system; meanwhile, the Doppler velocity log emits acoustic beams to the bottom or middle layer of the water body and receives echoes, and at least four independent direction beam radial velocity vector signals are generated by analysis, the signals taking the beam coordinate system as the reference system; the sonar imaging device emits acoustic pulses in the forward-looking or downward-looking scanning mode, and constructs a two-dimensional underwater acoustic image signal according to the echo intensity and time delay. In the signal processing stage, the carrier motion compensation is first performed on the beam radial velocity vector signal: the real-time angular velocity signal is combined to construct a posture rotation matrix, each beam radial velocity is projected to the carrier coordinate system, and the three-dimensional velocity observation values of the carrier forward movement, transverse movement and vertical movement are solved by least square fitting; the acoustic image signal is simultaneously subjected to spatial filtering enhancement and feature extraction, the geometric feature points of the environment such as reefs and sunken ships are identified by using the algorithm based on curvature extreme value detection, and the three-dimensional feature point cloud data with normal vector description are generated. Before multi-source data fusion, the time asynchronous problem needs to be solved: a time synchronization system taking the inertial measurement unit clock as the reference is established, the water depth pressure signal output by the depth sensor and the geomagnetic vector signal output by the magnetometer are timestamp calibrated, the depth observation value is solved by the hydrostatic pressure formula, and the heading observation value is obtained by the inverse tangent calculation of the horizontal projection of the geomagnetic vector, the angular velocity signal, the acceleration signal, the depth observation value, the heading observation value and the three-dimensional velocity observation value are uniformly interpolated to the common time node, and the time and space aligned fusion input signal stream is formed. The navigation optimization model is realized by using the adaptive factor graph architecture: the fusion input signal is input into the sensor confidence evaluation module, which calculates the reliability indicators of each signal source (such as the zero offset stability of the angular velocity signal and the beam consistency of the three-dimensional velocity observation value) in real time, and outputs dynamic weight coefficients in the range of 0 to 1; based on the coefficients, the information matrix weight of the inertial navigation calculation node in the factor graph is adjusted - when the weight coefficient tends to 1, the inertial prediction constraint is strengthened, and when the weight coefficient tends to 0, the constraint is weakened to prevent false data pollution; the environmental feature point cloud data is input into the closed loop detection module, the similarity between the current frame and the historical map is matched through the feature descriptor, when the matching is successful, the loop factor node is generated, and the node weight is adaptively modified according to the feature matching degree (the average re-projection error and spatial distribution uniformity of the matched point pairs), when the matching degree is higher than the threshold, the closed loop constraint strength is enhanced, and when the matching degree is lower than the threshold, the weight is reduced to suppress the false matching. Finally, the factor graph is iteratively solved by using the Levenberg-Marquardt nonlinear optimization algorithm, and the six-degree-of-freedom pose estimation signal (including three-dimensional position and Euler angle attitude) of the underwater robot in the global coordinate system is output, and the signal is used to incrementally update the global navigation trajectory, so as to realize the continuous positioning of the underwater environment.

[0034] Further, multi-dimension enhancement is implemented in the sensor signal acquisition stage: the Doppler velocity log adopts a four-beam Janus configuration, the central lines of the four beams are symmetrically distributed in a conical shape and form a preset inclination angle with the central axis of the carrier, ensuring that the beam directions are not coplanar to eliminate the singularity of velocity solution, and each beam original velocity measurement value is accompanied by a signal-to-noise ratio and echo intensity quality index; the sonar imaging device cooperatively controls the forward-looking mechanical scanning sonar and the downward-looking multi-beam depth sounding sonar, the forward-looking sonar acquires the acoustic image of the environment in front of the robot in a fan-shaped scanning mode, the downward-looking sonar acquires the acoustic image of the seabed topography in a strip scanning mode, and the two achieve acquisition time sequence synchronization through a hardware trigger signal to generate multi-view acoustic image signals with space-time correlation; the inertial measurement unit adopts a tactical micro-electromechanical system, the angular velocity and acceleration signal sampling rate of which is set to be more than three times that of the Doppler velocity log to meet the motion solution requirements under high-speed maneuvering, and the original inertial data is compensated for temperature drift and corrected for scale factor before output; a water acoustic communication machine is additionally added as an auxiliary signal source, when the underwater robot enters the preset acoustic beacon coverage area, receives the modulated acoustic signal transmitted by the beacon and decodes to generate an auxiliary positioning signal (including beacon number, relative distance and azimuth angle); all sensor signal outputs are embedded with high-precision time stamps, which are uniformly generated by the central processor through the IEEE 1588 precision clock protocol, and the time synchronization accuracy is controlled within microseconds. This design realizes four-fold optimization: the non-coplanar beam configuration improves the robustness of velocity solution, the multi-view sonar combination enhances the coverage rate of environmental features, the high-frequency sampling of inertia guarantees the integrity of motion state capture, and the external beacon auxiliary provides an absolute position correction anchor point, laying a high-quality data foundation for subsequent fusion positioning.

[0035] As Figure 2As shown, three-dimensional velocity observation value solving needs to overcome the dynamic disturbance of the carrier attitude: the beam radial velocity vector signal contains the scalar velocity value of each beam in its own coordinate system, and the conversion to the carrier coordinate system needs to compensate for the carrier rotation effect. Establish a kinematic coupling model: let the origin of the carrier coordinate system be O, the direction vector of the i-th beam be b_i, the beam radial velocity be v_i, and the carrier angular velocity be ω, then the true three-dimensional velocity v satisfies the equation v·b_i + (ω × r_i)·b_i = v_i, where r_i is the position vector of the beam emission point relative to O. Combine all beam equations into an overdetermined equation set, and solve the least squares optimal estimate of v combined with the real-time angular velocity signal. In order to suppress the error propagation caused by attitude measurement noise, the angular velocity signal is smoothed by introducing Kalman filtering, and when the carrier is moving at a constant speed in a straight line, an adaptive smoothing window is enabled to enhance the stability of the velocity solution. The environment feature extraction adopts a dual-mode adaptive mechanism: based on the acoustic image signal, the local area texture richness index is calculated, when the index is higher than the set threshold (such as there are obvious rock edges or artificial structures), the corner detection process based on gradient operator is started - first the acoustic image is anisotropically diffused to filter out spot noise, then the Hessian matrix eigenvalue of the pixel point is calculated, and the point whose maximum eigenvalue is greater than the average value of the neighborhood is judged as a corner point, generating a feature point cloud with direction descriptors; when the texture richness is lower than the threshold (such as flat sandy seabed), switch to the surface curvature analysis mode: construct a triangular mesh surface model for the acoustic image, calculate the Gaussian curvature and mean curvature at each vertex, extract the curvature extreme points as feature points, and record the principal curvature direction of the point as the feature descriptor. This dual-mode design ensures that effective geometric references can still be extracted in sparse feature environments, avoiding the failure of traditional single algorithms in complex underwater scenes.

[0036] Further, the multi-source data time synchronization mechanism adopts a two-way prediction-correction architecture to achieve high-precision alignment: taking the angular velocity signal and acceleration signal output by the inertial measurement unit as the reference, the high-frequency attitude quaternion and velocity vector are solved in real time through the inertial navigation differential equation to generate a predicted trajectory stream with a time resolution better than five milliseconds; the depth observation value comes from the pressure sensor, and its sampling rate is usually lower than that of inertial data. Each depth observation value is mapped to the nearest neighbor point of the predicted trajectory according to the time stamp, and a depth sequence with the same frequency as the inertial data is generated through cubic spline interpolation; the heading observation value is output by the three-axis magnetometer, after hard and soft magnetic interference compensation, the true heading angle is calculated by projecting it to the horizontal plane using the quaternion rotation matrix, and Lagrange interpolation is also used to synchronize it with the predicted trajectory; the three-dimensional velocity observation value has a large sampling delay due to the Doppler velocimeter, so a velocity transfer function model needs to be established, combined with the optimized pose at the previous moment to calculate the actual signal generation time of the carrier motion state, and then forward predicted to the current time node. After completing the preliminary alignment, the sliding window error elimination is started: within a time window of twenty times the main control period, the residuals of each sensor observation value and the predicted trajectory are least squares fitted, if the fitted curve shows a significant linear trend, it is determined that there is a fixed time delay error, and the sensor time stamp offset is corrected by time shift compensation; finally, the spatio-temporal aligned fusion input signal stream is generated, and the variance of each signal in the sliding window is calculated as the synchronization confidence, which participates in the subsequent sensor weight calculation. This mechanism breaks through the limitations of traditional single interpolation method, effectively suppressing the spatio-temporal mismatch problems caused by underwater transmission delay, sensor response difference and clock drift.

[0037] As Figure 2As shown, the sensor confidence assessment module is composed of five types of special evaluation functions in parallel: the angular velocity signal is analyzed by the zero bias stability evaluation function, the static data of one hundred sampling points are continuously collected to calculate the Allan variance curve, the quantization noise and random walk coefficient are extracted, and when the zero bias change rate is lower than the threshold, high confidence is output; the acceleration signal is input into the vibration noise spectrum analysis function, the three-axis acceleration frequency domain signal is wavelet packet decomposed, the energy entropy value of ten hertz to one kilohertz frequency band is extracted, the lower the entropy value, the smaller the carrier vibration interference, and the higher the confidence score; the three-dimensional velocity observation value is processed by the beam consistency test function, the residual norm of the four beam radial velocity projection to the carrier coordinate system is calculated, the norm value is inversely proportional to the volume of the preset error ellipsoid, and when the residual norm is less than the theoretical minimum value of the beam angle, it is determined as high confidence; the heading observation value is input into the magnetic interference detection function, the magnetic vector module length output by the magnetometer is compared with the theoretical geomagnetic field strength at the geographical location in real time, and when the deviation exceeds five percent or the vector direction suddenly changes, the magnetic interference flag is triggered, and the confidence is directly reduced to the lowest grade; the depth observation value is evaluated by the pressure sudden change judgment function, the ratio of the pressure difference between adjacent sampling points to the theoretical maximum diving speed is calculated, and when the ratio is greater than one, it is determined that the sensor is abnormal or the water body is violently disturbed, and the confidence is exponentially attenuated. The function outputs are normalized to a sub-confidence in the interval of zero to one, the weighted geometric mean is generated to generate the final weight coefficient, the weight coefficient and the sensor failure probability are negatively related exponentially, that is, the weight coefficient is zero point five corresponding to thirty percent of the failure probability, zero point two corresponding to eighty percent, realizing the quantitative mapping of the fault state.

[0038] Specifically, the dynamic adjustment of the inertial navigation solving node is essentially the online reconstruction of the factor graph topology: when the real-time weight coefficient of the angular velocity signal is higher than 0.8, a gyro zero bias online estimation node is added in the factor graph, which is combined with the three consecutive inertial navigation solving nodes to form a Gaussian constraint, and the constraint strength is linearly increased with the weight coefficient, and the maximum information matrix weight reaches five times the reference value, forcing the optimization algorithm to preferentially correct the gyro drift error; if the angular velocity weight coefficient is lower than 0.3 for three seconds, the zero bias estimation node is removed to avoid false correction. For the acceleration signal, a strong coupling constraint node of the vertical motion of the carrier and the direction of gravity is activated under high confidence, and the acceleration measurement value is decomposed into motion acceleration and gravity component through a projection matrix; when the weight coefficient drops below 0.4, the strong coupling node is automatically removed, and a vertical position constraint node is constructed using the depth observation value - the depth sensor output value is converted into the Z-axis coordinate of the world coordinate system, and a residual constraint is established between the vertical position of the inertial navigation solution. When the three-dimensional velocity observation value weight suddenly drops in a special scenario, the kinematic smoothing mechanism is started: a second-order motion model is fitted according to the optimization trajectory in the previous five seconds to generate a velocity prediction value to replace the invalid observation value for fusion. This dynamic adjustment strategy enables the factor graph to automatically degrade to a robust architecture when the sensor is partially invalid, for example, after the Doppler velocimeter beam is lost, the system relies on the high-weight inertial node and the depth constraint to maintain vertical positioning, and the horizontal plane continues the navigation capability through the heading constraint and the kinematic model.

[0039] As shown in Figure 3 The application also includes an underwater robot navigation and positioning system, comprising: a collection module that collects angular velocity and acceleration signals through an inertial measurement unit, collects beam radial velocity vector signals through a Doppler velocimeter, and collects underwater acoustic image signals through a sonar imaging device; a coordination module that collects angular velocity and acceleration signals through an inertial measurement unit, collects beam radial velocity vector signals through a Doppler velocimeter, and collects underwater acoustic image signals through a sonar imaging device; a comparison module that time synchronizes multiple source data of angular velocity signals, acceleration signals, depth observation values, heading observation values, and three-dimensional velocity observation values to generate spatio-temporally aligned fusion input signals; an analysis module that constructs an adaptive factor graph optimization model, inputs the fusion input signals into a pre-defined sensor confidence assessment module to generate real-time weight coefficients of each sensor, and dynamically adjusts inertial navigation solving nodes based on the real-time weight coefficients; and a loop detection module that inputs environmental feature point cloud data to generate a loop factor node, and adaptively corrects the weight of the node according to the feature matching degree.

[0040] As shown in Figure 3As shown, the adaptive correction strategy of loop closure factor node weight is established on the dynamic grading mechanism of feature matching degree: when the feature matching degree output by the loop closure detection module is in the high confidence interval (greater than or equal to 0.7), the loop closure factor node is coupled with the current inertial navigation solution node to form a tight coupling constraint, which converts the pose transformation matrix into a residual term in the six-dimensional manifold space through Lie group Lie algebra, and gives the information matrix a weight value of 1.5 times the reference weight, forcing the optimization algorithm to preferentially align the loop closure pose; if the matching degree is in the critical interval (0.4 to 0.7), the weight protection program is started - the existence of the loop closure factor node is maintained but its weight coefficient is attenuated according to a linear function, the specific formula is: weight equals reference weight multiplied by matching degree divided by 0.7, and the lowest is 35% of the reference value, and the re-projection error threshold is relaxed by 50% to reduce the constraint strength; when the matching degree is below the failure threshold of 0.4, the loop closure factor node is completely disabled and a three-level response mechanism is triggered: the first level interrupts the current loop closure data stream and discards the matching result to avoid polluting the optimization process; the second level activates the local map reconstruction process to create an independent sub-map centered on the current pose, and the subsequent ten times through the area skip loop detection; the third level checks the historical trajectory in reverse, if the area has triggered a high matching degree loop closure, it is marked as an environment dynamic change area and the related historical node weight is reduced. In special scenarios, when the matching degree is in the critical interval for three consecutive times, the Doppler speedometer speed observation value is automatically called to build a motion continuity constraint to replace the failed loop closure constraint to maintain positioning stability. This strategy realizes intelligent fault tolerance of loop closure detection and maintains system robustness under the interference of seabed terrain mutation or moving targets. The nonlinear optimization algorithm uses an incremental smoothing and map construction framework to achieve efficient solution: each iteration only processes the newly added sensor observation data and loop closure factor, and the overall optimization of the factor graph is converted into local incremental update through sparse matrix decomposition technology. The specific process is: the adaptive factor graph model is represented as a nonlinear least squares problem, and the objective function is the sum of the squares of all node residuals, which includes inertial navigation solution residuals, velocity observation residuals, depth constraint residuals, heading constraint residuals, and loop closure residuals; the Levenberg-Marquardt algorithm is used for iterative solution, and the Jacobian matrix and Hessian matrix approximation are calculated at each step. The core innovation lies in the incremental construction of the Hessian matrix - only the matrix block corresponding to the new factor is calculated, and the historical nodes are eliminated to the prior information matrix through Schur complement decomposition, and the Hessian submatrix corresponding to the new node is combined with the prior matrix to generate the current iteration equation. During the solving process, the covariance matrix eigenvalues of the pose estimation signal are monitored in real time, and if the maximum eigenvalue is detected to be more than three times the standard deviation of the historical mean or the condition number is greater than ten to the sixth power, it is determined that the optimization is in a pathological state, and the robust kernel function is immediately started to suppress the influence of outliers: the residuals are reweighted using the Cauchy kernel function, and the weight function is set to the residual divided by the inner point threshold squared plus the residual squared, and the inner point threshold is dynamically adjusted according to the sensor weight coefficient; at the same time, the inertial navigation prediction constraint is forcibly added as a regularization term to prevent optimization divergence.After the iteration converges, the underwater robot real-time pose estimation signal is output and the covariance ellipse radius is calculated, and when the radius exceeds the positioning accuracy threshold, the backtracking mechanism is automatically triggered: save the current factor graph state, and solve again from the stable state before the last five optimization periods. This design ensures the numerical stability and computational efficiency of the optimization process in long-time tasks.

[0041] The underwater robot navigation positioning method and system of the present application generates a fusion input through the spatio-temporal alignment of multi-source sensor signals, constructs an adaptive factor graph optimization model: dynamically evaluates the sensor confidence to generate a weight coefficient to adjust the constraint strength of the inertial navigation node, adaptively corrects the loop factor weight based on the acoustic image feature matching degree, and finally solves the real-time pose through nonlinear optimization. Robust positioning under sensor reliability fluctuations and environmental feature changes is realized, breaking through the limitations of fixed weights in traditional fusion algorithms.

[0042] Therefore, the underwater robot navigation positioning method and system of the present application can solve the problem of insufficient positioning accuracy of multi-source heterogeneous sensor fusion in underwater GPS-free environment.

[0043] The above embodiments only exemplarily illustrate the principles and effects of the present application, and are not intended to limit the present application. Any person skilled in the art can modify or change the above embodiments without departing from the spirit and scope of the present application. Therefore, all equivalent modifications or changes made by those skilled in the art without departing from the spirit and technical thought disclosed by the present application shall be covered by the claims of the present application.

Claims

1. A method for navigation and positioning of an underwater robot, characterized in that, The method comprises the following steps: S1: Collecting angular velocity and acceleration signals by an inertial measurement unit, collecting beam radial velocity vector signals by a Doppler velocimeter, and collecting underwater acoustic image signals by a sonar imaging device; S2: Calculating three-dimensional velocity observation values according to the beam radial velocity vector signals combined with the angular velocity signals, and extracting environmental feature point cloud data according to the acoustic image signals; S3: Time-synchronizing the angular velocity signals, acceleration signals, depth observation values, heading observation values, and three-dimensional velocity observation values, and generating spatio-temporally aligned fusion input signals; S4: Constructing an adaptive factor graph optimization model, inputting the fusion input signals into a predefined sensor confidence evaluation module to generate real-time weight coefficients of each sensor, and dynamically adjusting an inertial navigation calculation node based on the real-time weight coefficients; inputting the environmental feature point cloud data into a closed-loop detection module to generate a loop-back factor node, and adaptively correcting the weight of the node according to the feature matching degree; S5: Solving the adaptive factor graph optimization model by a nonlinear optimization algorithm, outputting real-time pose estimation signals of the underwater robot, and updating a global navigation trajectory according to the pose estimation signals.

2. The method of claim 1, wherein, In the step S1, the beam radial velocity vector signals collected by the Doppler velocimeter contain original velocity measurement values of at least four non-coplanar beams, the sonar imaging device synchronously collects forward-looking sonar images and downward-looking sonar images to generate multi-view acoustic image signals, the inertial measurement unit continuously outputs angular velocity and acceleration signals at a sampling rate more than three times higher than those of other sensors, and simultaneously receives auxiliary positioning signals of external reference beacons through a water acoustic communication machine, and all original signals are marked with time stamps with an accuracy of microseconds.

3. The method of claim 1, wherein, In the step S2, a beam vector projection compensation method is used to eliminate measurement deviations introduced by dynamic changes of carrier attitude angles when calculating three-dimensional velocity observation values, specifically including inputting the beam radial velocity vector signals and real-time angular velocity signals into a kinematic coupling model to solve three-dimensional velocity optimal estimates in a carrier coordinate system, and adaptively selecting a feature extraction algorithm based on a feature stability index of acoustic image signals, using a corner detection method based on a gradient operator to generate feature point cloud data when the environment is rich in texture, and switching to a feature extraction mode based on surface curvature analysis when the environment is sparse in texture.

4. The method of claim 1, wherein, The bidirectional timestamp interpolation alignment mechanism is used for time-synchronizing the multi-source data in the step S3, a high-frequency inertial navigation calculation thread is constructed for the angular velocity signals and acceleration signals to generate a predicted trajectory, the depth observation values, heading observation values, and three-dimensional velocity observation values are spatio-temporally matched with the predicted trajectory according to the nearest neighbor principle, a fixed time delay error between sensors is eliminated through a sliding window least squares fitting, spatio-temporally aligned fusion input signals are generated, and the synchronization confidence of each signal is recorded as an input parameter for subsequent weight calculation.

5. The method of claim 1, wherein, The sensor confidence evaluation module of the step S4 is implemented by establishing a zero bias stability evaluation function of the angular velocity signal, an acceleration signal vibration noise spectrum analysis function, a three-dimensional velocity observation value beam consistency test function, a heading observation value magnetic interference detection function, and a depth observation value pressure mutation judgment function, respectively, and outputting the real-time weight coefficient of each sensor through a multi-dimensional confidence score model, which is negatively correlated with the sensor failure probability.

6. The method of claim 1, wherein, The operation of the dynamic adjustment inertial navigation solution node specifically includes: when the real-time weight coefficient indicates that the angular velocity signal confidence is higher than the threshold value, adding a gyro zero bias online estimation node in the factor graph and enhancing its constraint strength, and when the acceleration signal confidence is lower than the threshold value, automatically releasing the strong coupling constraint between the carrier vertical motion and the gravity direction, and replacing it with a vertical position constraint node constructed by the depth observation value.

7. The method of claim 1, wherein, The workflow of the closed loop detection module includes: multi-resolution hierarchical matching of the current environment feature point cloud data with the historical feature map, first round fast retrieval based on curvature features to narrow down the candidate range, second round precision matching based on feature descriptors to calculate similarity scores, and finally generating a loop factor node according to the similarity score and spatial distribution consistency. The feature matching degree is calculated by the number of matching point pairs, distribution uniformity and geometric invariance error.

8. The method of claim 1, wherein, The adaptive correction loop factor node weight strategy is: when the feature matching degree is in the high confidence interval, constructing a tight coupling constraint between the loop factor node and the inertial navigation solution node and giving it the maximum weight, when the feature matching degree is in the critical interval, retaining the loop factor node but reducing its weight to less than fifty percent of the baseline value, and when the feature matching degree is lower than the failure threshold, completely disabling the loop factor node and triggering the local map reconstruction process.

9. The method of claim 1, wherein, The non-linear optimization algorithm of the step S5 adopts an incremental smoothing and map construction framework, processes only the newly added sensor observation data and loop factors in each iteration, reduces the computational complexity through sparse matrix decomposition technology, and monitors the covariance matrix eigenvalue of the pose estimation signal in real time during the solving process. If the eigenvalue abnormally increases, it automatically switches to a robust kernel function to suppress the influence of outliers.

10. A navigation positioning system using the navigation positioning method of the underwater robot according to any one of claims 1 to 9, characterized by, It comprises: a collection module that collects angular velocity and acceleration signals through an inertial measurement unit, collects beam radial velocity vector signals through a Doppler velocimeter, and collects underwater acoustic image signals through a sonar imaging device; a coordination module that collects angular velocity and acceleration signals through an inertial measurement unit, collects beam radial velocity vector signals through a Doppler velocimeter, and collects underwater acoustic image signals through a sonar imaging device; a comparison module that performs multi-source data time synchronization on the angular velocity signal, acceleration signal, depth observation value, heading observation value, and three-dimensional velocity observation value to generate a spatio-temporally aligned fusion input signal; An analysis module constructs an adaptive factor graph optimization model, which inputs the fusion input signal into a predefined sensor confidence assessment module, generates real-time weight coefficients of each sensor, and dynamically adjusts an inertial navigation solution node based on the real-time weight coefficients; inputs the environmental feature point cloud data into a loop detection module to generate a loop closing factor node, and adaptively corrects the node weight according to feature matching degrees.

Images