Unmanned underwater vehicle autonomous docking method based on binocular vision pose estimation

Abstract

The invention particularly relates to an unmanned underwater vehicle autonomous docking method based on binocular vision pose estimation. The method comprises the following steps: respectively collecting underwater images of a left camera and a right camera; preprocessing the underwater image, and extracting pixel coordinates of four LED light sources in the rectangular lamp array; constructing a PnP problem based on the world coordinates of the four LED light sources and the corresponding pixel coordinates in the two-dimensional image plane, solving the position and attitude of the camera relative to the coordinate system where the light sources are located, and obtaining the relative pose of the aircraft relative to the rectangular lamp array; fusing pose estimation results of the two cameras; and controlling the relative positions and postures of the aircraft and the lamp array to be converged to be 0 according to a pose fusion result, namely controlling the position and the posture of the aircraft to be consistent with those of the docking device. According to the method, high precision, autonomy and high robustness of docking operation of the underwater vehicle are realized.

Description

Technical Field

[0001] This invention relates to the field of autonomous docking and recovery of underwater robots, specifically to an autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation. Background Technology

[0002] With the increasing complexity of marine resource development and underwater operations, autonomous underwater vehicles (AUVs) are being used more and more widely in fields such as marine surveys, equipment maintenance, and underwater search and rescue. AUVs need to autonomously return to an underwater recovery platform or charging station after completing their missions; therefore, research into autonomous underwater docking technology for AUVs is necessary. Traditional underwater docking methods mostly rely on acoustic guidance, mechanical guidance, or manual intervention, which have significant shortcomings in complex water environments, areas with strong interference, and high precision requirements, exhibiting poor docking robustness, slow response speed, and low operational accuracy.

[0003] With the development of underwater optical sensing and image processing technologies, vision-based guidance using artificial light sources has become possible. In recent years, vision-based docking methods have gradually become a research hotspot. Among them, the use of monocular cameras for target recognition and localization is relatively common. However, due to the complexity of the underwater optical environment and the lack of distance perception, the success rate and attitude estimation accuracy still cannot meet the requirements of high-precision autonomous docking. In contrast, binocular vision systems can acquire more accurate depth information and have stronger three-dimensional environmental perception capabilities by utilizing a dual-view structure, providing a reliable technical means for underwater autonomous docking. However, underwater binocular vision systems still face practical challenges such as high image noise, unstable light source recognition, and unrobust pose calculation. Therefore, there is an urgent need for a new, highly robust autonomous docking method for underwater vehicles that can effectively solve technical problems such as unstable target feature extraction, large pose calculation errors, and untimely response of the control system.

[0004] It should be noted that the information disclosed in the background section above is only used to enhance the understanding of the background of the present invention, and therefore may include information that does not constitute prior art known to those skilled in the art. Summary of the Invention

[0005] This invention provides an autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation, which is used to solve the problem of how to stably identify light sources and achieve high-precision six-degree-of-freedom pose estimation in complex underwater environments. Based on the estimated pose, a control law is designed to achieve stable docking of the vehicle in six-degree-of-freedom space.

[0006] Other features and advantages of the invention will become apparent from the following detailed description, or may be learned in part by practice of the invention.

[0007] According to a first aspect of the present invention, an autonomous docking method for an unmanned underwater vehicle based on binocular visual pose estimation is provided. The docking target of the vehicle is to autonomously drive into a recovery device equipped with a rectangular light array. Each of the four corners of the rectangular light array is equipped with an LED underwater light source. Two industrial-grade network cameras are symmetrically mounted on the bow of the vehicle to acquire real-time images from left and right perspectives, respectively. The method includes: Underwater images were captured by the left and right cameras respectively; The underwater image is preprocessed to extract the pixel coordinates of the four LED light sources in the rectangular light array; Based on the world coordinates of the four LED light sources and the corresponding pixel coordinates in the two-dimensional image plane, a PnP problem is constructed to solve the position and attitude of the camera relative to the coordinate system of the light sources, and the relative pose of the vehicle relative to the rectangular light array is obtained. The pose estimation results of the two cameras are fused; Based on the pose fusion results, the control system ensures that the relative position and attitude of the vehicle and the light array converge to 0, that is, the position and attitude of the control system are consistent with those of the docking device.

[0008] In some exemplary embodiments, the image preprocessing includes: grayscale conversion, image enhancement, adaptive threshold binarization, image smoothing, contour extraction, pseudo-light source removal, contour centroid extraction, reflection processing, and target point matching.

[0009] In some exemplary embodiments, the step of constructing a PnP problem based on the world coordinates of the four LED light sources and the corresponding pixel coordinates in the two-dimensional image plane to solve for the position and orientation of the camera relative to the coordinate system of the light sources includes: The pixel coordinates of the centers of the four LED light sources Its pre-defined three-dimensional position in the world coordinate system One-to-one correspondence, and solve the camera extrinsic parameters based on the camera imaging principle. :

[0010] in, Represents the projection function. The camera intrinsic parameter matrix is ​​obtained through offline calibration, while the camera extrinsic parameters... It contains relative pose information; The specific process for solving the pose is as follows: First, the solvePnP and solvePnPRefineLM methods in OpenCV are used for initial solution and error optimization to obtain the rotation vector. With translation vector Then, the rotation vector is transformed using the Rodrigues transformation. The rotation matrix is ​​obtained from Finally, from the rotation matrix Translation vector Euler angles were calculated. and the position of the camera relative to the light array Use the following formula from the rotation matrix Solving Euler angles from the camera system to the world system:

[0011] in, Rotation matrix Elements in; The position of the camera's coordinate origin relative to the light array's coordinate origin is determined by the translation vector. The inverse transformation with the rotation matrix yields: .

[0012] In some exemplary embodiments, the fusion of the pose estimation results of the two cameras includes: First, determine whether both cameras have identified four target light sources. If both cameras have four valid light source points and the pose estimation results meet the requirements... If the result is a weighted fusion solution based on the reprojection error, then the result is taken from the single path that satisfies the error. If all results are invalid, the default pose 0 will be output. The weighted fusion method for the solution results is as follows: Assume the relative poses calculated by the left and right cameras are respectively... and The corresponding reprojection error is The merged position and orientation are as follows: .

[0013] In some exemplary embodiments, the reprojection error is calculated as follows: Based on the solved camera extrinsic parameters world coordinates of the light source Reprojecting onto the pixel coordinate system yields the reprojected coordinates of the light source. Then, the reprojection error is calculated using the reprojection coordinates and the coordinates obtained from image processing: .

[0014] In some exemplary embodiments, controlling the vehicle and the light array to converge to 0 in relative position and attitude based on the pose fusion result includes: Based on the characteristics of the docking mission, the control of the spacecraft is divided into three channels: horizontal plane control, vertical plane control, and axial motion control. The control strategy for each channel is as follows: Horizontal plane control: The control objective is to adjust the vehicle's heading angle. Orientation of the light array Consistent, and lateral error It needs to converge to 0; Vertical plane control: The control objective is to adjust the vehicle to be at the same depth as the light array, and to adjust the pitch angle. The convergence is 0; Axial motion control: The control objective is to adjust the vehicle's speed so that the vehicle can approach and enter the light array at a safe speed; Finally, after limiting and dead-zone processing of all control quantities for the three channels, they are sent to the thruster for execution; this is done when the following conditions are met simultaneously: The system determines that the spacecraft has entered the docking window and completes the docking.

[0015] According to a second aspect of the present invention, an autonomous docking device for an unmanned underwater vehicle based on binocular vision pose estimation is provided, comprising: The image acquisition module is used to acquire underwater images from the left and right cameras respectively. The image processing module is used to preprocess underwater images and extract the pixel coordinates of the four LED light sources in the rectangular light array; The pose estimation module is used to construct a PnP problem based on the world coordinates of the four LED light sources and the corresponding pixel coordinates in the two-dimensional image plane, to solve the position and attitude of the camera relative to the coordinate system of the light sources, and to obtain the relative pose of the vehicle relative to the rectangular light array. The binocular fusion module is used to fuse the pose estimation results of the two cameras; The docking control module is used to control the relative position and attitude of the vehicle and the light array to converge to 0 based on the pose fusion result, that is, to control the position and attitude of the vehicle to be consistent with the docking device.

[0016] According to a third aspect of the present invention, a storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation as described in the first aspect.

[0017] According to a fourth aspect of the present invention, a computer program product is provided, on which a computer program is stored, wherein when the computer program is executed by a processor, the autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation described in the first aspect is implemented.

[0018] According to a fifth aspect of the present invention, an electronic device is provided, comprising: Processor; and Memory for storing the executable instructions of the processor; The processor is configured to implement the autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation as described in the first aspect by executing the executable instructions.

[0019] The autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation provided by the embodiments of the present invention utilizes a binocular vision sensor to acquire images of a target light array. Through image processing, pose calculation, and fusion algorithms, the six-degree-of-freedom pose of the vehicle relative to the target light array is obtained. Based on the relative pose and a multi-channel control strategy, high precision, autonomy, and robustness of the underwater vehicle docking operation are achieved. Compared with existing technologies, it has the following advantages: 1. Highly robust target recognition: Combining various image processing techniques such as image enhancement, adaptive binarization, image smoothing, contour extraction, pseudo-light source removal, and target point matching, it can stably extract feature points in light arrays under harsh environments such as low light, turbid water, and water surface reflection, effectively improving the success rate and accuracy of light source recognition.

[0020] 2. High-precision pose calculation: The PnP algorithm and reprojection error optimization mechanism are used to accurately estimate the six-degree-of-freedom relative pose between the vehicle and the target light array. The estimation results from both the left and right view cameras are used simultaneously by a fusion algorithm, which significantly improves the accuracy of positioning and attitude estimation.

[0021] 3. Redundant fusion mechanism: The pose is estimated independently by the left and right cameras and then weighted and fused by reprojection error to ensure that the system still has stable output when a single camera fails or misidentifies, thereby improving the overall fault tolerance and reliability of the system.

[0022] 4. High-efficiency control strategy: Based on the six-degree-of-freedom pose error estimated by vision, a three-dimensional spatial channel control algorithm for horizontal, vertical and axial motion was designed. The control law is simple and efficient, with good convergence and real-time performance, ensuring that the vehicle enters the recovery device stably and smoothly.

[0023] 5. The system is highly modular and has strong real-time performance: image acquisition, processing, pose estimation and control tasks are executed in parallel by modules, all threads run independently and communicate with each other in real time, and the system has good scalability and deployment flexibility.

[0024] In summary, this invention enables stable and autonomous docking of vehicles and platforms in complex underwater environments, and has broad engineering application prospects in unmanned underwater operations.

[0025] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit the invention. Attached Figure Description

[0026] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention. It is obvious that the drawings described below are merely some embodiments of the invention, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort.

[0027] Figure 1 A schematic diagram of the configuration and rectangular light array for an underwater docking system vehicle. Figure 2 Flowchart of an autonomous docking method for underwater vehicles based on binocular vision pose estimation; Figure 3 The processing results of underwater images at each step of the image processing workflow are as follows: (a1) Image acquisition; (a2) Image enhancement; (a3) ​​Binarization; (a4) Local magnification of the binary image; (a5) Image smoothing; (a6) Contour extraction; (b1) Pseudo-light source interference; (b2) Pseudo-light source removal; (c) Contour centroid coordinate extraction. Figure 4 The results of visual pose estimation and control error convergence during autonomous docking are shown in the following figures: (a) relative position calculation and docking control; (b) relative attitude angle calculation and docking control. Figure 5 The diagram illustrates the docking process of the aircraft based on visual guidance: (a) the aircraft is 9m away from the light array; (b) the aircraft is 7m away from the light array; (c) the aircraft is 5m away from the light array; (d) the aircraft is 3m away from the light array. Detailed Implementation

[0028] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, they are provided so that the invention will be more comprehensive and complete, and will fully convey the concept of the exemplary embodiments to those skilled in the art. The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

[0029] Furthermore, the accompanying drawings are merely illustrative of the invention and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and therefore repeated descriptions of them will be omitted. Some block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities can be implemented in software, in one or more hardware modules or integrated circuits, or in different network and / or processor devices and / or microcontroller devices.

[0030] To address the shortcomings and deficiencies of existing technologies, this example embodiment provides an autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation. It utilizes binocular vision sensors to acquire images of a target light array, and obtains the vehicle's six-degree-of-freedom pose relative to the target light array through image processing, pose calculation, and fusion algorithms. Based on the relative pose and a multi-channel control strategy, it achieves high precision, autonomy, and robustness in the underwater vehicle docking operation. Figure 2 As shown, it includes the following steps: Step 1: Image Acquisition Two industrial cameras are symmetrically mounted on the bow of the vehicle to capture images of the target light array from the left and right perspectives, respectively. The light array consists of four artificial light sources arranged in a horizontal rectangle on the docking and recovery device. The vehicle is equipped with an image processing computer, and the images captured by the two cameras are processed by independent image processing threads within the computer.

[0031] Step 2: Image Processing The image processing thread is responsible for identifying and accurately extracting four light source points from the artificial light source array in the underwater image, providing robust input for subsequent pose estimation. The core image processing workflow includes image enhancement, adaptive threshold binarization, image smoothing, contour extraction, spurious light source removal, and target point matching, with the specific steps as follows: (1) Grayscale Conversion: While onboard cameras acquire and store images in color, image processing often requires converting three-channel color images to single-channel grayscale for processing. The main purpose of this conversion is to reduce the complexity of image data, enhance image features, and simplify subsequent analysis and processing. Color images typically use… The (red, green, blue) color space represents each pixel as composed of three components: red, green, and blue. To enhance image features, a weighted average method is used to transform the original color image... Convert to grayscale :

[0032] (2) Image Enhancement: Underwater images suffer from problems such as blurriness, low brightness, and low contrast, making it difficult to extract light sources. Therefore, image enhancement is necessary. The image enhancement method is as follows: First, calculate the average brightness of the image. :

[0033] in, The image resolution is set, and then enhancement parameters are configured based on the average brightness value. , : like If the contrast is too low, increase the contrast:

[0034] like If the brightness is too high, the image will be darkened.

[0035] in, The adjustable parameters are set according to different ambient brightness levels, resulting in the enhanced image:

[0036] (3) Adaptive Threshold Binarization: To further simplify image data, highlight feature points, remove noise, and improve contrast, thereby emphasizing and extracting image features or targets, binarization methods can be used to process the enhanced grayscale image into a black and white image. During binarization, the choice of threshold affects the accuracy and robustness of the final result. Therefore, to determine a suitable threshold, an adaptive threshold calculation based on image brightness is used. First, based on the average brightness of the enhanced image... Calculate the adaptive threshold Then perform binarization:

[0037] in, These are adjustable parameters that can be manually set based on water quality and environmental conditions.

[0038] (4) Image Smoothing: Binarized images often still contain noise interference. Therefore, median filtering is applied to the binary image to eliminate noise. The neighborhood size of the median filter is set to... The filtered image is:

[0039] To make the contours of artificial light sources in the image more continuous, the median-filtered binary underwater image was further smoothed, and a morphological closing operation (dilation followed by erosion) was performed:

[0040] (5) Contour Extraction: To identify the edges and contours of artificial light sources in an image, edge detection is required. Edges represent areas of drastic grayscale or color change in an image, which are very useful for locating object boundaries and shape analysis. Canny edge detection is performed on the image after morphological operations:

[0041] in, Low and high thresholds are set for edge intensity, and then the contour of the artificial light source is extracted. .

[0042] (6) False Light Source Removal: Bubbles and reflected light in water can easily form false light sources, interfering with the identification of the target light source. Therefore, false light sources must be removed from the image. The pixel area of ​​the outline of a false light source is generally much smaller than that of the outline of the target light source, and the outline of a false light source is often irregular, while the outline of the target light source is generally approximately circular. Therefore, false light sources can be removed by considering the pixel area and shape of the light source outline. The specific method is as follows: Calculate the pixel area of ​​each outline in the image. And the contour shape is approximated using a polygon fitting method. If the area is smaller than the set pixel area or the number of sides of the outline is less than the number of sides of the fitted graphic, then the outline is considered a pseudo-light source and needs to be discarded.

[0043] (7) Extracting the centroid of the contour: To obtain the pixel coordinates of the artificial light source center in the image, the geometric moments of the contour are used to solve for the centroid coordinates of the light source. The method for solving for the centroid of the contour using image geometric moments is as follows:

[0044] in, The image moments are for the contour.

[0045] (8) Reflection Processing: If the vehicle and light array are positioned near the water surface, the light array will create a reflection on the water surface. The light source in the reflection will affect the recognition of the real light source. If the number of centroid points of the contour in the image... This indicates the presence of water reflections or noise interference. The method for handling reflections is as follows: sort the ordinates of the centroid points in ascending order, and select the last four points as candidate target points. Other centroids are discarded as reflections or noise.

[0046] (9) Target point matching: In order to ensure that the four candidate target points are the image points corresponding to the rectangular light array, the four candidate target points are sorted in the order of top left, bottom left, top right, and bottom right to obtain the point set. Then, determine in sequence whether the four candidate points form an approximate rectangle. Let:

[0047] Calculate the cosine of the included angle for each of the four angles:

[0048] If all angles satisfy If the four candidate points are considered to form a valid approximate rectangular structure that matches the positions of the four light sources in the rectangular light array, then the extracted light source points are considered not to match the target points and these four points are discarded.

[0049] (10) Output target point coordinates: After successfully identifying four target points, their pixel coordinates are used as input in the order of top left, bottom left, top right, and bottom right, and provided to the subsequent pose estimation module.

[0050] Step 3: Pose Estimation The image processing thread, based on computer vision technology, extracted the pixel coordinates of the target light sources. If the world coordinates and corresponding pixel coordinates in the two-dimensional plane of the four target light sources are known, a PnP problem can be constructed to solve for the camera's position and attitude relative to the target light source coordinate system, thus obtaining the relative pose of the vehicle with respect to the light array. Solving the following PnP problem yields the relative pose between the vehicle and the light array: [The PnP problem is missing here, so the translation ends here.] Its preset three-dimensional position in the world coordinate system (with the origin selected as the center of the rectangular light array) One-to-one correspondence, and the camera's extrinsic parameters can be solved based on the camera's imaging principle. :

[0051] in, Represents the projection function. The camera intrinsic parameter matrix is ​​obtained through offline calibration, while the camera extrinsic parameters... It contains relative pose information.

[0052] The specific process for solving the pose is as follows: First, the EPnP algorithm is used in conjunction with the Levenberg-Marquardt optimization method for initial solution and error optimization to obtain the rotation vector. With translation vector Then, the rotation vector is transformed using the Rodrigues transformation. The rotation matrix is ​​obtained from Finally, from the rotation matrix Translation vector Euler angles were calculated. and the position of the camera relative to the light array From the rotation matrix The method for calculating the Euler angles (ZYX order) from the camera frame to the world frame is as follows:

[0053] in, Rotation matrix The elements in the array. The position of the camera coordinate origin relative to the light array coordinate origin is determined by the translation vector. The inverse transformation with the rotation matrix yields:

[0054] Step 4: Binocular Fusion Underwater images acquired by the left and right cameras are processed and pose estimated in separate threads. The results from each thread need to be fused based on reprojection error and target light source effectiveness. To fuse the pose estimation results from the two cameras, the solved camera extrinsic parameters are used... world coordinates of the light source Reprojecting onto the pixel coordinate system yields the reprojected coordinates of the light source. Then, the reprojection error is calculated using the reprojection coordinates and the coordinates obtained from image processing:

[0055] The specific implementation of the binocular fusion algorithm is as follows: First, it is determined whether both cameras have identified four target light source points. If both have four valid light source points and the estimated pose is reasonable, the fusion result is weighted according to the reprojection error. Otherwise, the reliable result from a single camera is used. If both are invalid, a default pose of 0 is output. The dual-camera data fusion method is as follows: Assume the relative poses calculated by the left and right cameras are respectively... and The corresponding reprojection error is The merged position and orientation are as follows:

[0056] Step 5: Dock Control The vehicle involved in this invention is a fully driven, slender-bodied underwater vehicle (AUV) with three groups of six thrusters: two auxiliary thrusters at the bow and stern in the vertical direction for controlling the AUV's heave and pitch; two auxiliary thrusters at the bow and stern in the horizontal direction for controlling the AUV's lateral and yaw movements; and two main thrusters at the stern for controlling the AUV's forward, backward, and roll movements. The autonomous underwater docking of the AUV requires controlling the relative position and attitude of the AUV and the light array to converge to zero based on the attitude fusion result; that is, the position and attitude of the AUV need to be consistent with the docking device. Based on the characteristics of the docking mission, the control of the AUV is divided into three channels: horizontal plane control, vertical plane control, and axial motion control. The control strategy for each channel is as follows: (1) Horizontal plane control: The control objective is to adjust the heading angle of the aircraft. Orientation of the light array Consistent, and lateral error It needs to converge to 0. Therefore, the speed commands for the bow and stern thrusters in the horizontal direction are as follows:

[0057] When visual pose estimation data is valid When visual pose estimation is ineffective, .in, and These are the heading angle and heading angular velocity of the aircraft, respectively. These are adjustable control parameters.

[0058] (2) Vertical plane control: The control objective is to adjust the vehicle to be at the same depth as the light array and to adjust the pitch angle. The convergence is 0. The depth of the vehicle. The depth of the light array is known, obtained from a depth gauge. The depth error in the vertical direction is expressed as To compensate for depth deviations caused by the vehicle's translational motion, and small-range deviations in depth gauge measurements, ,in and These represent the relative position errors of the vehicle with respect to the light array in the horizontal and vertical directions, respectively, calculated by visual pose estimation. Due to the vehicle's positive buoyancy, steady-state errors occur in depth control; therefore, an integral term is used to improve these errors.

[0059] In summary, the vertical rotational speed commands for the bow and stern thrusters of the aircraft are as follows:

[0060] in, For adjustable control parameters, These are the pitch angle and pitch rate of the aircraft.

[0061] (3) Axial motion control: The control objective is to adjust the vehicle's speed so that it can approach and enter the light array at a safe speed. When the bow of the vehicle is at different distances from the light array, the main thrusters of the vehicle use different speeds. When the vehicle is far from the light array, the main thrusters use a higher speed to ensure that the vehicle approaches the light array quickly. When the vehicle is two to three vehicle lengths away from the light array, the main thrusters use a lower speed to ensure that the vehicle can safely and smoothly align with the light array and complete the docking.

[0062] Finally, after limiting and dead-time processing of all control quantities for the three channels, they are sent to the thruster for execution. This is done when the following conditions are simultaneously met: The system determines that the spacecraft has entered the docking window and completes the docking.

[0063] The steps in this exemplary embodiment will now be described in more detail with reference to the accompanying drawings and embodiments.

[0064] The autonomous docking system of an unmanned underwater vehicle consists of the vehicle and a docking and recovery device, as follows: Figure 1As shown, the AUV is a fully driven, slender-bodied vehicle with three groups of six thrusters: two auxiliary thrusters at the bow and stern in the vertical direction to control the AUV's heave and pitch; two auxiliary thrusters at the bow and stern in the horizontal direction to control the AUV's lateral and yaw movements; and two main thrusters at the stern to control the AUV's forward, backward, and roll movements. Each thruster in each degree of freedom uses a counter-rotating propeller configuration to control the unbalanced torque generated by the propeller's interaction with the fluid. Two industrial-grade network cameras are symmetrically mounted at the bow to acquire real-time images from left and right perspectives. The AUV internally carries an image processing computer and a motion control computer, used for underwater binocular vision pose estimation and autonomous docking control, respectively. The image processing computer is equipped with the OpenCV computer vision library to perform image acquisition, image processing, pose estimation, and binocular fusion. The AUV's docking target is to autonomously navigate into a recovery device equipped with a rectangular light array. Each of the four corners of the rectangular light array is equipped with a high-brightness LED underwater light source. Based on the above configuration and Figure 2 The algorithm flow shown in this invention utilizes a binocular vision sensor to acquire images of the target light array. Through image processing, pose estimation, and fusion algorithms, the six-degree-of-freedom pose of the underwater vehicle relative to the light array is obtained. Then, based on the relative pose and a multi-channel control strategy, the docking operation of the underwater vehicle is completed. Specifically, this invention consists of the following algorithm modules: an image acquisition module, an image processing module, a pose estimation module, a binocular fusion module, and a docking control module. Each module is described in detail below.

[0065] Step 1: Image Acquisition The image acquisition module uses the VideoCapture interface in OpenCV to acquire image streams from two cameras via the RTSP protocol. Because camera lenses introduce radial and tangential distortion, causing distortion of the shape and size of objects in the image, camera calibration is necessary before visual localization to accurately describe the camera lens distortion model and correct image distortion, ensuring objects present their true shape and size. Camera calibration parameters include focal length. Principal point coordinates and distortion coefficient The values ​​are obtained through offline calibration and stored uniformly in the parameter configuration file.

[0066] Step 2: Image Processing The real-time images captured by the two cameras are calibrated by the image acquisition module and then processed by the image processing module. The image processing module is responsible for identifying and accurately extracting the four LED light sources from the rectangular light array in the underwater image, providing robust input for subsequent pose estimation. The core workflow of image processing includes image enhancement, adaptive threshold binarization, image smoothing, contour extraction, false light source removal, and target point matching. The specific steps are as follows: (1) Grayscale Conversion: While onboard cameras acquire and store images in color, image processing often requires converting three-channel color images to single-channel grayscale for processing. The main purpose of this conversion is to reduce the complexity of image data, enhance image features, and simplify subsequent analysis and processing. Color images typically use… The (red, green, blue) color space represents each pixel as composed of three components: red, green, and blue. To enhance image features, a weighted average method is used to transform the original color image... Convert to grayscale :

[0067] (2) Image Enhancement: Underwater images suffer from problems such as blurriness, low brightness, and low contrast, making it difficult to extract light sources. Therefore, image enhancement is necessary. The image enhancement method is as follows: First, calculate the average brightness of the image. :

[0068] in, The image resolution is set, and then enhancement parameters are configured based on the average brightness value. , : like If the contrast is too low, increase the contrast:

[0069] like If the brightness is too high, the image will be darkened.

[0070] The final enhanced image is obtained:

[0071] (3) Adaptive Threshold Binarization: To further simplify image data, highlight feature points, remove noise, and improve contrast, thereby emphasizing and extracting image features or targets, binarization methods can be used to process the enhanced grayscale image into a black and white image. When performing binarization, the choice of threshold affects the accuracy and robustness of the final result; therefore, selecting an appropriate threshold is crucial. This invention performs adaptive threshold calculation based on image brightness. First, based on the average brightness of the enhanced image… Calculate the adaptive threshold Then perform binarization:

[0072] in, This is an adjustable parameter; manually set it to 90 based on water quality and environmental conditions.

[0073] (4) Image Smoothing: Binarized images often still contain noise interference. Therefore, median filtering is applied to the binary image to eliminate noise. The neighborhood size of the median filter is set to 5, and the filtered image is as follows:

[0074] To make the outline of the LED light source in the image more continuous, the median-filtered binary underwater image was further smoothed, and a morphological closing operation (dilation followed by erosion) was performed:

[0075] (5) Contour Extraction: To identify the edges and contours of the LED light source in the image, edge detection is required. Edges represent areas in the image where grayscale or color changes drastically, and are very useful for locating object boundaries and shape analysis. Canny edge detection is performed on the image after morphological operations:

[0076] in, We set the low and high thresholds for edge intensity to 30 and 90 respectively, and then used the findContours method in OpenCV to extract the contours of the LED light source. .

[0077] (6) False Light Source Removal: Bubbles and reflected light in water can easily form false light sources, interfering with the identification of the target LED light source. Therefore, false light sources must be removed from the image. The pixel area of ​​the false light source outline is generally much smaller than that of the target light source outline, and the outline of the false light source is often irregular, while the outline of the target light source is generally approximately circular. Therefore, false light sources can be removed by considering the pixel area and shape of the light source outline. The contourArea method in OpenCV is used to calculate the pixel area of ​​each contour in the image. The contour shape is approximated using the polygon fitting method approxPolyDP.

[0078] in, The polygon complexity is controlled and set to 0.02. Let be the perimeter of the outline, if The outline is then considered a pseudo-light source and needs to be removed.

[0079] (7) Extracting the centroid of the contour: To obtain the pixel coordinates of the LED light source center in the image, the geometric moments of the contour are used to solve for the centroid pixel coordinates of the LED light source. The method for solving for the centroid of the contour using image geometric moments is as follows:

[0080] in, The image moments are for the contour.

[0081] (8) Reflection Processing: If the aircraft and LED light array are positioned near the water surface, the light array will create a reflection on the water surface. The light source in the reflection will affect the recognition of the real light source. If the number of centroid points of the contour in the image... This indicates the presence of water reflection or noise interference. After sorting the centroid points by their ordinates from smallest to largest, the last four points are selected as candidate target points. Other centroids are discarded as reflections or noise.

[0082] (9) Target point matching: In order to ensure that the four candidate target points are the image points corresponding to the rectangular LED array, the four candidate target points are sorted in the order of top left, bottom left, top right, and bottom right to obtain the point set. Then, determine in sequence whether the four candidate points form an approximate rectangle. Let:

[0083] Calculate the cosine of the included angle for each of the four angles:

[0084] If all angles satisfy If the four candidate points are found to form a valid approximate rectangular structure that matches the positions of the four light sources in the rectangular LED array, then the extracted light source points are considered not to match the target points and these four points are discarded.

[0085] (10) Output target point coordinates: After successfully identifying four target points, their pixel coordinates are used as input in the order of top left, bottom left, top right, and bottom right, and provided to the subsequent pose estimation module.

[0086] The results of each step in the image processing workflow are as follows: Figure 3 As shown. Through the above image processing procedure, the system can stably extract feature points of the lamp array in environments with strong noise, water reflection, and low light, greatly improving the reliability and accuracy of pose estimation.

[0087] Step 3: Pose Estimation The image processing module, based on computer vision technology, extracts the pixel coordinates of the light source centers in the image. If the world coordinates of the four LED light sources and their corresponding pixel coordinates in the two-dimensional image plane are known, a Proof-of-Practice (PnP) problem can be constructed to solve for the camera's position and orientation relative to the coordinate system of the light sources, thus obtaining the relative pose of the aircraft with respect to the light array. Solving the following PnP problem yields the relative pose between the aircraft and the light array: [The PnP problem is described in the original text, but the provided excerpt ends here.] Its preset three-dimensional position in the world coordinate system (with the origin selected as the center of the rectangular light array) One-to-one correspondence, and the camera's extrinsic parameters can be solved based on the camera's imaging principle. :

[0088] in, Represents the projection function. The camera intrinsic parameter matrix is ​​obtained through offline calibration, while the camera extrinsic parameters... It contains relative pose information.

[0089] The specific process for solving the pose is as follows: First, the solvePnP and solvePnPRefineLM methods in OpenCV are used for initial solution and error optimization to obtain the rotation vector. With translation vector Then, the rotation vector is transformed using the Rodrigues transformation. The rotation matrix is ​​obtained from Finally, from the rotation matrix Translation vector Euler angles were calculated. and the position of the camera relative to the light array Use the following formula from the rotation matrix Solve the Euler angles from the camera frame to the world frame (ZYX order):

[0090] in, Rotation matrix The elements in the array. The position of the camera coordinate origin relative to the light array coordinate origin is determined by the translation vector. The inverse transformation with the rotation matrix yields:

[0091] Step 4: Binocular Fusion Since the pose estimation module is based on monocular imaging principles, the underwater images acquired by the left and right cameras are processed and pose estimated in separate threads. Therefore, the results from each thread are fused based on reprojection error and target light source effectiveness. To fuse the pose estimation results from the two cameras, the solved camera extrinsic parameters are used... world coordinates of the light source Reprojecting onto the pixel coordinate system yields the reprojected coordinates of the light source. Then, the reprojection error is calculated using the reprojection coordinates and the coordinates obtained from image processing:

[0092] The specific implementation of the binocular fusion algorithm is as follows: First, it is determined whether both cameras have identified four target light sources. If both have four valid light source points and the pose estimation is reasonable, the result is weighted and fused based on the reprojection error. Otherwise, the reliable result from a single camera is used. If both are invalid, a default pose of 0 is output. The weighted fusion method for the solution results is as follows: Assume the relative poses calculated by the left and right cameras are respectively... and The corresponding reprojection error is The merged position and orientation are as follows:

[0093] The pose fusion result is updated every 50ms and is sent from the image processing computer to the motion control computer via TCP communication.

[0094] Step 5: Dock Control Underwater autonomous docking of the vehicle requires controlling the relative position and attitude of the vehicle and the light array to converge to zero based on the pose fusion results. In other words, the vehicle's position and attitude need to be consistent with the docking device. Based on the characteristics of the docking mission, the vehicle's control is divided into three channels: horizontal plane control, vertical plane control, and axial motion control. The control strategy for each channel is as follows: (1) Horizontal plane control: The control objective is to adjust the heading angle of the aircraft. Orientation of the light array Consistent, and lateral error It needs to converge to 0. Therefore, the speed commands for the bow and stern thrusters in the horizontal direction are as follows:

[0095] When visual pose estimation data is valid When visual pose estimation is ineffective, .in, and These are the heading angle and heading angular velocity of the aircraft, respectively. These are adjustable control parameters.

[0096] (2) Vertical plane control: The control objective is to adjust the vehicle to be at the same depth as the light array and to adjust the pitch angle. The convergence is 0. The depth of the vehicle. The depth of the light array is known, obtained from a depth gauge. The depth error in the vertical direction is expressed as To compensate for depth deviations caused by the vehicle's translational motion, and small-range deviations in depth gauge measurements, ,in and These represent the relative position errors of the vehicle with respect to the light array in the horizontal and vertical directions, respectively, calculated by visual pose estimation. Due to the vehicle's positive buoyancy, steady-state errors occur in depth control; therefore, an integral term is used to improve these errors.

[0097] In summary, the vertical rotational speed commands for the bow and stern thrusters of the aircraft are as follows:

[0098] in, For adjustable control parameters, These are the pitch angle and pitch rate of the aircraft.

[0099] (3) Axial motion control: The control objective is to adjust the vehicle's speed so that it can approach and enter the light array at a safe speed. When the bow of the vehicle is at different distances from the light array, the main thrusters of the vehicle use different speeds. When the vehicle is far from the light array, the main thrusters use a higher speed to ensure that the vehicle approaches the light array quickly. When the vehicle is two to three vehicle lengths away from the light array, the main thrusters use a lower speed to ensure that the vehicle can safely and smoothly align with the light array and complete the docking.

[0100] Finally, after limiting and dead-time processing of all control quantities for the three channels, they are sent to the thruster for execution. This is done when the following conditions are simultaneously met: The system determines that the spacecraft has entered the docking window and completes the docking.

[0101] The visual pose estimation results and control errors during the underwater autonomous docking process of the vehicle, as well as the process of the vehicle approaching the rectangular light array, are respectively as follows: Figure 4 and Figure 5 As shown, the system demonstrated good stability in actual lake trials, supporting rapid repositioning, error compensation, and multiple-attempt mechanisms, with an overall docking success rate exceeding 90%.

[0102] It should be noted that, as another aspect, this application also provides a storage medium, which may be included in an electronic device or may exist independently without being assembled into the electronic device. The aforementioned storage medium carries one or more programs, which, when executed by an electronic device, cause the electronic device to perform the methods described in the following embodiments. For example, the electronic device may perform... Figure 2 The steps of the method shown.

[0103] In one embodiment, this application provides a computer program product including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0104] Furthermore, the above figures are merely illustrative of the processes included in the method according to exemplary embodiments of the present invention, and are not intended to be limiting. It is readily understood that the processes shown in the above figures do not indicate or limit the temporal order of these processes. Additionally, it is readily understood that these processes may be executed synchronously or asynchronously, for example, in multiple modules.

[0105] Other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of the invention herein. This application is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not disclosed herein. The specification and embodiments are to be considered exemplary only, and the true scope and spirit of the invention are indicated by the claims.

[0106] It should be understood that the present invention is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope. The scope of the invention is defined only by the appended claims.

Claims

1. An autonomous docking method for an unmanned underwater vehicle based on binocular vision pose estimation, wherein the docking target of the vehicle is to autonomously drive into a recovery device equipped with a rectangular light array, wherein... An LED underwater light source is installed at each of the four corners of the rectangular light array, and two industrial-grade network cameras are symmetrically installed on the bow of the vehicle to acquire real-time images from left and right perspectives, respectively. The method is characterized by comprising: Underwater images were captured by the left and right cameras respectively; The underwater image is preprocessed to extract the pixel coordinates of the four LED light sources in the rectangular light array; Based on the world coordinates of the four LED light sources and the corresponding pixel coordinates in the two-dimensional image plane, a PnP problem is constructed to solve the position and attitude of the camera relative to the coordinate system of the light sources, and the relative pose of the vehicle relative to the rectangular light array is obtained. The pose estimation results of the two cameras are fused; Based on the pose fusion results, the control system ensures that the relative position and attitude of the vehicle and the light array converge to 0, that is, the position and attitude of the control system are consistent with those of the docking device.

2. The method according to claim 1, characterized in that, The image preprocessing includes: grayscale conversion, image enhancement, adaptive threshold binarization, image smoothing, contour extraction, pseudo-light source removal, contour centroid extraction, reflection processing, and target point matching.

3. The method according to claim 1, characterized in that, The PnP problem is constructed based on the world coordinates of the four LED light sources and the corresponding pixel coordinates in the two-dimensional image plane to solve for the position and orientation of the camera relative to the coordinate system of the light sources, including: The pixel coordinates of the centers of the four LED light sources Its pre-defined three-dimensional position in the world coordinate system One-to-one correspondence, and solve the camera extrinsic parameters based on the camera imaging principle. : in, Represents the projection function. The camera intrinsic parameter matrix is ​​obtained through offline calibration, while the camera extrinsic parameters... It contains relative pose information; The specific process for solving the pose is as follows: First, the solvePnP and solvePnPRefineLM methods in OpenCV are used for initial solution and error optimization to obtain the rotation vector. With translation vector Then, the rotation vector is transformed using the Rodrigues transformation. The rotation matrix is ​​obtained from Finally, from the rotation matrix Translation vector Euler angles were calculated. and the position of the camera relative to the light array Use the following formula from the rotation matrix Solving Euler angles from the camera system to the world system: in, Rotation matrix Elements in; The position of the camera's coordinate origin relative to the light array's coordinate origin is determined by the translation vector. The inverse transformation with the rotation matrix yields: 。 4. The method according to claim 3, characterized in that, The fusion of pose estimation results from the two cameras includes: First, determine whether both cameras have identified four target light sources. If both cameras have four valid light source points and the pose estimation results meet the requirements... If the result is a weighted fusion solution based on the reprojection error, then the result is taken from the single path that satisfies the error. If all results are invalid, the default pose 0 will be output. The weighted fusion method for the solution results is as follows: Assume the relative poses calculated by the left and right cameras are respectively... and The corresponding reprojection error is The merged position and orientation are as follows: 。 5. The method according to claim 4, characterized in that, The method for calculating the reprojection error is as follows: Based on the solved camera extrinsic parameters world coordinates of the light source Reprojecting onto the pixel coordinate system yields the reprojected coordinates of the light source. Then, the reprojection error is calculated using the reprojection coordinates and the coordinates obtained from image processing: 。 6. The method according to claim 1, characterized in that, The step of controlling the vehicle and the light array to converge to 0 in relative position and attitude based on the pose fusion result includes: Based on the characteristics of the docking mission, the control of the spacecraft is divided into three channels: horizontal plane control, vertical plane control, and axial motion control. The control strategy for each channel is as follows: Horizontal plane control: The control objective is to adjust the vehicle's heading angle. Orientation of the light array Consistent, and lateral error It needs to converge to 0; Vertical plane control: The control objective is to adjust the vehicle to be at the same depth as the light array, and to adjust the pitch angle. The convergence is 0; Axial motion control: The control objective is to adjust the vehicle's speed so that the vehicle can approach and enter the light array at a safe speed; Finally, after limiting and dead-zone processing of all control quantities for the three channels, they are sent to the thruster for execution; this is done when the following conditions are met simultaneously: The system determines that the spacecraft has entered the docking window and completes the docking.

7. An autonomous docking device for unmanned underwater vehicles based on binocular vision pose estimation, characterized in that, include: The image acquisition module is used to acquire underwater images from the left and right cameras respectively. The image processing module is used to preprocess underwater images and extract the pixel coordinates of the four LED light sources in the rectangular light array; The pose estimation module is used to construct a PnP problem based on the world coordinates of the four LED light sources and the corresponding pixel coordinates in the two-dimensional image plane, to solve the position and attitude of the camera relative to the coordinate system of the light sources, and to obtain the relative pose of the vehicle relative to the rectangular light array. The binocular fusion module is used to fuse the pose estimation results of the two cameras; The docking control module is used to control the relative position and attitude of the vehicle and the light array to converge to 0 based on the pose fusion result, that is, to control the position and attitude of the vehicle to be consistent with the docking device.

8. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation as described in any one of claims 1 to 6.

9. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation as described in any one of claims 1 to 6.

10. An electronic device, characterized in that, include: processor; as well as Memory for storing the executable instructions of the processor; The processor is configured to execute the autonomous docking method for unmanned underwater vehicles based on binocular vision pose estimation as described in any one of claims 1 to 6 by executing the executable instructions.

Images