Underwater robot navigation method, device and equipment based on side-scan image matching

Abstract

The application discloses a kind of underwater robot navigation method, device and equipment based on side scan image matching.It relates to underwater robot navigation technical field, wherein method includes: in response to receiving underwater robot travels to target depth, the side scan sonar image of the underwater robot is preprocessed, and preprocessed image is obtained;Based on the image features of the preprocessed image and reference image in reference image database, feature matching is carried out, and relative pose constraint is obtained;Based on the relative pose constraint, current inertia data, current speed data and current depth data, the running state of the underwater robot is optimized using target fusion optimization method, and the optimized relative pose is obtained;Based on the optimized relative pose, the motion trajectory of the underwater robot is solved, and the navigation trajectory of the underwater robot is obtained.The method of the application can improve the accurate navigation ability of underwater robot in the scene lacking external base station or absolute speed measurement.

Description

Technical Field

[0001] This invention relates to the field of underwater robot navigation technology, and in particular to an underwater robot navigation method, apparatus, storage medium, and electronic device based on side-scan image matching. Background Technology

[0002] In recent years, autonomous underwater vehicles (AUVs) have developed rapidly and are widely used in missions such as marine scientific research, seabed resource exploration, and military patrols. However, precise underwater navigation remains a significant factor restricting its further promotion. Because electromagnetic waves cannot penetrate seawater, GPS cannot directly provide positioning information for underwater robots. Existing underwater navigation technologies mainly include integrated navigation, acoustic navigation, cooperative navigation, and geophysical feature navigation. Among these, integrated navigation offers high accuracy but is complex and expensive, making it unsuitable for low-cost, long-endurance robots; acoustic navigation (USBL / LBL) relies on external base stations, making it difficult to use in open ocean or temporary operating environments; cooperative navigation requires multiple AUVs to work together, limiting its application scenarios; and geophysical feature-based navigation relies on prior maps, resulting in poor environmental adaptability.

[0003] In contrast, side-scan sonar (SSS) can acquire high-resolution, wide-coverage seabed images with rich texture and abundant information, making it an important navigation reference for low-cost, long-endurance AUVs. Therefore, researching a navigation method based on seabed side-scan image matching to constrain drift and improve positioning accuracy through image feature matching, optimization, and trajectory calculation in the absence of GPS is of great significance. Peter Vandrish et al. from Memorial University of Newfoundland proposed a navigation method based on side-scan image registration, using matching current and historical images to correct odometer errors, and conducted related research on "real-time image generation and registration" and "error matching filtering." Ove Kent Hagen et al. from the Norwegian Defence Research Institute proposed a terrain-aided navigation method based on Bayesian filtering in low-altitude operational scenarios, combining interferometric side-scan (HISAS 1030) and multibeam bathymetry, and verified the feasibility of the method in real sea trials. Zhang et al. from the Royal Institute of Technology in Sweden proposed a "fully automated side-scan image SLAM framework" and "subframe-compact matching SSS-SLAM," using the matching results as factor graph constraints to optimize AUV trajectories. The effectiveness of this method in reducing cumulative drift was verified using real-world data from Hugin AUVs. P. King et al. from the Australian Maritime Institute studied a side-scan image registration and positioning method for revisit survey lines, proposing to achieve long-term trajectory correction by matching historical images with current images.

[0004] In summary, navigation methods based on seabed side-scan image matching can not only reduce dependence on external base stations and expensive equipment, but also utilize natural seabed features as a reference, thereby achieving high-precision navigation for long-endurance, low-cost AUVs, and thus have become an important research direction. Summary of the Invention

[0005] In view of this, the present invention provides an underwater robot navigation method, device, storage medium and electronic device based on side-scan image matching, the main purpose of which is to solve the problem that underwater robot navigation currently relies on prior maps and has poor environmental adaptability.

[0006] To address the aforementioned problems, this application provides an underwater robot navigation method based on side-scan image matching, comprising: In response to receiving a message that the underwater robot has traveled to the target depth, the side-scan sonar image of the underwater robot is preprocessed to obtain a preprocessed image; Based on the image features of the preprocessed image, feature matching is performed with reference images in the reference image database to obtain relative pose constraints; Based on the relative pose constraints, current inertial data, current velocity data, and current depth data, a target fusion optimization method is used to optimize the operating state of the underwater robot to obtain an optimized relative pose. The underwater robot's motion trajectory is calculated based on the optimized relative pose to obtain the underwater robot's navigation trajectory.

[0007] Optionally, the preprocessing of the side-scan sonar image of the underwater robot to obtain a preprocessed image specifically includes: Radiometric correction is performed on the side-scan sonar image of the underwater robot to obtain a radiometrically corrected image; The radiometrically corrected image is geometrically corrected to obtain the preprocessed image.

[0008] Optionally, the step of performing radiometric correction on the side-scan sonar image of the underwater robot to obtain a radiometrically corrected image specifically includes: The distance-dependent intensity distortion of the side-scan sonar image is corrected using a time-varying gain compensation method to obtain a first corrected image; The intensity spatial distribution of the first corrected image is corrected using a beammap correction method to obtain a second corrected image; The noise signal of the second corrected image is filtered out using a seabed image defect masking method to obtain the radiometric corrected image.

[0009] Optionally, performing geometric correction on the radiometrically corrected image to obtain the preprocessed image specifically includes: The radiometrically corrected image is geometrically corrected using a strip geometric correction method to obtain a geometrically corrected image. The geometrically corrected image is post-processed using an intensity normalization method to obtain the pre-processed image.

[0010] Optionally, the step of performing feature matching between the image features of the preprocessed image and reference images in the reference image database to obtain relative pose constraints specifically includes: Based on the ground coordinate mapping information corresponding to the preprocessed image, the overlapping interval between any two preprocessed images is determined to be located in the first frame overlapping interval of the first preprocessed image and in the second frame overlapping interval of the second preprocessed image. The target navigation condition index is determined based on the first frame overlap interval and the second frame overlap interval. The target navigation condition index includes any one of the following: inter-frame heading change, trajectory curvature, and geometric offset of the overlap interval. Based on the target navigation condition indicators, determine the adaptive image feature matching parameters; Based on the adaptive image feature matching parameters, feature matching is performed on reference images in the reference image database to obtain key matching pairs; The geometric consistency between the key matching pairs is calculated using a random sampling consensus algorithm. Key matching pairs that do not meet any geometric model constraint are filtered out to obtain the relative pose constraint. The geometric model constraints include two-dimensional rigid body model constraints, two-dimensional similarity model constraints, and two-dimensional affine model constraints.

[0011] Optionally, the optimization of the underwater robot's operating state using a target fusion optimization method based on the relative pose constraints, current inertial data, current velocity data, and current depth data to obtain an optimized relative pose specifically includes: Based on the relative pose constraints, current inertial data, current velocity data, and current depth data, an objective function is constructed to optimize the operating state of the underwater robot with the goal of minimizing inertial odometry error and image matching error. The objective function is iteratively solved using a factor graph optimization algorithm to obtain the optimized relative pose.

[0012] Optionally, the step of iteratively solving the objective function using a factor graph optimization algorithm to obtain the optimized relative pose specifically includes: Step 1: Initialize the algorithm parameters of the factor graph optimization algorithm and construct a factor graph with pose state variables as vertices and pose constraints as edges according to the objective function. Step 2: Calculate the pose state estimate at the current moment based on the current inertial data, the current velocity data, and the current depth data; Step 3: Calculate the error vector for each error factor based on the pose state estimation at the current moment; Step 4: Calculate the Jacobian matrix of the error vector and its corresponding error factor; Step 5: Perform calculations based on the Jacobian matrix and Jacobian inverse matrix corresponding to different error factors to obtain the Hessian matrix and the right-hand apex contribution. Step 6: Based on the Hessian matrix and the right-hand apex contribution, the sparse Cholesky decomposition method is used to calculate the state gain; Step 7: Update the pose state of the factor graph at the current time based on the state gain to obtain the current relative pose; Step 8: When the state gain satisfies the preset increment norm criterion or the current iteration round is greater than or equal to the preset iteration number threshold, the current relative pose is determined as the optimized relative pose; when the state gain does not satisfy the preset increment norm criterion or the current iteration round is less than the preset iteration number threshold, the observation state of the underwater robot in the factor graph is corrected based on the state gain, and steps 1 to 8 are repeated.

[0013] To address the aforementioned problems, this application provides an underwater robot navigation device based on side-scan image matching, comprising: The preprocessing module is used to preprocess the side-scan sonar image of the underwater robot in response to receiving a message that the underwater robot has traveled to the target depth, so as to obtain a preprocessed image. The feature matching module is used to perform feature matching between the image features of the preprocessed image and reference images in the reference image database to obtain relative pose constraints; The optimization module is used to optimize the operating state of the underwater robot based on the relative pose constraints, current inertial data, current velocity data, and current depth data using a target fusion optimization method to obtain an optimized relative pose. The calculation module is used to calculate the motion trajectory of the underwater robot based on the optimized relative pose, so as to obtain the navigation trajectory of the underwater robot.

[0014] To address the aforementioned problems, this application provides a storage medium storing a computer program that, when executed by a processor, implements the steps of the underwater robot navigation method based on side-scan image matching described above.

[0015] To address the aforementioned problems, this application provides an electronic device, comprising at least a memory and a processor. The memory stores a computer program, and the processor, when executing the computer program in the memory, implements the steps of the underwater robot navigation method based on side-scan image matching described above.

[0016] The beneficial effects of this application are as follows: By extracting and matching features between adjacent images and existing images in the database, stable relative pose observations are obtained, and fused with IMU and depth information, significantly reducing the cumulative drift error of pure inertial navigation. The side-scan sonar used is conventional equipment with a simple hardware structure and low power consumption, suitable for underwater robots of various specifications and platforms, and has good economic efficiency and applicability. During navigation, the current image can be matched with images at adjacent time points to form local continuous constraints; it can also be searched and matched with candidate images in the above two types of image databases. If the similarity of the matching result exceeds the threshold and passes the geometric consistency verification, the system determines that the robot has returned to the visited area, thereby triggering relocalization, and adding loop closure constraints in the global trajectory optimization to correct the cumulative error and improve global consistency. The high resolution and rich texture of the side-scan images provide a reliable reference for navigation. Accurate estimation of position and attitude is achieved through optimized filtering algorithms, which can be directly used in task scenarios lacking external base stations or absolute velocity measurements, and can also serve as an effective supplement to other navigation methods.

[0017] The above description is merely an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention and to implement it in accordance with the contents of the specification, and to make the above and other objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention are described below. Attached Figure Description

[0018] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings: Figure 1 A flowchart illustrating an underwater robot navigation method based on side-scan image matching provided in an embodiment of this application is shown. Figure 2 This paper illustrates a flowchart of an underwater robot navigation method based on side-scan image matching, according to another embodiment of this application. Figure 3 A structural block diagram of an underwater robot navigation device based on side-scan image matching, according to another embodiment of this application, is shown. Detailed Implementation

[0019] Various embodiments and features of this application are described herein with reference to the accompanying drawings.

[0020] It should be understood that various modifications can be made to the embodiments described herein. Therefore, the above description should not be considered as limiting, but merely as an example of embodiments. Other modifications within the scope and spirit of this application will be apparent to those skilled in the art.

[0021] The accompanying drawings, which are included in and form part of this specification, illustrate embodiments of the present application and, together with the general description of the present application given above and the detailed description of the embodiments given below, serve to explain the principles of the present application.

[0022] These and other features of this application will become apparent from the following description of preferred forms of embodiments given as non-limiting examples, with reference to the accompanying drawings.

[0023] It should also be understood that although this application has been described with reference to some specific examples, those skilled in the art can certainly implement many other equivalent forms of this application.

[0024] The above and other aspects, features and advantages of this application will become more apparent when taken in conjunction with the accompanying drawings and in view of the following detailed description.

[0025] Specific embodiments of this application are described thereafter with reference to the accompanying drawings; however, it should be understood that the claimed embodiments are merely examples of this application, which can be implemented in various ways. Well-known and / or repeated functions and structures are not described in detail to avoid unnecessary or redundant details that could obscure the application. Therefore, the specific structural and functional details claimed herein are not intended to be limiting, but merely to teach those skilled in the art to use this application in a variety of substantially any suitable detailed structures.

[0026] This specification may use the phrases “in one embodiment,” “in another embodiment,” “in yet another embodiment,” or “in other embodiments,” all of which may refer to one or more of the same or different embodiments according to this application.

[0027] This application provides an underwater robot navigation method based on side-scan image matching, such as... Figure 1 As shown, it includes: Step S101: In response to receiving that the underwater robot has traveled to the target depth, preprocess the side-scan sonar image of the underwater robot to obtain a preprocessed image; In the specific implementation process, the underwater robot's current depth data is collected in real time from the depth gauge to determine the target depth. Navigation of the underwater robot generally involves phased execution, including surface initialization, diving, underwater cruising, and surfacing correction. GPS positioning can be used for navigation during surface initialization, diving, and surfacing correction. The core phase of this application is the underwater cruising phase. During underwater cruising, GPS signals are weak, so a side-scan sonar image matching method is used for navigation. The high resolution and rich texture of the side-scan images provide a reliable reference for navigation. Accurate estimation of position and attitude is achieved through optimized algorithms. Radiometric correction is performed on the side-scan sonar images of the underwater robot to obtain a radiometrically corrected image; geometric correction is then performed on the radiometrically corrected image to obtain the preprocessed image.

[0028] Step S102: Based on the image features of the preprocessed image, perform feature matching with the reference image in the reference image database to obtain the relative pose constraint; In the specific implementation process, based on the image features of the preprocessed image, a target feature operator is used to perform feature matching on the reference images in the reference image database. The key point with the smallest distance to the feature points of the current image features in the reference image database is taken as the best matching point, and a key matching pair is obtained. The random sampling consensus algorithm is used to calculate the geometric consistency between the key matching pairs. Key matching pairs that do not meet any geometric model constraint are filtered to obtain the relative pose constraint. The target feature operator includes at least one of the ORB feature operator, SIFT feature operator, and SURF feature operator. The geometric model constraint includes two-dimensional rigid body model constraint, two-dimensional similarity model constraint, and two-dimensional affine model constraint.

[0029] Step S103: Based on the relative pose constraints, current inertial data, current velocity data, and current depth data, the target fusion optimization method is used to optimize the operating state of the underwater robot to obtain the optimized relative pose; In the specific implementation process, based on the relative pose constraints, current inertial data, current velocity data, and current depth data, an objective function is constructed to optimize the operating state of the underwater robot with the goal of minimizing inertial odometry error and image matching error; the objective function is iteratively solved using a factor graph optimization algorithm to obtain the optimized relative pose.

[0030] Step S104: Calculate the motion trajectory of the underwater robot based on the optimized relative pose to obtain the navigation trajectory of the underwater robot.

[0031] In the specific implementation process, the current speed is calculated based on the optimized relative pose and time step, and the position and attitude are obtained by recursion based on the current speed, so as to obtain the navigation trajectory of the underwater robot.

[0032] This application achieves stable relative pose observations by extracting and matching features between adjacent images and existing images in a database, and then fuses these with IMU and depth information, significantly reducing the cumulative drift error of pure inertial navigation. The side-scan sonar used is conventional equipment with a simple hardware structure and low power consumption, suitable for various specifications and platforms of underwater robots, demonstrating good economy and applicability. During navigation, the current image can be matched with images from adjacent time points to form local continuity constraints, or it can be searched and matched with candidate images in the aforementioned two types of image databases. If the similarity of the matching result exceeds a threshold and passes geometric consistency verification, the system determines that the robot has returned to the visited area, thereby triggering relocalization. Loop closure constraints are added to the global trajectory optimization to correct cumulative errors and improve global consistency. The high resolution and rich texture of the side-scan images provide a reliable reference for navigation. Accurate estimation of position and attitude is achieved through optimized filtering algorithms, which can be directly used in task scenarios lacking external base stations or absolute velocity measurements, and can also serve as an effective supplement to other navigation methods.

[0033] Another embodiment of this application provides another underwater robot navigation method based on side-scan image matching, such as... Figure 2 As shown, it includes: Step S201: In response to receiving that the underwater robot has traveled to the target depth, perform radiometric correction on the side scan sonar image of the underwater robot to obtain a radiometric corrected image; In this step, a time-varying gain compensation method is used to correct the distance-dependent intensity distortion of the side-scan sonar image, resulting in a first corrected image. Specifically, a compensation gain model is constructed, including a compensation term for spherical diffusion loss and a compensation term for exponential attenuation caused by seawater absorption. Based on the compensation gain model, a point-by-point compensation algorithm is used to compensate the gain value of the original echo intensity value of each pixel in each column of the original side-scan sonar image, resulting in the first corrected image. The sonar signal intensity decreases sharply with increasing propagation distance due to spherical diffusion and seawater absorption. This results in an excessively strong, bright white seabed echo signal near the sensor and an excessively weak, dark seabed signal far from the sensor in the original image, with complete loss of detail. This solves the problem of sound wave propagation attenuation in seawater. A beam pattern correction method is used to correct the intensity spatial distribution of the first corrected image to obtain a second corrected image. Specifically, the angle value of each pixel in the first corrected image relative to the beam center is calculated; the beam pattern of the sonar transducer is obtained; intensity compensation is performed on the first corrected image based on the beam pattern and the angle value to obtain the second corrected image; a seabed image defect masking method is used to filter the noise signal of the second corrected image to obtain the radiation-corrected image. Specifically, a water column masking method is used to mask the second corrected image to obtain a first masked image; the first masked image is filtered to obtain a second masked image; the filtering method can use median filtering, mean filtering, or more advanced wavelet denoising methods to smooth random water noise while preserving edge and texture features as much as possible; the second masked image is subjected to defect detection and interpolation to obtain the radiation-corrected image. For obvious linear burst noise or data missing areas, abnormal highlights or dark lines can be detected and then filled by interpolation with adjacent normal pixels to remove interference information, making subsequent feature extraction more focused on the real seabed topography and texture.

[0034] Step S202: Perform geometric correction on the radiometrically corrected image to obtain the preprocessed image; In this step, a strip geometric correction method is used to geometrically correct the radiometrically corrected image to obtain a geometrically corrected image. Specifically, the slant range of the radiometrically corrected image is converted into ground distance to obtain a first geometrically corrected image. Specifically, for each pixel, the horizontal ground distance corresponding to that pixel is calculated using trigonometric functions, and the beam illumination point is projected onto a horizontal plane. The first geometrically corrected image is geocoded and resampled to obtain a second geometrically corrected image. Specifically, each pixel is mapped from the slant range coordinate system to the along-heading distance and horizontal ground distance coordinate system. Since the new coordinate grid is usually irregular, interpolation resampling is required to generate a new second geometrically corrected image where each pixel represents the actual horizontal distance to the seabed. The interpolation resampling method can use a bilinear interpolation algorithm; this application does not limit the interpolation resampling method. The geometrically corrected image is post-processed using an intensity normalization method to obtain the preprocessed image. Histogram matching, inter-strip equalization, and adaptive local normalization are performed on the geometrically corrected image to obtain the preprocessed image. Ensure that all image patches to be matched are on a relatively uniform intensity scale to guarantee that the feature matching algorithm can work stably.

[0035] Step S203: Determine the overlapping interval between any two preprocessed images based on the ground coordinate mapping information corresponding to the preprocessed image. The overlapping interval is located in the first frame overlapping interval of the first preprocessed image and in the second frame overlapping interval of the second preprocessed image. In this step, based on the geometrically corrected ground coordinate mapping information, such as the ground coordinate range corresponding to each preprocessed image frame, the effective overlap interval of two preprocessed images in the heading distance dimension is estimated, thus obtaining the overlap interval between the two preprocessed images located within the first frame overlap interval of the first preprocessed image. Overlapping region with the second frame of the preprocessed image The first frame overlap region Overlapping area with the second frame These represent the corresponding position range of the same physically overlapping area in the respective index coordinate systems of the two images, so the start and end indices are generally different.

[0036] Step S204: Determine the target navigation condition index based on the overlapping interval of the first frame and the overlapping interval of the second frame; In this step, a moving average is calculated on the DR heading angle sequence within the overlapping interval based on the first and second frame overlapping intervals to obtain the heading change between frames; the mathematical formula is as follows:

[0037] in, This represents the maximum change in heading over an overlapping period, i.e., the most dramatic turning amplitude during this time. The frame interval indicates a comparison. and The difference in heading between two moments; For interval K Changes in the heading of the frame; To find all valid cases within the overlapping interval t， For example Take the maximum value. Threshold. , The threshold for classifying operating conditions. When At that time, the underwater robot was traveling in a straight line; when If the underwater robot makes a small turn, then it makes a large turn; otherwise, it makes a large turn.

[0038] Calculate the ratio of total turning angle to total distance within the overlapping intervals of the first and second frames to obtain the trajectory curvature; the mathematical formula is as follows:

[0039] in, The curvature of the trajectory within the overlapping interval; i A point-by-point cumulative index within the interval; This refers to gradual course increments, that is, changes in course between two adjacent points. This is also to handle changes in angle. Molecules This is the total accumulated turning angle within the overlapping interval; The planar position coordinates are derived from the DR calculation; the denominator is... Accumulated total distance within the overlapping interval. Threshold interval: using... The size of the turn can be distinguished as a straight flight, a small turn, or a large turn.

[0040] Calculate the coordinates of the first center point of the overlapping region in the first frame. And calculate the coordinates of the second center point of the overlapping region of the second frame. The geometric offset of the overlapping interval is obtained by calculating the coordinates of the first and second center points. The mathematical formula for the calculation is as follows:

[0041] in, This represents the geometric offset of the two overlapping intervals on the plane, in meters (m). : The coordinates of the midpoint of the overlapping area in the first frame; The coordinates of the midpoint of the overlapping interval in the second frame. The target navigation condition index is determined by filtering the changes in heading, trajectory curvature, and geometric offset of the overlapping interval between frames. When multiple condition indices are used simultaneously, the condition labels output by each index belong to discrete categories with a natural order. The labels are sorted, and the median label is taken as the final condition type. This is because the median, in ordered category fusion, is equivalent to a robust center estimate of the condition level and is not sensitive to extreme misjudgments caused by DR noise, local anomalies, or short-term jitter in a single index. If one index abnormally outputs an extreme condition of "1 or 3," while the other two indices still fall within a reasonable range, the sorted median is still determined by the median value, thus avoiding the final condition being skewed by extreme values. For example, when the three indices output (1, 2, 3) respectively, taking the median of 2 can reduce condition jumps, improve the stability of parameter adaptation, and reduce mismatch propagation caused by incorrect parameter settings.

[0042] Step S205: Determine adaptive image feature matching parameters based on the target navigation condition indicators; In this step, the matching parameter set is automatically set according to the final operating condition, such as search radius, pixel error threshold, candidate point density control, and grid thinning intensity. During sharp turns, the thinning intensity for dense areas is increased, the upper limit of matching points retained per grid is reduced, and a dense / sparse area determination threshold is enabled to reduce mismatches caused by local texture repetition. During straight-line or small-turn operations, a more lenient retention strategy is adopted to obtain more stable constraints.

[0043] Step S206: Perform feature matching on the reference images in the reference image database based on the adaptive image feature matching parameters to obtain key matching pairs; In this step, the search radius, pixel error threshold, candidate matching point density threshold, grid size after thinning, or upper limit of the number of matching points retained per grid are used to adaptively match reference images in the reference image database using adaptive image feature matching parameters to obtain key matching pairs. The target feature operators include ORB, SIFT, and SURF feature operators. When using the ORB feature operator, the key point with the smallest Hamming distance to the feature points of the current image in the reference image database is selected as the best matching point, resulting in a key matching pair. When using the SIFT and SURF feature operators, the key point with the smallest Euclidean distance to the feature points of the current image in the reference image database is selected as the best matching point, resulting in a key matching pair. When the similarity between the current image and candidate images in the database exceeds a preset threshold, and geometric consistency verification is successful, the system triggers relocalization and introduces closure constraints in global trajectory optimization, thereby achieving a combination of local continuous localization and global error correction, improving navigation accuracy and robustness. The mathematical expression for a closure constraint is as follows:

[0044] in, The global pose at two moments separated by a predetermined time interval; In order to seek The inverse operation; The relative pose transformation is directly calculated for image matching; Indicates index pairs It belongs to the Loop constraint set; This represents the pose error calculation in the pose space; the physical process is to bring the relative relationship between i and j closer to match the image observation, thereby instantly "flattening" the drift error accumulated in the middle onto the entire trajectory.

[0045] Step S207: The geometric consistency between the key matching pairs is calculated using a random sampling consensus algorithm. Key matching pairs that do not meet any geometric model constraint are filtered out to obtain the relative pose constraint. In this step, the geometric model constraints include two-dimensional rigid body model constraints, two-dimensional similarity model constraints, and two-dimensional affine model constraints. The two-dimensional rigid body model constraints are used to constrain images where there is only rotation and translation, without scale changes. The two-dimensional similarity model constraints are used to constrain images that allow rotation, translation, and uniform scale changes. The two-dimensional affine model constraints are used to constrain images that, in addition to rotation, translation, and scale changes, also allow certain shear transformations. The random sampling consensus algorithm can automatically eliminate incorrect matches that do not meet geometric consistency from a large number of matching points, retaining only correct matches that satisfy specific model constraints to obtain the relative pose constraints. The mathematical expression for the relative position constraints is as follows:

[0046] in, Represents the set of candidate matching points; This represents a geographic backprojection function based on prior pose (mapping image pixels to seabed coordinates); This indicates a search radius constraint. Indicates the first in the reference image k The pixel coordinates of the candidate feature points; This represents the pixel coordinates of the feature point to be matched in the current image; This represents the index number of a feature point in the reference image. It is used to enumerate all candidate points and filter those that meet the criteria. Candidate index set This allows for matching within the pre-allowed search range, thereby reducing the risk of global mismatches.

[0047] Step S208: Based on the relative pose constraints, current inertial data, current velocity data, and current depth data, construct an objective function to optimize the operating state of the underwater robot with the goal of minimizing inertial odometry error and image matching error; In the specific implementation process of this step, the mathematical expression of the objective function is as follows:

[0048] in, Let be the globally optimal trajectory to be solved, and be the set of positions and attitudes at all times; Here, is the Mahalanobis distance, representing the weighted square of the error; Let be the noise covariance matrix of the inertial navigation system, representing the uncertainty of the IMU / DVL. To represent the error term / residual vector of an inertial odometry system, i.e., the error term generated by inertial and velocity sensors such as IMU / DVL at adjacent time points... k 1 and kThe inconsistency between the given motion increment and the current pose estimate is used to constrain the continuity of the trajectory; The image matching error term / residual vector represents the inconsistency between the relative pose observation obtained from the side scan image matching and the relative pose derived from the current pose estimation. It is used to provide geometric constraints across time. Both are minimized under their respective weighted norms, thereby achieving the fusion optimization of minimizing the inertial odometry error and the image matching error. For the set of all time pairs for which image matching was successfully established; For adaptive weights; observation noise matrix The expression is as follows:

[0049] The mathematical expression for the range of consistency confidence scores is as follows:

[0050] in, This is the dynamically adjusted observation noise matrix; The consistency confidence score range is (0, 1). A higher score indicates a higher confidence level. The basic noise parameters for image matching; This reflects the probability weights; when the matching quality is high, the covariance is small, which has a strong pulling force on optimization; when the matching is poor, the covariance tends to infinity, and this constraint is automatically ignored in optimization. To indicate the first i With the j Observational associations between frames are constructed through image matching; The first one obtained by DR i With the j The relative translation increment between frames; The first one obtained by image matching / registration estimation i With the j The relative translation increment between frames; The first one obtained by DR i With the j The relative heading angle increment between frames; The first one obtained by image matching / registration estimation i With the j The relative heading angle increment between frames; The variance of the consistency score is used to normalize the translation and heading difference residuals, thereby... The smaller the difference, the higher the score.

[0051] Step S209: The objective function is iteratively solved using a factor graph optimization algorithm to obtain the optimized relative pose; In its implementation, this step specifically includes the following steps: Step 1: Initialize the algorithm parameters of the factor graph optimization algorithm and construct a factor graph with pose state variables as vertices and pose constraints as edges according to the objective function. Specifically, each state variable is considered a node in a "factor graph," and each odometry constraint, image constraint, velocity constraint, and depth constraint is considered an "edge" connecting the nodes. The optimization process involves adjusting the positions of the nodes to minimize the error of all edges. The solved... It represents a series of compromise states that smoothly conform to the robot's inertial motion while satisfying all reliable sensor observations as much as possible, especially effectively fitting the geometric constraints provided by image matching to correct drift. Based on current inertial data, current velocity data, and current depth data, an initial pose sequence is obtained through calculation and processing; this can be acquired through an inertial measurement unit (IMU) and a depth detection unit (DVL). All sensor observations are converted into corresponding factors and added to the factor graph to update the pre-constructed initial factor graph. The algorithm parameters for initializing the factor graph optimization algorithm include initial values ​​for adaptive weights, initial values ​​for matching quality scores, and initial values ​​for IMU confidence. Adaptive weights... Determined jointly based on image texture intensity and IMU confidence:

[0052] in, A measure of image texture intensity or feature point density; The IMU confidence level, with a value range of [0,1]. These are the weighting coefficients; To restrict the value a to the interval [0,1].

[0053] Step 2: Calculate the pose state estimate at the current moment based on the current inertial data, the current velocity data, and the current depth data; Specifically, the pose state estimate at the current moment is obtained by calculating and processing the current inertial data, the current velocity data, and the current depth data. The mathematical expression is as follows:

[0054] Specifically parameterized as a 6-DOF vector :

[0055] in, For the three-dimensional attitude of underwater robots: roll, pitch, and yaw; Let T represent the robot's three-dimensional absolute position in the north, east, and depth directions, and let T be the transpose matrix used for calculation. It is a set of three-dimensional pose transformations.

[0056] Step 3: Calculate the error vector for each error factor based on the pose state estimation at the current moment; Specifically, for each error factor, an error vector is calculated based on the state estimate at the current moment.

[0057] Step 4: Calculate the Jacobian matrix of the error vector and its corresponding error factor; Specifically, for each error factor, the Jacobian matrix of the error function with respect to the relevant state variables is calculated; based on the error vector, the right Jacobian matrix of the corresponding error factor is calculated; for the target error factors connecting adjacent pose states, the Jacobian of the error factor and the two adjacent pose states is calculated respectively to obtain the Jacobian matrix of the error factor.

[0058] Step 5: Perform calculations based on the Jacobian matrix and Jacobian inverse matrix corresponding to different error factors to obtain the Hessian matrix and the right-hand apex contribution. Specifically, the Jacobian matrix and Jacobian inverse matrix corresponding to different error factors are calculated to obtain the Hessian submatrix corresponding to different error factors; the Hessian submatrix corresponding to different error factors are globally assembled to obtain the Hessian matrix; the Jacobian matrix and Jacobian inverse matrix corresponding to different error factors are calculated to obtain the right-hand top contribution corresponding to different error factors; and the right-hand top contribution is globally assembled to obtain the right-hand top contribution.

[0059] Step 6: Based on the Hessian matrix and the right-hand apex contribution, the sparse Cholesky decomposition method is used to calculate the state gain; Specifically, a linear system is constructed based on the Hessian matrix and the right-hand vertices contribution; the sparse Cholesky decomposition method is used to solve the linear system to obtain the state gain; in specific implementations, the damped Gauss-Newton method or the Levenberg-Marquardt method can also be used to solve the linear system to obtain the state gain.

[0060] Step 7: Update the pose state of the factor graph at the current time based on the state gain to obtain the current relative pose; In this step, the state gain is decomposed into the Lie algebra increment corresponding to each state; the pose state of the factor graph is updated based on the Lie algebra increment to obtain the current relative pose.

[0061] Step 8: When the state gain satisfies the preset increment norm criterion or the current iteration round is greater than or equal to the preset iteration number threshold, the current relative pose is determined as the optimized relative pose; when the state gain does not satisfy the preset increment norm criterion or the current iteration round is less than the preset iteration number threshold, the observation state of the underwater robot in the factor graph is corrected based on the state gain, and steps 1 to 8 are repeated.

[0062] In this step, when the norm of the state gain is less than or equal to a first preset threshold (which can be set according to actual needs), or when the current iteration round is greater than or equal to a preset iteration number threshold, the current relative pose is determined as an optimized relative pose. The preset iteration number threshold can be 200, which can be set according to actual needs. When the state gain does not meet the preset increment norm criterion or the current iteration round is less than the preset iteration number threshold, the observation state of the underwater robot in the factor graph is corrected based on the state gain. Steps one to eight are repeated until the norm of the state gain is less than or equal to the first preset threshold or the current iteration round is greater than or equal to the preset iteration number threshold, at which point the current relative pose is determined as an optimized relative pose.

[0063] Step S210: Calculate the motion trajectory of the underwater robot based on the optimized relative pose to obtain the navigation trajectory of the underwater robot.

[0064] In this step, the optimized relative pose includes the corrected velocity and attitude increments. Combined with the time step, and using kinematic recursive formulas, the precise absolute position and attitude at the current moment are calculated from the absolute position of the previous moment. The mathematical expression is as follows:

[0065] in, ; Vertical velocity approximation Therefore, we can conclude that:

[0066] in, These are the forward / lateral / vertical velocity components of the underwater robot within the machine system, in m / s. This is the heading angle, provided by the IMU, and is measured in rad or °. The eastward / northward / vertical ground velocity components under the navigation system, in m / s; This is the ground velocity measurement vector obtained by the underwater robot through a directional rotation; Let K be the eastward position coordinates of the navigation system at discrete times k and k+1. Here are the northward position navigation system coordinates at discrete times k and k+1, in meters. This represents the time step between adjacent moments, measured in seconds (s).

[0067] This application achieves stable relative pose observations by extracting and matching features between adjacent images and existing images in a database, and then fuses these with IMU and depth information, significantly reducing the cumulative drift error of pure inertial navigation. The side-scan sonar used is conventional equipment with a simple hardware structure and low power consumption, suitable for various specifications and platforms of underwater robots, demonstrating good economy and applicability. During navigation, the current image can be matched with images from adjacent time points to form local continuity constraints, or it can be searched and matched with candidate images in the aforementioned two types of image databases. If the similarity of the matching result exceeds a threshold and passes geometric consistency verification, the system determines that the robot has returned to the visited area, thereby triggering relocalization. Loop closure constraints are added to the global trajectory optimization to correct cumulative errors and improve global consistency. The high resolution and rich texture of the side-scan images provide a reliable reference for navigation. Accurate estimation of position and attitude is achieved through optimized filtering algorithms, which can be directly used in task scenarios lacking external base stations or absolute velocity measurements, and can also serve as an effective supplement to other navigation methods.

[0068] Another embodiment of this application provides an underwater robot navigation device 300 based on side-scan image matching, such as... Figure 3 As shown, it includes: Preprocessing module 301 is used to preprocess the side-scan sonar image of the underwater robot in response to receiving a message that the underwater robot has traveled to the target depth, so as to obtain a preprocessed image; Feature matching module 302 is used to perform feature matching between the image features of the preprocessed image and reference images in the reference image database to obtain relative pose constraints; The optimization module 303 is used to optimize the operating state of the underwater robot based on the relative pose constraints, current inertial data, current velocity data and current depth data using a target fusion optimization method to obtain an optimized relative pose; The calculation module 304 is used to calculate the motion trajectory of the underwater robot based on the optimized relative pose to obtain the navigation trajectory of the underwater robot.

[0069] In the specific implementation process, the preprocessing module 301 is specifically used to perform radiometric correction on the side-scan sonar image of the underwater robot to obtain a radiometrically corrected image; and to perform geometric correction on the radiometrically corrected image to obtain the preprocessed image.

[0070] In the specific implementation process, the preprocessing module 301 is further used to: correct the distance-dependent intensity distortion of the side-scan sonar image using a time-varying gain compensation method to obtain a first corrected image; correct the intensity spatial distribution of the first corrected image using a beammap correction method to obtain a second corrected image; and filter the noise signal of the second corrected image using a seabed image defect masking method to obtain the radiation corrected image.

[0071] In the specific implementation process, the preprocessing module 301 is further used to perform geometric correction on the radiometric correction image using the strip geometric correction method to obtain a geometric correction image; and to perform post-processing on the geometric correction image using the intensity normalization method to obtain the preprocessed image.

[0072] In specific implementation, the optimization module 303 is specifically used to: determine, based on the ground coordinate mapping information corresponding to the preprocessed image, the overlapping interval between any two preprocessed images is located in the first frame overlapping interval of the first preprocessed image and the second frame overlapping interval of the second preprocessed image; determine the target navigation condition index based on the first frame overlapping interval and the second frame overlapping interval, wherein the target navigation condition index includes any one of the following: frame-by-frame heading change, trajectory curvature, and overlapping interval geometric offset; determine adaptive image feature matching parameters based on the target navigation condition index; perform feature matching on reference images in the reference image database based on the adaptive image feature matching parameters to obtain key matching pairs; calculate the geometric consistency between the key matching pairs using a random sampling consensus algorithm, and filter key matching pairs that do not meet any geometric model constraint conditions to obtain the relative pose constraint; wherein the geometric model constraint conditions include two-dimensional rigid body model constraint, two-dimensional similarity model constraint, and two-dimensional affine model constraint.

[0073] In the specific implementation process, the optimization module 303 is also used to construct an objective function for optimizing the operating state of the underwater robot based on the relative pose constraints, current inertial data, current velocity data and current depth data, with the goal of minimizing inertial odometry error and image matching error; and to iteratively solve the objective function using a factor graph optimization algorithm to obtain the optimized relative pose.

[0074] In the specific implementation process, the optimization module 303 is also used for: Step 1, initializing the algorithm parameters of the factor graph optimization algorithm and constructing a factor graph with pose state variables as vertices and pose constraints as edges according to the objective function; Step 2, calculating the pose state estimate at the current moment based on the current inertial data, the current velocity data, and the current depth data; Step 3, calculating the error vector of each error factor based on the pose state estimate at the current moment; Step 4, calculating the Jacobian matrix of the error factor corresponding to the error vector; Step 5, performing calculation processing based on the Jacobian matrix and the inverse Jacobian matrix corresponding to different error factors to obtain the Hessian matrix and the right-hand vertex matrix. Step 6: Calculate the state gain using the sparse Cholesky decomposition method based on the Hessian matrix and the right-hand vertices contribution; Step 7: Update the pose state of the factor graph at the current time based on the state gain to obtain the current relative pose; Step 8: When the state gain satisfies the preset increment norm criterion or the current iteration round is greater than or equal to the preset iteration number threshold, determine the current relative pose as the optimized relative pose; when the state gain does not satisfy the preset increment norm criterion or the current iteration round is less than the preset iteration number threshold, correct the observation state of the underwater robot in the factor graph based on the state gain, and repeat steps 1 to 8.

[0075] This application achieves stable relative pose observations by extracting and matching features between adjacent images and existing images in a database, and then fuses these with IMU and depth information, significantly reducing the cumulative drift error of pure inertial navigation. The side-scan sonar used is conventional equipment with a simple hardware structure and low power consumption, suitable for various specifications and platforms of underwater robots, demonstrating good economy and applicability. During navigation, the current image can be matched with images from adjacent time points to form local continuity constraints, or it can be searched and matched with candidate images in the aforementioned two types of image databases. If the similarity of the matching result exceeds a threshold and passes geometric consistency verification, the system determines that the robot has returned to the visited area, thereby triggering relocalization. Loop closure constraints are added to the global trajectory optimization to correct cumulative errors and improve global consistency. The high resolution and rich texture of the side-scan images provide a reliable reference for navigation. Accurate estimation of position and attitude is achieved through optimized filtering algorithms, which can be directly used in task scenarios lacking external base stations or absolute velocity measurements, and can also serve as an effective supplement to other navigation methods.

[0076] Another embodiment of this application provides a storage medium storing a computer program, which, when executed by a processor, implements the following method steps: Step 1: In response to receiving that the underwater robot has traveled to the target depth, preprocess the side-scan sonar image of the underwater robot to obtain a preprocessed image; Step 2: Based on the image features of the preprocessed image, perform feature matching with the reference images in the reference image database to obtain relative pose constraints; Step 3: Based on the relative pose constraints, current inertial data, current velocity data, and current depth data, the target fusion optimization method is used to optimize the operating state of the underwater robot to obtain the optimized relative pose; Step 4: Calculate the motion trajectory of the underwater robot based on the optimized relative pose to obtain the navigation trajectory of the underwater robot.

[0077] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0078] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is used as an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above.

[0079] The specific implementation process of the above method steps can be found in any of the above embodiments of underwater robot navigation methods based on side-scan image matching, and will not be repeated here.

[0080] This application achieves stable relative pose observations by extracting and matching features between adjacent images and existing images in a database, and then fuses these with IMU and depth information, significantly reducing the cumulative drift error of pure inertial navigation. The side-scan sonar used is conventional equipment with a simple hardware structure and low power consumption, suitable for various specifications and platforms of underwater robots, demonstrating good economy and applicability. During navigation, the current image can be matched with images from adjacent time points to form local continuity constraints, or it can be searched and matched with candidate images in the aforementioned two types of image databases. If the similarity of the matching result exceeds a threshold and passes geometric consistency verification, the system determines that the robot has returned to the visited area, thereby triggering relocalization. Loop closure constraints are added to the global trajectory optimization to correct cumulative errors and improve global consistency. The high resolution and rich texture of the side-scan images provide a reliable reference for navigation. Accurate estimation of position and attitude is achieved through optimized filtering algorithms, which can be directly used in task scenarios lacking external base stations or absolute velocity measurements, and can also serve as an effective supplement to other navigation methods.

[0081] Another embodiment of this application provides an electronic device, which can be a server. The electronic device includes a processor, a memory, a network interface, and a database connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile and / or volatile storage media and internal memory. The non-volatile storage media stores an operating system, computer programs, and a database. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The network interface is used to communicate with external clients via a network connection. When the program is executed by the processor, it implements the functions or steps of a server-side underwater robot navigation method based on side-scan image matching.

[0082] In one embodiment, an electronic device is provided, which can be a client. The electronic device includes a processor, memory, a network interface, a display screen, and an input device connected via a system bus. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores an operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The network interface is used to communicate with an external server via a network connection. When the program of the electronic device is executed by the processor, it implements the functions or steps of a client-side underwater robot navigation method based on side-scan image matching.

[0083] Another embodiment of this application provides an electronic device, including at least a memory and a processor. The memory stores a computer program, and the processor, when executing the computer program in the memory, performs the following method steps: Step 1: In response to receiving that the underwater robot has traveled to the target depth, preprocess the side-scan sonar image of the underwater robot to obtain a preprocessed image; Step 2: Based on the image features of the preprocessed image, perform feature matching with the reference images in the reference image database to obtain relative pose constraints; Step 3: Based on the relative pose constraints, current inertial data, current velocity data, and current depth data, the target fusion optimization method is used to optimize the operating state of the underwater robot to obtain the optimized relative pose; Step 4: Calculate the motion trajectory of the underwater robot based on the optimized relative pose to obtain the navigation trajectory of the underwater robot.

[0084] The specific implementation process of the above method steps can be found in any of the above embodiments of underwater robot navigation methods based on side-scan image matching, and will not be repeated here.

[0085] This application achieves stable relative pose observations by extracting and matching features between adjacent images and existing images in a database, and then fuses these with IMU and depth information, significantly reducing the cumulative drift error of pure inertial navigation. The side-scan sonar used is conventional equipment with a simple hardware structure and low power consumption, suitable for various specifications and platforms of underwater robots, demonstrating good economy and applicability. During navigation, the current image can be matched with images from adjacent time points to form local continuity constraints, or it can be searched and matched with candidate images in the aforementioned two types of image databases. If the similarity of the matching result exceeds a threshold and passes geometric consistency verification, the system determines that the robot has returned to the visited area, thereby triggering relocalization. Loop closure constraints are added to the global trajectory optimization to correct cumulative errors and improve global consistency. The high resolution and rich texture of the side-scan images provide a reliable reference for navigation. Accurate estimation of position and attitude is achieved through optimized filtering algorithms, which can be directly used in task scenarios lacking external base stations or absolute velocity measurements, and can also serve as an effective supplement to other navigation methods.

[0086] The above embodiments are merely exemplary embodiments of this application and are not intended to limit this application. Those skilled in the art can make various modifications or equivalent substitutions to this application within the scope and nature of this application, and such modifications or equivalent substitutions should also be considered to fall within the scope of protection of this application.

Claims

1. A navigation method for underwater robots based on side-scan image matching, characterized in that, include: In response to receiving a message that the underwater robot has traveled to the target depth, the side-scan sonar image of the underwater robot is preprocessed to obtain a preprocessed image; Based on the image features of the preprocessed image, feature matching is performed with reference images in the reference image database to obtain relative pose constraints; Based on the relative pose constraints, current inertial data, current velocity data, and current depth data, a target fusion optimization method is used to optimize the operating state of the underwater robot to obtain an optimized relative pose. The underwater robot's motion trajectory is calculated based on the optimized relative pose to obtain the underwater robot's navigation trajectory.

2. The method as described in claim 1, characterized in that, The preprocessing of the side-scan sonar images of the underwater robot to obtain preprocessed images specifically includes: Radiometric correction is performed on the side-scan sonar image of the underwater robot to obtain a radiometrically corrected image; The radiometrically corrected image is geometrically corrected to obtain the preprocessed image.

3. The method as described in claim 2, characterized in that, The step of performing radiometric correction on the side-scan sonar image of the underwater robot to obtain a radiometrically corrected image specifically includes: The distance-dependent intensity distortion of the side-scan sonar image is corrected using a time-varying gain compensation method to obtain a first corrected image; The intensity spatial distribution of the first corrected image is corrected using a beammap correction method to obtain a second corrected image; The noise signal of the second corrected image is filtered out using a seabed image defect masking method to obtain the radiometric corrected image.

4. The method as described in claim 2, characterized in that, The step of performing geometric correction on the radiometrically corrected image to obtain the preprocessed image specifically includes: The radiometrically corrected image is geometrically corrected using a strip geometric correction method to obtain a geometrically corrected image. The geometrically corrected image is post-processed using an intensity normalization method to obtain the pre-processed image.

5. The method as described in claim 1, characterized in that, The step of performing feature matching between the image features of the preprocessed image and reference images in the reference image database to obtain relative pose constraints specifically includes: Based on the ground coordinate mapping information corresponding to the preprocessed image, the overlapping interval between any two preprocessed images is determined to be located in the first frame overlapping interval of the first preprocessed image and in the second frame overlapping interval of the second preprocessed image. The target navigation condition index is determined based on the first frame overlap interval and the second frame overlap interval. The target navigation condition index includes any one of the following: inter-frame heading change, trajectory curvature, and geometric offset of the overlap interval. Based on the target navigation condition indicators, determine the adaptive image feature matching parameters; Based on the adaptive image feature matching parameters, feature matching is performed on reference images in the reference image database to obtain key matching pairs; The geometric consistency between the key matching pairs is calculated using a random sampling consensus algorithm. Key matching pairs that do not meet any geometric model constraint are filtered out to obtain the relative pose constraint. The geometric model constraints include two-dimensional rigid body model constraints, two-dimensional similarity model constraints, and two-dimensional affine model constraints.

6. The method as described in claim 1, characterized in that, The underwater robot's operational state is optimized using a target fusion optimization method based on the relative pose constraints, current inertial data, current velocity data, and current depth data to obtain an optimized relative pose. Specifically, this includes: Based on the relative pose constraints, current inertial data, current velocity data, and current depth data, an objective function is constructed to optimize the operating state of the underwater robot with the goal of minimizing inertial odometry error and image matching error. The objective function is iteratively solved using a factor graph optimization algorithm to obtain the optimized relative pose.

7. The method as described in claim 6, characterized in that, The step of iteratively solving the objective function using a factor graph optimization algorithm to obtain the optimized relative pose specifically includes: Step 1: Initialize the algorithm parameters of the factor graph optimization algorithm and construct a factor graph with pose state variables as vertices and pose constraints as edges according to the objective function. Step 2: Calculate the pose state estimate at the current moment based on the current inertial data, the current velocity data, and the current depth data; Step 3: Calculate the error vector for each error factor based on the pose state estimation at the current moment; Step 4: Calculate the Jacobian matrix of the error vector and its corresponding error factor; Step 5: Perform calculations based on the Jacobian matrix and Jacobian inverse matrix corresponding to different error factors to obtain the Hessian matrix and the right-hand apex contribution. Step 6: Based on the Hessian matrix and the right-hand apex contribution, the sparse Cholesky decomposition method is used to calculate the state gain; Step 7: Update the pose state of the factor graph at the current time based on the state gain to obtain the current relative pose; Step 8: When the state gain satisfies the preset increment norm criterion or the current iteration round is greater than or equal to the preset iteration number threshold, the current relative pose is determined as the optimized relative pose; when the state gain does not satisfy the preset increment norm criterion or the current iteration round is less than the preset iteration number threshold, the observation state of the underwater robot in the factor graph is corrected based on the state gain, and steps 1 to 8 are repeated.

8. An underwater robot navigation device based on side-scan image matching, characterized in that, include: The preprocessing module is used to preprocess the side-scan sonar image of the underwater robot in response to receiving a message that the underwater robot has traveled to the target depth, so as to obtain a preprocessed image. The feature matching module is used to perform feature matching between the image features of the preprocessed image and reference images in the reference image database to obtain relative pose constraints; The optimization module is used to optimize the operating state of the underwater robot based on the relative pose constraints, current inertial data, current velocity data, and current depth data using a target fusion optimization method to obtain an optimized relative pose. The calculation module is used to calculate the motion trajectory of the underwater robot based on the optimized relative pose, so as to obtain the navigation trajectory of the underwater robot.

9. A storage medium, characterized in that, The storage medium stores a computer program, which, when executed by a processor, implements the steps of the underwater robot navigation method based on side-scan image matching as described in any one of claims 1-7.

10. An electronic device, characterized in that, It includes at least a memory and a processor, wherein the memory stores a computer program, and the processor, when executing the computer program in the memory, implements the steps of the underwater robot navigation method based on side-scan image matching as described in any one of claims 1-7.

Images