Underwater robot self-positioning platform based on multi-feature and multi-modal information fusion

Abstract

The invention provides an underwater robot self-positioning platform based on multi-feature and multi-modal information fusion, and the platform is installed on an underwater robot, and comprises a visual sensing module which is used for collecting image information of an underwater environment, and extracting visual point features and visual line features from the image information; the acoustic measurement module is used for providing three-axis speed information and low-frequency pose information of the underwater robot based on the Doppler principle; the pressure measuring module is used for measuring water depth information of the underwater robot on the basis of a fluid statics principle; and the data processing and optimizing module is used for fusing the visual point features, the visual line features, the three-axis speed information, the low-frequency pose information and the water depth information on the basis of a factor graph optimizing framework so as to realize positioning and mapping. Compared with a traditional system, the self-positioning platform provided by the invention realizes tight coupling joint optimization of point-line features and acoustics and pressure sensor information, can stably operate in extreme underwater environments with low texture, turbidity and the like, and realizes high-precision underwater synchronous positioning and map construction.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous navigation and positioning of underwater robots, and in particular to a design and implementation method of an underwater robot positioning system based on tight coupling optimization of point-line visual features and acoustic and pressure multimodal information. Background Art

[0002] In recent years, underwater robots (AUVs) have been widely used in a variety of fields, including marine resource exploration, terrain reconstruction, underwater archaeology, and ecological monitoring. With the growing demand for marine development and utilization, higher requirements are being placed on the autonomous positioning accuracy of AUVs. In particular, with the continuous advancement of underwater platforms in fluid dynamics and biomimetic propulsion, underwater positioning technology has become a key support for the successful completion of underwater operations.

[0003] Traditional underwater positioning methods rely primarily on sensors such as sonar and inertial measurement units (IMUs) for navigation and state estimation. However, with the development of visual perception technology, vision-based simultaneous localization and mapping (SLAM) methods have shown great promise in underwater applications due to their low cost and compact structure.

[0004] Despite this, images are often blurred and low-contrast due to the absorption and scattering of light in underwater environments, making the extraction and matching of visual features difficult. Point feature extraction is particularly effective in low-light or low-texture scenes, further reducing the stability and accuracy of pure visual positioning systems. Line features, on the other hand, have stronger structural representation capabilities and can be extracted more stably in structured environments, compensating for the shortcomings of point features in sparse or blurred conditions. Therefore, the fusion of point-line features is of great significance for improving the robustness of underwater visual positioning.

[0005] Furthermore, in complex underwater environments, single-visual SLAM methods are prone to problems such as cumulative drift. In contrast, acoustic and pressure sensors offer greater stability underwater. For example, Doppler velocimeters (DVLs) provide stable three-axis velocity and pose predictions, playing a key role in state constraints in visual SLAM. Pressure sensors provide depth information that is insensitive to changing environmental conditions, effectively complementing the shortcomings of visual sensors.

[0006] In summary, purely visual positioning methods have many limitations in complex underwater environments. To address these issues, there is an urgent need for an underwater robot self-localization technology that integrates multi-feature and multi-modal information. This technology fully utilizes point-line visual features as well as data from various sensors, such as acoustics and pressure, to improve positioning accuracy and robustness in complex underwater conditions, providing solid technical support for autonomous underwater robot operations. Summary of the Invention

[0007] The present invention provides a design and method for an underwater robot self-positioning platform based on the fusion of multi-feature and multi-modal information, which is used to solve the technical problems of pure visual SLAM in the existing technology, such as poor positioning accuracy, severe drift, sparse features, and difficulty in stable mapping in underwater environments.

[0008] According to the present invention, a self-positioning platform for an underwater robot based on the fusion of multi-feature and multi-modal information is provided. The underwater robot self-positioning platform is installed on the underwater robot and includes: a visual sensing module for collecting image information of the underwater environment; an acoustic measurement module for measuring the three-axis velocity information and low-frequency posture information of the underwater robot based on the Doppler principle; a pressure measurement module for measuring the water depth information of the underwater robot's location based on the principle of fluid statics; a data processing and optimization module for extracting visual point features and visual line features from the image information, and fusing the visual point features, visual line features, three-axis velocity information, low-frequency posture information and water depth information based on a factor graph optimization framework to achieve positioning and mapping of the underwater environment in which the underwater robot is located.

[0009] In an embodiment, the visual sensing module includes: a binocular camera system configured to collect image information, wherein the image information includes a left image and a right image opposite to the left image that are collected synchronously.

[0010] In an embodiment, the acoustic measurement module includes a Doppler velocimeter configured to measure the following information: three-axis velocity information as mid-frequency linear velocity information; and low-frequency posture information.

[0011] In an embodiment, the pressure measurement module includes a pressure sensor having an update frequency higher than an image sampling rate and configured to measure water depth information and align it with a timestamp of the image information by interpolation.

[0012] In an embodiment, the data processing and optimization module includes: a preprocessing module, configured to extract visual point features using the ORB method, extract and match visual line features based on the LSD and LBD algorithms, obtain three-dimensional map points and three-dimensional map line segments through stereo matching and triangulation based on the visual point features and visual line features, and associate visual point features, visual line features, three-axis velocity information, low-frequency pose information and water depth information through time difference; a front-end inter-frame tracking module, configured to construct a front-end factor graph, and optimize the front-end factor graph using a sliding window, and perform inter-frame pose prediction based on frames of image information, wherein the front-end factor graph includes: three-dimensional map points and three-dimensional map line segments as front-end optimization fixed variables, frame poses and front-end factor edges, the front-end factor edges include: visual point feature factors connecting three-dimensional map points and frame poses to describe point feature reprojection residuals, visual line feature factors connecting three-dimensional map line segments and frame poses to describe line feature reprojection residuals, speed factors connected between frame poses of multiple frames to describe speed residuals generated by three-axis velocity information, and speed factors connecting to frame pose for describing the pressure depth factor of the depth residual generated by the water depth information; the back-end local mapping module is configured to construct a back-end factor graph for multiple key frames, wherein the back-end factor graph includes: 3D map points and 3D map segments corresponding to multiple key frames as fixed variables for back-end optimization, key frame poses and back-end factor edges, the back-end factor edges include: visual point feature factors connecting 3D map points and key frame poses, visual line feature factors connecting 3D map segments and key frame poses, pose factors connected between key frame poses of multiple key frames for describing the pose residual generated by low-frequency pose information, and pressure depth factors of key frames connected to key frame poses. The back-end local mapping module uses the back-end factor edges to jointly optimize the key frame poses and 3D map points and 3D map segments and simultaneously update the 3D map points, 3D map segments and key frame poses to suppress the accumulated error when the front-end inter-frame tracking module performs inter-frame pose prediction; the key frame management module is configured to perform key frame selection using a disparity threshold strategy to obtain multiple key frames and trigger local mapping updates.

[0013] In an embodiment, the visual point feature factor is calculated by the following steps: using the internal parameters of the binocular camera to project the three-dimensional map point in the camera coordinate system of the current frame to obtain the projection point; comparing the pixel coordinates of the projection point with the pixel coordinates of the visual point feature extracted by the preprocessing module from the image of the current frame to obtain the point feature reprojection residual of the visual point feature; applying the Huber robust loss function to weight the point feature reprojection residual to construct the visual point feature factor.

[0014] In an embodiment, the visual line feature factor is calculated by the following steps: obtaining a three-dimensional map line segment in a previous frame relative to the current frame, and obtaining a two-dimensional image line segment that matches the three-dimensional map line segment of the previous frame in the visual line feature extracted by the preprocessing module in the image of the current frame; projecting the three-dimensional map line segment in the camera coordinate system of the current frame to obtain a predicted line segment; calculating the area of ​​a quadrilateral formed by the predicted line segment and the two-dimensional image line segment as the quadrilateral area residual of the visual line feature, and applying the Huber robust loss function and the line feature information matrix weighting to the quadrilateral area residual to construct a visual line feature factor, wherein the quadrilateral area residual is obtained by cross-producting the vectors of the two endpoints of the predicted line segment and the vectors of the two endpoints of the two-dimensional image segment in sequence.

[0015] In an embodiment, the velocity factor, the posture factor and the pressure depth factor are calculated respectively by the following steps: in the front-end inter-frame tracking module, the velocity factor is based on the three-axis velocity information of the current frame measured by the Doppler velocimeter, and the inter-frame displacement is predicted by linear interpolation, and the velocity residual is formed by subtracting the displacement from the visual estimation, wherein the visual estimation displacement is the displacement of the frame posture of the current frame relative to the frame posture of the previous frame; in the back-end local mapping module, the posture factor is based on the low-frequency posture information of the key frame provided by the Doppler velocimeter, and the posture difference between the low-frequency posture information of the key frame and the frame posture of the key frame is obtained by logarithmically mapping the posture difference between the low-frequency posture information of the key frame and the frame posture of the key frame to the so(3) space using the posture residual definition method under the Lie group; the pressure depth factor is obtained by the depth difference calculated by the depth change of the current frame relative to the initial frame and the water depth information measured by the pressure sensor.

[0016] This invention provides a design for an underwater robot self-positioning platform, comprising a visual sensing module, an acoustic measurement module, a pressure measurement module, and a data processing and optimization module. The visual sensing module includes a binocular camera system for capturing left and right images of the underwater environment and simultaneously extracting point and line features from the images. Point features are extracted using the ORB method, which exhibits rotational invariance and high computational efficiency. Line features are extracted and matched using the LSD and LBD algorithms, enabling structural boundary perception and suitability for underwater scenarios with sparse features.

[0017] According to the present invention, a self-positioning platform design for an underwater robot features an acoustic measurement module, a Doppler velocimeter (DVL), which provides mid-frequency linear velocity information and low-frequency pose information. The mid-frequency velocity information is used for pose prediction during front-end tracking, while the low-frequency DVL pose is incorporated into the graph optimization process as a relative constraint between back-end keyframes. The DVL data exhibits excellent underwater stability, effectively compensating for short-term errors and feature drift in the visual system.

[0018] According to the design of a self-positioning platform for underwater robots, the pressure measurement module is a high-frequency pressure sensor that measures the water depth corresponding to the current frame in real time. Based on the principles of hydrostatics, water depth and pressure have a linear relationship. The system uses temporal interpolation to align pressure data with image frame timestamps, thereby constructing a stable and reliable vertical observation factor for graph optimization.

[0019] According to the design scheme for an underwater robot self-positioning platform provided by the present invention, the data processing and optimization module includes a measurement preprocessing module, a front-end tracking module, and a back-end mapping module. The measurement preprocessing module is used to extract ORB point features and LSD line features and perform time alignment and interpolation of visual, acoustic, and pressure multi-source observation data. The front-end tracking module constructs a multimodal factor graph within a sliding window based on initial DVL velocity values ​​and visual features for pose estimation and update. The back-end mapping module constructs visual residuals, DVL relative pose factors, and pressure depth factors between key frames, achieving high-precision local dense mapping through nonlinear optimization. Furthermore, the back-end mapping module filters key frames using a disparity threshold to dynamically maintain the map optimization window.

[0020] The present invention also provides a multi-feature multi-modal fusion positioning method based on the above-mentioned underwater robot platform, which includes the following four steps: a feature extraction step: using binocular images to extract ORB point features and LSD line features, and restoring three-dimensional map points and map segments through stereo matching and triangulation; a visual-acoustic-pressure information association step: aligning the speed and posture information provided by the DVL and the depth information provided by the pressure sensor with the current image frame through time interpolation to establish a multimodal observation link; a front-end inter-frame tracking step: constructing a sliding window optimization graph, the factors of which include the visual point feature reprojection residual, the line feature quadrilateral area residual, the DVL speed residual and the pressure depth residual, and estimating the precise posture of the current frame through nonlinear optimization; a back-end local mapping step: performing joint optimization on multiple key frames, constructing the relative posture factor, pressure depth factor and multi-frame visual residual factor between the DVL key frames, and completing the local three-dimensional map construction through local bundling adjustment.

[0021] According to a method for constructing visual point feature factors provided by the present invention, the system uses camera intrinsic parameters to project map points onto the image plane, calculates the deviation between the feature point position in the actual image and the projected position, forms the point feature reprojection residual, and constructs the optimization factor through Huber robust function weighting.

[0022] According to a method for constructing visual line feature factors provided by the present invention, a line segment quadrilateral residual definition method is adopted: the three-dimensional line segments reconstructed in the previous frame are projected onto the current image respectively, and the geometric area of ​​the quadrilateral formed by the predicted line segment and the current observed line segment is calculated as the line feature residual, which is further weighted by the information matrix to form an optimization item.

[0023] According to a method for defining acoustic and pressure residuals provided by the present invention, the DVL velocity factor estimates the inter-frame displacement through linear interpolation and forms a residual by comparing it with the visual estimation result. The DVL pose factor constructs the rotation error on SO(3) through Lie group logarithmic mapping. The pressure depth factor defines the optimized residual based on the depth difference between the current frame and the initial key frame.

[0024] In summary, this invention achieves the first tightly coupled optimization of point-line features with acoustic velocity, acoustic attitude, and pressure-depth information in an underwater robot self-positioning system. This method offers significant advantages, including strong stability, high positioning accuracy, high feature utilization, and excellent robustness. It is particularly suitable for complex and variable underwater operations, such as those in clear and deep water. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the technical solutions in the present invention or the prior art, a brief introduction will be given below to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0026] Figure 1 This is a structural diagram of an underwater robot positioning platform that integrates point-line visual features with sound pressure information provided by the present invention; Figure 2 It is a block diagram of the data processing and optimization module provided by the present invention; Figure 3 This is an example diagram of point and line feature extraction in a real underwater scene; Figure 4 It is a schematic diagram of the inter-frame reprojection error of line features; Figure 5 It is an optimized schematic diagram of the front-end factor graph; Figure 6 It is a schematic diagram of the optimization of the backend factor graph. DETAILED DESCRIPTION

[0027] In order to make the purpose, technical solutions and beneficial effects of the present invention more clear, the following Figures 1 to 6 This article clearly and completely describes the technical solutions for the underwater robot positioning method and its implementation platform, which integrates point and line visual features with sound pressure information. The aforementioned implementations are merely preferred examples of the present invention. The specific module divisions, parameter configurations, and feature extraction methods employed are subject to refinement and expansion by those skilled in the art without departing from the core principles of the present invention.

[0028] Figure 1This is a structural schematic diagram of an underwater robot positioning platform that integrates point-line visual features with sound pressure information provided by the present invention.

[0029] like Figure 1 As shown in FIG, the present invention provides a point-line visual feature fusion sound pressure information underwater robot self-positioning platform, which is installed on the underwater robot. The overall structure of the platform mainly includes: a visual sensing module 100, an acoustic measurement module 110, a pressure measurement module 120 and a data processing and optimization module ( Figure 1 Not shown, see Figure 2 ).

[0030] The visual sensing module 100 is used to collect image information of an underwater environment. The visual sensing module 100 includes a binocular camera system configured to collect the image information, which includes a left image and a right image opposite to the left image that are collected synchronously.

[0031] Acoustic measurement module 110 measures the underwater robot's three-axis velocity information and low-frequency pose information (e.g., low-frequency acoustic odometry pose) based on the Doppler principle. Acoustic measurement module 110 includes a Doppler velocimeter (DVL) configured to measure the robot's three-axis velocity information (mid-frequency linear velocity information) relative to the water body, as well as low-frequency pose information.

[0032] The pressure measurement module 120 measures the water depth of the underwater robot based on the principles of hydrostatics. The pressure measurement module 120 includes a pressure sensor with an update rate higher than the image sampling rate. The pressure sensor is configured to measure the robot's relative water depth and align it with the image timestamp through interpolation.

[0033] The data processing and optimization module extracts visual point features and visual line features from the image information collected by the visual sensing module 100, and fuses the visual point features, the visual line features, the three-axis velocity information, the low-frequency pose information and the water depth information based on the factor graph optimization framework to achieve positioning and mapping of the underwater environment in which the underwater robot is located.

[0034] In addition, the platform may also include a global detection module 130. The global detection module 130 may be equipped with an AprilTag board for positioning accuracy assessment; all sensors are uniformly processed by the data preprocessing module 210 in the data processing and optimization module (see Figure 2 ) for time alignment and information fusion.

[0035] The pose prediction process is divided into the front-end inter-frame tracking module 220 (see Figure 2 ) and the back-end local mapping module 230 (see Figure 2), together forming a complete positioning system. The three measurement subsystems (100, 110, and 120) collaboratively provide visual features, velocity constraints, and vertical positioning information. Fusion of the data processing module enables high-precision, robust underwater positioning.

[0036] Figure 2 It is a block diagram of the data processing and optimization module provided by the present invention.

[0037] like Figure 2 As shown, the data processing and optimization module includes: a pre-processing module 210 , a front-end inter-frame tracking module 220 , a back-end local mapping module 230 and a key frame management module 240 .

[0038] The preprocessing module 210 is configured to extract the visual point features using the ORB method, extract and match the visual line features based on the LSD and LBD algorithms, obtain three-dimensional map points and three-dimensional map segments through stereo matching and triangulation based on the visual point features and the visual line features, and associate the visual point features, the visual line features, the three-axis velocity information, the low-frequency pose information, and the water depth information through time difference.

[0039] The front-end inter-frame tracking module 220 is configured to construct a front-end factor graph, and optimize the front-end factor graph using a sliding window, and perform inter-frame pose prediction based on the frames of the image information; wherein the front-end factor graph includes: the three-dimensional map points and the three-dimensional map segments, frame poses and front-end factor edges as front-end optimization fixed variables; the front-end factor edges include: visual point feature factors connecting the three-dimensional map points and the frame poses to describe the point feature reprojection residuals, visual line feature factors connecting the three-dimensional map segments and the frame poses to describe the line feature reprojection residuals, speed factors connected between the frame poses of multiple frames to describe the speed residuals generated by the three-axis speed information, and pressure depth factors connected to the frame poses to describe the depth residuals generated by the water depth information. More specifically, as Figure 2 As shown in the figure, a reference keyframe is obtained based on an acoustically aided motion model. The visual point feature / line feature factors, DVL pose factors, and pressure depth factors described above are calculated based on the reference keyframe. Using a sliding window factor graph, point features, line features, DVL velocity, and pressure depth are fused to perform inter-frame pose prediction.

[0040] The backend local mapping module 230 is configured to construct a backend factor graph for a plurality of keyframes, wherein the backend factor graph comprises a plurality of three-dimensional map points and three-dimensional map line segments corresponding to the keyframes as backend optimization fixed variables, keyframe poses, and backend factor edges; the backend factor edges comprise visual point feature factors connecting the three-dimensional map points and the keyframe poses, visual line feature factors connecting the three-dimensional map line segments and the keyframe poses, pose factors connecting between keyframe poses of a plurality of keyframes for describing pose residuals generated by the low-frequency pose information, and pressure depth factors connecting to the keyframes of the keyframe poses; the backend local mapping module jointly optimizes the keyframe poses and the three-dimensional map points and three-dimensional map line segments using the backend factor edges and simultaneously updates the three-dimensional map points, the three-dimensional map line segments and the keyframe poses to suppress errors accumulated when the front-end inter-frame tracking module performs inter-frame pose prediction. More specifically, as shown in Figure 2 the backend factor graph (also referred to as an inter-keyframe factor graph) is constructed, and the keyframe poses and the three-dimensional map are jointly optimized (e.g., by local visual-acoustic pressure bundle adjustment). The backend mapping module 230 further suppresses drift errors by multi-feature-multi-modal beam adjustment optimizing the keyframe poses and the sparse map within a window.

[0041] The keyframe management module 240 is configured to perform keyframe selection using a parallax threshold strategy to obtain the plurality of keyframes and trigger local mapping updates. As an example, the keyframe management module 240 can perform keyframe selection using a visual feature parallax threshold strategy, e.g., when a large parallax (e.g., exceeding a parallax threshold) occurs in visual matching between a current frame and a most recent reference keyframe (e.g., a previous keyframe or an initial keyframe), a keyframe insertion is triggered. Examples of trigger conditions can include: A) the average image space parallax between the ORB point features or the LSD line features extracted and matched between the current frame and the reference keyframe exceeds a preset threshold (e.g., 20-30 pixels); B) the number of valid visual feature points (or line segments) matched with the reference keyframe drops below 50% of the reference keyframe; C) the average value of re-projection residuals based on visual point and line features exceeds a set threshold (e.g., 1 2 pixels), etc. In embodiments, the keyframe management module 240 can use one or more of the above conditions for detection, or fuse them into a weighted scoring mechanism. Only when the cumulative score exceeds the parallax threshold, the current frame is selected as a keyframe to reduce redundancy and ensure the stability and efficiency of map optimization. The keyframe management module 240 can dynamically maintain an optimization window to improve computational efficiency and map structure stability.

[0042] As shown in Figure 2 the modules cooperate to ensure a closed loop from raw sensor input to the final three-dimensional map and accurate trajectory.

[0043] In an embodiment, the visual point feature factor as described above is calculated by the following steps: using the internal parameters of the binocular camera to project the three-dimensional map point in the camera coordinate system of the current frame to obtain a projection point; comparing the pixel coordinates of the projection point with the pixel coordinates of the visual point feature extracted by the preprocessing module from the image of the current frame to obtain the point feature reprojection residual of the visual point feature; applying the Huber robust loss function to weight the point feature reprojection residual to construct the visual point feature factor.

[0044] In an embodiment, the visual line feature factor as described above is calculated by the following steps: obtaining a three-dimensional map line segment in a previous frame relative to the current frame, and obtaining a two-dimensional image line segment that matches the three-dimensional map line segment of the previous frame in the visual line feature extracted by the preprocessing module in the image of the current frame; projecting the three-dimensional map line segment in the camera coordinate system of the current frame to obtain a predicted line segment; calculating the area of ​​a quadrilateral formed by the predicted line segment and the two-dimensional image line segment as the quadrilateral area residual of the visual line feature, and applying the Huber robust loss function and the line feature information matrix weighting to the quadrilateral area residual to construct a visual line feature factor, wherein the quadrilateral area residual is obtained by cross-producting the vectors of the two endpoints of the predicted line segment and the vectors of the two endpoints of the two-dimensional image segment in sequence.

[0045] In an embodiment, in the front-end inter-frame tracking module, the velocity factor predicts the inter-frame displacement through linear interpolation based on the three-axis velocity information of the current frame measured by the Doppler velocimeter, and subtracts it from the visually estimated displacement to form a velocity residual, wherein the visually estimated displacement is the displacement of the frame pose of the current frame relative to the frame pose of the previous frame.

[0046] In an embodiment, in the back-end local mapping module, the pose factor is obtained by logarithmically mapping the pose difference between the low-frequency pose information of the key frame and the frame pose of the key frame to the so(3) space based on the low-frequency pose information of the key frame provided by the Doppler velocimeter using the pose residual definition method under the Lie group.

[0047] In an embodiment, the pressure depth factor is obtained by calculating the depth difference between the depth change of the current frame relative to the initial frame and the water depth information measured by the pressure sensor.

[0048] Figure 3 This is an example diagram of point feature and line feature extraction in real underwater scenes.

[0049] like Figure 3 As shown, it is a schematic diagram of point and line visual feature extraction in a real underwater scene: the image 310 obtained by the binocular camera is pre-processed to extract two types of features: point features (visual point features) 311 and line features (visual line features) 312.

[0050] Point features 311 are detected, described, and matched using the ORB algorithm, making them suitable for high-texture areas. Line features 312 extract image edge segments based on the LSD algorithm and use LBD for description and matching, making them suitable for low-texture or turbid scenes. The resulting local 3D map 320 contains both 3D map points and 3D map segments, providing richer and more stable geometric constraints.

[0051] Figure 4 This is a schematic diagram of the inter-frame reprojection error of line features.

[0052] Figure 4 The geometric interpretation of the inter-frame projection error of line features is shown. Figure 4 Describe the steps for calculating the visual line characteristic factor.

[0053] The actual position of the underwater robot at time t-1 is , the picture taken by its visual sensor module 100 is as follows Figure 4 As shown in (a) in the figure. At time t, the actual position of the underwater robot is , the picture taken by its visual sensor module 100 is as follows Figure 4 As shown in (b) in .

[0054] 3D map line segment reconstructed at time t-1 The endpoints are ( , ), the predicted line segment is obtained by projecting the binocular positioning parameters to the current frame image plane (i.e., projecting in the camera coordinate system of the current frame) , predict line segment The two endpoints of , ); and the line segment matched in the current frame The two endpoints ( , ) forms a quadrilateral area 400; the area of ​​the quadrilateral is calculated as the reprojection error of the line feature 410 by cross-product of the end points; the residual value is introduced into the robust function to construct the visual line feature factor 523 (such as Figure 5 This quadrilateral area method is more stable than the point-to-point error and can effectively cope with the effects of posture changes and image distortion.

[0055] Figure 5 This is an optimized diagram of the front-end factor graph. The main variables and factors included in the front-end factor graph include: State nodes: current frame pose 510 , 3D map points (also called local map points) 501 , and 3D map segments (also called local map segments) 502 .

[0056] Optimization factors: visual point feature factor (also referred to as visual point re-projection factor) 521, visual line feature factor (also referred to as visual line re-projection factor) 523, DVL velocity factor (also referred to as velocity factor) 522, and pressure depth factor 521.

[0057] In an embodiment, the visual point feature factor 521 connects the three-dimensional map point and the frame pose for describing a point feature re-projection residual, that is, the point feature re-projection residual is calculated based on a pixel error between the three-dimensional map point and a current image point.

[0058] The visual line feature factor 523 connects the three-dimensional map line segment and the frame pose for describing a line feature re-projection residual, as described with reference to Figure 4 The line feature re-projection residual is calculated by the quadrilateral area residual as described above.

[0059] The DVL velocity factor 522 connects between frame poses of multiple frames for describing a velocity residual generated by the three-axis velocity information. For example, the DVL velocity factor 522 predicts an inter-frame velocity by linear interpolation based on the three-axis velocity information of a current frame measured by a Doppler velocity log, and forms a velocity residual with a visual estimated displacement, where the visual estimated displacement is a displacement of the frame pose of the current frame relative to a frame pose of a previous frame.

[0060] The pressure depth factor 521 is connected to the frame pose for describing a depth residual generated by the water depth information. For example, the pressure depth factor 521 can represent a residual of a depth difference between a current frame and an initial frame. More specifically, the pressure depth factor 521 obtains the depth difference between the current frame and the initial frame by calculating a depth difference between a depth change of the current frame relative to the initial frame and the water depth information measured by the pressure sensor.

[0061] By jointly optimizing the above factors, the front-end can estimate the pose of the current frame in real time, providing an initial value for the back-end mapping.

[0062] Figure 6 is an optimization schematic diagram of the back-end factor graph.

[0063] As shown in Figure 6 The back-end local mapping module focuses on global consistency optimization between key frames. Main variables and factors include: State nodes: multiple key frame poses 610, three-dimensional map points (also referred to as local map points) 611, and three-dimensional map line segments (also referred to as local map line segments) 612.

[0064] Optimization factors: visual point feature factor 620, visual line feature factor 623, DVL pose factor 621, and pressure depth factor 622.

[0065] The key frame pose 610, 3D map point 611, 3D map line segment 612 and pressure depth factor 622 have the same Figure 5 The current frame's frame pose 510, 3D map point 501, 3D map line segment 502, and pressure-depth factor 522 have the same or similar meanings as those of the keyframe, and redundant descriptions are omitted here. For example, the pressure-depth factor 622 compares the water depth information of the keyframe with the initial depth to form an optimization factor.

[0066] The DVL pose factor 621 is obtained by logarithmically mapping the pose difference between the low-frequency pose information of the key frame and the frame pose of the key frame to the so(3) space based on the low-frequency pose information of the key frame provided by the Doppler velocimeter (for example, the low-frequency acoustic odometry pose). Specifically, the DVL can provide the relative pose between the key frames, and the residual is calculated by logarithmic mapping of the Lie group so(3). The pose difference refers to the positional relationship between the low-frequency pose information of the key frame and the frame pose of the key frame. This difference takes into account both translation and rotation components.

[0067] The backend optimization process is implemented using the Ceres Solver, using a pre-calculated Hessian matrix and marginalization strategies to improve efficiency. The optimization results simultaneously update the map and trajectory, eliminating frontend error accumulation and enhancing system stability.

[0068] In summary, the present invention solves the problem of the difficulty of stable mapping in existing underwater visual SLAM in low-feature, low-light and motion-disturbance scenarios by constructing a multimodal self-positioning system that integrates visual point and line features, DVL acoustic measurements and pressure-depth observations, and provides a reliable positioning support platform for complex underwater tasks.

[0069] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

[0070] Reference numerals: 100: visual sensing module (binocular camera); 110: Acoustic measurement module (DVL); 120: pressure measurement module (pressure sensor); 130: Global camera detection code April Tag; 200: sensor; 210: data preprocessing module; 220: front-end inter-frame tracking module; 230: back-end local mapping module; 240: key frame management module; 300: Underwater real scene example; 310: Point and line feature extraction of underwater visual images; 311: visual point features; 312: visual line features; 320: Instance of local map; 400: Line feature inter-frame reprojection; 410: Line feature reprojection residual calculation; 500: Front-end optimization fixed variables; 501: 3D map point in the local map; 502: 3D map line segment in the local map; 510: frame pose; 520: visual point feature factor; 521: pressure depth factor; 522: speed factor; 523: visual line characteristic factor; 600: Backend optimization fixed variables; 610: keyframe pose; 611: 3D map point in the local map; 612: 3D map line segments in a local map; 620: visual point feature factor; 621: pose factor; 622: pressure depth factor; 623: Visual line characteristic factor.

Claims

1. An underwater robot self-positioning platform based on multi-feature and multi-modal information fusion, wherein the underwater robot self-positioning platform is installed on the underwater robot, characterized in that: include: Visual sensing module, used to collect image information of underwater environment; an acoustic measurement module, which measures the three-axis velocity information and low-frequency posture information of the underwater robot based on the Doppler principle; A pressure measurement module, which measures the water depth information of the underwater robot based on the principle of fluid statics; A data processing and optimization module extracts visual point features and visual line features from the image information, and fuses the visual point features, the visual line features, the three-axis velocity information, the low-frequency pose information, and the water depth information based on a factor graph optimization framework to achieve positioning and mapping of the underwater environment in which the underwater robot is located.

2. The underwater robot self-positioning platform according to claim 1, characterized in that: The visual sensing module includes: The binocular camera system is configured to collect the image information, wherein the image information includes a left image and a right image opposite to the left image that are collected synchronously.

3. The underwater robot self-positioning platform according to claim 1, characterized in that: The acoustic measurement module includes a Doppler velocimeter configured to measure the following information: The three-axis velocity information as intermediate frequency linear velocity information; and The low-frequency pose information.

4. The underwater robot self-positioning platform according to claim 1, characterized in that: The pressure measurement module includes a pressure sensor having an update frequency higher than an image sampling rate and configured to measure the water depth information and align it with a timestamp of the image information by interpolation.

5. The underwater robot self-positioning platform according to claim 1, characterized in that: The data processing and optimization module includes: a preprocessing module configured to extract the visual point features using an ORB method, extract and match the visual line features based on LSD and LBD algorithms, obtain three-dimensional map points and three-dimensional map line segments through stereo matching and triangulation based on the visual point features and the visual line features, and associate the visual point features, the visual line features, the three-axis velocity information, the low-frequency pose information, and the water depth information through time difference; The front-end inter-frame tracking module is configured to construct a front-end factor graph, optimize the front-end factor graph using a sliding window, and perform inter-frame pose prediction based on the frames of the image information; wherein the front-end factor graph includes: the 3D map points and the 3D map line segments, frame poses, and front-end factor edges as fixed variables for front-end optimization; the front-end factor edges include: a visual point feature factor connecting the 3D map points and the frame poses to describe the point feature reprojection residual, a visual line feature factor connecting the 3D map line segments and the frame poses to describe the line feature reprojection residual, a velocity factor connected between the frame poses of multiple frames to describe the velocity residual generated by the three-axis velocity information, and a pressure depth factor connected to the frame poses to describe the depth residual generated by the water depth information; A back-end local mapping module is configured to construct a back-end factor graph for multiple key frames, wherein the back-end factor graph includes: 3D map points and 3D map segments corresponding to multiple key frames as fixed variables for back-end optimization, key frame poses, and back-end factor edges; the back-end factor edges include: visual point feature factors connecting the 3D map points and the key frame poses, visual line feature factors connecting the 3D map segments and the key frame poses, pose factors connected between the key frame poses of multiple key frames to describe the pose residuals generated by the low-frequency pose information, and pressure depth factors of the key frames connected to the key frame poses; the back-end local mapping module uses the back-end factor edges to jointly optimize the key frame poses and the 3D map points and 3D map segments and simultaneously update the 3D map points, the 3D map segments, and the key frame poses to suppress the accumulated errors when the front-end inter-frame tracking module performs inter-frame pose prediction; The key frame management module is configured to perform key frame selection by adopting a disparity threshold strategy to obtain the multiple key frames and trigger a local map update.

6. The underwater robot self-positioning platform according to claim 5, characterized in that: The visual point feature factor is calculated by the following steps: Projecting the three-dimensional map point in the camera coordinate system of the current frame using the internal parameters of the binocular camera to obtain a projection point; Comparing the pixel coordinates of the projection point with the pixel coordinates of the visual point feature extracted by the preprocessing module from the image of the current frame to obtain the point feature reprojection residual of the visual point feature; The Huber robust loss function is applied to the point feature reprojection residual to weight the visual point feature factor.

7. The underwater robot self-positioning platform according to claim 5, characterized in that: The visual line characteristic factor is calculated by the following steps: Acquire a three-dimensional map line segment in a previous frame relative to the current frame, and obtain a two-dimensional image line segment matching the three-dimensional map line segment in the previous frame from the visual line features extracted by the pre-processing module from the image of the current frame; Projecting the three-dimensional map line segment in the camera coordinate system of the current frame to obtain a predicted line segment; Calculating the area of ​​the quadrilateral formed by the predicted line segment and the two-dimensional image line segment as the quadrilateral area residual of the visual line feature, and applying the Huber robust loss function and the line feature information matrix weighting to the quadrilateral area residual to construct the visual line feature factor, The quadrilateral area residual is obtained by sequentially cross-producting the vectors of the two endpoints of the predicted line segment and the vectors of the two endpoints of the two-dimensional image line segment.

8. The underwater robot self-positioning platform according to claim 5, characterized in that: The velocity factor, the posture factor, and the pressure depth factor are calculated respectively by the following steps: In the front-end inter-frame tracking module, the velocity factor predicts the inter-frame displacement through linear interpolation based on the three-axis velocity information of the current frame measured by the Doppler velocimeter, and subtracts it from the visually estimated displacement to form the velocity residual, wherein the visually estimated displacement is the displacement of the frame pose of the current frame relative to the frame pose of the previous frame; In the back-end local mapping module, the pose factor is obtained by logarithmically mapping the pose difference between the low-frequency pose information of the key frame and the frame pose of the key frame to the so(3) space using a pose residual definition method under the Lie group based on the low-frequency pose information of the key frame provided by the Doppler velocimeter; and The pressure depth factor is obtained by calculating the depth difference between the depth change of the current frame relative to the initial frame and the water depth information measured by the pressure sensor.

Images