AUV (Autonomous Underwater Vehicle) dynamic docking system for multiple docking stations

Abstract

The AUV dynamic docking system based on the multi-port docking station comprises the multi-port docking station and an AUV, and the dynamic docking control method of the AUV and the multi-port docking station comprises the following specific steps: S1, extracting center coordinates of a guide light source of each dock port of the multi-port docking station; s2, in a long-distance stage, the AUV performs global pose calculation by combining an EPnP algorithm with all the guide light sources, and the AUV autonomously docks to a target dock entrance according to a calculation result; s3, when the AUV gradually gets close to the target dock entrance and is in the middle distance stage, the AUV estimates the azimuth included angle between the AUV and the target dock entrance based on the monocular double-light-source set relation, and the AUV adjusts the course according to the direction included angle and continues to be in butt joint with the target dock entrance; and S4, when the AUV is close to the target dock entrance and is about to enter the target dock entrance, the AUV is in a final docking stage at the moment, the docking dock is still in a moving state, the AUV identifies the Stag visual mark of the target dock entrance to carry out pose calculation of tail end docking, and the AUV is accurately docked with the target dock entrance according to the pose calculation.

Description

Technical Field

[0001] The present invention relates to the technical field of robot docking, and in particular to an AUV dynamic docking system for a multi-port docking station. Background Art

[0002] Autonomous underwater vehicles (AUVs) are widely used in ocean exploration, resource exploration, and environmental monitoring, but due to energy constraints, they need to be resupplied through underwater docking. Depending on the motion state of the docking station, underwater docking is divided into static docking and dynamic docking. Static docking usually fixes the docking station to the seabed or shore, which has low requirements for AUV guidance and control, but this method limits the operating range of the AUV. Dynamic docking allows AUV operations to have a wider coverage and higher flexibility because both the AUV and the docking station are in motion, but requires higher-precision real-time perception and motion control. At present, AUV docking is still dominated by the single-port docking mode in which a single AUV docks with a single dock. However, as the future trend of AUV applications develops from single operations to clustered operations, the traditional single-port docking mode is difficult to meet the application requirements of multi-machine collaborative operations.

[0003] The existing technology has the following shortcomings:

[0004] 1) Single docking mode: The dock is fixed and cannot cope with the future trend of AUV applications from single operation to cluster operation.

[0005] 2) During the AUV's docking process, the docking method is relatively simple, ignoring the fact that the relative position between the AUV and the docking port continues to change during the docking process, and the visual recognition target will also have different situations. Summary of the Invention

[0006] To overcome the shortcomings of the existing technology, the present invention proposes an AUV dynamic docking system for a multi-port docking dock, which improves the system's response efficiency and docking stability. The phased docking strategy can ensure the precise docking of the AUV.

[0007] The technical solution adopted in the present invention is:

[0008] The AUV dynamic docking system for a multi-port docking station includes a multi-port docking station and an AUV. The specific steps of the dynamic docking control method of the AUV and the multi-port docking station are as follows:

[0009] S1, extracting the center coordinates of the guiding light sources of each docking port of a multi-port docking station, where each docking port is provided with two guiding light sources and a visual marker;

[0010] S2: When in the long-range phase, the AUV uses the extracted center coordinates of the guiding light source to determine the 3D coordinates of the corresponding light source in the world coordinate system, selects the target dock to be docked and defines the center of the target dock as the origin of the world coordinate system. The light source information identified by the AUV is then converted into a 3D-2D point pair in the corresponding new coordinate system and input into the EPnP algorithm for global pose solution. The EPnP algorithm is used to calculate the rotation matrix and translation vector of the AUV relative to the target dock. The AUV autonomously docks to the target dock based on the rotation matrix and translation vector.

[0011] S3: As the AUV gradually approaches the target dock, it is in the mid-range stage. The AUV determines the position of the target dock center on the camera pixel plane based on the center coordinates of the two guiding light sources extracted from the target dock. The AUV estimates the azimuth angle with the target dock based on the deviation between the position of the target dock center in the current image and the image center. The AUV adjusts its heading based on the azimuth angle and continues to dock to the target dock.

[0012] S4, when the AUV approaches the target dock and is about to enter the target dock, it is in the final docking stage. The dock is still in motion. The AUV recognizes the Stag visual mark of the target dock and performs terminal docking posture solution. The AUV accurately docks with the target dock based on the posture solution.

[0013] Furthermore, the AUV is provided with a main control module, a drive system, a sensor system, and a power supply system, and the drive system, the sensor system, and the power supply system are all connected to the main control module.

[0014] Furthermore, the guiding light source of each docking port has a different color, and guiding light sources are arranged at the center of the upper and lower ends of each docking port, and the visual mark is set at the dock end of the dock.

[0015] Furthermore, the steps for extracting the center coordinates of the guiding light sources of each docking port of the multi-port docking station in step S1 are as follows:

[0016] S11, cropping the target area according to the detection frame coordinates and converting the cropped image into a grayscale image;

[0017] S12, applying a threshold segmentation method to highlight light source features, and performing contour detection, marking and drawing the detected contour on the cropped image;

[0018] S13, calculating the centroid coordinates of the light source through the image moment and drawing the center point on the original image;

[0019] S14, output the center coordinates of each light source.

[0020] Furthermore, the steps of calculating the centroid coordinates of the light source by using the image moment in step S13 are as follows:

[0021] For a binary image, let its pixel intensity value be I(x,y), then the (p,q)-order geometric moment is defined as follows:

[0022]

[0023] Where (x, y) is the pixel coordinate in the image, p, q is the order of the moment, and I(x, y) is the pixel value of the point;

[0024] The centroid of a light source is the center position of all pixels within the light source area. The calculation formula is as follows:

[0025]

[0026] Among them, M 10 =∑ x ∑ y xI(x, y) represents the weighted sum of the horizontal coordinates of all pixels, M 01 =∑ x ∑ y I(x, y) represents the weighted sum of the ordinates of all pixels, M 00 =Σ x Σ y I(x, y) represents the total number of pixels in the region, that is, the area.

[0027] Furthermore, the specific steps of the EPnP algorithm in step S2 for global pose solution are as follows:

[0028] S21, select control points: In the world coordinate system, select four non-coplanar control points. Usually, first calculate the center of gravity of all 3D reference points as the first control point C1 w , and then determine the remaining three control points C2 through principal component analysis w 、C3 w 、C4 w ;

[0029] S22, calculate the homogeneous barycentric coordinates: For each 3D reference point P i w , whose coordinates can be expressed as a linear combination of four control points:

[0030]

[0031] Among them, α ij is the homogeneous barycentric coordinate, the subscript i represents the number of the 3D reference point, that is, the key point or feature point on the real object; the subscript j represents the number of the control point, that is, the four artificially selected virtual reference points (not coplanar); satisfying:

[0032]

[0033] S23, establish a linear relationship in the camera coordinate system: In the camera coordinate system, the reference point can also be expressed as a linear combination of control points:

[0034]

[0035] Among them, P i c and C j c are the coordinates of the reference point and control point in the camera coordinate system;

[0036] S24, projection relationship: According to the camera projection model, the camera coordinates of the reference point and its image coordinates (u i ,v i ) has the following relationship:

[0037]

[0038] Among them, s i is the scaling factor, N is the camera intrinsic parameter matrix;

[0039] S25, construct a linear equation system: Substitute the expression in step S23 into step S24 to obtain the linear equation system about C j c The linear equations of

[0040] S26, solve the coordinates of the control point in the camera coordinate system: combine all equations into matrix form:

[0041] MC=B

[0042] Among them, M is a known matrix, C is a matrix containing all C j c The unknown vector is B, and the known vector is C. By solving this linear equation system, the coordinates of the control point in the camera coordinate system can be obtained. j c ;

[0043] S27, calculate the camera pose: Once the coordinates C of the control point in the camera coordinate system are obtained j c , and their corresponding C in the world coordinate system j w , the camera’s rotation matrix R and translation vector t can be determined by solving the rigid body transformation.

[0044] Furthermore, the control points in step S21 are four random non-coplanar guiding light source points among all the identified guiding light sources. There are multiple groups of control points, which are solved separately to obtain the solution results, and consistency verification is performed to eliminate outliers. The weighted mean of the multiple groups of solution results is calculated to obtain the final global posture.

[0045] Furthermore, the estimation steps of the AUV azimuth in step S3 are as follows:

[0046] The optical center coordinates of the two guiding light sources at the target dock are (x1, y1) and (x2, y2) respectively. The coordinates of the center of the dock can be obtained by geometric calculation (x d ,y d )for:

[0047]

[0048] The azimuth angle θ can be calculated by the following formula:

[0049]

[0050] Among them, the parameter h θ It is an intermediate calculation quantity used to simplify the subsequent derivation parameters of tanθ; parameter v0 is the vertical axis pixel coordinate of the reference point; parameter u0 is the horizontal axis pixel coordinate of the reference point; parameter x d Is the horizontal pixel coordinate of the target point corresponding to the azimuth to be calculated; parameter y d is the vertical pixel coordinate of the target point corresponding to the azimuth to be calculated; the parameter vFOV represents the vertical field of view, in rad; the parameter height represents the height of the image, in px; the parameter dy is the scaling factor from the pixels on the v-axis to the actual length. The above camera parameters are all known quantities.

[0051] Furthermore, the pose calculation steps for the end docking based on the Stag visual marker in step S4 are as follows:

[0052] S41, placing STag markers in the target area of ​​the docking station and defining the ID number, physical size, and camera parameters of the STag markers in the ROS configuration file so as to correctly detect the markers and calculate the pose;

[0053] S42, the AUV camera captures images in real time during the docking process and performs marker detection through the STag ROS node. The STag ROS package uses edge detection and corner extraction algorithms to identify the location of the STag marker and output the pixel coordinates of its four corner points on the image plane.

[0054] S43, matching the two-dimensional image coordinates of the four corner points of the STag marker with the pre-set three-dimensional world coordinates, and solving the rotation matrix and translation vector of the STag marker relative to the camera coordinate system based on the EPnP algorithm;

[0055] S44, the STag ROS package publishes the calculated pose information through the / stag_ros / pose topic. The AUV control system subscribes to this topic and obtains the position of the STag marker in real time.

[0056] At S45, the AUV control system adjusts the thrusters and servos in real time using the pose information calculated by the STag ROS package, ensuring that the AUV always maintains the correct pose and docking accurately in the terminal phase.

[0057] Beneficial effects of the present invention:

[0058] 1. The multi-port docking dock arranges light sources of different colors (blue, white, and green) at the center of the upper and lower ends of each dock, and sets visual markers at the tail. By optimizing the dock body size and the layout of the guide light sources, while adding a small number of light sources, visual interference is reduced, and the stability and efficiency of AUV multi-port docking are improved, which is different from the traditional single-port docking structure.

[0059] 2. The AUV integrates a real-time control main module, power supply, drive, and sensor systems to ensure stable operation in dynamic environments. Based on the ROS architecture, the AUV's communication, docking control, and data processing systems were built, and the coordinated control between the host and slave computers was optimized to improve the system's response efficiency and docking stability.

[0060] 3. In light of the AUV's dynamic docking requirements for multiple docking ports, a phased visual positioning strategy was proposed. At long distances, the EPnP+ multi-light source redundancy algorithm was used to calculate the AUV's global pose. This leverages the visibility of all light sources, improves the robustness of pose calculations through redundant data, and reduces the long-distance error amplification effect. At medium distances, dual-light source geometric positioning ensures the AUV's pose adjustment in the absence of some light sources. When switching algorithms (EPnP to dual-light source method), pose jumps are prone to occur due to model differences, requiring smooth transitions to maintain control continuity. At close ranges, STag visual markers are used for precise pose calculations, combined with residual light source information to maintain pose continuity, ensuring the AUV can stably complete autonomous docking. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 It is a schematic diagram of the main structure of the multi-port docking station of the present invention.

[0062] Figure 2 It is a schematic diagram of the top view of the multi-port docking station of the present invention.

[0063] Figure 3 It is a structural schematic diagram of the present invention.

[0064] Figure 4 It is a flow chart of the docking control method of the present invention.

[0065] Figure 5 It is a schematic diagram of the optical center coordinate extraction process of the present invention.

[0066] Figure 6 Schematic diagram of AUV azimuth estimation according to the present invention.

[0067] Figure 7 It is a schematic diagram of the Stag visual marking structure of the present invention. DETAILED DESCRIPTION

[0068] The present invention will be further described below with reference to specific embodiments, but the present invention is not limited to these specific embodiments. Those skilled in the art should recognize that the present invention covers all possible alternatives, improvements and equivalents within the scope of the claims.

[0069] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "clockwise", "counterclockwise" and the like indicate positions or positional relationships based on the positions or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first" and "second" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "multiple" means two or more, unless otherwise clearly defined.

[0070] In the present invention, unless otherwise expressly specified or limited, the terms "mounted," "connected," "connect," "fixed," etc. should be understood broadly. For example, they may refer to fixed, detachable, or integral connections; mechanical or electrical connections; direct or indirect connections through an intermediary; or internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0071] In the present invention, unless otherwise expressly specified or limited, a first feature being "above" or "below" a second feature may include the first and second features being in direct contact, or may include the first and second features being in contact not directly but through another feature between them. Furthermore, a first feature being "above," "above," and "above" a second feature may include the first feature being directly above or obliquely above the second feature, or may simply mean that the first feature is higher in level than the second feature. A first feature being "below," "below," and "below" a second feature may include the first feature being directly below or obliquely below the second feature, or may simply mean that the first feature is lower in level than the second feature.

[0072] See also Figure 1-7 This embodiment provides an AUV dynamic docking system for a multi-port docking dock, including a multi-port docking dock and an AUV. The AUV is provided with a main control module, a drive system, a sensor system, and a power supply system, and the drive system, sensor system, and power supply system are all connected to the main control module. The main control module, as the core, should adopt a main control card that supports a real-time operating system (such as μCOS-III) to achieve precise control and data processing, so that the AUV can quickly adapt to the environment and successfully complete docking. The functional module includes a power supply, a drive, and a sensor system. The power supply system provides stable energy guarantee, the drive system ensures the flexibility and accuracy of the AUV's underwater movement, and the sensor system provides positioning, attitude, and environmental perception data, providing key support for the docking algorithm and control system. Through careful design and integration, the main control module ensures that the AUV has real-time control capabilities, while the functional module enhances its system stability and multi-tasking processing capabilities, thereby laying a solid hardware foundation for the AUV to successfully perform complex underwater dynamic docking tasks. The main control module can also communicate with the host computer, which sends control instructions, monitors the operating status, and records data.

[0073] The specific steps of the dynamic docking control method of the AUV and the multi-port docking dock described in this embodiment are as follows:

[0074] S1. Extract the center coordinates of the guiding light sources of each docking port of a multi-port docking dock. Each docking port is provided with two guiding light sources and a visual marker. The guiding light sources of each docking port are different colors. Guiding light sources are arranged at the centers of the upper and lower ends of each docking port. The visual marker is provided at the end of the docking port.

[0075] Specifically, this embodiment features three docking ports. Blue, white, and green light sources are positioned at the top and bottom centers of Docks 1, 2, and 3, respectively. Visual markers are also placed at the rear of each dock. By optimizing dock dimensions and rationally arranging guidance light sources, this docking station can better meet practical application needs. The number of guidance light sources required for a multi-docking system is only slightly more than one or two compared to traditional single-dock docking. This reduces unnecessary visual interference while ensuring docking accuracy, thereby improving the stability and efficiency of AUV multi-docking.

[0076] The steps for extracting the center coordinates of the guiding light sources of each docking port of the multi-port docking station are as follows:

[0077] S11, crop the target area according to the detection frame coordinates and convert the cropped image into a grayscale image to reduce the computational complexity; the detection frame is output by the visual processing algorithm

[0078] S12, applying a threshold segmentation method to highlight light source features, and performing contour detection, marking and drawing the detected contour on the cropped image;

[0079] S13, calculating the centroid coordinates of the light source through the image moment and drawing the center point on the original image;

[0080] The steps for calculating the center of mass coordinates of the light source through image moments are as follows:

[0081] For a binary image, let its pixel intensity value be I(x,y), then the (p,q)-order geometric moment is defined as follows:

[0082]

[0083] Where (x, y) is the pixel coordinate in the image, p, q is the order of the moment, and I(x, y) is the pixel value of the point;

[0084] The centroid of a light source is the center position of all pixels within the light source area. The calculation formula is as follows:

[0085]

[0086] Among them, M 10 =∑ x ∑ y xI(x, y) represents the weighted sum of the horizontal coordinates of all pixels, M 01 =∑ x ∑ y I(x, y) represents the weighted sum of the ordinates of all pixels, M 00 =∑ x ∑ y I(x,y) represents the total number of pixels in the region, that is, the area.

[0087] S14, output the center coordinates of each light source.

[0088] After step S1, the center coordinates of the guiding light sources of dock 1, dock 2 and dock 3 can be obtained, and these coordinate data can be input into the position matrix to provide necessary input information for the subsequent AUV posture solution algorithm. Based on the calculated posture relative to the docking dock, the AUV can autonomously adjust its heading to achieve precise docking.

[0089] S2: When in the long-range phase, the AUV uses the extracted center coordinates of the guiding light source to determine the 3D coordinates of the corresponding light source in the world coordinate system, selects the target dock to be docked and defines the center of the target dock as the origin of the world coordinate system. The light source information identified by the AUV is then converted into a 3D-2D point pair in the corresponding new coordinate system and input into the EPnP algorithm for global pose solution. The EPnP algorithm is used to calculate the rotation matrix and translation vector of the AUV relative to the target dock. The AUV autonomously docks to the target dock based on the rotation matrix and translation vector.

[0090] Specifically, based on (optical center extraction), the pixel coordinates of the light source on the docking dock, that is, the 2D image coordinates, can be obtained. Combined with the docking dock size and the specific distribution information of the light source, the 3D coordinates of each light source in the world coordinate system can be determined. Since the world coordinates of the light source are known, in the dynamic docking process for multi-port docking docks, the target docking port of the AUV can be specified by adjusting the origin of the world coordinate system. For example, if the AUV needs to dock to dock 1, the center of dock 1 can be defined as the origin of the world coordinate system, and then the light source information identified by the AUV is converted into a three-dimensional-two-dimensional point pair in the corresponding new coordinate system, and input into the EPnP algorithm for pose solution. By calculating the rotation matrix and translation vector of the AUV relative to the target dock through the EPnP algorithm, the mission goal of the AUV autonomously docking to the specified dock can be achieved.

[0091] The core idea behind using the EPnP algorithm to solve the AUV's pose relative to the dock is to linearly weight the key points on the real object using four virtual control points, thereby representing the object's spatial position in the camera coordinate system. The specific steps for the EPnP algorithm to solve the global pose are as follows:

[0092] S21, select control points: In the world coordinate system, select four non-coplanar control points. Usually, first calculate the center of gravity of all 3D reference points as the first control point C1 w , and then determine the remaining three control points C2 through principal component analysis w 、C3 w 、C4 w ;

[0093] S22, calculate the homogeneous barycentric coordinates: For each 3D reference point Pi w , whose coordinates can be expressed as a linear combination of four control points:

[0094]

[0095] Among them, α ij is the homogeneous barycentric coordinate, the subscript i represents the number of the 3D reference point, that is, the key point or feature point on the real object. The subscript j represents the number of the control point, that is, the four artificially selected virtual reference points (not coplanar). Satisfy:

[0096]

[0097] S23, establish a linear relationship in the camera coordinate system: In the camera coordinate system, the reference point can also be expressed as a linear combination of control points:

[0098]

[0099] Among them, P i c and C j c are the coordinates of the reference point and control point in the camera coordinate system;

[0100] S24, projection relationship: According to the camera projection model, the camera coordinates of the reference point and its image coordinates (u i ,v i ) has the following relationship:

[0101]

[0102] Among them, s i is the scaling factor, N is the camera intrinsic parameter matrix;

[0103] S25, construct a linear equation system: Substitute the expression in step S23 into step S24 to obtain the linear equation system about C j c The linear equations of

[0104] S26, solve the coordinates of the control point in the camera coordinate system: combine all equations into matrix form:

[0105] MC=B

[0106] Among them, M is a known matrix, C is a matrix containing all C j c The unknown vector is B, and the known vector is C. By solving this linear equation system, the coordinates of the control point in the camera coordinate system can be obtained. j c ;

[0107] S27, calculate the camera pose: Once the coordinates C of the control point in the camera coordinate system are obtained j c , and their corresponding C in the world coordinate system j w , the camera’s rotation matrix R and translation vector t can be determined by solving the rigid body transformation.

[0108] Specifically,

[0109] The control points in step S21 of this embodiment are four random non-coplanar guiding light source points from all the identified guiding light sources. There are multiple groups of control points, which are solved separately to obtain the solution results, and a consistency check is performed to eliminate outliers. The weighted mean of the multiple groups of solution results is calculated to obtain the final global posture.

[0110] Specifically, the EPnP algorithm and multi-light source collaboration mechanism are as follows:

[0111] (1) Redundant calculation framework: Identify all guiding light sources (e.g., 3 docking ports × 2 light sources = 6 point pairs) and perform EPnP solutions in groups. For each group, select 4 non-coplanar light source points as EPnP inputs (e.g., randomly select 4 points each time, generating a total of C(6,4) = 15 groups). Perform a consistency check on each group of solution results (rotation matrix R, translation vector t).

[0112] (2) Error suppression principle: The pixel deviation Δp of the light source imaging at a long distance will lead to a posture error ΔP = Δp × D / f (D is the distance, f is the focal length). By taking the average of multiple groups of calculations, ΔP can be made to obey the central limit theorem, and the variance is reduced to 1 / n of a single group (n is the number of redundant groups).

[0113] Mean fusion and outlier removal are as follows:

[0114] (1) Weighted mean calculation: Assume that the poses of each group are (R i ,t i ), weight w i Determined based on the following indicators: Light source point reprojection error: Weight formula: Normalized and weighted fusion: t=(∑ i w i ) -1 ×∑ i w i t i ,(R i , t i ) represents the pose of the i-th group solution, where R i is a 3×3 rotation matrix (belonging to the special orthogonal group SO(3)), t iIs a 3×1 translation vector. Light source point pair: P j (j=1,2,3,4) is the coordinate of the light source point in three-dimensional space, u j The camera intrinsic parameter matrix K represents the camera intrinsic parameter matrix, which is used to project the 3D point onto the 2D image plane. i Using exponential decay function, based on the error e i calculate: When e i =0, w i =1(maximum weight);e i The larger the w i The smaller σ is, the more Gaussian distribution characteristics are. σ controls the sensitivity of the weight to the error. When σ is small, the weight of the pose with small error is significantly higher than that of the pose with large error. When σ is large, the difference in weight of poses with different errors is reduced. is the Frobenius norm. Translation vector fusion (weighted) t=(∑ i w i ) -1 ∑ i w i t i Directly perform weighted averaging on the translation vectors. Since the translation space is a linear space, it can be directly linearly combined.

[0115] (2) RANSAC iterative optimization: Randomly sample 4 points to calculate the pose, use the remaining points to verify the reprojection error, iterate to the maximum inlier set, and eliminate outlier light source points (such as light sources blocked by water plants).

[0116] When the AUV is far from the dock, the camera's field of view is large enough to fully cover all guiding light sources on the dock, thereby acquiring complete 3D-2D point pair information. At this stage, the AUV uses monocular vision to identify all guiding light sources at the three docking ports. Combining their known coordinates in the world coordinate system, the EPnP algorithm and multi-light source redundant calculation are used to perform pose calculations, thereby obtaining the AUV's global pose information relative to the target docking port. This utilizes the advantage of full light source visibility and redundant data to improve the robustness of pose calculations and reduce the long-distance error amplification effect.

[0117] The EPnP algorithm offers higher accuracy than pose calculation methods based on the geometric relationship between a monocular and dual light sources. Therefore, during long-distance docking, the AUV does not directly use the dual light sources for pose calculation. Instead, it chooses to identify all guiding light sources to ensure higher pose calculation accuracy. If pose calculation based on the geometric relationship between a monocular and dual light sources were used directly when the AUV is far from the dock, the long distance between the AUV and the dock would result in large calculation errors, hindering subsequent precise docking. Therefore, while the AUV can still fully identify all light sources, the EPnP algorithm is preferred for global pose calculation to provide more accurate initial pose information.

[0118] S3: As the AUV gradually approaches the target dock, it is in the mid-range stage. The AUV determines the position of the target dock center on the camera pixel plane based on the center coordinates of the two guiding light sources extracted from the target dock. The AUV estimates the azimuth angle with the target dock based on the deviation between the position of the target dock center in the current image and the image center. The AUV adjusts its heading based on the azimuth angle and continues to dock to the target dock.

[0119] The estimation steps of the AUV azimuth are as follows:

[0120] The optical center coordinates of the two guiding light sources at the target dock are (x1, y1) and (x2, y2) respectively. The coordinates of the center of the dock can be obtained by geometric calculation (x d ,y d )for:

[0121]

[0122] The azimuth angle θ can be calculated by the following formula:

[0123]

[0124] Among them, the parameter h θ It is an intermediate calculation quantity used to simplify the subsequent derivation parameters of tanθ; parameter v0 is the vertical axis pixel coordinate of the reference point; parameter u0 is the horizontal axis pixel coordinate of the reference point; parameter x d Is the horizontal pixel coordinate of the target point corresponding to the azimuth to be calculated; parameter y d is the vertical pixel coordinate of the target point corresponding to the azimuth to be calculated; the parameter vFOV represents the vertical field of view, in rad; the parameter height represents the height of the image, in px; the parameter dy is the scaling factor from the pixels on the v-axis to the actual length. The above camera parameters are all known quantities.

[0125] Through the above calculations, the AUV can continuously update its azimuth angle relative to the target dock and transmit the calculation results to the controller, so that it can adjust its posture according to the real-time angle and dock to the target dock.

[0126] Dual-light source geometric positioning ensures the AUV's posture adjustment when some light sources are missing. When switching algorithms (EPnP → dual-light source method), posture jumps are easily caused by model differences, and smooth transition is required to maintain control continuity. The historical posture smooth transition strategy is as follows:

[0127] (1) Kalman filter fusion: state definition: Contains angle and angular velocity. State transition: x k =Fx k-1 +w k ,in Observation equation: z k =Hx k +v k , H = [1, 0, 0, 0; 0, 1, 0, 0]. Key: When initializing the covariance matrix P, the initial uncertainty needs to be set in combination with the error of the last frame of EPnP.

[0128] (2) Sliding window smoothing: Maintain the pose queue of the last N frames {θ1, ..., θ N}, current output: Where λ is the smoothing coefficient, which suppresses mutations while ensuring dynamic response.

[0129] As the AUV approaches the target docking port, the present invention's dynamic motion causes the AUV and dock to undergo a constant change in their relative position. This causes the AUV's camera's field of view to gradually decrease, making it unable to continuously cover all the guiding light sources on the docking port. In this situation, the EPnP algorithm struggles to provide stable position and pose calculations due to an insufficient number of 3D-2D point pairs. If the AUV loses valid position and pose information at this stage, it enters an unguided navigation mode, where its motion is influenced solely by inertia in the water and cannot be adjusted in real time. This can cause the AUV to drift, enter an unexpected docking port, or even collide with the docking port, thereby reducing the success rate and safety of autonomous docking. To address this issue, based on the geometric relationship of the monocular camera imaging model, the AUV's heading and pitch angles are calculated using the dual guiding light sources at the target docking port, ensuring that the AUV can still obtain the necessary position and pose information during this phase. Although this method's calculation accuracy is slightly lower than that of the EPnP algorithm, since the EPnP algorithm is no longer able to perform position and pose calculations, it still provides an effective basis for attitude adjustment. In addition, the AUV is close to the docking station at this stage, and a moderate pose solution error has limited impact on the final docking, which is acceptable in practical applications.

[0130] Traditional monocular single-light guidance typically places the guidance light source at the center of the docking station. The AUV's azimuth relative to the guidance light source's center is estimated by calculating the pixel deviation between the guidance light source's center pixel coordinates and the center of the camera plane, enabling autonomous docking. Since each docking port is equipped with two guidance light sources, if only the deviation between the pixel coordinates of one light source and the camera center is used to estimate the AUV's azimuth, the AUV's final docking target will be above or below the docking port, rather than directly in the center. This approach will cause the AUV to deviate from the ideal docking position, affecting docking accuracy and stability.

[0131] To avoid the misalignment between the AUV and the dock center caused by this problem, the extracted center pixel coordinates of the two guiding light sources can be combined with geometric calculation methods to determine the exact position of the actual dock center on the camera pixel plane. Based on this, the AUV can estimate the azimuth angle with the target dock based on the deviation between the dock center in the current image and the image center, thereby adjusting its own heading. This method relies on the dual-light source features in the monocular image to complete azimuth estimation, offering the advantages of simple calculation and strong real-time performance.

[0132] S4, when the AUV approaches the target dock and is about to enter the target dock, it is in the final docking stage. The dock is still in motion. The AUV recognizes the Stag visual mark of the target dock and performs terminal docking posture solution. The AUV accurately docks with the target dock based on the posture solution.

[0133] To facilitate the integration of STag for pose calculation in the ROS environment, Unmanned Systems & RoboticsLab-UofSC developed the STag ROS package, which can detect STag markers in real time in ROS and perform pose calculation based on the PnP algorithm. The pose calculation steps for end-to-end docking based on Stag visual markers are as follows:

[0134] S41, placing STag markers in the target area of ​​the docking station and defining the ID number, physical size, and camera parameters of the STag markers in the ROS configuration file so as to correctly detect the markers and calculate the pose;

[0135] S42, the AUV camera captures images in real time during the docking process and performs marker detection through the STag ROS node. The STag ROS package uses edge detection and corner extraction algorithms to identify the location of the STag marker and output the pixel coordinates of its four corner points on the image plane.

[0136] S43, matching the two-dimensional image coordinates of the four corner points of the STag marker with the pre-set three-dimensional world coordinates, and solving the rotation matrix and translation vector of the STag marker relative to the camera coordinate system based on the EPnP algorithm;

[0137] S44, the STag ROS package publishes the calculated pose information through the / stag_ros / pose topic. The AUV control system subscribes to this topic and obtains the position of the STag marker in real time.

[0138] At S45, the AUV control system adjusts the thrusters and servos in real time using the pose information calculated by the STag ROS package, ensuring that the AUV always maintains the correct pose and docking accurately in the terminal phase.

[0139] Since STag is prone to losing its positioning when blocked by the docking structure, it is necessary to combine the residual light source information to maintain the posture continuity. The multi-source data fusion algorithm can effectively solve this problem.

[0140] (1) Weighted fusion model: Let the STag pose be (R s , t s ), the light source pose is (R l , t l ), fused pose: t=t s +α(t l -t s ), where the weight α is determined by the following factors: STag detection confidence (such as corner detection response strength) and the number of light sources (2 light sources α = 0.3). STag pose (R s , t s ): The visual SLAM system obtains the four corner points detected by STag. Its confidence is quantified by the corner point detection response strength. When the response strength is greater than the threshold Tconf, the detection is considered valid, and the response strength is positively correlated with the weight coefficient. Light source pose (R l , t l ): Calculated through multi-view geometric constraints of ambient light sources, and triangulation is used to improve accuracy in dual-light source scenarios.

[0141] (2) Motion prediction compensation: When both STag and light source are lost, the uniform velocity model is used to predict the pose based on the previous three frames: t pred =t k-1 +v k-1 Δt, At the same time, the thruster damping control is activated to reduce the impact of inertial drift.

[0142] In the present invention, when the AUV is close to the docking port and is about to enter the interior of the target docking dock, the docking dock is still in motion. If the AUV continues to move in a straight line along the current trajectory, it may collide with the internal structure of the target docking dock, affecting the safety of docking. At this stage, the field of view of the AUV camera is extremely limited, and it is no longer possible to identify the aforementioned guiding light source. The traditional light source guidance method can no longer provide effective posture information. In order to ensure that the AUV can still stably adjust its posture and dock smoothly in the absence of light source guidance, a STag visual marker (such as Figure 7 (as shown in the figure), the AUV recognizes the visual marker and calculates the pose for terminal docking. The STag marker is highly robust and accurate, providing a stable visual positioning reference as the AUV enters the docking station. This allows the AUV to maintain accurate heading and pitch attitude during the final stage of dynamic docking, ensuring successful docking. The STag is prone to losing its position when obscured by docking structures, requiring residual light source information to maintain pose continuity.

[0143] The multi-port docking dock of the present invention arranges light sources of different colors (blue, white and green) at the center of the upper and lower ends of each docking port, and sets a visual mark at the tail; by optimizing the dock body size and the layout of the guide light source, while adding a small amount of light sources, the visual interference is reduced, and the stability and efficiency of the AUV multi-dock docking are improved, which is different from the traditional single-dock docking structure. The AUV integrates a real-time control main control module, power supply, drive and sensor system to ensure that the AUV can operate stably in a dynamic environment. Based on the ROS architecture, the AUV's communication, docking control and data processing systems are constructed, and the collaborative control of the upper and lower computers is optimized to improve the system's response efficiency and docking stability. In combination with the dynamic docking requirements of the AUV for multi-port docking docks, a phased visual positioning strategy is proposed. In the long-distance stage, the EPnP+multi-light source redundant algorithm is used to calculate the global pose of the AUV. The advantage of full light source visibility is utilized, and the robustness of the pose solution is improved through redundant data, thereby reducing the long-distance error amplification effect. In the medium-distance stage, dual-light source geometric positioning is used to ensure the pose adjustment of the AUV in the case of partial light source loss. When the algorithm switches (EPnP→dual-light source method), pose jumps are easily caused by model differences, and control continuity needs to be maintained through smooth transition. In the close-range stage, STag visual markers are used for precise pose solution, and residual light source information is combined to maintain pose continuity to ensure that the AUV can stably complete autonomous docking.

Claims

1. An AUV dynamic docking system for a multi-port docking station includes a multi-port docking station and an AUV. The specific steps of the dynamic docking control method of the AUV and the multi-port docking station are as follows: S1, extracting the center coordinates of the guiding light sources of each docking port of a multi-port docking station, where each docking port is provided with two guiding light sources and a visual marker; S2: When in the long-range phase, the AUV uses the extracted center coordinates of the guiding light source to determine the 3D coordinates of the corresponding light source in the world coordinate system, selects the target dock to be docked and defines the center of the target dock as the origin of the world coordinate system. The light source information identified by the AUV is then converted into a 3D-2D point pair in the corresponding new coordinate system and input into the EPnP algorithm for global pose solution. The EPnP algorithm is used to calculate the rotation matrix and translation vector of the AUV relative to the target dock. The AUV autonomously docks to the target dock based on the rotation matrix and translation vector. S3: As the AUV gradually approaches the target dock, it is in the mid-range stage. The AUV determines the position of the target dock center on the camera pixel plane based on the center coordinates of the two guiding light sources extracted from the target dock. The AUV estimates the azimuth angle with the target dock based on the deviation between the position of the target dock center in the current image and the image center. The AUV adjusts its heading based on the azimuth angle and continues to dock to the target dock. S4, when the AUV approaches the target dock and is about to enter the target dock, it is in the final docking stage. The dock is still in motion. The AUV recognizes the Stag visual mark of the target dock and performs terminal docking posture solution. The AUV accurately docks with the target dock based on the posture solution.

2. The AUV dynamic docking system for multiple docking ports according to claim 1, characterized in that: The AUV is provided with a main control module, a drive system, a sensor system, and a power supply system, and the drive system, the sensor system, and the power supply system are all connected to the main control module.

3. The AUV dynamic docking system for multiple docking ports according to claim 1, characterized in that: The guiding light source of each docking port has a different color, and guiding light sources are arranged at the center of the upper and lower ends of each docking port, and the visual mark is set at the dock end of the dock.

4. The AUV dynamic docking system for multiple docking ports according to claim 1, characterized in that: The steps for extracting the center coordinates of the guiding light sources of each docking port of the multi-port docking station in step S1 are as follows: S11, cropping the target area according to the detection frame coordinates and converting the cropped image into a grayscale image; S12, applying a threshold segmentation method to highlight light source features, and performing contour detection, marking and drawing the detected contour on the cropped image; S13, calculating the centroid coordinates of the light source through the image moment and drawing the center point on the original image; S14, output the center coordinates of each light source.

5. The AUV dynamic docking system for multiple docking ports according to claim 4 is characterized by: The steps for calculating the centroid coordinates of the light source by using the image moment in step S13 are as follows: For a binary image, let its pixel intensity value be I(x,y), then the (p,q)-order geometric moment is defined as follows: Where (x, y) is the pixel coordinate in the image, p, q is the order of the moment, and I(x, y) is the pixel value of the point; The centroid of a light source is the center position of all pixels within the light source area. The calculation formula is as follows: Among them, M 10 =∑ x ∑ x I(x, y) represents the weighted sum of the horizontal coordinates of all pixels, M 01 =∑ x ∑ y yI(x, y) represents the weighted sum of the ordinates of all pixels, M 00 =∑ x ∑ y I(x, y) represents the total number of pixels in the region, that is, the area.

6. The AUV dynamic docking system for multiple docking ports according to claim 1, characterized in that: The specific steps of the EPnP algorithm in step S2 for global pose solution are as follows: S21, select control points: In the world coordinate system, select four non-coplanar control points. Usually, first calculate the center of gravity of all 3D reference points as the first control point C1 w , and then determine the remaining three control points C2 through principal component analysis w 、C3 w 、C4 w ; S22, calculate the homogeneous barycentric coordinates: For each 3D reference point Its coordinates can be expressed as a linear combination of four control points: Among them, α ij is the homogeneous barycentric coordinate, the subscript i represents the number of the 3D reference point, that is, the key point or feature point on the real object; the subscript j represents the number of the control point, that is, the four artificially selected virtual reference points that satisfy: S23, establish a linear relationship in the camera coordinate system: In the camera coordinate system, the reference point can also be expressed as a linear combination of control points: in, and are the coordinates of the reference point and control point in the camera coordinate system; S24, projection relationship: According to the camera projection model, the camera coordinates of the reference point and its image coordinates (u i ,v i ) has the following relationship: Among them, s i is the scaling factor, N is the camera intrinsic parameter matrix; S25, constructing a linear equation system: Substitute the expression in step S23 into step S24 to obtain The linear equations of S26, solve the coordinates of the control point in the camera coordinate system: combine all equations into matrix form: MC=B Among them, M is a known matrix, C is a matrix containing all The unknown vector is B, and the known vector is B. By solving this linear equation system, the coordinates of the control point in the camera coordinate system can be obtained. S27, calculate the camera pose: Once the coordinates of the control point in the camera coordinate system are obtained And their correspondence in the world coordinate system The camera's rotation matrix R and translation vector t can be determined by solving the rigid body transformation.

7. The AUV dynamic docking system for multiple docking ports according to claim 6, characterized in that: The control points in step S21 are four random non-coplanar guide light source points from all the identified guide light sources. There are multiple groups of control points, which are solved separately to obtain the solution results, and a consistency check is performed to eliminate outliers. The weighted mean of the multiple groups of solution results is calculated to obtain the final global posture.

8. The AUV dynamic docking system for multiple docking ports according to claim 1, characterized in that: The estimation steps of the AUV azimuth in step S3 are as follows: The optical center coordinates of the two guiding light sources at the target dock are (x1, y1) and (x2, y2) respectively. The coordinates of the center of the dock can be obtained by geometric calculation (x d ,y d )for: The azimuth angle θ can be calculated by the following formula: Among them, the parameter h θ It is an intermediate calculation quantity used to simplify the subsequent derivation parameters of tanθ; parameter v0 is the vertical axis pixel coordinate of the reference point; parameter u0 is the horizontal axis pixel coordinate of the reference point; parameter x d Is the horizontal pixel coordinate of the target point corresponding to the azimuth to be calculated; parameter y d is the vertical pixel coordinate of the target point corresponding to the azimuth to be calculated; the parameter vFOV represents the vertical field of view, in rad; the parameter height represents the height of the image, in px; the parameter dy is the scaling factor from the pixels on the v-axis to the actual length. The above camera parameters are all known quantities.

9. The AUV dynamic docking system for multiple docking ports according to claim 1, characterized in that: The pose calculation steps for the end-to-end docking based on the Stag visual marker in step S4 are as follows: S41, placing STag markers in the target area of ​​the docking station and defining the ID number, physical size, and camera parameters of the STag markers in the ROS configuration file so as to correctly detect the markers and calculate the pose; S42, the AUV camera captures images in real time during the docking process and performs marker detection through the STag ROS node; the STagROS package uses edge detection and corner extraction algorithms to identify the location of the STag marker and output the pixel coordinates of its four corner points on the image plane; S43, matching the two-dimensional image coordinates of the four corner points of the STag marker with the pre-set three-dimensional world coordinates, and solving the rotation matrix and translation vector of the STag marker relative to the camera coordinate system based on the EPnP algorithm; S44, the STag ROS package publishes the calculated pose information through the / stag_ros / pose topic. The AUV control system subscribes to this topic and obtains the position of the STag marker in real time. At S45, the AUV control system adjusts the thrusters and servos in real time using the pose information calculated by the STag ROS package, ensuring that the AUV always maintains the correct pose and docking accurately in the terminal phase.

Images