Underwater robot real-time three-dimensional map reconstruction system and method fused with SLAM algorithm

Abstract

The invention discloses an underwater robot real-time three-dimensional map reconstruction system and method fused with an SLAM algorithm, and belongs to the technical field of three-dimensional map reconstruction. By constructing a confidence coefficient system of three-dimensional points, a spatial trajectory fluctuation index and an inter-frame texture change rate are introduced as dynamic feature evaluation indexes; and a regression model is adopted to score and grade the severity of the false points, and differential processing of different grades of point clouds is realized in combination with a dynamic suppression mechanism, so that an unstable structure is effectively eliminated, and the stability and reliability of the sparse three-dimensional map are remarkably improved. Therefore, the positioning precision, the path planning capability and the task execution safety of the underwater robot in a complex environment are enhanced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of three-dimensional map reconstruction, in particular to a real-time three-dimensional map reconstruction system and method for underwater robots based on SLAM algorithm fusion. BACKGROUND

[0002] Real-time three-dimensional map reconstruction for underwater robots refers to using sensor data (such as sonar, camera or laser radar, etc.) collected by underwater robots during movement to construct a three-dimensional map of the underwater environment in real time through computer vision and image processing technology. This technology enables robots to achieve autonomous navigation, obstacle avoidance and task execution in unknown or complex underwater environments, and is a key foundation for intelligent underwater operations.

[0003] The prior art has the following shortcomings: In underwater SLAM three-dimensional reconstruction, since the SLAM algorithm defaults to a static environment, when there are moving objects such as fish, water plants or dust in the water, these dynamic elements may be misidentified as stable feature points and involved in triangulation, thereby generating pseudo point clouds that drift over time. For example, near coral reefs, water plants sway due to ocean currents, but the system considers them as fixed structures, resulting in the appearance of unrealistic floating objects in the map. In addition, such misbuilt maps can seriously interfere with the positioning accuracy of the robot, path planning judgment, and even mislead the reinforcement learning algorithm to learn an incorrect environmental model, affecting the reliability and safety of overall task execution. SUMMARY

[0004] The purpose of the present application is to provide a real-time three-dimensional map reconstruction system and method for underwater robots based on SLAM algorithm fusion to solve the problems in the background art.

[0005] In order to achieve the above-mentioned purpose, the present application provides the following technical solution: a real-time three-dimensional map reconstruction method for underwater robots based on SLAM algorithm fusion, comprising: obtaining an underwater image sequence and pose information, extracting image feature points and performing inter-frame matching; triangulating the matched feature points according to the pose estimation results of the image frames to generate a sparse three-dimensional point cloud; For each point S in the three-dimensional point cloud, calculate the re-projection error, the angle of view, the number of observations and the distance reasonableness to obtain an initial confidence value; Further obtain the spatial trajectory volatility index STV for point S, which is used to measure the volatility degree of the three-dimensional position of point S in consecutive frames, and the inter-frame texture change rate TVD, which is used to evaluate the texture consistency change of the image block corresponding to point S in multiple frames; Based on the STV and TVD parameters, evaluate the severity of the drift pseudo point cloud of point S, and divide the point cloud into multiple levels; According to the level, the point cloud is dynamically suppressed, wherein the point cloud of the trusted level participates in map optimization and loop detection; the point cloud of the suspicious level has a reduced weight and only participates in local optimization; and the point cloud of the serious level is directly removed. A stable sparse three-dimensional map processed according to the confidence and the level is output.

[0006] Preferably, the image feature points are extracted and inter-frame matching is performed, including: The RGB image is converted into a grayscale image to enhance edge features; an image enhancement and denoising method is used to process low-light or turbid images; an ORB algorithm is used to extract feature points and calculate descriptors; a Hamming distance is used for brute-force matching, and high-quality matching points are screened by combining bidirectional consistency and distance ratio tests; and a RANSAC method is used to remove outliers, and only matching pairs satisfying a geometric model are reserved as subsequent triangulation inputs.

[0007] Preferably, a projection matrix is constructed for two image frames. ; wherein: represents that the first frame is the starting point of the world coordinate system; represents the relative pose of the second frame; high-confidence matching point pairs screened by RANSAC are extracted: points on frame 1; points on frame 2; a linear equation set is constructed: ; wherein A is a matrix containing and , X is the homogeneous three-dimensional coordinates of the points; SVD is used to decompose the matrix A, and the vector corresponding to the smallest singular value is obtained as the three-dimensional point X, and is standardized to ; T is the matrix transpose; X / W represents the real space coordinates of the point in the x direction; Y / W represents the real space coordinates of the point in the y direction; and Z / W represents the real space coordinates of the point in the z direction; the obtained three-dimensional points are subjected to positive depth checking and reprojection error verification, and unqualified points are discarded.

[0008] Preferably, the three-dimensional point S is projected back to the observation image frame, and the difference is compared with the actual image point x; for each observation frame, the projection error is: ; wherein: is the projection matrix of the camera used to project the point S to the image plane; represents the observed pixel coordinates of the point in the image frame i; the angle of view represents the size of the light ray angle when two cameras observe the point S, and is used to measure the geometric accuracy of triangulation, given the positions of the centers of the two cameras and , the angle is: ; The observation times represent the image frame numbers in which the three-dimensional point is observed; Calculating the rationality score ; wherein, represents the rationality distance center; ; For controlling the tolerance width; represents the Euclidean distance from the three-dimensional point S to the current camera center; After the obtained re-projection error, the view angle included angle, the observation times and the distance rationality score are processed by dimensionless normalization, the weighted average summation calculation is performed to obtain the initial confidence value.

[0009] Preferably, the method for obtaining the spatial trajectory fluctuation index STV is as follows: Suppose the observation positions of the three-dimensional point S in the continuous N frames are: ; R is a real set; each column corresponds to the three-dimensional coordinates St=(xt,yt,zt) of the t-th frame; represents the three-dimensional coordinates of the point S in the t-th frame; N is the frame number of the observation window; Calculate the first-order difference of the three-dimensional position: ; Then, it is unfolded as a vector according to the column: ; construct a sparse optimization target, calculate the spatial trajectory fluctuation index STV, and the expression is: ; wherein, represents the component value of the three-dimensional coordinate change amount of the point S between the continuous frames.

[0010] Preferably, the method for obtaining the inter-frame texture variation rate TVD is as follows: obtaining an image block sequence in the continuous image frames, inputting the image frame sequence in which the three-dimensional point S is observed ; t represents the total frame number; for each frame Project the point S into the pixel coordinates ; extract a fixed-size image block at the position ; suppose that the first frame is the reference frame, and the image block is ; for the image block of each frame , compare with the reference block Calculate the similarity value SSIM, and establish the corresponding data set, calculate the variance of the SSIM sequence as the inter-frame texture variation rate TVD.

[0011] Preferably, based on the STV and TVD parameters, the drift false point cloud severity of the point S is evaluated, and the point cloud is divided into multiple levels, which specifically includes: The spatial trajectory fluctuation index and the inter-frame texture change rate are converted into a comprehensive feature vector, the comprehensive feature vector is taken as an input of a machine learning model, the machine learning model takes a drift pseudo point cloud severity score value label of a point S predicted by each set of comprehensive feature vectors as a prediction target, minimizes a sum of prediction errors of drift pseudo point cloud severity score value labels of all points S as a training target, and is trained until the sum of prediction errors converges, and the model training is stopped. The drift pseudo point cloud severity score value of the point S is determined according to an output result of the model, wherein the machine learning model is a polynomial regression model.

[0012] Preferably, the obtained drift pseudo point cloud severity score value of the point S is compared with a gradient threshold value, the gradient threshold value includes a first threshold value and a second threshold value, and the first threshold value is less than the second threshold value; the dispatching over-frequency index is compared with the first threshold value and the second threshold value, respectively; If the dispatching over-frequency index is greater than the second threshold value, the drift pseudo point cloud severity of the point S is classified as a severe level; If the dispatching over-frequency index is greater than or equal to the first threshold value and less than or equal to the second threshold value, the drift pseudo point cloud severity of the point S is classified as a suspicious level; If the dispatching over-frequency index is less than the first threshold value, the drift pseudo point cloud severity of the point S is classified as a trusted level.

[0013] Preferably, for the points S in the suspicious level, that is, the severity score value is greater than the first threshold value and less than the second threshold value, the point cloud optimization weight adjustment includes: defining an optimization graph residual weight of the point S as: ; in the formula, , wherein, represents a residual term weight coefficient of the point S in an optimizer; γ represents a maximum weight attenuation ratio; defining a local optimization window, and setting a local key frame set as: ; represents a last observed key frame timestamp of the point S, and Δt is a local window size; represents a key frame with a timestamp t; represents a selected key frame sequence in a SLAM system; represents a last observed key frame timestamp of the point S; represents a key frame set in which the point S is observed, and the timestamp is close to , and the frame nodes constitute a local optimization subgraph; In the local optimization, only the frames associated with the point S are added to the optimization graph; the point S only appears in the local subgraph and is not added to the global map graph structure; a local optimization graph structure is constructed, and satisfies: ; and in the optimization process, Minimize the residual error; denotes a set of nodes in the local optimization graph, including point S and all key frames observed to it; denotes a set of edges in the optimization graph, i.e. observation constraints between point S and key frames; denotes the observation relationship between point S and frame , i.e. point S is observed on frame and forms a residual edge.

[0014] The application also provides an underwater robot real-time three-dimensional map reconstruction system integrating a SLAM algorithm, comprising a visual perception module, a sparse mapping module, a three-dimensional point credibility evaluation module, a dynamic feature identification module, a pseudo point cloud detection module, an optimization control module, and a map output module; Visual perception module: obtain underwater image sequences and pose information, extract image feature points and perform inter-frame matching; Sparse mapping module: triangulate matched feature points according to pose estimation results of image frames to generate a sparse three-dimensional point cloud; Three-dimensional point credibility evaluation module: for each point S in the three-dimensional point cloud, calculate re-projection error, viewing angle, number of observations, and distance reasonableness to obtain an initial credibility value; Dynamic feature identification module: further obtain a spatial trajectory volatility index STV for point S, which is used to measure the volatility degree of the three-dimensional position of point S in consecutive frames, and a frame-to-frame texture change rate TVD, which is used to evaluate the texture consistency change of the image block corresponding to point S in multiple frames; Pseudo point cloud detection module: based on the STV and TVD parameters, evaluate the severity of the drift pseudo point cloud of point S, and divide the point cloud into multiple levels; Optimization control module: dynamically suppress the point cloud according to the levels, wherein the credible level point cloud participates in map optimization and loop detection; the suspicious level point cloud has reduced weight and only participates in local optimization; the severe level point cloud is directly excluded; Map output module: output a stable sparse three-dimensional map processed according to the credibility and levels.

[0015] In the above technical solution, the application provides technical effects and advantages: 1. The application introduces spatial trajectory fluctuation index and inter-frame texture change rate as dynamic feature recognition indicators, and innovatively constructs a pseudo-point cloud recognition and grading mechanism suitable for underwater SLAM systems, effectively solving the problem of dynamic interference (such as water grass, fish, dust) being misrecognized as static structure and generating drift pseudo points. Using STV and TVD to form a feature vector, combined with a polynomial regression model to score the severity of each three-dimensional point, and dividing the point cloud into levels of trust, suspicion, and severity, the point cloud dynamic suppression processing is realized, ensuring that the three-dimensional structure in the map has high stability and accuracy.

[0016] 2. The application further introduces a differentiated optimization strategy, which adjusts the participation range and residual weight of points in the optimization map according to the level of the point cloud. The points of severe level are directly excluded, the points of suspicious level are limited to participate in the optimization within the local subgraph and the weight is downgraded, and the trusted points are fully involved in the global map optimization and loop detection. The final output three-dimensional map not only has clear structure, low noise and good robustness to dynamic interference, but also has additional information such as confidence label and optimization participation flag, which can be widely used in underwater robot autonomous navigation, target recognition, path planning and other tasks, greatly improving the precision, stability and environmental adaptability of task execution. BRIEF DESCRIPTION OF DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed in the embodiments. Obviously, the drawings described below are only some embodiments described in the present application, and other drawings can also be obtained by those skilled in the art based on these drawings.

[0018] Figure 1 The method mind map of the present application.

[0019] Figure 2 The system module mind map of the present application. DETAILED DESCRIPTION

[0020] In order to make the purpose, technical scheme and advantages of the embodiments of the present application more clear, the technical scheme in the embodiments of the present application will be described clearly and completely in the following with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor belong to the scope of protection of the present application.

[0021] Embodiment 1, please refer to Figure 1 The underwater robot real-time three-dimensional map reconstruction method of the fusion SLAM algorithm described in this embodiment includes: Obtain underwater image sequence and pose information, extract image feature points and perform inter-frame matching; Triangulate the matching feature points according to the pose estimation results of the image frames to generate a sparse three-dimensional point cloud; For each point S in the three-dimensional point cloud, calculate the re-projection error, the angle of view, the number of observations, and the distance rationality to obtain an initial confidence value; Further obtain the spatial trajectory fluctuation index STV for point S, which is used to measure the fluctuation degree of the three-dimensional position of point S in consecutive frames, and the inter-frame texture change rate TVD, which is used to evaluate the texture consistency change of the image block corresponding to point S in multiple frames; Based on the STV and TVD parameters, evaluate the severity of the drift false point cloud of point S, and divide the point cloud into multiple levels; According to the levels, perform dynamic suppression processing on the point cloud, wherein the point cloud of the trusted level participates in map optimization and loop detection; the point cloud of the suspicious level reduces the weight and only participates in local optimization; the point cloud of the severe level is directly removed; Output the stable sparse three-dimensional map after confidence and level processing.

[0022] Obtain underwater image sequence and pose information, underwater image acquisition includes: carrying visual sensor (such as RGB monocular, binocular camera, or low-light underwater industrial camera). Set the sampling frequency (such as 10-30 frames / second) to obtain continuous frame image sequence. Use hardware or software to remove lens distortion (correct based on camera calibration parameters). Pose information acquisition includes: carrying an inertial measurement unit (IMU) to obtain linear acceleration and angular velocity in real time. Fuse IMU data into the visual system (such as using VIO or EKF method) to calculate the initial camera pose. If fused with sonar or DVL, higher precision motion estimation can be obtained. In turbid water or low light environment, infrared, low light imaging or image enhancement module needs to be used.

[0023] Preprocess the obtained images, convert the RGB images to grayscale images, reduce the computational complexity and enhance the edge saliency. Enhance the details of low-contrast images and improve the feature clarity in dark areas. Remove image noise caused by seawater suspended matter and retain edge features. Strengthen the edge area and improve the stability of feature points.

[0024] Select a robust feature extraction algorithm suitable for underwater environment, for example: ORB (Oriented FAST and Rotated BRIEF), advantages: fast speed, strong rotation invariance, suitable for real-time system. Use FAST algorithm to detect corner points. Calculate the direction for each corner point. Use BRIEF descriptor plus direction encoding.

[0025] Generate Descriptor for each extracted feature point for later matching: ORB: generate 256-bit binary descriptor. SIFT: generate 128-dimension float vector descriptor.

[0026] Inter-frame feature matching includes: Brute Force Matcher: suitable for ORB descriptor, use Hamming distance. Match all feature descriptors of current frame and previous frame, select minimum distance matching pair. FLANN matcher (Fast Library for Approximate Nearest Neighbors): suitable for float type descriptors such as SIFT. Build KD-Tree index to improve matching efficiency. Only keep consistent matching in both A->B and B->A, remove false matches. Keep matching with ratio of nearest neighbor distance to second nearest neighbor distance below a certain threshold (e.g. 0.75) to enhance stability. Input matching point pairs, iteratively sample randomly and solve geometric transformation. Remove "outliers" that do not conform to the transformation model. Only keep "inliers" that pass RANSAC verification as input for subsequent triangulation.

[0027] Output results include: high-quality feature matching set between image frame pairs (including position and descriptor); preliminary pose information for each frame (such as rotation matrix, translation vector); matching score / confidence value for each pair of matching points (can be used for subsequent weighted triangulation).

[0028] According to the pose transformation between adjacent image frames, perform geometric triangulation calculation on the matched feature point pairs to recover their three-dimensional space coordinates and form a sparse point cloud.

[0029] Obtain the intrinsic matrix K of the camera through pre-calibration, which contains the focal length and principal point coordinates. Use feature matching results to calculate the relative pose (rotation matrix R, translation vector t) between cameras: monocular: obtain by solving the essential matrix (E) and decomposing. Binocular or RGB-D: can directly use depth or known extrinsic parameters to calculate relative pose.

[0030] Construct the projection matrix for two image frames (denoted as frame 1 and frame 2): ; where: represents the first frame as the origin of the world coordinate system; represents the relative pose of the second frame.

[0031] Extract high-confidence matching point pairs filtered by RANSAC: points in frame 1 ; points in frame 2 ; Construct linear equations: ; where Xi represents the i-th row of the projection matrix, and X is the homogeneous coordinates of the unknown 3D point.

[0032] The matrix A is decomposed using SVD, and the vector corresponding to the smallest singular value is obtained as the 3D point X, expressed as: ; T is the matrix transpose; (X / W, Y / W, Z / W) represents the normalization of the homogeneous coordinates to the Cartesian coordinates (regular 3D coordinates). X / W represents the real space coordinates of the point in the x direction. Y / W represents the real space coordinates of the point in the y direction. Z / W represents the real space coordinates of the point in the z direction.

[0033] Positive depth check (Cheirality Check): Verify whether the 3D point is in front of the two cameras (i.e. depth > 0); if a point pair does not satisfy the positive depth condition, discard the triangulation result.

[0034] Reprojection error calculation: Project the 3D point back to the original image frame and calculate the pixel error with the original matching point. Discard points with error greater than the threshold (such as 2 pixels).

[0035] If the current 3D point is recovered from the inter-frame coordinate system, it needs to be uniformly converted to the global map coordinate system (using the cumulative pose transformation chain), and all valid 3D points are added to the sparse point cloud set.

[0036] Optional point cloud filtering and compression, including the following filtering operations that can be applied to improve the quality of the point cloud: Voxel Grid filtering: compresses dense areas; Statistical Outlier Removal: removes abnormal floating points; redundant point merging based on descriptor clustering.

[0037] Output a sparse, aligned, and denoised 3D point cloud set corresponding to the identifiable static structural features in the underwater environment.

[0038] For each triangulation-generated 3D point S=(X, Y, Z), evaluate its reliability from the geometric and observation angles, and output the normalized confidence value CS.

[0039] Project the 3D point S back to its observed image frame and compare the difference with the actual image point x. For each observed frame, the projection error is: ; where: is the projection matrix using the camera Project the point S to the image plane. Xi represents the observed pixel coordinates of the point in the image frame i.

[0040] The angle of view represents the angle of the light ray when two cameras observe point S, and is used to measure the geometric accuracy of triangulation. Given the positions of the centers of two cameras and , the angle is: ; the number of observations represents how many frames of images the three-dimensional point is observed by.

[0041] The distance reasonableness represents whether the depth value of the detection point S is in a reasonable range, to prevent extremely far points or floating points from participating in the map. The reasonable distance range is set as , such as [0.5m, 20m]; the reasonableness score is calculated as ; in the formula, represents the center of the reasonableness distance; ; , which is the control tolerance width (such as 5m); represents the Euclidean distance from the three-dimensional point S to the center of the current camera.

[0042] After the obtained re-projection error, angle of view, number of observations, and distance reasonableness score are dimensionless and normalized, they are weighted and averaged to obtain the initial confidence value.

[0043] The method for obtaining the spatial trajectory fluctuation index STV is: Let the observation positions of the three-dimensional point S in consecutive N frames be: ; R is a real set; each column corresponds to the three-dimensional coordinates St=(xt,yt,zt) of the t-th frame; represents the three-dimensional coordinates of point S in the t-th frame; N is the number of observation window frames (recommended 5-10 frames); Calculate the first-order difference of the three-dimensional position (i.e., the inter-frame motion change): ; then, it is expanded into a vector according to the column: ; Sparse modeling (LASSO constraint) is performed, and the sparse hypothesis is that if the point trajectory is stable, most of the values in the difference vector should be close to 0; if it is not stable, most of the values are non-zero jumps.

[0044] The sparse optimization target (not to solve the optimization, only to measure the sparsity) is constructed, and the spatial trajectory fluctuation index STV is calculated, and the expression is: ; in the formula, represents the component value of the three-dimensional coordinate change of point S between consecutive frames. Specifically, it is the i-th element after expanding the position difference between each frame into a one-dimensional vector.

[0045] The method for obtaining the inter-frame texture change rate TVD is: obtaining an image block sequence in consecutive image frames, inputting the image frame sequence in which the three-dimensional point S is observed ; t represents the total number of frames; the camera intrinsic parameters + pose → can project the three-dimensional point S onto each frame of image coordinates; for each frame projecting the point S into pixel coordinates ; extracting a fixed-size image block at this position , for example 15x15 or 21x21 pixels; Let the first frame be the reference frame, and let the image block be ; for each frame of the image block , calculate the similarity value SSIM with the reference block , and establish the corresponding data set, calculate the variance of the SSIM sequence as the inter-frame texture change rate TVD.

[0046] Based on the STV and TVD parameters, the drift false point cloud severity of the point S is evaluated, and the point cloud is divided into multiple levels, which specifically includes: The spatial trajectory fluctuation index and the inter-frame texture change rate are converted into a comprehensive feature vector, the comprehensive feature vector is taken as an input of a machine learning model, the machine learning model takes a drift false point cloud severity score value label of the point S as a prediction target, a sum of prediction errors of the drift false point cloud severity score value labels of all points S is minimized as a training target, the machine learning model is trained until the sum of prediction errors converges, and the model training is stopped, and a drift false point cloud severity score value of the point S is determined according to a model output result, wherein the machine learning model is a polynomial regression model.

[0047] The obtained drift false point cloud severity score value of the point S is compared with a gradient threshold value, the gradient threshold value includes a first threshold value and a second threshold value, and the first threshold value is less than the second threshold value, and the overscheduling index is compared with the first threshold value and the second threshold value respectively; If the overscheduling index is greater than the second threshold value, the drift false point cloud severity of the point S is divided into a serious level; If the overscheduling index is greater than or equal to the first threshold value and less than or equal to the second threshold value, the drift false point cloud severity of the point S is divided into a suspicious level; If the overscheduling index is less than the first threshold value, the drift false point cloud severity of the point S is divided into a trusted level.

[0048] The trusted level point cloud processing includes: normally joining the map point set in the SLAM backend optimization. It can be used for key frame matching and repositioning in loop closure recognition. Add a complete edge (full weight) between the optimized map and the camera node.

[0049] The serious level point cloud processing includes: the point is directly excluded and does not participate in map construction or pose estimation. It can be added to the false point cloud buffer area (for abnormal analysis, visualization or learning feedback). It is removed from the map point container; or it is marked as "invalid" and skipped in the downstream processing flow.

[0050] The suspicious level point cloud processing includes: For points in suspicious level, i.e. severity score value Between the first threshold and the second threshold , the three-dimensional point S is not completely rejected, but is suppressed by: reducing the residual weight in the optimization graph; limiting its participation in the optimization of the space / time range (localization), to suppress the error propagation caused by potential dynamic false points, and to improve the stability of the SLAM system.

[0051] The point cloud optimization weight adjustment includes: defining the optimization graph residual weight (information matrix scaling factor) of point S as: ; In the formula, , represents the residual term weight coefficient of point S in the optimizer (for information matrix scaling); γ represents the maximum weight attenuation ratio (such as 0.8, which means that the minimum weight is 0.2).

[0052] It can be used in a graph optimizer (such as g2o, Ceres) to set or scale the information matrix of the edge residual term of point S and the camera frame.

[0053] Limit the suspicious level point S to participate in the optimization process only within the local key frame window, and not participate in the global map optimization and loop repositioning optimization.

[0054] Define a local optimization window, and set the local key frame set as: ; , represents the last observed key frame timestamp of point S, and Δt is the local window size (such as ±5 frames); , represents the key frame with timestamp t; , represents the selected key frame sequence (sparse frames, representing the global map) in the SLAM system; , represents the last observed key frame timestamp of point S; , represents the key frame set in which point S is observed, and the timestamp is close to , which constitutes the frame nodes of the local optimization subgraph.

[0055] In local optimization, only the frames associated with point S are added to the optimization graph; point S only appears in the local subgraph and is not added to the global map graph structure; the point is not introduced in loop detection or global pose graph optimization.

[0056] Construct a local optimization graph structure , which satisfies: ; and minimize the residual in . , represents the node set in the local optimization graph, including point S and all key frames observed by it; , represents the edge set in the optimization graph, i.e.: the observation constraint (such as the re-projection error term) between point S and the key frame; represents the observed relationship between point S and frame , i.e. the point is observed on the frame and forms a residual edge.

[0057] In the process of real-time three-dimensional map reconstruction of underwater robots using the fusion SLAM algorithm, after feature point extraction, triangulation mapping, confidence evaluation, and dynamic point classification, the system will enter the stable output stage of the sparse three-dimensional map. The core goal of this stage is to output a high-reliability sparse point cloud map, which only retains three-dimensional points that have been verified or controlled, and eliminates false point clouds caused by dynamic interference, drift error, or unstable structure, thereby ensuring the practicality and robustness of the map in subsequent navigation, positioning, and modeling tasks.

[0058] Firstly, each three-dimensional point has been attached with two key evaluation indicators: spatial trajectory fluctuation index (STV) and inter-frame texture variation rate (TVD), and its drift false point cloud severity score is generated through a polynomial regression model. According to the score results, the point cloud is divided into "trusted level", "suspicious level", and "serious level". In this output stage, the system executes differentiated processing strategies according to the level of the points: trusted level point cloud is retained in its entirety, participating in global map optimization, loop detection, and key frame graph construction; suspicious level point cloud only participates in local map optimization within its local time window, and its optimization residual is processed with reduced weight in the graph optimizer to control its influence range; serious level point cloud is eliminated and does not participate in any form of map construction and is not used for pose estimation or matching.

[0059] After the above screening and optimization, the sparse three-dimensional map generated by the system is composed of stable and reliable three-dimensional points. Each output point contains not only its three-dimensional coordinates, but also confidence score, point cloud level label, optimization participation flag, observation frequency, and other additional attributes. These information not only supports map quality evaluation, but also provides a structured data foundation for subsequent dense mapping, path planning, and semantic recognition tasks.

[0060] The final output map can be saved in the standard point cloud data format, or directly integrated into the map database of the SLAM system, supporting incremental updating and online maintenance. The map has advantages in clear structure, resistance to dynamic interference, and controllable precision, and is an important information basis for realizing complex underwater tasks.

[0061] Embodiment 2, please refer to Figure 2 The underwater robot real-time three-dimensional map reconstruction system using the fusion SLAM algorithm described in this embodiment includes a visual perception module, a sparse mapping module, a three-dimensional point confidence evaluation module, a dynamic feature recognition module, a false point cloud detection module, an optimization control module, and a map output module. Visual perception module: obtain underwater image sequence and pose information, extract image feature points and perform inter-frame matching; Sparse mapping module: triangulate the matching feature points according to the pose estimation results of the image frames to generate a sparse three-dimensional point cloud; Three-dimensional point credibility evaluation module: for each point S in the three-dimensional point cloud, calculate the re-projection error, the angle of view, the number of observations, and the distance rationality to obtain an initial confidence value; Dynamic feature recognition module: for point S, further obtain a spatial trajectory fluctuation index STV for measuring the fluctuation degree of the three-dimensional position of point S in consecutive frames, and a frame-to-frame texture change rate TVD for evaluating the texture consistency change of the image block corresponding to point S in multiple frames; Pseudo-point cloud detection module: based on the STV and TVD parameters, evaluate the drift pseudo-point cloud severity of point S, and divide the point cloud into multiple levels; Optimization control module: according to the levels, perform dynamic suppression processing on the point cloud, wherein the credible level point cloud participates in map optimization and loop detection; the suspicious level point cloud has a reduced weight and only participates in local optimization; the severe level point cloud is directly excluded; Map output module: output a stable sparse three-dimensional map processed according to the confidence and levels.

[0062] The above formulas are all dimensionless numerical calculations, and the formulas are obtained by software simulation of a large amount of data to obtain a formula closest to the actual situation. The preset parameters in the formula are set by a person skilled in the art according to the actual situation.

[0063] It should be understood that the term "and / or" herein merely describes an association relationship of associated objects, and indicates that there can be three relationships, for example, A and / or B can represent three cases of A alone, A and B together, and B alone, where A and B can be singular or plural. In addition, the character " / " herein generally represents an "or" relationship between the front and rear associated objects, but can also represent an "and / or" relationship. The specific meaning can be understood according to the context before and after.

[0064] Those skilled in the art can appreciate that the units and algorithm steps of the examples described in combination with the embodiments disclosed herein can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether the functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. A person skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present application.

[0065] The above merely provides the specific implementation of the present application, but the protection scope of the present application is not limited to this. Any person skilled in the art can easily think of the changes or replacements within the technical range disclosed by the present application, which should be covered in the protection scope of the present application.

Claims

1. A real-time 3D map reconstruction method for underwater robots fusing SLAM algorithms, characterized in that: The method comprises the following steps: Obtain underwater image sequences and pose information, extract image feature points and perform inter-frame matching; Triangulate the matching feature points according to the pose estimation results of the image frames to generate a sparse three-dimensional point cloud; For each point S in the three-dimensional point cloud, calculate the re-projection error, the angle of view, the number of observations and the distance rationality to obtain an initial confidence value; Further obtain the spatial trajectory fluctuation index STV of the point S, which is used to measure the fluctuation degree of the three-dimensional position of the point S in the continuous frames, and the inter-frame texture change rate TVD, which is used to evaluate the texture consistency change of the image block corresponding to the point S in multiple frames; Based on the STV and TVD parameters, evaluate the drift false point cloud severity of the point S, and divide the point cloud into multiple levels; According to the levels, perform dynamic suppression processing on the point cloud, wherein the point cloud of the trusted level participates in map optimization and loop detection; the point cloud of the suspicious level reduces the weight and only participates in local optimization; the point cloud of the serious level is directly removed; Output the stable sparse three-dimensional map after confidence and level processing.

2. The method of claim 1, wherein: The extraction of image feature points and inter-frame matching comprises the following steps: Convert the RGB image to a grayscale image to enhance the edge features; use image enhancement and denoising methods to process low-light or turbid images; use the ORB algorithm to extract feature points and calculate the descriptors; use Hamming distance for brute-force matching, combined with bidirectional consistency and distance ratio test to filter high-quality matching points; use the RANSAC method to remove outliers, and only keep the matching pairs that meet the geometric model as the input for subsequent triangulation.

3. The method of claim 1, wherein: Construct the projection matrix for two image frames: ; wherein: represents the first frame as the origin of the world coordinate system; represents the relative pose of the second frame; high-confidence matching point pairs filtered by RANSAC are extracted: points on frame 1 ; points on frame 2 ; construct a linear equation set: ; wherein A is a matrix containing and , X is the homogeneous three-dimensional coordinates of the points; use SVD to decompose the matrix A to obtain the vector corresponding to the smallest singular value as the three-dimensional point X, and normalize it to ; T is the matrix transpose; X / W represents the real space coordinates of the point in the x direction; Y / W represents the real space coordinates of the point in the y direction; Z / W represents the real space coordinates of the point in the z direction; perform positive depth checking and reprojection error verification on the obtained three-dimensional points, and discard unqualified points.

4. The method of claim 3, wherein: Project the three-dimensional point S back to its observed image frame and compare the difference to the actual image point x; for each observed frame, its projection error is: ; where: is the projection matrix using the camera Project the point S to the image plane; denotes the observed pixel coordinate of this point on image frame i; The angle of view represents the size of the angle of the light rays when two camera observation points S are observed, and is used to measure the geometric accuracy of triangulation. Given two camera center positions and , the angle is: ; The number of observations represents the number of image frames in which the three-dimensional point is observed; Computing the plausibility score ; where plausibility distance center represents the plausibility distance center; ; to control the tolerance width; represents the Euclidean distance of the three-dimensional point S to the current camera center; After the obtained re-projection error, angle of view, number of observations and distance rationality scores are dimensionless and normalized, they are weighted and averaged to obtain the initial confidence value.

5. The method of claim 1, wherein: The method for obtaining the spatial trajectory fluctuation index STV comprises the following steps: Let the observed positions of a three-dimensional point S in successive N frames be: ; R is the set of real numbers; each column corresponds to the three-dimensional coordinates of the t-th frame St= (xt, yt, Zt); St represents the three-dimensional coordinates of the point S in the t-th frame; N is the number of observation window frames; Compute the first difference of the three-dimensional position: ; then, expand it as a vector by column: ; A sparse optimization objective is constructed, and a spatial trajectory variation index STV is calculated, expressed as: ; wherein, represents a component value of the three-dimensional coordinate variation of the point S between consecutive frames.

6. The method of claim 5, wherein: The method for obtaining the inter-frame texture variation rate TVD is: obtaining an image block sequence in a continuous image frame, inputting a three-dimensional point S observed image frame sequence ; t represents the total number of frames; for each frame , the point S is projected into a pixel coordinate ; a fixed-size image block is extracted at the position ; the first frame is taken as a reference frame, and the image block is denoted as ; for each frame of the image block with the reference block calculating a similarity value SSIM and building a corresponding data set, calculating the variance of the SSIM sequence as the inter-frame texture variation rate TVD.

7. The method of claim 6, wherein: Based on the STV and TVD parameters, evaluate the drift false point cloud severity of the point S, and divide the point cloud into multiple levels, which specifically comprises: Convert the spatial trajectory fluctuation index and the inter-frame texture change rate into a comprehensive feature vector, use the comprehensive feature vector as the input of a machine learning model, use the machine learning model to predict the drift false point cloud severity score value label of the point S as the prediction target, minimize the sum of prediction errors of the drift false point cloud severity score value labels of all points S as the training target, train the machine learning model until the sum of prediction errors converges, and then stop the model training, and determine the drift false point cloud severity score value of the point S according to the model output result, wherein the machine learning model is a polynomial regression model.

8. The method of claim 7, wherein: Compare the obtained drift false point cloud severity score value of the point S with a gradient threshold value, the gradient threshold value comprises a first threshold value and a second threshold value, and the first threshold value is less than the second threshold value, and compare the dispatching over-frequency index with the first threshold value and the second threshold value respectively; If the dispatching over-frequency index is greater than the second threshold value, the drift false point cloud severity of the point S is divided into a serious level; If the dispatching over-frequency index is greater than or equal to the first threshold value and less than or equal to the second threshold value, the drift false point cloud severity of the point S is divided into a suspicious level. If the scheduling frequency index is less than the first threshold, the drift false point cloud severity of the point S is classified into a trust level.

9. The method of claim 8, wherein: For points in the suspicious class, i.e. severity score value between a first threshold and a second threshold , the point cloud optimization weight adjustment comprises: defining the optimization graph residual weight of point S as: ; in which, represents the residual term weight coefficient of point S in the optimizer; γ represents the maximum weight decay ratio; Define a local optimization window, let the local keyframe set be: ; denote the last observed keyframe timestamp of point S, and Δt is the local window size; denote the keyframe with timestamp t; denote the selected keyframe sequence in the SLAM system; denote the last observed keyframe timestamp of point S; denote the keyframe set in which point S is observed, and the timestamps are close to , the frame nodes of the local optimization subgraph; In local optimization, only the frames associated with point S are added to the optimization graph; point S only appears in the local sub-graph and is not added to the global map graph structure; a local optimization graph structure is constructed , satisfying: ; and minimizing the residual in ; represents the node set in the local optimization graph, including point S and all key frames observed to it; represents the edge set in the optimization graph, i.e. the observation constraint between point S and the key frame; represents the observation relationship between point S and frame , i.e. point S is observed on the frame and forms a residual edge.

10. A real-time 3D mapping system for an underwater vehicle implementing the method of any one of claims 1 to 9, characterized in that: The system comprises a visual perception module, a sparse mapping module, a three-dimensional point confidence evaluation module, a dynamic feature recognition module, a false point cloud detection module, an optimization control module, and a map output module. The visual perception module acquires underwater image sequences and pose information, extracts image feature points, and performs inter-frame matching. The sparse mapping module triangulates the matched feature points based on the pose estimation results of the image frames to generate a sparse three-dimensional point cloud. The three-dimensional point confidence evaluation module calculates the re-projection error, the viewing angle, the number of observations, and the distance rationality for each point S in the three-dimensional point cloud to obtain an initial confidence value. The dynamic feature recognition module further acquires the spatial trajectory fluctuation index STV for the point S to measure the fluctuation degree of the three-dimensional position of the point S in consecutive frames, and the inter-frame texture change rate TVD to evaluate the texture consistency change of the image block corresponding to the point S in multiple frames. The false point cloud detection module evaluates the drift false point cloud severity of the point S based on the STV and TVD parameters, and classifies the point cloud into multiple levels. The optimization control module performs dynamic suppression processing on the point cloud according to the levels, wherein the trust level point cloud participates in map optimization and loop detection; the suspicious level point cloud has a reduced weight and only participates in local optimization; and the severe level point cloud is directly excluded. The map output module outputs a stable sparse three-dimensional map after confidence and level processing.

Images