Underwater robot navigation positioning method and system

Abstract

The invention relates to an underwater robot navigation positioning method and system. The method comprises the following steps: S1, acquiring angular velocity and acceleration signals through an inertial measurement unit; s2, resolving a three-dimensional velocity observation value according to the beam radial velocity vector signal in combination with the angular velocity signal, and extracting environment feature point cloud data according to the acoustic image signal; s3, multi-source data time synchronization is carried out, and a fusion input signal with time-space alignment is generated; s4, constructing an adaptive factor graph optimization model, and dynamically adjusting an inertial navigation solution node based on a real-time weight coefficient; inputting the environment feature point cloud data into a closed-loop detection module to generate a loopback factor node, and adaptively correcting the weight of the node according to the feature matching degree; and S5, solving the adaptive factor graph optimization model through a nonlinear optimization algorithm. According to the underwater robot navigation positioning method and system, the problem that the fusion positioning precision of a multi-source heterogeneous sensor is insufficient in an underwater GPS-free environment can be solved.

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] The 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 velocity meter. The acoustic positioning needs to preset the seabed beacon array, and the deployment cost is high and the coverage range is limited. 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 velocity meter measures distortedly in turbulent water flow, and the magnetometer is disturbed near the metal structure, causing the reliability of the fusion system to drop sharply; secondly, the underwater acoustic image is affected by reverberation and multipath effect, 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 easily cause the entire system to crash.

[0003] In recent years, although some researches have introduced factor graph optimization to improve the accuracy, the problem of static weight distribution has not been solved, and the positioning drift occurs in long-time tasks due to the performance degradation of sensors or environmental mutations. In addition, the flat area of underwater terrain lacks distinctive features, and the 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 an autonomous navigation and positioning method that can dynamically evaluate the reliability of sensors, adaptively adjust the constraint strength and intelligently process the sparse environmental features is urgently needed. 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 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 fluctuation and environmental feature change is realized, and the limitation of fixed weight in traditional fusion algorithm is broken through.

[0005] The present application provides an underwater robot navigation and positioning method, comprising: S1: acquiring angular velocity and acceleration signals through an inertial measurement unit, acquiring beam radial velocity vector signals through a Doppler velocity meter, and acquiring underwater acoustic image signals through a sonar imaging device; S2: According to the beam radial velocity vector signal, the three-dimensional velocity observation value is solved in combination with the angular velocity signal, and the environmental feature point cloud data is extracted according to the acoustic image signal; S3: The angular velocity signal, the acceleration signal, the depth observation value, the heading observation value and the three-dimensional velocity observation value are time-synchronized for multi-source data, and a time-space aligned fusion input signal is generated; S4: An adaptive factor graph optimization model is constructed, the fusion input signal is input into a pre-defined sensor confidence evaluation module, real-time weight coefficients of each sensor are generated, and the inertial navigation solving node is dynamically adjusted based on the real-time weight coefficients; the environmental feature point cloud data is input into a closed loop detection module to generate a loop factor node, and the node weight is adaptively corrected according to the feature matching degree; S5: The adaptive factor graph optimization model is solved by a nonlinear optimization algorithm, and the real-time pose estimation signal of the underwater robot is output, and the global navigation trajectory is updated according to the pose estimation signal.

[0006] In an embodiment of the present application, in step S1, the beam radial velocity vector signal collected by the Doppler velocimeter contains at least four non-coplanar beam original velocity measurement values, the multiview acoustic image signal is generated by synchronously collecting the forward-looking sonar image and the downward-looking sonar image by the sonar imaging device, the angular velocity and acceleration signals are continuously output by the inertial measurement unit at a sampling rate more than three times of other sensors, and the auxiliary positioning signal of the external reference beacon is received through the underwater acoustic communication machine, and all original signals are marked with time stamps with an accuracy of microseconds.

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

[0008] In an embodiment of the present application, the multi-source data time synchronization in step S3 uses 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 time-space 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 time-space aligned fusion input signal is generated, and the synchronization confidence of each signal is recorded as an input parameter for subsequent weight calculation.

[0009] In one embodiment of the application, the sensor confidence evaluation module of 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.

[0010] In one embodiment of the application, the operation of dynamically adjusting the inertial navigation solution node 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 instead using the depth observation value to construct a vertical position constraint node.

[0011] In one embodiment of the application, 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, and the feature matching degree is calculated by the number of matching point pairs, distribution uniformity and geometric invariance error.

[0012] In one embodiment of the application, the strategy for adaptively modifying the loop factor node weight 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 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 50% of the reference 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.

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

[0014] The application also includes an underwater robot navigation and positioning system, comprising: 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. The coordination module collects angular velocity and acceleration signals through an inertial measurement unit, collects beam radial velocity vector signals through a Doppler speedometer, and collects underwater acoustic image signals through a sonar imaging device; The comparison module performs multi-source data time synchronization on the angular velocity signal, the acceleration signal, the depth observation value, the heading observation value and the three-dimensional velocity observation value, and generates a spatio-temporally aligned fusion input signal; The analysis module constructs an adaptive factor graph optimization model, inputs the fusion input signal into a predefined sensor confidence assessment module, generates real-time weight coefficients of each sensor, and dynamically adjusts the inertial navigation solution node based on the real-time weight coefficients; inputs the environmental feature point cloud data into a closed loop detection module to generate a loop factor node, and adaptively corrects the node weight according to the feature matching degree.

[0015] The underwater robot navigation positioning method and system provided by the application generate a fusion input through spatio-temporal alignment of multi-source sensor signals, construct an adaptive factor graph optimization model: dynamically evaluate the sensor confidence to generate weight coefficients to adjust the constraint strength of the inertial navigation node, adaptively correct the loop factor weight based on the acoustic image feature matching degree, and finally solve the real-time pose through nonlinear optimization. Robust positioning under sensor reliability fluctuation and environmental feature change is realized, and the limitation of fixed weight in traditional fusion algorithm is broken through. BRIEF DESCRIPTION OF DRAWINGS

[0016] In order to more clearly illustrate the technical solutions of the embodiments of the 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 application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0017] Figure 1 It is a method flow chart of an underwater robot navigation positioning method. Figure 2 It is a schematic diagram of step three and step four of the underwater robot navigation positioning method. Figure 3 It is a system architecture diagram of an underwater robot navigation positioning system. DETAILED DESCRIPTION

[0018] Following, through specific examples, the embodiments of the present application are illustrated, and other advantages and effects of the present application can be easily understood by those skilled in the art from the disclosure of this specification. The present application can also be implemented or applied by means of other different specific embodiments, and the details in the specification can be modified or changed in various ways based on different views and applications without departing from the spirit of the present application. It should be noted that the following examples and features in the examples can be combined with each other without conflict.

[0019] It should be noted that the diagrams provided in the following examples only illustrate the basic concepts of the present application in a schematic manner, and only the components related to the present application are shown in the diagrams, not the number, shape and size of the components when actually implemented. The actual implementation of each component can be a random change, and the component layout pattern can be more complex.

[0020] In the following description, a large number of details are discussed to provide a more thorough explanation of the embodiments of the present application, however, it is obvious to those skilled in the art that the embodiments of the present application can be implemented without these specific details, and in other embodiments, well-known structures and devices are shown in the form of block diagrams rather than in detail, to avoid making the embodiments of the present application difficult to understand.

[0021] Please refer to Figures 1-3 , which shows a method and system for navigation and positioning of an underwater robot. The method for navigation and positioning of an underwater robot comprises: 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, 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 assessment module, generating real-time weight coefficients of each sensor, and dynamically adjusting the 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 by a nonlinear optimization algorithm, outputting real-time pose estimation signals of the underwater robot, and updating the global navigation trajectory according to the pose estimation signals.

[0022] 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 radial velocity vector signals of the beams are generated, the signals being referenced to the beam coordinate 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 radial velocity vector signals of the beams: the real-time angular velocity signals are combined to construct a posture rotation matrix, the radial velocities of the beams are projected to the carrier coordinate system, and the three-dimensional velocity observation values of the carrier in the forward, transverse and vertical directions are solved by least squares fitting; at the same time, spatial filtering enhancement and feature extraction are performed on the acoustic image signal, the geometric feature points of the environment such as reefs and sunken ships are identified by using the algorithm based on the 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 is established based on the clock of the inertial measurement unit, 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 consistency between the beams 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.

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

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

[0025] 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 residual error of each sensor observation value and the predicted trajectory is 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 problem caused by underwater transmission delay, sensor response difference and clock drift.

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

[0027] 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 loses lock, 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.

[0028] 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 multi-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.

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

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

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

[0032] 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 idea disclosed by the present application shall be covered by the claims of the present application.

Claims

1. A navigation and positioning method for an underwater robot, characterized in that, include: S1: Angular velocity and acceleration signals are acquired through an inertial measurement unit, beam radial velocity vector signals are acquired through a Doppler velocimeter, and underwater acoustic image signals are acquired through a sonar imaging device; S2: Calculate the three-dimensional velocity observation value based on the beam radial velocity vector signal and the angular velocity signal, and extract environmental feature point cloud data based on the acoustic image signal; S3: Perform 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 spatiotemporally aligned fused input signal; S4: Construct an adaptive factor graph optimization model. The adaptive factor graph optimization model inputs the fused input signal into a predefined sensor confidence evaluation module to generate real-time weight coefficients for each sensor, and dynamically adjusts the inertial navigation solution node based on the real-time weight coefficients; inputs the environmental feature point cloud data into the closed loop detection module to generate a loop closure factor node, and adaptively corrects the weight of the node according to the feature matching degree. S5: Solve the adaptive factor graph optimization model using a nonlinear optimization algorithm, output the real-time pose estimation signal of the underwater robot, and update the global navigation trajectory based on the pose estimation signal.

2. The underwater robot navigation and positioning method according to claim 1, characterized in that, In step S1, the beam radial velocity vector signal acquired by the Doppler velocimeter contains the original velocity measurements of at least four non-coplanar beams. The sonar imaging device simultaneously acquires forward-looking 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 that of other sensors. At the same time, it receives auxiliary positioning signals from external reference beacons through an underwater acoustic communication device. All original signals are marked with timestamps accurate to the microsecond level.

3. The underwater robot navigation and positioning method according to claim 1, characterized in that, In step S2, when calculating the three-dimensional velocity observation value, a beam vector projection compensation method is used to eliminate the measurement deviation introduced by the dynamic change of the carrier attitude angle. Specifically, the beam radial velocity vector signal and the real-time angular velocity signal are input into the kinematic coupling model to solve the optimal estimate of the three-dimensional velocity in the carrier coordinate system. At the same time, based on the feature stability index of the acoustic image signal, the feature extraction algorithm is adaptively selected. When the environmental texture is rich, the corner detection method based on the gradient operator is used to generate feature point cloud data. When the environmental texture is sparse, the feature extraction mode based on surface curvature analysis is switched.

4. The underwater robot navigation and positioning method according to claim 1, characterized in that, The multi-source data time synchronization in step S3 adopts a bidirectional timestamp interpolation alignment mechanism. A high-frequency inertial navigation solution thread is constructed for the angular velocity signal and acceleration signal to generate a predicted trajectory. The depth observation value, heading observation value and 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 by sliding window least squares fitting to generate a spatiotemporally aligned fused input signal. At the same time, the synchronization confidence of each signal is recorded as the input parameter for subsequent weight calculation.

5. The underwater robot navigation and positioning method according to claim 1, characterized in that, The sensor confidence assessment module in step S4 is implemented as follows: a zero-bias stability evaluation function for angular velocity signals, a vibration noise spectrum analysis function for acceleration signals, a beam consistency test function for three-dimensional velocity observations, a magnetic interference detection function for heading observations, and a pressure mutation judgment function for depth observations are established respectively. The real-time weight coefficients of each sensor are output through a multi-dimensional confidence scoring model. These weight coefficients have a negative exponential relationship with the sensor failure probability.

6. The underwater robot navigation and positioning method according to claim 1, characterized in that, The operation of dynamically adjusting the inertial navigation solution node specifically includes: when the real-time weight coefficient indicates that the confidence level of the angular velocity signal is higher than the threshold, adding a gyroscope zero-bias online estimation node in the factor graph and strengthening its constraint strength; when the confidence level of the acceleration signal is lower than the threshold, automatically releasing the strong coupling constraint between the vertical motion of the carrier and the gravity direction, and instead using depth observations to construct a vertical position constraint node.

7. The underwater robot navigation and positioning method according to claim 1, characterized in that, The workflow of the closed-loop detection module includes: performing multi-resolution hierarchical matching between the current environmental feature point cloud data and the historical feature map; in the first round, using fast retrieval based on curvature features to narrow down the candidate range; in the second round, using fine matching based on feature descriptors to calculate the similarity score; and finally generating loop closure factor nodes based on the similarity score and spatial distribution consistency. The feature matching degree is calculated by comprehensively considering the number of matching point pairs, distribution uniformity, and geometric invariance error.

8. The underwater robot navigation and positioning method according to claim 1, characterized in that, The adaptive correction strategy for the weight of the loop closure factor node is as follows: when the feature matching degree is in the high confidence interval, the loop closure factor node and the inertial navigation solution node are tightly coupled and given the maximum weight. When the feature matching degree is in the critical interval, the loop closure factor node is retained but its weight is reduced to less than 50% of the baseline value. When the feature matching degree is lower than the failure threshold, the loop closure factor node is completely disabled and the local map reconstruction process is triggered.

9. The underwater robot navigation and positioning method according to claim 1, characterized in that, The nonlinear optimization algorithm in step S5 is implemented using an incremental smoothing and map building framework. Each iteration only processes newly added sensor observation data and closure factors. The computational complexity is reduced by sparse matrix factorization. During the solution process, the eigenvalues ​​of the covariance matrix of the pose estimation signal are monitored in real time. If the eigenvalues ​​increase abnormally, the algorithm automatically switches to a robust kernel function to suppress the influence of outliers.

10. A navigation and positioning system using the underwater robot navigation and positioning method according to any one of claims 1-9, characterized in that, include: The acquisition module acquires angular velocity and acceleration signals through an inertial measurement unit, beam radial velocity vector signals through a Doppler velocimeter, and underwater acoustic image signals through a sonar imaging device. The coordination module acquires angular velocity and acceleration signals through an inertial measurement unit, beam radial velocity vector signals through a Doppler velocimeter, and underwater acoustic image signals through a sonar imaging device. The comparison module 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 spatiotemporally aligned fused input signal; The analysis module constructs an adaptive factor graph optimization model. This model inputs the fused input signal into a predefined sensor confidence evaluation module to generate real-time weight coefficients for each sensor and dynamically adjusts the inertial navigation solution nodes based on these real-time weight coefficients. It also inputs the environmental feature point cloud data into a closed-loop detection module to generate loop closure factor nodes and adaptively corrects the node weights based on feature matching.

Images