Underwater robot trajectory tracking method and related device

Abstract

The invention discloses an underwater robot trajectory tracking method and a related device, and relates to the technical field of underwater robot autonomous navigation, and the method comprises the steps: synchronously collecting the original data of an underwater robot camera, an IMU and a DVL; performing time synchronization on IMU and DVL data by taking a camera timestamp as a reference, and converting DVL speed data into an IMU coordinate system; calculating a relative translation vector and a time interval based on the relative pose transformation matrix of the adjacent key frames, and determining an average speed estimation value of the underwater robot between the adjacent key frames; comparing the average speed estimation value with the actual measurement speed of the DVL, and determining a speed error function of the DVL; and constructing a weighted error cost function containing visual re-projection, IMU pre-integration and DVL speed error terms, solving optimal estimation of key frame poses and map point states through nonlinear least square optimization, and outputting an optimized trajectory of the underwater robot. According to the invention, the precision of trajectory tracking of the underwater robot is effectively improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of autonomous navigation of underwater robots, in particular to an underwater robot trajectory tracking method and related device. BACKGROUND

[0002] Underwater robots need to rely on high-precision trajectory tracking to achieve autonomous navigation in tasks such as ocean exploration and resource exploration. However, there are many challenges in the underwater environment: GPS signal failure leads to the inability to rely on satellite positioning; the absorption and scattering of light by water cause visual degradation (image blurring, sparse features), and pure visual trajectory tracking methods (such as ORB-SLAM3 visual mode) have low positioning accuracy and insufficient robustness in this scenario; methods that only fuse vision and IMU can partially alleviate the problem, but there is still trajectory drift under long-term operation or dynamic water flow interference.

[0003] In the prior art, multi-sensor fusion methods (such as the SVIn system) introduce imaging sonar to assist positioning, but sonar data processing is complex and limited by the field of view; traditional loosely coupled fusion of DVL and inertial navigation only uses DVL output results, which has low information utilization rate and is difficult to meet high-precision requirements. Therefore, there is an urgent need for a tightly coupled method that can fully integrate the advantages of vision, IMU, and DVL to improve the trajectory tracking performance of underwater robots in complex environments. SUMMARY

[0004] The purpose of the present application is to provide an underwater robot trajectory tracking method and related device, which can effectively solve the problem of low trajectory tracking accuracy in complex underwater environments.

[0005] To achieve the above purpose, the present application provides the following solutions: In a first aspect, the present application provides an underwater robot trajectory tracking method, comprising: synchronously collecting raw data of a camera, an inertial measurement unit (IMU), and a Doppler velocity log (DVL) of an underwater robot; the raw data includes camera data, IMU data, and DVL data; time-synchronizing the IMU data and the DVL data by linear interpolation based on the timestamp of the camera, and converting the speed data in the DVL data from the self-coordinate system to the IMU coordinate system; calculating a relative translation vector and a time interval based on a relative pose transformation matrix of adjacent keyframes i and j, and determining an average speed estimate of the underwater robot between the adjacent keyframes; the relative pose transformation matrix is used to represent the pose change of the underwater robot from the keyframe i to the keyframe j ; comparing the average speed estimate with the actual speed measured by the Doppler velocity log in the corresponding time period to determine a DVL speed error function; A weighted error cost function is constructed, which includes visual reprojection error, IMU pre-integration error, and DVL velocity error. The optimal estimates of keyframe pose and map point state are solved by nonlinear least squares optimization, and the optimized trajectory of the underwater robot is output. The map point is the residual between the reprojected pixel coordinates of the image plane and the actual observed pixel coordinates.

[0006] Optionally, based on the camera's timestamp, time synchronization of IMU data and DVL data is performed using linear interpolation, specifically including: With camera timestamp T c Based on the baseline, collect N Group data synchronization; Regarding the first i Group data synchronization, according to the formula T imu,i = k 1 T c,i + b 1. Linear regression fitting of inertial measurement unit timestamps T imu,i With camera timestamp T c,i The relationship is used to obtain the time offset of the inertial measurement unit relative to the camera. b 1 ; According to the formula T dvl,i = k 2 T c,i + b 2. Linear regression fitting of Doppler velocimeter timestamps T dvl,i With camera timestamp T c,i The relationship was used to obtain the time offset of the Doppler velocimeter relative to the camera. b 2 ; When the camera keyframe timestamp is received T c,k At that time, linear interpolation is performed separately in the buffer windows of IMU data and DVL data to obtain... T c,k Velocity data in IMU data and DVL data at any given time.

[0007] Optionally, when a camera keyframe timestamp is received T c,k At that time, linear interpolation is performed separately in the buffer windows of IMU data and DVL data to obtain... T c,kThe velocity data in the IMU data and DVL data at each moment specifically includes: According to the formula ,calculate T c,k IMU data at any given time; According to the formula ,calculate T c,k Velocity data in the DVL data at any given time; in, T imu1 <T c,k <T imu2 , a x1 for T imu1 x-axis acceleration data of the IMU at time t. v x1 This represents the most recent x-axis velocity value measured by DVL before the target time. a x2 for T imu2 time imu x-axis acceleration data v x2 The closest distance to the target time actually measured by DVL after the target time. x The velocity value in the direction.

[0008] Optionally, the velocity data in the DVL data is transformed from its own coordinate system to the IMU coordinate system, specifically including: The rotation matrix R from the DVL coordinate system {D} to the IMU coordinate system {B} is obtained through offline calibration. BD ;in, ; According to the formula The velocity data in the DVL data is in its own coordinate system. v D Velocity converted to IMU coordinate system v B .

[0009] Optionally, the formula for the relative pose transformation matrix is: ; in, T i,j Keyframe i The relative pose transformation matrix to keyframe j. It is a 1×3 zero vector. , , , , , It is a 3×3 identity matrix.

[0010] Optionally, the formula for calculating the average velocity estimate of the underwater robot between adjacent keyframes is as follows: ; in, p i,j Δ is a relative translation vector. t = t j -t i Δ t For keyframes i To keyframe j The time interval.

[0011] Optionally, the formula for the weighted error cost function is: ; Among them, Ω vis Ω IMU Ω DVL These are the information matrices for visual reprojection constraints, IMU pre-integration constraints, and DVL velocity constraints, respectively. e vis,k,m For map points P m In the k The reprojection pixels of the frame and the residual of the observation, e IMU,i For the IMU pre-integration error term, e DVL,i,j This is the DVL pre-integration error term.

[0012] Secondly, this application provides an underwater robot trajectory tracking device, comprising: The data acquisition module is used to simultaneously acquire raw data from the underwater robot's camera, inertial measurement unit (IMU), and Doppler velocimeter (DVL); the raw data includes camera data, IMU data, and DVL data. The synchronization module is used to synchronize IMU data and DVL data in time using linear interpolation based on the camera's timestamp, and to transform the velocity data in the DVL data from its own coordinate system to the IMU coordinate system. The velocity calculation module is used to calculate the relative translation vector and time interval based on the relative pose transformation matrix of adjacent keyframes i and j, and to determine the average velocity estimate of the underwater robot between adjacent keyframes; the relative pose transformation matrix is ​​used to represent the pose change of the underwater robot from keyframe i to keyframe j. The function construction module is used to compare the average speed estimate with the actual speed measured by the Doppler velocimeter within the corresponding time period to determine the DVL speed error function. The output module is used to construct a weighted error cost function that includes visual reprojection error term, IMU pre-integration error term and DVL velocity error term, and solves the optimal estimate of keyframe pose and map point state through nonlinear least squares optimization, and outputs the optimized trajectory of the underwater robot; the map point is the residual between the reprojected pixel coordinates of the image plane and the actual observed pixel coordinates.

[0013] Thirdly, this application provides a computer device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the underwater robot trajectory tracking method described in any one of the above.

[0014] Fourthly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the underwater robot trajectory tracking method described in any one of the above descriptions.

[0015] According to the specific embodiments provided in this application, the following technical effects are disclosed: This application provides an underwater robot trajectory tracking method and related apparatus. By tightly coupling vision, IMU, and DVL data, it effectively solves the problem of low trajectory tracking accuracy in complex underwater environments. The method utilizes camera timestamps to achieve time alignment of multi-sensor data, ensuring accurate data synchronization. Through linear interpolation and coordinate transformation, DVL velocity data is unified to the IMU coordinate system, improving the consistency of data fusion. The average velocity estimate is calculated based on the relative pose transformation matrix, and a DVL velocity error function is constructed by combining actual measurements from a Doppler velocimeter, further improving the accuracy of velocity estimation. Finally, by constructing a weighted error cost function including visual reprojection error, IMU pre-integration error, and DVL velocity error, and employing nonlinear least squares optimization to solve for the optimal estimation of keyframe pose and map point states, high-precision tracking of the underwater robot trajectory is achieved. Attached Figure Description

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

[0017] Figure 1This is an application environment diagram of an underwater robot trajectory tracking method according to an embodiment of this application; Figure 2 A flowchart illustrating an underwater robot trajectory tracking method provided in an embodiment of this application; Figure 3 A schematic diagram of the tracking logic provided in one embodiment of this application; Figure 4 A functional module diagram of an underwater robot trajectory tracking device provided in an embodiment of this application; Figure 5 This is a schematic diagram of the structure of a computer device provided in an embodiment of this application. Detailed Implementation

[0018] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0019] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0020] The underwater robot trajectory tracking method provided in this application embodiment can be applied to, for example... Figure 1 In the application environment shown, terminal 102 communicates with server 104 via a network. A data storage system can store the data that server 104 needs to process. The data storage system can be set up independently, integrated into server 104, or placed in the cloud or on another server.

[0021] The terminal 102 can be, but is not limited to, various desktop computers, laptops, smartphones, tablets, IoT devices, and portable wearable devices. IoT devices can include smart speakers, smart TVs, smart air conditioners, and smart in-vehicle devices. Portable wearable devices can include smartwatches, smart bracelets, and head-mounted devices. The server 104 can be implemented using a standalone server or a server cluster composed of multiple servers, or it can be a cloud server.

[0022] In one exemplary embodiment, such as Figure 2 As shown, an underwater robot trajectory tracking method is provided. This method is executed by a computer device, specifically a terminal or server, or both. In this embodiment, the method is applied to... Figure 1Taking server 104 as an example, the explanation includes the following steps 201 to 205. Wherein: Step 201: Synchronously acquire raw data from the underwater robot's camera, inertial measurement unit (IMU), and Doppler velocimeter (DVL); the raw data includes camera data, IMU data, and DVL data. Step 202: Using the camera's timestamp as a reference, time synchronization is performed on the IMU data and DVL data through linear interpolation, and the velocity data in the DVL data is transformed from its own coordinate system to the IMU coordinate system. Step 203: Based on the relative pose transformation matrix of adjacent keyframes i and j, calculate the relative translation vector and time interval, and determine the average velocity estimate of the underwater robot between adjacent keyframes; the relative pose transformation matrix is ​​used to represent the pose change of the underwater robot from keyframe i to keyframe j. Step 204: Compare the estimated average speed with the actual speed measured by the Doppler velocimeter within the corresponding time period to determine the DVL speed error function; Step 205: Construct a weighted error cost function that includes visual reprojection error term, IMU pre-integration error term, and DVL velocity error term, and solve for the optimal estimation of keyframe pose and map point state through nonlinear least squares optimization, and output the optimized trajectory of the underwater robot; the map point is the residual between the reprojected pixel coordinates of the image plane and the actual observed pixel coordinates.

[0023] In one exemplary embodiment, when performing steps 201-202, the specific steps may be as follows: Specifically, based on the camera's timestamp, time synchronization of IMU data and DVL data is achieved through linear interpolation, including: For the i-th set of synchronized data, according to the formula T imu,i = k 1 T c,i + b 1. Linear regression fitting of inertial measurement unit timestamps T imu,i With camera timestamp T c,i The relationship is used to obtain the time offset of the inertial measurement unit relative to the camera. b 1; According to the formula T dvl,i = k 2 T c,i + b 2. Linear regression fitting of Doppler velocimeter timestamps T dvl,iWith camera timestamp T c,i The relationship was used to obtain the time offset of the Doppler velocimeter relative to the camera. b 2; When the camera keyframe timestamp is received T c,k At that time, linear interpolation is performed separately in the buffer windows of IMU data and DVL data to obtain... T c,k Velocity data in IMU data and DVL data at any given time.

[0024] Specifically, when the camera keyframe timestamp is received T c,k At that time, linear interpolation is performed separately in the buffer windows of IMU data and DVL data to obtain... T c,k The velocity data in the IMU data and DVL data at each moment specifically includes: According to the formula ,calculate T c,k IMU data at any given time; According to the formula ,calculate T c,k Velocity data in the DVL data at any given time; in, T imu1 <T c,k <T imu2 , a x1 for T imu1 real-time IMU x Axis acceleration data, v x1 The closest distance to the target time actually measured by DVL before the target time. x The velocity value in the direction, a x2 For T imu2 time imu of x Axis acceleration data, v x2 The closest distance to the target time actually measured by DVL after the target time. x The velocity value in the direction.

[0025] The transformation of velocity data in DVL data from its own coordinate system to the IMU coordinate system specifically includes: The rotation matrix R from the DVL coordinate system {D} to the IMU coordinate system {B} is obtained through offline calibration.BD ;in, ; According to the formula The velocity v in the DVL data in its own coordinate system D velocity v transformed to IMU coordinate system B .

[0026] Specifically, when performing time synchronization of multi-sensor data, the following can be done: The specific parameters of sensors such as cameras, IMUs, and DVLs are determined, and a dedicated data receiving node is developed based on the ROS Noetic framework to realize the real-time acquisition and encapsulation of multi-sensor data.

[0027] With camera timestamp T c Based on the baseline, collect N Group synchronization data: Using the camera's timestamp as a reference, the timestamps of the IMU and DVL are calibrated. A fixed number of group synchronization data are collected for each sensor, and the timestamp offset is calculated. This time offset is used to address the issue of inconsistent timestamps across multiple sensors, taking into account differences in sampling rates. Because the sampling frequencies and start times of the camera, IMU, and DVL differ, data may become misaligned in the time dimension.

[0028] Let the camera timestamp be T c IMU timestamp is T imu DVL timestamp is T dvl .collection N Group synchronization data ( N ≥10 (to ensure statistical validity), for the first i Group data, camera time is T c,i IMU time is T imu,i DVL time is T dvl,i 。

[0029] For IMU data interpolation, an IMU data buffer window is constructed, and the time offset Δ between the IMU and the camera is... t imu-c Fitting via linear regression T imu,i = k 1 T c,i + b 1, of which b 1 represents the time offset of the IMU relative to the camera, when a camera keyframe is received.T c,k At that time, search the cache window for timestamps less than [a certain value]. T c,k The most recent IMU data point and timestamp are greater than T c,k The most recent IMU data points, among which, T imu1 <T c,k <T imu2 Linear interpolation calculation k Time-based IMU data.

[0030] Similarly, for DVL data interpolation, a DVL data cache window is constructed, and linear interpolation is used to calculate the DVL speed.

[0031] Due to their different installation locations, each sensor's own coordinate system differs from the robot's body coordinate system (defined as the {B} system, based on the IMU coordinate system). x Axial forward, y Axis to the right, z There is a difference (axis downwards). The velocity output by DVL is in its own coordinate system {D}, while optimization is usually performed in the IMU's body coordinate system {B}. Therefore, coordinate transformation is needed to convert the DVL velocity data from its own coordinate system {D} to the IMU's coordinate system {B}. The least-squares problem for constructing the coordinate transformation relationship is as follows: ; Solving for the given information, we get: .

[0032] To address the issues of blurry and color distortion in underwater images, an image preprocessing module is added after the camera data receiving node, based on the distortion coefficients in the camera's intrinsic parameters. k 1 , k 2 , p 1 , p 2 ） The OpenCV undistort function is used to correct the distortion of the left and right images, eliminating the feature point offset caused by lens distortion, providing more accurate image data for subsequent feature extraction, and ensuring the accuracy of feature point positions.

[0033] .

[0034] .

[0035] Wherein, formula t ( x(This is used for transmittance calculation, formula) J c ( x ) is used for image calculation after dehazing, where t(x) is the number of pixels. x Transmittance at that location w It is the dehazing weighting coefficient, Ω ( x () is a local window centered on a pixel. I c (y) is the pixel value of the pixel within the local window in the channel. A c That is the corresponding atmospheric light value. J c ( x () represents the pixel value in the channel after dehazing; I c ( x ) is the pixel value of a pixel in the original hazy image under the channel.

[0036] In one exemplary embodiment, step 203 can be performed as follows: Construct DVL speed constraint factors to transform the high-frequency, stable speed information provided by DVL into constraints that can be used for backend optimization.

[0037] The trajectory of an underwater robot is composed of poses from consecutive keyframes: ; in, (Special orthogonal group, satisfying) and ), , It is a 1×3 zero vector. It is a 3×3 identity matrix.

[0038] According to the inverse operation rules of pose transformation, the relative pose transformation formula is: ; satisfy ; Substituting into the relative pose transformation formula and expanding, we get: ; Define the relative rotation matrix: ; but It can be simplified to: .

[0039] In the formula, T i,j Keyframe i To keyframej The relative pose transformation matrix, which fully describes the underwater robot's pose transformation from keyframes. i Time to keyframe j The pose changes at any given moment, including rotation and translation information, are the core foundational data for subsequent construction of DVL velocity constraint factors and backend optimization.

[0040] Since DVL measures the robot's instantaneous velocity, while keyframe pose represents discrete-time states, a relationship needs to be established between the two through average velocity. Assuming the robot moves at a constant velocity between two keyframes, using... T i,j Calculate keyframes i arrive j relative translation vector p i,j and time interval Δ t = t j - t i Then the first i Frame to the j The average velocity between keyframes can be calculated by the ratio of the relative translation vector to the time interval, as shown in the following formula: .

[0041] This average speed will then be compared with the actual speed measured by DVL to construct a constraint error. V meas,i,j It is the actual speed measured by DVL during the time period from keyframe i to j.

[0042] Backend optimization requires calculating the gradient direction of the error using the Jacobian matrix of the state variables through the error function, which necessitates derivation.

[0043] in, e DVL right T W,i and T W,j Jacobian matrix J DVL,i and J DVL,j : ; ; The above matrices are all 3×6 matrices (the derivative of the 3D error vector with respect to the 6D pose perturbation). In the graph optimization framework, they are unified with the Jacobian matrices of visual reprojection error and IMU pre-integration error, and the two Jacobian matrices are merged into the total Jacobian matrix of DVL constraint edges.

[0044] in, This matrix is ​​3×12 in dimension and corresponds to the derivative of the error vector with respect to the two 6-dimensional pose state variables. Subsequent optimization will use... J T DVL Ω DVL J DVL Calculate an approximate value for the Hessian matrix, which is used to iteratively update the keyframe pose.

[0045] In one exemplary embodiment, when performing steps 204-205, the specific steps may be as follows: The backend optimization, which integrates multiple constraints, uses a graph optimization framework to fuse visual, IMU, and DVL constraints to solve for the globally optimal trajectory.

[0046] The core of the optimization is to construct a weighted error cost function that includes three types of constraints: vision, IMU, and DVL. Each type of constraint corresponds to an error term, and its confidence level is reflected by weighting through an information matrix. A set of state variables is defined, containing the poses of N keyframes and M map points within the optimization window.

[0047] ; The key pose is represented as follows: ; in, For rotation matrix, It is a translation vector.

[0048] First, the visual reprojection error term is obtained. The visual constraint comes from the observation of map points by the binocular camera. The error is defined as the residual between the reprojected pixel coordinates of the map point on the image plane and the actual observed pixel coordinates.

[0049] The residual between the reprojected pixels of map point Pm in the k-th frame and the observation: .

[0050] The residual reprojection formula is used to quantify the error of visual observation, map points. The formula for reprojecting the reprojected pixels in the k-th frame with the observed residual is: ; The formula for the cost term, i.e., the weighted sum of squared errors, is: ; Similarly, the formula for the IMU pre-integration error term is: ; The formula for the cost term is: ; Similarly, the formula for the DVL pre-integration error term is: ; The formula for the cost term is: ; The formula for the nonlinear least squares problem that integrates three types of constraints is as follows: .

[0051] The error term is expanded in first order at the current state Xt, and the formula is: .

[0052] Convergence is determined when |et| < ϵ, and the optimal trajectory is output.

[0053] By fusing multiple constraints, the optimization results can suppress trajectory loss and IMU drift during visual degradation, while maintaining global consistency under DVL velocity constraints.

[0054] This application uses underwater surveying, a typical and complex scenario, as the test background. By combining a real hardware platform with a public dataset, it verifies the accuracy, robustness, and real-time performance of the underwater robot trajectory tracking method based on tight coupling of visual-inertial DVL.

[0055] The camera module resolution was set to 640×480 (balancing data volume and real-time processing), with a fixed frame rate of 20Hz (50ms exposure time per frame to avoid motion blur). The camera intrinsic parameter matrix was obtained using Zhang Zhengyou's calibration method, specifically as follows: ; Radial distortion coefficients k1=-0.1256, k2=0.0532, k3=0, and tangential distortion coefficients p1=9.94×10-5, p2=9.55×10-5.

[0056] The DVL module obtains the extrinsic parameters of the DVL relative to the IMU coordinate system through precision turntable calibration, including the rotation matrix: ; A dedicated data receiving module was developed based on the ROS node architecture to achieve real-time acquisition and standardized encapsulation of data from three types of sensors. Due to the differences in sampling rates among the sensors (camera 20Hz, IMU 100Hz, DVL 10Hz), time synchronization and coordinate transformation were performed.

[0057] When the camera keyframe timestamp T k At adjacent sampling time of DVL Tdvl,m With T dvl,m+1 When T is between, linear interpolation is used to calculate T. k DVL speed at time ; ; ; Transform the interpolated DVL velocity from its own coordinate system {D} to the IMU coordinate system {B} using the calibrated extrinsic matrix, the formula is: .

[0058] Set adjacent keyframes within the optimization window K i and K j ( j = i +1), their world coordinate system poses are respectively T W,i =[ R W,i , p W,i ]and T W,j =[ R W,j , p W,j ] Calculate the relative translation vector between two frames; .

[0059] Combined time interval Δ t = t j - t i Derivation of keyframe estimation speed: .

[0060] Take K i With K j The mean DVL velocity at time step is used as the measured velocity, and an error function is defined as follows: .

[0061] This error is a 3-dimensional vector, corresponding to x / y / z Directional velocity residual.

[0062] Fusion visual reprojection error e vis IMU pre-integration error e IMU With DVL speed error e DVL Construct a global optimization objective function: ; Among them, Ωvis Ω IMU Ω DVL These are information matrices representing three types of constraints.

[0063] The consistency between the trajectory and the ground truth is evaluated using absolute trajectory error (APE), and the pose error for each frame is calculated: ; Among them, T gt,k For the ground true pose, T est,k To optimize the estimated pose, ∥⋅∥F is the Frobenius norm, and the root mean square error (RMSE), mean error (Mean), and maximum error (Max) are calculated. A 3D trajectory is plotted using Pangolin (true value in red, estimated trajectory in blue), and an APE error curve is generated using the evo tool to visually display the error distribution. Simultaneously, a keyframe pose file (TUM format) is output, containing timestamps, x / y / z coordinates, and quaternion pose.

[0064] The core of the underwater robot trajectory tracking provided in this application is to achieve accurate approximation of the "estimated trajectory" to the "real trajectory" through multi-sensor collaboration in complex environments such as GPS failure, visual degradation, and water flow interference. Taking underwater cave exploration and archaeology as examples, robots need to operate continuously in environments with low visibility and strong dynamic interference. As multi-sensor data fusion and optimization progresses, trajectory drift is gradually suppressed. When the error between the estimated trajectory and the true trajectory (such as the absolute trajectory error RMSE) is significantly reduced and stabilized within a high-precision range, the requirements for autonomous navigation and operation can be met.

[0065] To overcome the limitations of single sensors and traditional fusion methods, this application designs a trajectory tracking method based on tightly coupled visual-inertial (VVL) tracking. First, using ORB-SLAM3 as the basic framework, a baseline scheme for joint visual-inertial tracking is preset. However, in actual underwater environments, pure visual mode suffers from low positioning accuracy (RMSE reaching 0.051m) due to image blurring and sparse features. Visual-inertial fusion alone is still affected by water flow interference, leading to accumulated IMU integration errors. Therefore, this study introduces a Doppler velocimeter (DVL), using ROS nodes to achieve spatiotemporal synchronization (time error ≤ 5ms) and coordinate transformation (transformation error ≤ 0.001m / s) of visual, IMU, and DVL data. Then, optimal control principles are used to construct the DVL velocity constraint factor (error function e). DVL =v est,i,j -v meas,i,j This involves integrating the g2o optimizer and iterative strategies (such as the Levenberg-Marquardt algorithm) into the backend local BA optimization in a tightly coupled manner, while improving the g2o optimizer and iterative strategies to achieve the synergistic minimization of visual reprojection error, IMU pre-integration error and DVL velocity error.

[0066] This method is fully adaptable to the characteristics of the underwater environment, improving trajectory tracking accuracy and robustness while ensuring real-time performance (single frame processing time ≤ 50ms). Experimental verification shows that on underwater datasets such as Aqualoc, the absolute trajectory error RMSE of the system after fusing DVL is reduced from 0.051m to 0.026m, an average improvement of over 48%, and a maximum error reduction of 43%. It can effectively suppress trajectory drift caused by visual degradation and water flow interference, providing stable technical support for underwater robot operations in complex scenarios. Furthermore, the scope of protection of this application covers various algorithm improvements, parameter optimizations, and scene adaptations within its core ideas and technical framework.

[0067] Based on the same inventive concept, this application also provides an underwater robot trajectory tracking device for implementing the underwater robot trajectory tracking method described above. The solution provided by this device is similar to the implementation described in the above method; therefore, the specific limitations in one or more embodiments of the underwater robot trajectory tracking device provided below can be found in the limitations of the underwater robot trajectory tracking method described above, and will not be repeated here.

[0068] In one exemplary embodiment, such as Figure 4 As shown, an underwater robot trajectory tracking device is provided, comprising: The data acquisition module 401 is used to simultaneously acquire raw data from the underwater robot's camera, inertial measurement unit (IMU), and Doppler velocimeter (DVL); the raw data includes camera data, IMU data, and DVL data. Synchronization module 402 is used to synchronize IMU data and DVL data in time using the camera's timestamp as a reference through linear interpolation, and to transform the velocity data in the DVL data from its own coordinate system to the IMU coordinate system. The velocity calculation module 403 is used to calculate the relative translation vector and time interval based on the relative pose transformation matrix of adjacent keyframes i and j, and to determine the average velocity estimate of the underwater robot between adjacent keyframes; the relative pose transformation matrix is ​​used to represent the pose change of the underwater robot from keyframe i to keyframe j. The function construction module 404 is used to compare the average speed estimate with the actual speed measured by the Doppler velocimeter within the corresponding time period to determine the DVL speed error function. The output module 405 is used to construct a weighted error cost function that includes visual reprojection error term, IMU pre-integration error term and DVL velocity error term, and solves the optimal estimate of keyframe pose and map point state through nonlinear least squares optimization, and outputs the optimized trajectory of the underwater robot; the map point is the residual between the reprojected pixel coordinates of the image plane and the actual observed pixel coordinates.

[0069] In one exemplary embodiment, a computer device is provided, which may be a server or a terminal, and its internal structure diagram may be as follows. Figure 5 As shown, this computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and a database. The internal memory provides the environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The database stores optimized trajectories. The I / O interfaces are used for exchanging information between the processor and external devices. The communication interface is used for communication with external terminals via a network connection. When the computer program is executed by the processor, it implements a trajectory tracking method for an underwater robot.

[0070] Those skilled in the art will understand that Figure 5 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0071] In one exemplary embodiment, a computer device is also provided, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps in the above-described method embodiments.

[0072] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0073] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data must comply with relevant regulations.

[0074] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments described above. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

[0075] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0076] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0077] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method of underwater robotic trajectory tracking, the method comprising: The method comprises: synchronously collecting raw data of a camera, an inertial measurement unit (IMU) and a Doppler velocity log (DVL) of the underwater robot; the raw data comprises camera data, IMU data and DVL data; time-synchronizing the IMU data and the DVL data by linear interpolation based on the timestamp of the camera, and converting the speed data in the DVL data from the self-coordinate system to the IMU coordinate system; Based on adjacent key frames i and key frames j a relative pose transformation matrix, a relative translation vector and a time interval are calculated, and an average speed estimate of the underwater robot between the adjacent key frames is determined; the relative pose transformation matrix is used to represent the change in the pose of the underwater robot from the key frame i time to the key frame j time; comparing the average speed estimation value with the actual speed measured by the Doppler velocity log in the corresponding time period to determine a DVL speed error function; constructing a weighted error cost function comprising a visual re-projection error term, an IMU pre-integration error term and a DVL speed error term, and solving the optimal estimation of the key frame pose and the map point state by nonlinear least squares optimization to output the optimized trajectory of the underwater robot; the map point is the residual error of the re-projection pixel coordinate on the image plane and the actual observation pixel coordinate.

2. The method of claim 1, wherein, The time-synchronizing the IMU data and the DVL data by linear interpolation based on the timestamp of the camera specifically comprises: With camera timestamp T c To base, collect N Group synchronization data; For the first i group of synchronized data, according to the formula T imu,i = k 1 T c,i + b 1, linear regression fitting inertial measurement unit timestamp T imu,i and camera timestamp T c,i relationship, get the time offset of the inertial measurement unit relative to the camera b 1 ; According to the formula T dvl,i = k 2 T c,i + b 2, linear regression fit of Doppler velocimeter timestamps T dvl,i to camera timestamps T c,i to obtain the time offset of the Doppler velocimeter relative to the camera b 2 ; When receiving the synchronization key frame timestamp T c,k Linear interpolation is performed in the buffer window of IMU data and DVL data respectively to obtain the speed data in the IMU data and DVL data at the moment T c,k of 3. The method of claim 2, wherein, When receiving the synchronization key frame timestamp T c,k Linear interpolation is performed in the buffer window of the IMU data and the DVL data respectively to obtain the speed data in the IMU data and the DVL data at the moment T c,k Linear interpolation is performed in the buffer window of the IMU data and the DVL data respectively to obtain the speed data in the IMU data and the DVL data at the moment According to the formula , the T c,k IMU data at the moment; According to the formula , the T c,k velocity data in the DVL data at the moment; in, T imu1 <T c,k <T imu2 , a x1 for T imu1 real-time IMU x Axis acceleration data, v x1 The closest distance to the target time actually measured by DVL before the target time. x The velocity value in the direction, a x2 for T imu2 time The converting the speed data in the DVL data from the self-coordinate system to the IMU coordinate system specifically comprises: of x Axis acceleration data, v x2 The closest distance to the target time actually measured by DVL after the target time. x The velocity value in the direction.

4. The method of claim 1, wherein, The formula expression of the relative pose transformation matrix is: The rotation matrix R of the DVL coordinate system {D} to the IMU coordinate system {B} is obtained by offline calibration BD ; wherein ; According to the formula The velocity data in the DVL data is in its own coordinate system. v D Velocity converted to IMU coordinate system v B .

5. The method of claim 1, wherein, The formula of the average speed estimation value of the underwater robot between adjacent key frames is: ； wherein T i,j is a key frame i is a relative pose transformation matrix from key frame j to key frame j, is a 1x3 zero vector, , , , , , is a 3x3 identity matrix.

6. The method of claim 1, wherein, The formula expression of the weighted error cost function is: ； wherein, p i,j is the relative translation vector, Δ t = t j -t i is the time interval from key frame t to key frame i to key frame j .

7. The method of claim 1, wherein, The method comprises: ； Among them, Ω vis Ω IMU Ω DVL These are the information matrices for visual reprojection constraints, IMU pre-integration constraints, and DVL velocity constraints, respectively. e vis,k,m For map points P m In the k The reprojection pixels of the frame and the residual of the observation, e IMU,i For the IMU pre-integration error term, e DVL,i,j This is the DVL pre-integration error term.

8. An underwater robotic trajectory tracking apparatus, characterized by, a data collection module configured to synchronously collect raw data of a camera, an inertial measurement unit (IMU) and a Doppler velocity log (DVL) of the underwater robot; the raw data comprises camera data, IMU data and DVL data; a synchronization module configured to time-synchronize the IMU data and the DVL data by linear interpolation based on the timestamp of the camera, and convert the speed data in the DVL data from the self-coordinate system to the IMU coordinate system; a function construction module configured to compare the average speed estimation value with the actual speed measured by the Doppler velocity log in the corresponding time period to determine a DVL speed error function; The velocity calculation module is used to calculate based on adjacent keyframes. i and keyframes j The relative pose transformation matrix is ​​used to calculate the relative translation vector and time interval, and to determine the average velocity estimate of the underwater robot between adjacent keyframes; the relative pose transformation matrix is ​​used to represent the pose change of the underwater robot from keyframe i to keyframe j. an output module configured to construct a weighted error cost function comprising a visual re-projection error term, an IMU pre-integration error term and a DVL speed error term, and solve the optimal estimation of the key frame pose and the map point state by nonlinear least squares optimization to output the optimized trajectory of the underwater robot; the map point is the residual error of the re-projection pixel coordinate on the image plane and the actual observation pixel coordinate. A memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that the processor executes the computer program to implement the underwater robot trajectory tracking method of any one of claims 1-7.

9. A computer device comprising: The computer program is executed by the processor to implement the underwater robot trajectory tracking method of any one of claims 1-7.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, ​

Images