AUV vertical docking method based on light beacon fusion perception

Abstract

The application discloses an AUV vertical docking method based on light beacon fusion perception, comprising the following steps: (1) obtaining the inertial navigation information and depth of the current AUV through an inertial navigation unit and a depth gauge; (2) obtaining the real-time image information of the light beacon position on the docking base station after calibrating the camera; (3) time synchronizing the inertial navigation unit, the depth gauge and the underwater camera; (4) predicting the position of the light beacon at the next time based on the inertial navigation information and the image Jacobian matrix on the basis of the current position; (5) introducing TensorRT in the target detection model, and measuring the position of the light beacon through the target detection of the image; (6) fusing the results of steps (4) and (5) through the unscented Kalman filter to obtain the optimal estimation of the light beacon position; (7) calculating the docking target between the AUV and the docking base station according to the light beacon position, and then completing the docking. The application can effectively deal with the scenes that the existing schemes cannot deal with, such as light source interference and light beacon shielding.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of autonomous docking of underwater robots, in particular to an AUV vertical docking method based on light beacon fusion perception. BACKGROUND

[0002] Autonomous Underwater Vehicle (AUV) is the main tool and carrier for marine resource exploration and environmental monitoring. Its long-term autonomous operation in special environments can effectively meet various scientific research needs. However, due to the limited energy carried by AUV itself, it must be returned to the base station for energy replenishment. During the process of AUV autonomous docking with the base station, the recognition and positioning of the base station guide light by AUV are crucial, otherwise it may cause serious consequences such as energy depletion and even loss of AUV.

[0003] The Chinese patent document with publication number CN113034399A discloses a method for removing pseudo-light source for guiding autonomous underwater robot recovery based on binocular vision, the Chinese patent document with publication number CN113901870A discloses an AUV underwater docking guide light signal extraction and recognition method based on adaptive threshold image binarization, the Chinese patent document with publication number CN113034590A discloses an AUV dynamic docking positioning method based on visual fusion, and the Chinese patent document with publication number CN117647998A discloses an underwater vehicle docking method and system based on dynamic vision sensor.

[0004] The existing disclosed technologies all identify the light beacon based on pure vision scheme. However, in the real AUV autonomous docking process, uncontrollable water flow disturbance will cause a large change in the attitude of AUV. When the light beacon deviates from the camera field of view, the existing vision scheme will inevitably fail. In addition, the feature of underwater light source is not obvious, and the scheme based on pure vision is difficult to cope with the misjudgment problem caused by similar interference light beacon, which is a potential hidden danger for the safe and accurate autonomous docking of AUV. SUMMARY

[0005] The present application discloses an AUV vertical docking method based on light beacon fusion perception, which can enhance the perception ability of AUV during docking based on light beacon, and effectively cope with scenes such as light source interference and light beacon shielding that existing schemes cannot cope with.

[0006] An AUV vertical docking method based on light beacon fusion perception, comprising the following steps:

[0007] (1) Obtain the inertial navigation information of the current AUV through the inertial navigation unit, including linear velocity and angular velocity; obtain the depth of the current AUV through the depth gauge;

[0008] (2) calibrate the underwater camera, and obtain real-time image information of the optical beacon position on the docking base station by using the calibrated underwater camera;

[0009] (3) time synchronization of the inertial navigation unit, depth gauge and underwater camera, and whenever the real-time image is updated, the inertial navigation information and depth of the AUV are calculated synchronously;

[0010] (4) based on the inertial navigation information and the image Jacobian matrix, the position of the optical beacon at the next time is predicted based on the current position;

[0011] (5) introducing TensorRT in the target detection model, and measuring the position of the optical beacon through target detection of the image taken by the underwater camera;

[0012] (6) fusing the position prediction based on the image Jacobian matrix in step (4) and the position measurement based on target detection in step (5) through an unscented Kalman filter to obtain the optimal estimation of the pixel position of the optical beacon;

[0013] (7) calculating the docking target between the AUV and the docking base station according to the pixel position of the optical beacon, and guiding the AUV to travel towards the center of the docking base station by controlling the propeller, and then completing the docking.

[0014] The application adds inertial navigation data and depth data to optical beacon perception, and proposes an iterative position estimation framework based on unscented Kalman, which combines high-precision target detection algorithm and strong-robust image Jacobian matrix method to realize an anti-interference and anti-occlusion optical beacon perception scheme.

[0015] In step (2), the Zhang calibration method is used to obtain the intrinsic parameters of the underwater camera.

[0016] In step (3), the specific method for time synchronization of the inertial navigation unit, depth gauge and underwater camera is as follows:

[0017] In the AUV hardware circuit, a pulse generator is used as a unified clock trigger source of the inertial navigation unit, depth gauge and underwater camera to eliminate the cumulative error of the clock source; after hardware synchronization, whenever the underwater camera collects an image, the inertial navigation information and depth at the same time are calculated based on the current timestamp interpolation.

[0018] In step (4), the optical beacon in the camera coordinate system {C} is L c (x c ,y c ,z c ), and the coordinates in the image coordinate system {I} are L i (x i ,y i ), and the representation form of the image Jacobian matrix J img is as follows:

[0019]

[0020] where J img denotes the image Jacobian matrix, and f is the camera focal length.

[0021] In step (5), the target detection model adopts the YOLOv8 network, and TensorRT accelerates the inference process by restructuring the internal structure of the network model, including vertical fusion and horizontal fusion; wherein the vertical fusion integrates the CONV layer, the BN layer and the ReLU layer in the YOLOv8 network into a CBR layer, and the horizontal fusion fuses the CBR layers with the same input tensor and the same operation, thereby compressing the network model.

[0022] In step (6), the execution of the unscented Kalman filter is divided into state prediction and measurement update; the state prediction step is to preliminarily estimate the state quantity of the system through the process state equation:

[0023]

[0024] where denotes the predicted state, P k|k-1 is the covariance matrix of the predicted state, and n represents the dimension after merging and augmenting the system noise state and the AUV state, denotes the weight of the i-th sigma point in the state prediction, denotes the propagation result of the i-th sigma point, denotes the weight of the corresponding i-th sigma point in the calculation of the state covariance matrix. The measurement update step propagates the sigma points through the observation equation, and according to the observation value and the self-covariance matrix and the cross-covariance matrix performs measurement update:

[0025]

[0026]

[0027] where K k is the Kalman gain matrix, is the estimated state, P k|k is the covariance matrix corresponding to the final estimated state; therefore, after initializing the state variable and the covariance matrix, the optimal estimation of the pixel position of the optical beacon can be obtained by iterative update.

[0028] In step (7), the optical beacon is represented as L w (x w ,y w ,zw ), in the camera coordinate system {C} is L c (x c ,y c ,z c ), projected to the image coordinate system {I} coordinate is L i (x i ,y i ), corresponding to the pixel coordinate system {P} coordinate is L p (x p ,y p );

[0029] The relative orientation between the AUV and the docking base station is as follows:

[0030]

[0031] Wherein, α and β are the horizontal and vertical angles between the AUV and the center line of the docking base station, -W P and H P are the width and height of the pixel screen, F H and F V represent the horizontal and vertical field of view angles of the camera; after solving α and β, they are input as control quantities into the motion control system, and the output expectation of the system when α = 0 and β = 0, the AUV will guide the center of the docking base station through the control of the thruster, and then complete the docking.

[0032] The transformation from the world coordinate system {W} to the camera coordinate system {C} is a rigid body transformation, which is completed by rotation and translation:

[0033]

[0034] Wherein, R is a 3 × 3 rotation matrix, and T is a translation vector; the transformation from the camera coordinate system {C} to the image coordinate system {I} satisfies the perspective projection relationship:

[0035]

[0036] Wherein, f represents the focal length of the camera, and the final light beacon coordinate in the pixel coordinate system {P} is further obtained:

[0037]

[0038] Wherein, u0, v0 are the coordinates of the center point of the imaging plane in the pixel coordinate system {P}, and dx, dy represent the physical size of each pixel in the X P and Y P axis direction, respectively. Thus, the conversion relationship from the world coordinate system {W} to the pixel coordinate system {P} of the light beacon is obtained.

[0039] Compared with the prior art, the present application has the following beneficial effects:

[0040] 1、 The present application proposes a light beacon position calculation based on an image Jacobian matrix, which can predict the light beacon position transfer at the next time based on inertial navigation information and depth data on the basis of the current position. Even in the case of complete occlusion of the light beacon, the current position can be calculated according to the light beacon vanishing point and inertial navigation and depth information, ensuring reliable and continuous perception information for AUV docking.

[0041] 2、 The present application proposes a light beacon position measurement based on TensorRT acceleration, which can effectively distinguish light beacons from general interference stray light and can be deployed on an AUV main control platform with limited computing power. In the case of visible light beacons, high-precision and low-latency perception information is provided.

[0042] 3、 The present application proposes a light beacon position optimal estimation framework based on unscented Kalman, which combines the above two perception results to overcome the nonlinearity problem in perception fusion. In normal docking, the fusion of predicted values and measured values realizes higher-precision position estimation; in the case of occlusion of the light beacon, continuous and reliable perception output can be realized based on the predicted value; in the presence of interference light sources, the predicted value is used to select a reliable measured value to avoid misjudgment. BRIEF DESCRIPTION OF DRAWINGS

[0043] Figure 1 A flow chart of an AUV vertical docking method based on light beacon fusion perception is provided for the embodiments of the present application. DETAILED DESCRIPTION

[0044] The present application will be further described in detail below in conjunction with the drawings and embodiments, and it should be noted that the following embodiments are intended to facilitate the understanding of the present application and do not limit the present application in any way.

[0045] As shown in the drawings, Figure 1 A method for AUV vertical docking based on light beacon fusion perception includes the following steps:

[0046] S10: updating inertial navigation information and depth

[0047] The existing AUV system perception part usually includes at least a camera, an inertial navigation unit and a depth gauge, and the present application adopts a fusion perception method to add inertial navigation information and depth to the light beacon tracking scheme, realizing strong robustness of AUV vertical docking perception.

[0048] First, the linear velocity v1 and angular velocity v2 of the current AUV are obtained by the inertial navigation unit:

[0049] [v1 v2] T =[u v w p q r] T

[0050] where u, v, w and p, q, r represent the linear and angular velocities in x, y, z directions respectively. The depth d of the current AUV can be obtained by the depth gauge. For the vertical docking scenario, the depth d of the docking base station s is fixed and known, the remaining vertical docking distance Z of the AUV c = d s -d.

[0051] S20: Subscribe to real-time image information

[0052] The light beacon identification is mainly based on images, so it is necessary to subscribe to real-time image information. In order to obtain accurate light beacon position estimation, camera calibration needs to be performed in advance. The present application uses Zhang's calibration method to obtain the camera intrinsic parameters:

[0053]

[0054] where f is the camera focal length, u0, v0 are the coordinates of the center point of the imaging plane in the pixel coordinate system, and dx, dy represent the physical size of each pixel in the X P and Y P axis directions of the pixel coordinate system, respectively.

[0055] S30: Time synchronization of multi-source perception

[0056] The fusion perception scheme of the present application is based on the joint estimation of light beacon position by vision, inertial navigation, and depth multi-source sensors, so it is necessary to ensure the consistency of the time of each sensor data.

[0057] Considering the clock drift of different clock sources, even if the timestamps of each sensor are aligned, they will deviate after running for a period of time. In the present patent, a pulse generator is used as a unified clock trigger source for each sensor in the AUV hardware circuit, eliminating the cumulative error of the clock source. After solving the hardware synchronization, since the sampling times of each sensor are not completely consistent, the interpolation method is further used to obtain the information at the same time. The present patent takes the camera as the core sensor, and whenever the real-time image is updated, the inertial navigation information and the depth are calculated based on the current timestamp, so as to realize comprehensive multi-source perception time synchronization in hardware and software.

[0058] S40: Predicting the position of the light beacon based on inertial navigation data and image Jacobian matrix

[0059] The image Jacobian matrix describes the approximate linear transformation relationship between the image feature space velocity and the AUV motion velocity. In order to calculate the state transition of the light beacon in the prediction step, unlike the passive velocity measurement method of image target tracking, the present application innovatively combines inertial navigation data and image Jacobian matrix to calculate the target velocity, so as to obtain more accurate position estimation through active measurement.

[0060] The relative pose between the docking station and the AUV is an important information for the docking process, and the light beacon L c (x c ,y c ,z c ) velocity transformation from camera coordinate system {C} to pixel coordinate system {I} is:

[0061]

[0062] Since the docking station is fixed, the position of the light beacon in the camera coordinate system {C} is updated by the six degrees of freedom translation and rotation motion of the AUV:

[0063]

[0064] where u, v, w and r, p, q represent the velocity and angular velocity of the AUV in the X c , Y c , Z c direction respectively. Thus the velocity of the light beacon in the pixel coordinate system {I} can be expressed as:

[0065]

[0066] Further, it can be expressed in matrix form:

[0067]

[0068] This formula maps the AUV motion velocity to the velocity of the target point in the pixel coordinate system, where the image Jacobian matrix J img is updated according to the altimeter data:

[0069]

[0070] The six degrees of freedom velocity matrix [v1 v2] T = [u v w p q r] T is updated by the inertial navigation module.

[0071] S50: Light beacon detection algorithm based on TensorRT acceleration

[0072] The traditional threshold segmentation algorithm cannot effectively distinguish the light beacon from the interference light source, the present application uses YOLOv8 to directly extract the information features of the light source target from the image, and the network structure is composed of four parts: Input, Backbone, Neck and Head.The Input layer is responsible for inputting the light beacon image subjected to Mosaic data enhancement into the network, the Backbone is composed of CBL, C2 and SPPF modules, wherein C2f is the main module for feature learning, the features are extracted through the residual module, and the features are spliced and fused to form a module with stronger feature representation capability;CBL represents ordinary feature extraction operation, mainly composed of convolution, batch normalization and activation;SPPF mainly performs serial maximum pooling calculation to realize the fusion of local features and global features.The Neck layer adopts PAN structure, which can enhance the network's ability to fuse features of objects with different scaling scales.The Head layer reasons the target category and position of the extracted features of the network, and performs positive and negative sample screening to calculate the loss.

[0073] Although the light beacon recognition method based on the target detection model shows the potential of real-time performance, it may be limited when deployed on the AUV platform with limited computing power, especially in the docking scene of AUV high-speed motion. Therefore, the present application introduces TensorRT to optimize the speed of the model. The main optimization strategy is to reconstruct the internal structure of the network model to speed up the inference process, including vertical fusion and horizontal fusion. Vertical fusion integrates CONV layer, BN layer and ReLU layer into a CBR layer, thereby compressing the network model;Horizontal fusion fuses the input tensors and layers that perform the same operation, which can speed up the inference process within an acceptable range of accuracy.

[0074] S60: Optimal estimation of light beacon position

[0075] The present application fuses the position prediction based on the image Jacobian matrix of S40 and the position measurement based on target detection of S50 through the unscented Kalman filter to comprehensively realize the optimal estimation of the light beacon position. The core idea is to generate a set of deterministic sampling points sigma points, and to approximate the distribution of state variables by weighted statistical linear regression to overcome the nonlinearity problem in perception fusion. The execution of unscented Kalman can be divided into two steps: state prediction and measurement update. The state prediction step is to preliminarily estimate the state quantity of the system through the process state equation:

[0076]

[0077] wherein, represents the predicted state, P k|k-1is the covariance matrix of the predicted state, n represents the dimension after the system noise state and the AUV state are merged and augmented, Represents the weight of the i-th sigma point during state prediction, Represents the propagation result of the i-th sigma point, Represents the weight of the i-th sigma point corresponding to the calculation of the state covariance matrix. The measurement update step propagates the sigma point through the observation equation, according to the observation value and its autocovariance matrix and the cross-covariance matrix To perform a measurement update:

[0078]

[0079] Among them, K k is the Kalman gain matrix, is the estimated state, P k|k is the covariance matrix corresponding to the final estimated state. Therefore, after initializing the state variables and covariance matrix, the optimal estimate of the pixel position of the optical beacon can be obtained by iterative update.

[0080] S70: Docking target calculation

[0081] For the AUV docking scenario guided by a single camera and a single light, this patent defines four main reference frames: the world coordinate system {W} is fixedly connected to the north-east coordinate system; the camera coordinate system {C} is based on the camera optical center O C is the coordinate origin, Z C Axis is the camera optical axis; the image coordinate system {I} is centered on the imaging plane O I is the coordinate origin, X I Axis and Y I The axes are parallel to the two sides of the imaging plane; the pixel coordinate system {P} is centered at the upper left corner of the imaging plane O P is the coordinate origin, X P Axis and Y P Axes parallel to the X I Axis and Y I axis.

[0082] The optical beacon can be expressed as L in the world coordinate system {W} w (x w ,y w ,z w ), which is L in the camera coordinate system {C} c (x c ,y c ,z c ), projected to the image coordinate system {I} coordinate is L i (x i ,y i), the corresponding coordinate in the pixel coordinate system {P} is L p (x p ,y p ). Among them, the transformation from the world coordinate system {W} to the camera coordinate system {C} is a rigid body transformation, which can be completed by rotation and translation:

[0083]

[0084] Where R is a 3×3 rotation matrix and T is a translation vector. The perspective projection relationship from the camera coordinate system {C} to the image coordinate system {I} is:

[0085]

[0086] f represents the focal length of the camera, and the coordinates of the final optical beacon in the pixel coordinate system {P} can be further calculated:

[0087]

[0088] Where u0, v0 are the coordinates of the center point of the imaging plane in the pixel coordinate system {P}, dx, dy represent X P Axis and Y P The physical size of each pixel in the axis direction. From this, we obtain the conversion relationship between the optical beacon from the world coordinate system {W} to the pixel coordinate system {P}. For the docking scenario, we focus on the relative orientation between the AUV and the base station:

[0089]

[0090] Among them, α and β are the horizontal angle and vertical angle between the AUV and the center line of the base station, respectively, W P With H P is the width and height of the pixel screen, F H With F V Represents the camera's horizontal and vertical field of view. After solving for α and β, they are input into the motion control system as control variables. With α = 0 and β = 0, the desired output of the control system is set. The AUV will control the thrusters to move toward the center of the base station according to the guidance, thus completing the docking.

[0091] The embodiments described above provide a detailed description of the technical solutions and beneficial effects of the present invention. It should be understood that the above are only specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, supplements and equivalent substitutions made within the scope of the principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. An AUV vertical docking method based on optical beacon fusion perception, characterized in that, The method comprises the following steps: (1) obtaining inertial navigation information of the current AUV through an inertial navigation unit, including linear velocity and angular velocity; obtaining the depth of the current AUV through a depth gauge; (2) calibrating an underwater camera, and obtaining real-time image information of the position of an optical beacon on a docking base station by using the calibrated underwater camera; (3) time-synchronizing the inertial navigation unit, the depth gauge and the underwater camera, and synchronously calculating the inertial navigation information and the depth of the AUV every time the real-time image is updated; (4) predicting the position of the optical beacon at the next time based on the inertial navigation information and the image Jacobian matrix on the basis of the current position; (5) introducing TensorRT into a target detection model, and measuring the position of the optical beacon through target detection of the image shot by the underwater camera; (6) obtaining the optimal estimation of the pixel position of the optical beacon by fusing the position prediction based on the image Jacobian matrix in step (4) and the position measurement based on target detection in step (5) through an unscented Kalman filter; (7) calculating the docking target between the AUV and the docking base station according to the pixel position of the optical beacon, and guiding the AUV to move towards the center of the docking base station by controlling the propeller, thereby completing the docking.

2. The AUV vertical docking method based on light beacon fusion perception according to claim 1, wherein, In step (2), Zhang's calibration method is used to obtain the intrinsic parameters of the underwater camera.

3. The AUV vertical docking method based on light beacon fusion perception according to claim 1, characterized in that, In step (3), the specific method of time-synchronizing the inertial navigation unit, the depth gauge and the underwater camera is as follows: In the AUV hardware circuit, a pulse generator is used as a unified clock trigger source of the inertial navigation unit, the depth gauge and the underwater camera to eliminate the cumulative error of the clock source; after hardware synchronization, the inertial navigation information and the depth at the same time are calculated based on the current time stamp interpolation every time the underwater camera collects an image.

4. The AUV vertical docking method based on light beacon fusion perception according to claim 1, wherein, In step (4), the light beacon in the camera coordinate system {C} is L c (x c ,y c ,z c ), the coordinates projected into the image coordinate system {I} are L i (x i ,y i ), and the representation form of the image Jacobian matrix J img is as follows: In the formula, J img denotes the image Jacobian matrix, and f is the camera focal length.

5. The AUV vertical docking method based on light beacon fusion perception according to claim 1, wherein, In step (5), the target detection model adopts the YOLOv8 network, and TensorRT accelerates the inference process by restructuring the internal structure of the network model, including vertical fusion and horizontal fusion; wherein the vertical fusion layer integrates the CONV layer, the BN layer and the ReLU layer in the YOLOv8 network into a CBR layer, and the horizontal fusion layer fuses the CBR layers with the same input tensor and the same operation, thereby compressing the network model.

6. The light beacon fusion perception based AUV vertical docking method according to claim 1, wherein, In step (6), the execution of the unscented Kalman filter is divided into state prediction and measurement update; the state prediction step is to preliminarily estimate the state quantity of the system through the process state equation: wherein, denotes the predicted state, P k|k-1 is the covariance matrix of the predicted state, n denotes the dimension of the augmented state which merges the system noise state with the AUV state, denotes the weight of the i-th sigma point in the state prediction, denotes the propagation result of the i-th sigma point, denotes the weight of the i-th sigma point in the state covariance matrix calculation; the measurement update step propagates the sigma points through the observation equation, and according to the observation value and the self-covariance matrix and the cross-covariance matrix performs measurement update: where K k is the Kalman gain matrix, is the estimated state, P k|k is the covariance matrix corresponding to the final estimated state; thus, after initializing the state variable and the covariance matrix, the optimal estimation of the pixel position of the optical beacon can be obtained by iterative updating.

7. The AUV vertical docking method based on light beacon fusion perception according to claim 1, wherein, In step (7), the light beacon is represented in the world coordinate system {W} as L w (x w ,y w ,z w ), in the camera coordinate system {C} as L c (x c ,y c ,z c ), projected into the image coordinate system {I} as L i (x i ,y i ), and in the pixel coordinate system {P} as L p (x p ,y p ). The relative position between the AUV and the docking base station is as follows: where, α and β are the horizontal and vertical angles between the AUV and the center line of the docking station, -W P and H P are the width and height of the pixel screen, F H and F V represent the horizontal and vertical field of view angles of the camera; after solving α and β, they are input into the motion control system as control variables, and the output of the control system is expected to be α = 0 and β = 0, so that the AUV will be guided to move towards the center of the docking station by controlling the thrusters, and then the docking will be completed.

8. The AUV vertical docking method based on light beacon fusion perception according to claim 7, characterized in that, The transformation from the world coordinate system {W} to the camera coordinate system {C} is a rigid body transformation, which is completed through rotation and translation: Wherein, R is a 3*3 rotation matrix, and T is a translation vector; the transformation from the camera coordinate system {C} to the image coordinate system {I} satisfies the perspective projection relationship: Wherein, f represents the focal length of the camera, and the final coordinates of the optical beacon in the pixel coordinate system {P} are further obtained: where u0, v0are the coordinates of the center point of the imaging plane in the pixel coordinate system {P}, and dx, dy represent the physical size of each pixel in the X P and Y P axis direction, respectively. Thus, the conversion relationship of the optical beacon from the world coordinate system {W} to the pixel coordinate system {P} is obtained.

Images