Underwater robot positioning method and device, electronic equipment and storage medium

Abstract

The application provides an underwater robot positioning method and device, electronic equipment and storage medium, the method comprises the following steps: based on the speed measurement data of DVL, the first speed component of the robot at the first frame time and the DVL error term at the second frame time are obtained; the first frame time is the last frame time of the second frame time; based on the first speed component of the robot at the first frame time, the initial pose estimation value of the robot at the second frame time is determined; based on the DVL error term of the robot at the second frame time, the initial pose estimation value of the robot at the second frame time is optimized, and the final pose estimation value of the robot at the second frame time is determined, which improves the positioning accuracy of the underwater robot.

Description

Technical Field

[0001] This invention relates to the field of visual image technology, and in particular to an underwater robot positioning method, apparatus, electronic device, and storage medium. Background Technology

[0002] Underwater robots are playing an increasingly important role in marine resource exploration and offshore platform maintenance. Accurate self-positioning is crucial for the successful completion of these tasks. Traditional positioning methods typically utilize sensors such as sonar, inertial measurement units (IMUs), depth sensors, and Doppler velocimeters (DVLs) for navigation and state estimation. In recent years, due to the ability of visual sensors to provide rich image information at a lower cost, vision-based simultaneous localization and mapping (V-SLAM) systems have received widespread attention, and underwater visual simultaneous localization and mapping (SLAM) systems have been validated in numerous studies.

[0003] DVL (Direct Visual Odometry) can obtain linear velocities in the XYZ directions by emitting acoustic signals and measuring their Doppler frequency shift as they reflect back from the bottom. These measurements contain information about position changes and can be used for robot position estimation. Although some work has attempted to fuse visual sensors with DVL sensors, these methods fuse DVL velocity measurements with gyroscopes and depth sensors. Due to the use of trajectory extrapolation methods that involve cumulative calculations, error accumulation is inevitable. Therefore, these fusion methods cannot significantly improve positioning accuracy based on visual odometry. Summary of the Invention

[0004] To address the problems existing in the prior art, the present invention provides an underwater robot positioning method, device, electronic device, and storage medium.

[0005] In a first aspect, the present invention provides an underwater robot positioning method, comprising:

[0006] Based on the velocity measurement data from the Doppler velocity meter (DVL), the first velocity component of the robot at the first frame time and the DVL error term at the second frame time are obtained; the first frame time is the previous frame time of the second frame time.

[0007] Based on the first velocity component of the robot at the first frame time, the initial pose estimate of the robot at the second frame time is determined;

[0008] Based on the DVL error term of the robot at the second frame time, the initial pose estimate of the robot at the second frame time is optimized to determine the final pose estimate of the robot at the second frame time.

[0009] Optionally, the DVL-based velocity measurement data, which obtains the robot's first velocity component at the first frame time, includes:

[0010] The first velocity component of the robot at the first frame time is obtained based on the product of the second velocity component of the robot at the first frame time and the rotation component of the robot when it is transformed from the visual coordinate system to the DVL coordinate system, the difference between the translation component of the robot when it is transformed from the DVL coordinate system to the visual coordinate system and the displacement of the DVL in the DVL coordinate system between the first frame time and the second frame time, and the translation component of the robot when it is transformed from the visual coordinate system to the DVL coordinate system.

[0011] Optionally, the first velocity component of the robot at the first frame time is determined based on the following formula:

[0012]

[0013] Among them, the This represents the first velocity component of the robot at the first frame time. This represents the second velocity component of the robot at the first frame time. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system. This represents the displacement of the DVL in the DVL coordinate system between the first frame time and the second frame time. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system.

[0014] Optionally, determining the initial pose estimate of the robot at the second frame time based on the first velocity component of the robot at the first frame time includes:

[0015] Based on the robot's first velocity component at the first frame time, the robot's second velocity component at the first frame time, and the robot's final pose estimate at the first frame time, the robot's initial pose estimate at the second frame time is determined.

[0016] Optionally, the DVL-based velocity measurement data, used to obtain the robot's DVL error term at the second frame time, includes:

[0017] Based on the difference between the displacements in the DVL coordinate system between the first frame and the second frame, the rotation component of the robot transforming from the visual coordinate system to the DVL coordinate system, the rotation component of the robot at the first frame, the rotation component of the robot relative to the visual coordinate system at the second frame, the translation component of the robot transforming from the visual coordinate system to the DVL coordinate system, the translation component of the robot relative to the visual coordinate system at the second frame, the translation component of the robot at the first frame, and the translation component of the robot transforming from the DVL coordinate system to the visual coordinate system, the DVL error term of the robot at the second frame is obtained.

[0018] Optionally, the DVL error term of the robot at the second frame time is determined based on the following formula:

[0019]

[0020] Among them, the This represents the DVL error term of the robot at the second frame time. This represents the displacement of the DVL in the DVL coordinate system between the first frame time and the second frame time. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the rotation component of the robot at the first frame time. This represents the rotation component of the robot relative to the visual coordinate system at the second frame time. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot relative to the visual coordinate system at the second frame time. This represents the translation component of the robot at the first frame time. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system.

[0021] Optionally, optimizing the initial pose estimate of the robot at the second frame time based on the DVL error term of the robot at the second frame time to determine the final pose estimate of the robot at the second frame time includes:

[0022] Based on the DVL error term and reprojection error of the robot at the second frame time, a nonlinear least squares optimization problem is solved with the robot's pose at the second frame time as the objective. The initial pose estimate of the robot at the second frame time is optimized to determine the final pose estimate of the robot at the second frame time.

[0023] Secondly, the present invention also provides an underwater robot positioning device, comprising:

[0024] The acquisition module is used to acquire the robot's first velocity component at the first frame time and the DVL error term at the second frame time based on DVL velocity measurement data; the first frame time is the previous frame time of the second frame time.

[0025] The determination module is used to determine the initial pose estimate of the robot at the second frame time based on the first velocity component of the robot at the first frame time.

[0026] An optimization module is used to optimize the initial pose estimate of the robot at the second frame time based on the DVL error term of the robot at the second frame time, and determine the final pose estimate of the robot at the second frame time.

[0027] Thirdly, the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the underwater robot positioning method described in the first aspect above.

[0028] Fourthly, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the underwater robot positioning method described in the first aspect above.

[0029] The underwater robot localization method, apparatus, electronic device, and storage medium provided by this invention improve the initial estimation of robot pose during visual tracking by utilizing DVL data measurement data, and then optimize the initial pose estimation value of the robot by constructing a new DVL error term, thereby improving the accuracy of underwater robot localization. Attached Figure Description

[0030] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0031] Figure 1 This is a flowchart illustrating the underwater robot positioning method provided by the present invention;

[0032] Figure 2 This is a flowchart illustrating the vision-Doppler velocimeter fusion framework provided by the present invention;

[0033] Figure 3 This is a schematic diagram of the coordinate system relationships of the camera-DVL system provided by the present invention;

[0034] Figure 4 This is a schematic diagram of the factors of the vision-DVL fusion system provided by the present invention;

[0035] Figure 5 This is a schematic diagram of the robot motion trajectory of the binocular vision-DVL fusion mode localization method provided by the present invention;

[0036] Figure 6 This is a schematic diagram of the underwater robot positioning device provided by the present invention;

[0037] Figure 7 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0038] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0039] Figure 1 This is a flowchart illustrating the underwater robot positioning method provided by the present invention, as shown below. Figure 1 As shown, the method includes the following steps:

[0040] Step 100: Based on the velocity measurement data from the Doppler velocimeter (DVL), obtain the first velocity component of the robot at the first frame time and the DVL error term at the second frame time; the first frame time is the frame time preceding the second frame time.

[0041] Step 101: Based on the robot's first velocity component at the first frame time, determine the robot's initial pose estimate at the second frame time.

[0042] Step 102: Based on the robot's DVL error term at the second frame time, optimize the robot's initial pose estimation value at the second frame time to determine the robot's final pose estimation value at the second frame time.

[0043] Specifically, DVL (Direct Visualization) can obtain the linear velocity of a target in the XYZ directions by emitting acoustic signals and measuring the Doppler frequency shift as the target reflects back from the bottom. In robot pose estimation, the robot's velocity at a given frame can be represented by its translational and yaw velocities. The first velocity component can be used to represent the robot's translational velocity. The second frame can be used to represent the current frame, and the first frame can be used to represent the frame preceding the current frame. The DVL error term can be used to represent the error between the measured and derived displacement values ​​in the DVL coordinate system between the first and second frame times.

[0044] After obtaining the robot's first velocity component at the first frame time based on DVL velocity measurement data, the robot's initial pose estimate at the second frame time can be determined based on the robot's first velocity component at the first frame time.

[0045] After determining the robot's initial pose estimate at the second frame, the initial pose estimate can be optimized based on the robot's DVL error term at the second frame to obtain the robot's final pose estimate at the second frame. The final pose estimate obtained in this invention can be used as the final pose estimate for the first-stage visual tracking thread in the original ORB-SLAM3 system. Based on the final pose estimate of the first-stage visual tracking thread, the second-stage operation of the visual tracking thread is performed to complete robot localization. The operation of the second-stage visual tracking thread can be the same as in existing technologies or can employ other methods, which are not specifically limited here.

[0046] The underwater robot localization method provided by this invention improves the initial estimation of robot pose during visual tracking by using DVL data measurement data, and then optimizes the initial pose estimation value of the robot by constructing a new DVL error term, thereby improving the accuracy of underwater robot localization.

[0047] Optionally, based on the velocity measurement data from DVL, the first velocity component of the robot at the first frame moment is obtained, including:

[0048] The robot's first velocity component at the first frame time is obtained by multiplying the second velocity component of the robot at the first frame time with the rotation component of the robot from the visual coordinate system to the DVL coordinate system, the translation component of the robot from the DVL coordinate system to the visual coordinate system with the difference between the displacement of the DVL in the DVL coordinate system between the first and second frames time, and the translation component of the robot from the visual coordinate system to the DVL coordinate system.

[0049] Specifically, the second velocity component can represent the robot's velocity in the deflection direction at a given frame time. The robot's relative position between the visual coordinate system and the DVL coordinate system is constant. The displacement of the DVL in the DVL coordinate system between the first and second frame times can be represented by the product of the DVL's velocity measurement data at the first frame time and the time difference between the first and second frame times.

[0050] In one implementation, the robot's first velocity component at the first frame time can be determined based on the following formula:

[0051]

[0052] in, This represents the robot's first velocity component at the first frame. This represents the robot's second velocity component at the first frame. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system. This represents the displacement of DVL in the DVL coordinate system between the first frame and the second frame. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system.

[0053] The displacement of DVL in the DVL coordinate system between the first frame and the second frame can be calculated based on the following formula:

[0054]

[0055] in, This represents the displacement of DVL in the DVL coordinate system between the first frame and the second frame. dt represents the velocity measurement data of DVL at the first frame time, and dt represents the time difference between the first frame time and the second frame time.

[0056] Optionally, based on the robot's first velocity component at the first frame, the initial pose estimate of the robot at the second frame is determined, including:

[0057] Based on the robot's first velocity component, second velocity component, and final pose estimate in the first frame, the robot's initial pose estimate in the second frame is determined.

[0058] Specifically, the robot's velocity at the first frame can be obtained based on the first and second velocity components of the robot at the first frame. Then, based on the robot's velocity at the first frame and the robot's final pose estimate at the first frame, the robot's initial pose estimate at the second frame can be determined.

[0059] In one implementation, the initial pose estimate of the robot at the second frame time can be determined based on the following formula:

[0060]

[0061] in, This represents the robot's initial pose estimate at the second frame. This indicates the robot's velocity at the first frame. This represents the robot's final pose estimate at the first frame.

[0062] Optionally, based on the velocity measurement data from the DVL (Dynamic Value Chain) algorithm, the DVL error term for the robot at the second frame time is obtained, including:

[0063] Based on the difference between the displacements in the DVL coordinate system between the first and second frame times, the rotation component of the robot transforming from the visual coordinate system to the DVL coordinate system, the rotation component of the robot in the first frame time, the rotation component of the robot relative to the visual coordinate system in the second frame time, the translation component of the robot transforming from the visual coordinate system to the DVL coordinate system in the second frame time, the translation component of the robot relative to the visual coordinate system in the second frame time, the translation component of the robot in the first frame time, and the translation component of the robot transforming from the DVL coordinate system to the visual coordinate system, the DVL error term of the robot in the second frame time is obtained.

[0064] Specifically, the DVL error term of the robot at the second frame time can be used to represent the error between the measured and derived displacement values ​​in the DVL coordinate system between the first and second frame times.

[0065] The displacement measurement of DVL in the DVL coordinate system between the first frame time and the second frame time can be represented by the difference between the displacements of DVL in the DVL coordinate system between the first frame time and the second frame time.

[0066] The displacement values ​​in the DVL coordinate system between the first and second frame times can be determined based on the rotation components of the robot transforming from the visual coordinate system to the DVL coordinate system, the robot's rotation components at the first frame time, the robot's rotation components relative to the visual coordinate system at the second frame time, the translation components of the robot transforming from the visual coordinate system to the DVL coordinate system, the robot's translation components relative to the visual coordinate system at the second frame time, the robot's translation components at the first frame time, and the translation components of the robot transforming from the DVL coordinate system to the visual coordinate system.

[0067] In one implementation, the robot's DVL error term at the second frame time can be determined based on the following formula:

[0068]

[0069] in, This represents the DVL error term of the robot at the second frame time. This represents the displacement of DVL in the DVL coordinate system between the first frame and the second frame. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the robot's rotation component at the first frame. This represents the rotation component of the robot relative to the visual coordinate system at the second frame. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot relative to the visual coordinate system at the second frame time. This represents the translation component of the robot at the first frame. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system.

[0070] Optionally, based on the robot's DVL error term at the second frame time, the initial pose estimate of the robot at the second frame time is optimized to determine the final pose estimate of the robot at the second frame time, including:

[0071] Based on the robot's DVL error term and reprojection error at the second frame, a nonlinear least squares optimization problem is solved with the robot's pose at the second frame as the objective. This optimizes the robot's initial pose estimate at the second frame and determines the robot's final pose estimate at the second frame.

[0072] Specifically, the reprojection error of the robot at the second frame time can be obtained using existing visual tracking methods.

[0073] With the robot's pose at the second frame as the objective, and the robot's DVL error term and reprojection error at the second frame as known terms, an objective function can be constructed.

[0074] In one implementation, the objective function can be constructed using the following formula:

[0075]

[0076] in, This represents the robot's rotation component at the first frame. This represents the robot's rotation component at the first frame, based on... and The final pose estimate of the robot at the second frame can be determined, J V This represents the reprojection error of the robot at the second frame. ∑ represents the DVL error term of the robot at the second frame time. d This represents the covariance of the DVL error term.

[0077] By solving the nonlinear least squares optimization problem of the objective function, the final pose estimate of the robot at the second frame can be determined. The solution method can be the Gauss-Newton (GN) or Levenberg-Marquardt (LM) method implemented in the nonlinear optimization library g2o, etc., and no specific limitation is made here.

[0078] The underwater robot positioning method provided by this invention will be illustrated with specific application scenarios below.

[0079] This invention proposes a tightly coupled Visual-Doppler Velocimetry (Visual-DVL) fusion method for underwater robot localization, aiming to integrate DVL velocity measurement data into Visual Odometry (VO). Considering that fusing DVL measurement data in a Dead Reckoning (DR) system can easily lead to error accumulation and suboptimal results, this invention directly integrates it into the visual tracking process. Specifically, firstly, DVL velocity measurements are used to improve the initial estimation of camera pose during visual tracking, providing better initial values ​​for subsequent pose optimization. Then, by constructing a new DVL velocity error term, the DVL velocity measurements are directly used to constrain the camera's positional changes between adjacent frames, and this is jointly optimized with the visual reprojection error term to obtain a more accurate camera pose. The proposed tightly coupled Visual-Doppler Velocimetry fusion method has been validated on datasets collected from multiple scenarios in the HoloOcean underwater simulation environment. Experimental results show that, compared to pure visual odometry, the proposed fusion method can effectively improve the localization accuracy of underwater robots. This invention provides a theoretical basis and technical guidance for the accurate positioning of underwater robots.

[0080] Figure 2 This is a flowchart illustrating the vision-Doppler velocimeter fusion framework provided by the present invention, as shown below. Figure 2 As shown, based on the ORB-SLAM3 framework, this invention designs a fusion module related to the DVL sensor. In the original ORB-SLAM3 system, the visual tracking thread can be divided into two stages. The first stage includes three modes: constant velocity motion model tracking, reference keyframe tracking, and relocalization tracking. The latter two modes are only used in case of system initialization or tracking failure, while the first mode is used in most cases. Therefore, constant velocity motion model tracking is the main process affecting the visual tracking performance in the first stage. The second stage of the visual tracking thread is called local map tracking. This stage projects the local map points corresponding to the current frame of the camera onto the current frame to obtain more feature point matching relationships, and then further optimizes the camera pose obtained in the first stage to obtain a more accurate pose estimate. The improvement of this invention focuses on the tracking thread in the first stage mentioned above. The proposed visual-DVL tightly coupled fusion method specifically includes the following steps:

[0081] 1. Associate DVL measurements with image frames. Considering that the data frame rate of DVL is lower than the image frame rate of the camera, this invention associates DVL measurement information with each camera image frame according to the "most recent in time" principle, that is, uses the most recent DVL data in time to add velocity measurement information to each image frame.

[0082] 2. Improving initial camera pose estimation in the current frame using DVL measurement data. In the first stage of the tracking thread, this invention jointly utilizes visual information and DVL measurement information to predict the current camera pose. Specifically, this invention uses the constant velocity model from the first stage of the visual tracking process to obtain the rotation component of the camera pose in the current frame, while using DVL velocity measurement data to obtain the position component of the camera pose in the current frame.

[0083] Figure 3 This is a schematic diagram of the coordinate system relationships of the camera-DVL system provided by the present invention, as shown below. Figure 3 The diagram illustrates the relationship between the various coordinate systems in the camera-DVL system as it moves from time k-2 to time k. In this invention, the 3D pose is represented by a transformation matrix T∈SE(3), where T consists of a rotation matrix R∈SO(3) and a translation vector p∈R. 3 Composition. Furthermore, the transformation matrix. Let {N} represent the pose of coordinate system {N} at time j relative to coordinate system {M} at time i. In this invention, the camera coordinate system and DVL coordinate system at time k are represented as {C}. k} and {D k The relative pose between the two coordinate systems is fixed, represented as... Furthermore, this invention represents the global world coordinate system as {W}, and defines the position of the camera in the first frame as the origin of the world coordinate system {W}.

[0084] In the original ORB-SLAM3 system, if visual tracking was successful in the previous frame, a constant velocity motion model is used to predict the camera pose in the current frame. This is based on the assumption that the camera moves at a constant velocity between adjacent frames. The camera pose in the current frame can be predicted using the camera's pose and velocity in the previous frame, as follows:

[0085]

[0086] In the formula, It is the predicted pose of the camera in the current frame. It is the camera pose of the previous frame. It is the camera's speed in the previous frame, which is considered to be the same as the camera's speed in the penultimate frame. equal:

[0087]

[0088] The method proposed in this invention relaxes the constant velocity assumption and uses velocity measurement data from a DVL sensor to predict the camera's position at the current frame. Since the DVL sensor only provides XYZ three-dimensional linear velocity measurements and lacks attitude information, this invention derives the position using DVL velocity measurement data. The translation component, while preserving The rotational component is consistent with the derivation results of the constant velocity model, as follows:

[0089]

[0090] According to equation (3), Translation components It can be represented as:

[0091]

[0092] In the formula, It is a DVL sensor in {D k} and {D k-1 The displacement between} can be calculated as:

[0093]

[0094] In the formula, dt is the measurement value of the DVL sensor at time k-1, and dt is the time difference between two image frames. This invention assumes that the measurement noise of the DVL sensor follows a Gaussian distribution, i.e., n v ~N(0,σ v ·I).

[0095] Once the camera's velocity in the previous frame is obtained through visual and DVL measurements, its pose in the current frame can be predicted based on the camera's pose in the previous frame. Then, the first optimization is performed based on the reprojection relationship of map points, completing the first stage of tracking.

[0096] 3. Design the DVL error term based on the measurements from the DVL sensor and the estimated camera pose values ​​from two adjacent frames.

[0097] In the second stage of the original ORB-SLAM3 system's tracking thread, local map points in the local window are projected onto the current frame to find more feature point matching relationships. Then, the reprojection constraints of these matching feature points are used to perform secondary optimization on the camera pose estimated in the first stage for the current frame. Based on the original system, this invention designs a DVL velocity constraint between two camera image frames. It uses the velocity measurements from the DVL sensor to constrain the camera's positional changes between adjacent frames, thereby improving the accuracy of pose estimation.

[0098] Given the camera pose in the previous frame and pose in the current frame Then DVL in {D k} and {D k-1 The displacement between} can be derived as:

[0099]

[0100] Combining (5) and (6), the DVL error term can be defined as:

[0101]

[0102] According to (7), The covariance can be derived from the measurement noise of DVL, i.e. Then the weights of the DVL error term can be determined according to... The covariance is obtained.

[0103] 4. Based on the visual reprojection error, and combined with the DVL velocity error term designed in step 3, perform visual-DVL joint optimization.

[0104] In the second stage of the tracking thread, namely the local map tracking stage, this invention uses visual information and DVL measurement information to jointly optimize the current frame pose of the camera. Through visual reprojection constraints and DVL velocity error term constraints, the current frame camera pose estimate obtained in the first stage is optimized a second time by Bundle Adjustment (BA).

[0105] Specifically, this invention constructs an objective function to minimize the reprojection error term of the matched local map points and the DVL velocity error term between two camera image frames. Given the camera pose of the current frame... And the matching world coordinate system 3D point X i ∈R 3 and the corresponding 2D image pixel position u i ∈R 2 Where i∈χ, and χ is the set of all 3D-2D matching relationships, the visual reprojection error can be expressed as:

[0106]

[0107] In the formula, ρ is the robust kernel function, Π represents the camera projection model, and Σ v It is the covariance matrix related to the keypoint scale. Then, the camera pose optimization problem in the second stage of the tracking process using the vision-DVL fusion method proposed in this invention can be expressed as:

[0108]

[0109] This is a nonlinear least squares optimization problem, which can be solved using methods such as Gauss-Newton (GN) or Levenberg-Marquardt (LM) implemented in the nonlinear optimization library g2o. Figure 4This is a schematic diagram of the factors in the visual-DVL fusion system provided by the present invention, as shown below. Figure 4 As shown, the parameter to be optimized is the camera pose in the current frame, while the camera pose in the previous frame is fixed. The DVL velocity factor is a residual term proposed in this invention, used to constrain the positional change of the camera between two frames.

[0110] After obtaining the current camera pose through the two stages of the tracking thread, local map maintenance and keyframe processing are performed in the local mapping thread.

[0111] HoloOcean, an underwater simulation environment, includes two scenarios: PierHarbor and OpenWater. These scenarios are used to simulate underwater robots performing underwater inspection and surveillance tasks in a near-shore port setting and searching for and inspecting submarine wreckage in an open water setting, respectively.

[0112] Figure 5 This is a schematic diagram of the robot motion trajectory for the binocular vision-DVL fusion pattern localization method provided by the present invention, as shown below. Figure 5 As shown, the motion trajectories obtained by the binocular vision mode of the ORB-SLAM3 system and the binocular vision-DVL fusion mode proposed in this invention are presented in the OpenWater scene. The results show that the trajectory estimated by the fusion method proposed in this invention is closer to the real trajectory than the binocular vision mode of the original ORB-SLAM3 system.

[0113] Table 1 shows the absolute translation error between the trajectory estimated by the binocular vision mode of the ORB-SLAM3 system and the proposed binocular vision-DVL fusion mode and the true trajectory. The results in Table 1 show that, compared with the binocular vision mode of the original ORB-SLAM3 system, the proposed vision-DVL fusion method achieves a smaller absolute translation error, verifying the effectiveness of the proposed fusion method in improving positioning accuracy.

[0114] Table 1

[0115]

[0116] 1 Each sequence was evaluated 11 times, and the mean and median are shown.

[0117] 2 DVL measurements are only used to provide error constraints, not initial attitude estimation;

[0118] 3 DVL measurements are used to provide both initial attitude estimation and error constraints;

[0119] 4This improvement refers to the percentage reduction in the median error of the proposed method compared to the ORB-SLAM3 stereo mode.

[0120] Table 2 shows the velocity errors in the X, Y, and Z directions between the trajectory estimated by the binocular vision mode of the ORB-SLAM3 system and the trajectory by the binocular vision-DVL fusion mode proposed in this invention, respectively. The results in Table 2 demonstrate that, compared to the binocular ORB-SLAM3 method, the velocity obtained by the binocular-DVL fusion method is more consistent with the true value, verifying that the fusion method proposed in this invention effectively utilizes the velocity measurement information from the DVL sensor.

[0121] Table 2

[0122]

[0123]

[0124] The underwater robot positioning device provided by the present invention is described below. The underwater robot positioning device described below can be referred to in correspondence with the underwater robot positioning method described above.

[0125] Figure 6 This is a schematic diagram of the underwater robot positioning device provided by the present invention, as shown below. Figure 6 As shown, the device includes:

[0126] The acquisition module 600 is used to acquire velocity measurement data based on DVL, and to acquire the first velocity component of the robot at the first frame time and the DVL error term at the second frame time; the first frame time is the previous frame time of the second frame time.

[0127] The determination module 610 is used to determine the initial pose estimate of the robot at the second frame time based on the first velocity component of the robot at the first frame time.

[0128] The optimization module 620 is used to optimize the initial pose estimate of the robot at the second frame time based on the DVL error term of the robot at the second frame time, and to determine the final pose estimate of the robot at the second frame time.

[0129] Optionally, based on the velocity measurement data from DVL, the first velocity component of the robot at the first frame moment is obtained, including:

[0130] The robot's first velocity component at the first frame time is obtained by multiplying the second velocity component of the robot at the first frame time with the rotation component of the robot from the visual coordinate system to the DVL coordinate system, the translation component of the robot from the DVL coordinate system to the visual coordinate system with the difference between the displacement of the DVL in the DVL coordinate system between the first and second frames time, and the translation component of the robot from the visual coordinate system to the DVL coordinate system.

[0131] Optionally, the robot's first velocity component at the first frame time is determined based on the following formula:

[0132]

[0133] in, This represents the robot's first velocity component at the first frame. This represents the robot's second velocity component at the first frame. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system. This represents the displacement of DVL in the DVL coordinate system between the first frame and the second frame. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system.

[0134] Optionally, based on the robot's first velocity component at the first frame, the initial pose estimate of the robot at the second frame is determined, including:

[0135] Based on the robot's first velocity component, second velocity component, and final pose estimate in the first frame, the robot's initial pose estimate in the second frame is determined.

[0136] Optionally, based on the velocity measurement data from the DVL (Dynamic Value Chain) algorithm, the DVL error term for the robot at the second frame time is obtained, including:

[0137] Based on the difference between the displacements in the DVL coordinate system between the first and second frame times, the rotation component of the robot transforming from the visual coordinate system to the DVL coordinate system, the rotation component of the robot in the first frame time, the rotation component of the robot relative to the visual coordinate system in the second frame time, the translation component of the robot transforming from the visual coordinate system to the DVL coordinate system in the second frame time, the translation component of the robot relative to the visual coordinate system in the second frame time, the translation component of the robot in the first frame time, and the translation component of the robot transforming from the DVL coordinate system to the visual coordinate system, the DVL error term of the robot in the second frame time is obtained.

[0138] Optionally, the robot's DVL error term at the second frame time is determined based on the following formula:

[0139]

[0140] in, This represents the DVL error term of the robot at the second frame time. This represents the displacement of DVL in the DVL coordinate system between the first frame and the second frame. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the robot's rotation component at the first frame. This represents the rotation component of the robot relative to the visual coordinate system at the second frame. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot relative to the visual coordinate system at the second frame time. This represents the translation component of the robot at the first frame. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system.

[0141] Optionally, based on the robot's DVL error term at the second frame time, the initial pose estimate of the robot at the second frame time is optimized to determine the final pose estimate of the robot at the second frame time, including:

[0142] Based on the robot's DVL error term and reprojection error at the second frame, a nonlinear least squares optimization problem is solved with the robot's pose at the second frame as the objective. This optimizes the robot's initial pose estimate at the second frame and determines the robot's final pose estimate at the second frame.

[0143] It should be noted that the device provided by the present invention can implement all the method steps implemented in the above method embodiments and can achieve the same technical effect. Therefore, the parts and beneficial effects that are the same as those in the method embodiments will not be described in detail here.

[0144] Figure 7 This is a schematic diagram of the structure of the electronic device provided by the present invention, such as... Figure 7As shown, the electronic device may include a processor 710, a communications interface 720, a memory 730, and a communication bus 740, wherein the processor 710, the communications interface 720, and the memory 730 communicate with each other via the communication bus 740. The processor 710 can call logical instructions in the memory 730 to execute any of the underwater robot positioning methods provided in the above embodiments.

[0145] Furthermore, the logical instructions in the aforementioned memory 730 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, essentially, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0146] It should be noted that the electronic device provided by the present invention can implement all the method steps implemented in the above method embodiments and can achieve the same technical effect. Therefore, the parts and beneficial effects that are the same as those in the method embodiments will not be described in detail here.

[0147] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to perform any of the underwater robot positioning methods provided in the above embodiments.

[0148] It should be noted that the non-transitory computer-readable storage medium provided by the present invention can implement all the method steps implemented in the above method embodiments and can achieve the same technical effect. Here, the parts that are the same as those in the method embodiments and the beneficial effects will not be described in detail.

[0149] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0150] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0151] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for locating an underwater robot, characterized in that, include: Based on the velocity measurement data from the Doppler velocimeter (DVL), the first velocity component of the robot at the first frame time and the DVL error term at the second frame time are obtained. The first frame time is the frame time preceding the second frame time; Based on the first velocity component of the robot at the first frame time, the initial pose estimate of the robot at the second frame time is determined; Based on the DVL error term of the robot at the second frame time, the initial pose estimation value of the robot at the second frame time is optimized to determine the final pose estimation value of the robot at the second frame time. The DVL-based velocity measurement data is used to obtain the robot's DVL error term at the second frame time, including: Based on the difference between the displacements in the DVL coordinate system between the first frame and the second frame, the rotation component of the robot transforming from the visual coordinate system to the DVL coordinate system, the rotation component of the robot at the first frame, the rotation component of the robot relative to the visual coordinate system at the second frame, the translation component of the robot transforming from the visual coordinate system to the DVL coordinate system, the translation component of the robot relative to the visual coordinate system at the second frame, the translation component of the robot at the first frame, and the translation component of the robot transforming from the DVL coordinate system to the visual coordinate system, the DVL error term of the robot at the second frame is obtained. The DVL error term of the robot at the second frame time is determined based on the following formula: ； Among them, the This represents the DVL error term of the robot at the second frame time. This represents the displacement of the DVL in the DVL coordinate system between the first frame time and the second frame time. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the rotation component of the robot at the first frame time. This represents the rotation component of the robot relative to the visual coordinate system at the second frame time. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot relative to the visual coordinate system at the second frame time. This represents the translation component of the robot at the first frame time. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system.

2. The underwater robot positioning method according to claim 1, characterized in that, The DVL-based velocity measurement data, which obtains the robot's first velocity component at the first frame time, includes: The first velocity component of the robot at the first frame time is obtained based on the product of the second velocity component of the robot at the first frame time and the rotation component of the robot when it is transformed from the visual coordinate system to the DVL coordinate system, the difference between the translation component of the robot when it is transformed from the DVL coordinate system to the visual coordinate system and the displacement of the DVL in the DVL coordinate system between the first frame time and the second frame time, and the translation component of the robot when it is transformed from the visual coordinate system to the DVL coordinate system.

3. The underwater robot positioning method according to claim 2, characterized in that, The first velocity component of the robot at the first frame time is determined based on the following formula: ； Among them, the This represents the first velocity component of the robot at the first frame time. This represents the second velocity component of the robot at the first frame time. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system. This represents the displacement of the DVL in the DVL coordinate system between the first frame time and the second frame time. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system.

4. The underwater robot positioning method according to claim 1, characterized in that, Determining the initial pose estimate of the robot at the second frame time based on the robot's first velocity component at the first frame time includes: Based on the robot's first velocity component at the first frame time, the robot's second velocity component at the first frame time, and the robot's final pose estimate at the first frame time, the robot's initial pose estimate at the second frame time is determined.

5. The underwater robot positioning method according to claim 1, characterized in that, The step of optimizing the initial pose estimate of the robot at the second frame time based on the DVL error term of the robot at the second frame time, and determining the final pose estimate of the robot at the second frame time, includes: Based on the DVL error term and reprojection error of the robot at the second frame time, a nonlinear least squares optimization problem is solved with the robot's pose at the second frame time as the objective. The initial pose estimate of the robot at the second frame time is optimized to determine the final pose estimate of the robot at the second frame time.

6. An underwater robot positioning device, characterized in that, include: The acquisition module is used to acquire velocity measurement data based on DVL, and to acquire the robot's first velocity component at the first frame time and the DVL error term at the second frame time. The first frame time is the frame time preceding the second frame time; The determination module is used to determine the initial pose estimate of the robot at the second frame time based on the first velocity component of the robot at the first frame time. An optimization module is used to optimize the initial pose estimation value of the robot at the second frame time based on the DVL error term of the robot at the second frame time, and determine the final pose estimation value of the robot at the second frame time. The DVL-based velocity measurement data is used to obtain the robot's DVL error term at the second frame time, including: Based on the difference between the displacements in the DVL coordinate system between the first frame and the second frame, the rotation component of the robot transforming from the visual coordinate system to the DVL coordinate system, the rotation component of the robot at the first frame, the rotation component of the robot relative to the visual coordinate system at the second frame, the translation component of the robot transforming from the visual coordinate system to the DVL coordinate system, the translation component of the robot relative to the visual coordinate system at the second frame, the translation component of the robot at the first frame, and the translation component of the robot transforming from the DVL coordinate system to the visual coordinate system, the DVL error term of the robot at the second frame is obtained. The DVL error term of the robot at the second frame time is determined based on the following formula: ； Among them, the This represents the DVL error term of the robot at the second frame time. This represents the displacement of the DVL in the DVL coordinate system between the first frame time and the second frame time. This represents the rotational component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the rotation component of the robot at the first frame time. This represents the rotation component of the robot relative to the visual coordinate system at the second frame time. This represents the translation component of the robot's transformation from the visual coordinate system to the DVL coordinate system. This represents the translation component of the robot relative to the visual coordinate system at the second frame time. This represents the translation component of the robot at the first frame time. This represents the translation component of the robot's transformation from the DVL coordinate system to the visual coordinate system.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, When the processor executes the program, it implements the underwater robot positioning method as described in any one of claims 1 to 5.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the underwater robot positioning method as described in any one of claims 1 to 5.

Images