Autonomous berthing navigation system and method for unmanned surface vehicle based on infrared vision tight coupling

Abstract

The application discloses an unmanned ship autonomous berthing navigation system and method based on infrared vision tight coupling. The system comprises an infrared vision sensing module, a vision processing module and a multi-sensor fusion pose estimation module. The application installs an active infrared light source array at a berth and uses an improved deep learning target detection network at the end of the unmanned ship to detect the light source array. Secondly, through the constructed tight coupling integrated navigation system based on factor graph optimization, the information fusion of heterogeneous and asynchronous multi-sensors is realized in a sliding window. The method can detect the infrared light source array, ensure the realization of robust visual feature extraction under complex light changes, fuse the information of heterogeneous and asynchronous multi-sensors, realize the distance and attitude estimation of the unmanned ship relative to the light source array of the berth, overcome the vulnerability of a single sensor, provide accurate self-state and relative berth attitude information for the autonomous berthing of the unmanned ship, and improve the long-term navigation consistency.

Description

Technical Field

[0001] This invention relates to a berthing navigation system, specifically to an autonomous berthing navigation system and method for unmanned surface vessels based on infrared vision tight coupling. Background Technology

[0002] In recent years, unmanned surface vessels (USVs) have garnered increasing attention due to their ability to autonomously perform various complex maritime missions, such as maritime rescue, maritime counter-terrorism, and resource exploration. Their deployment has brought significant benefits, improving operational efficiency and reducing operational risks for personnel in high-risk environments. While substantial progress has been made in autonomous navigation technologies in open waters (such as formation cruising, target encirclement, and cooperative control), autonomous berthing of USVs still faces significant challenges. Developing a reliable autonomous berthing system will reduce human intervention during berthing, further improve the operational efficiency of USVs, and open up new application scenarios such as the transfer of equipment from unmanned offshore platforms. To achieve autonomous berthing of USVs, a complete berthing perception system is first needed to provide accurate and stable self-pose (position and attitude) and relative pose estimation results between the USV and the berth for berthing navigation.

[0003] However, existing research in the field of autonomous berthing of unmanned surface vessels (USVs) mainly focuses on berthing path planning and autonomous control. These approaches do not address pose perception or specific navigation requirements, and therefore cannot be directly applied to such tasks. For the task of pose perception during USV berthing, current solutions are not particularly mature. The sensors used are susceptible to environmental interference and lighting conditions, failing to meet all-weather application requirements. Furthermore, considering that USVs often carry heterogeneous and asynchronous sensors for self-state monitoring, integrating heterogeneous measurement data into a unified framework to improve state estimation accuracy is also a challenge. This includes how to handle asynchronous data and avoid sensor failures affecting estimation accuracy. Although this problem is of great significance, there is little research addressing it. Summary of the Invention

[0004] To address the problems existing in the background art, this invention provides an autonomous docking navigation system and method for unmanned surface vessels (USVs) based on tightly coupled infrared and visual systems. The method of this invention solves the problems of relative pose estimation and multi-sensor fusion navigation in the autonomous docking of USVs.

[0005] The technical solution adopted in this invention is:

[0006] I. An autonomous berthing and navigation system for unmanned surface vessels based on infrared vision tight coupling, comprising:

[0007] An infrared vision sensing module is installed in the berth area and on the unmanned surface vessel (USV) to acquire infrared images of the berth area when the USV autonomously berths to the berth area.

[0008] The vision processing module, installed in the computer device, includes an improved target detection network and a berth location algorithm. It is used to process infrared images of the berth area and locate the unmanned surface vessel's berthing position in the berth area to obtain an infrared positioning image.

[0009] The multi-sensor fusion pose estimation module, installed in the computer device, is a factor graph pose estimation optimization model that includes infrared visual measurement geometric constraint factor nodes. It is used to determine the state variables of the unmanned surface vessel and the berthing position based on the infrared positioning image, so as to control the unmanned surface vessel to navigate to the berthing position.

[0010] The infrared visual perception module includes an active infrared light source array and a shipborne infrared camera. The active infrared light source array includes four infrared LED light sources arranged in an array. The active infrared light source array is installed on the water surface at the berthing position in the berthing area. The shipborne infrared camera is installed at the front of the unmanned surface vessel and captures infrared images of the active infrared light source array while the unmanned surface vessel is navigating.

[0011] The improved object detection network of the visual processing module is specifically an improved YOLOv10-N deep learning object detection network obtained by replacing the convolutional layers, the convolutional neural network C2f (Cross Stage Partial Networks with FeatureFusion) layer, and the lightweight downsampling SCDown (Selective Channel Down-sampling) layer in the YOLOv10-N deep learning object detection network with the feature extraction UFSC (Unflatten and Shortcut Layer) layer.

[0012] The feature extraction UFSC layer consists of three two-dimensional convolutional layers and a flattening layer. The input to the feature extraction UFSC layer is first processed in the first two-dimensional convolutional layer and the flattening layer, and then fed into the second and third two-dimensional convolutional layers. The processed outputs are summed and used as the output of the feature extraction UFSC layer.

[0013] The berth location algorithm of the aforementioned visual processing module includes sequential adaptive binarization, light source center extraction, and potential outlier removal operations. An improved target detection network outputs a location detection infrared image with the target bounding box of an active infrared light source array. This image is then processed through adaptive binarization to obtain an infrared location image. Next, light source center extraction is performed, followed by Canny edge detection and contour moment calculation to determine the center pixel coordinates of the four infrared LEDs in the active infrared light source array. Finally, potential outlier removal is performed. If the center pixel coordinates of the four infrared LEDs in the infrared location image do not meet the outlier condition, the current infrared location image is discarded, and the next obtained infrared location image is processed. If the outlier condition is met, the infrared location image is further processed through a multi-sensor fusion pose estimation module.

[0014] The outlier conditions are as follows:

[0015] |cθ|≥1-ε(d)

[0016] ε(d)=ε min -k(dd min )

[0017] Where || represents the absolute value, cθ represents the cosine value between the two vertical line segments formed by the x and y coordinates of the centers of the four infrared LED light sources; ε(d) represents the outlier adaptive threshold, and d represents the distance between the unmanned surface vessel and the active infrared light source array; ε min d represents the minimum value of the adaptive threshold for outliers; k represents the linear variation coefficient of the adaptive threshold; d min This represents the theoretical minimum distance between the unmanned surface vessel and the active infrared light source array.

[0018] The specific optimization model for factor graph pose estimation is as follows:

[0019]

[0020] X = [p, v, θ] T

[0021] X b =[p b ,θ b ] T

[0022] Among them, X and These are the attitude and velocity state variable nodes of the unmanned surface vessel and their optimal estimates, X. b and These are the berth pose state variable nodes and their optimal estimates; N is the length of the sliding window; f i IMU(), f i Biase (), f i GPS (), f i AHRS () and f light () represent the pre-integration factor node, inertial measurement unit bias factor node, global positioning system measurement factor node, attitude reference system measurement factor node, and infrared vision measurement geometric constraint factor node of the i-th inertial measurement unit, respectively; X i and X i-1 These are the pose and velocity state variable nodes for the i-th and (i-1)-th unmanned surface vessels, respectively. Let be the node for the bias error variable of the i-th inertial measurement unit; p is the pose state variable node for the i-th berth; lj Let be the coordinates of the j-th infrared LED light source in the active infrared light source array coordinate system; let p be the position of the unmanned surface vessel in the world three-dimensional coordinate system, p = [x, y, z]. T Let x, y, z be the three-dimensional coordinates of the unmanned surface vessel's position, and v be the velocity of the unmanned surface vessel in the world three-dimensional coordinate system, v = [u, v, w]. T Let u, v, and w represent the sway, roll, and heave velocities of the unmanned surface vessel (USV), respectively, and let θ represent the USV's attitude in the world's three-dimensional coordinate system, where θ = [ψ, θ, φ]. T ψ, θ, φ represent the unmanned surface vessel's roll, pitch, and yaw attitudes, respectively; p b p represents the position of the berth in the world's three-dimensional coordinate system. b =[x b ,y b ,z b ] T θ b θ represents the attitude of the berth in the world's three-dimensional coordinate system. b =[ψ b ,θ b ,φ b ] T .

[0023] The state variables of the unmanned surface vessel (USV) and the berthing position include the USV's pose and velocity state variable node X and the berthing position's pose state variable node X. b .

[0024] The optimal state estimate within the sliding window is obtained by minimizing the objective function. It is assumed that the state estimate outside the sliding window has already reached the optimal state estimate. The variable nodes within the sliding window are optimized by utilizing the factor nodes within the sliding window.

[0025] The specific nodes of the infrared vision measurement geometric constraint factor are as follows:

[0026]

[0027] Among them, Z cj Let K be the z-axis coordinate of the j-th infrared LED light source in the camera coordinate system; K and These are the standard and augmented camera intrinsic parameter matrices for submarine-mounted infrared cameras, respectively. and These are the transformation matrices from the unmanned surface vessel coordinate system to the world coordinate system, from the camera coordinate system to the unmanned surface vessel coordinate system, and from the light source array coordinate system to the camera coordinate system, respectively. and The measurements of the center of the infrared LED light source and its augmented form are shown respectively. H is the covariance matrix of the j-th infrared LED light source corresponding to the i-th infrared vision measurement geometric constraint factor node; Pixel () represents the measurement process of the geometric constraint factor in infrared vision measurement; The measurement noise of the j-th infrared LED light source corresponding to the i-th infrared vision measurement geometric constraint factor node.

[0028] II. A navigation method for an unmanned surface vessel autonomous berthing navigation system based on infrared vision tight coupling, comprising:

[0029] Step 1) Establish an infrared visual perception module and install it in the berth area and on the unmanned surface vessel. Acquire several infrared images of the active infrared light source array in the berth area and add target box labels to construct a training set.

[0030] Step 2) Establish a visual processing module and install it in a computer device. Input the training set into the improved object detection network for training to obtain the trained improved object detection network.

[0031] Step 3) Establish a multi-sensor fusion pose estimation module.

[0032] Step 4) When the unmanned surface vessel (USV) autonomously berths to the berth area, it acquires an infrared image of the berth area to be berthed and inputs it into the vision processing module to obtain an infrared positioning image. Based on the infrared positioning image, it obtains infrared visual measurement geometric constraint factor nodes and inputs the infrared visual measurement geometric constraint factor nodes into the multi-sensor fusion pose estimation module. After processing, it outputs the state variables of the USV and the berth position to obtain the relative pose. Finally, it controls the USV to navigate until it reaches the position of the active infrared light source array in the berth area.

[0033] The electronic device of the present invention includes: a memory and a processor coupled to each other, wherein the memory stores program data, and the processor invokes the program data to execute the method described above.

[0034] The present invention provides a computer-readable storage medium having program data stored thereon, which, when executed by a processor, implements the method described above.

[0035] This invention addresses the pose perception problem during autonomous berthing of unmanned surface vessels (USVs). It designs an infrared sensing scheme to achieve stable all-weather perception and utilizes an improved YOLOv10-N deep learning target detection network to perform target detection and light source center extraction adaptive image processing on infrared light source array images captured by infrared cameras. This enables robust all-weather light source array detection and light source center extraction under complex lighting conditions. Furthermore, this invention designs a dynamic outlier removal algorithm based on a light source parallelism tolerance function to filter invalid data in real time, ensuring perception reliability. In addition, this invention optimizes a multi-sensor tightly coupled fusion pose estimation framework based on factor graphs, achieving deep fusion of asynchronous and heterogeneous sensors. This avoids the vulnerability of single sensors, maintains long-term consistency of pose estimation results, and improves the accuracy and stability of pose estimation. Simultaneously, to ensure the real-time computation of multi-sensor tightly coupled fusion pose estimation, a sliding window is used to optimize the factor graph structure, retaining only variable factors and node factors within the sliding window. Ultimately, this achieves stable and high-precision relative pose estimation between the USV and the berth.

[0036] The beneficial effects of this invention are:

[0037] 1. This invention addresses the position and attitude perception problem during autonomous berthing of unmanned surface vessels by designing an infrared perception scheme, including an active infrared light source array at the berth and an onboard infrared camera, to achieve stable perception in all weather conditions.

[0038] 2. This invention addresses the problem of unstable feature extraction caused by day and night lighting interference and environmental background interference. It uses an improved YOLOv10-N deep learning light source array target detection network and a collaborative light source center extraction image processing algorithm to achieve all-weather robust light source array detection and light source center extraction under complex lighting conditions.

[0039] 3. This invention addresses the perception anomaly problem caused by water surface reflection or image processing anomalies by designing a dynamic outlier removal algorithm based on the light source parallelism tolerance function to achieve real-time filtering of invalid data and ensure perception reliability.

[0040] 4. This invention achieves deep fusion of asynchronous and heterogeneous sensors through a tightly coupled fusion framework based on factor graph optimization, avoiding the vulnerability of single sensors, maintaining the long-term consistency of pose estimation results, and improving the accuracy and stability of pose estimation.

[0041] 5. Considering the computational complexity of factor graph optimization and the real-time requirements of multi-sensor tightly coupled fusion pose estimation, this invention uses a sliding window to optimize the factor graph structure, retaining only the variable factors and node factors within the sliding window. This achieves real-time computation of the tightly coupled fusion framework based on factor graph optimization while ensuring the accuracy of pose estimation.

[0042] This invention involves installing an active infrared light source array at the berth and employing an improved deep learning target detection network at the unmanned surface vessel (USV) end for array detection. Secondly, a tightly coupled integrated navigation system based on factor graph optimization is constructed, achieving information fusion from heterogeneous and asynchronous multiple sensors within a sliding window. This method enables infrared light source array detection, ensuring robust visual feature extraction under complex lighting conditions. By fusing information from heterogeneous and asynchronous multiple sensors, it achieves distance and attitude estimation of the USV relative to the berth's light source array, overcoming the vulnerability of single sensors and providing accurate self-state and relative berth attitude information for autonomous berthing, thus improving long-term navigation consistency. Attached Figure Description

[0043] Figure 1 This is a diagram of the autonomous docking and navigation system for unmanned surface vessels based on infrared vision tight coupling of the present invention;

[0044] Figure 2 This is a diagram of the improved YOLOv10-N deep learning object detection network structure proposed in this invention;

[0045] Figure 3 This is a schematic diagram of the workflow of the vision processing module proposed in this invention, wherein, Figure 3 (a) is the original image during the day. Figure 3 (b) is the target detection result image during the day. Figure 3 (c) is the binarized result image during the day. Figure 3 (d) is the result of extracting the center of the light source during the day. Figure 3 (e) is the original image of the night. Figure 3 (f) is the target detection result image at night. Figure 3 (g) is the binarized result image for nighttime. Figure 3 (h) is the result of extracting the center of the light source at night;

[0046] Figure 4 This is a schematic diagram of the trajectory estimation results of the unmanned surface vessel in the simulation experiment of this embodiment of the invention;

[0047] Figure 5 This is a schematic diagram illustrating the relative position estimation error between the unmanned surface vessel and the berth in a simulation experiment of an embodiment of the present invention;

[0048] Figure 6This is a schematic diagram of the relative attitude estimation error between the unmanned surface vessel and the berth in the simulation experiment of an embodiment of the present invention. Detailed Implementation

[0049] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0050] like Figure 1 As shown, the unmanned surface vessel (USV) autonomous berthing navigation system based on tightly coupled infrared vision of the present invention includes an infrared vision perception module, a vision processing module, and a multi-sensor fusion pose estimation module. The infrared vision perception module is installed in the berth area and on the USV and is used to acquire infrared images of the berth area when the USV autonomously berths to the berth area. The vision processing module is installed in the computer device and includes an improved target detection network and a berth position localization algorithm. It is used to process the infrared images of the berth area and locate the USV's berthing position in the berth area to obtain an infrared positioning image. The multi-sensor fusion pose estimation module is installed in the computer device and is a factor graph pose estimation optimization model that includes infrared vision measurement geometric constraint factor nodes. It is used to determine the state variables of the USV and the berthing position based on the infrared positioning image to control the USV to navigate to the berthing position.

[0051] The infrared vision perception module includes an active infrared light source array and a shipborne infrared camera. The active infrared light source array consists of four infrared LED light sources arranged in an array. The active infrared light source array is installed on the water surface at the berthing position in the berthing area. Specifically, it is composed of four infrared LEDs with a center wavelength of 940±5nm, arranged in a rectangular layout of 60cm×45cm. The shipborne infrared camera is installed at the front of the unmanned surface vessel and captures infrared images of the active infrared light source array while the unmanned surface vessel is navigating. The wavelength of the shipborne infrared camera is 940±10nm.

[0052] The improved object detection network in the vision processing module is obtained by replacing the convolutional layers, the C2f (Cross Stage Partial Networks with Feature Fusion) layer, and the SCDown (Selective Channel Down-sampling) layer in the YOLOv10-N deep learning object detection network with the UFSC (Unflattened and Shortcut Layer) layer for feature extraction. The network structure diagram of the improved YOLOv10-N deep learning object detection network is shown below. Figure 2 As shown, the network input is first processed sequentially into a convolutional module, a two-dimensional max pooling layer, three UFSC layers, a c2f layer, an SPPF layer, and an upsampling layer. The output of the upsampling layer is concatenated with the output of the second UFSC layer and then processed sequentially into the next c2f layer and the upsampling layer. The output of the second upsampling layer is concatenated with the output of the first UFSC layer and then processed sequentially into the next c2f layer and the convolutional module. The output of the second convolutional module is concatenated with the output of the second c2f layer and then processed sequentially into the next c2f layer and the SCDown layer. The processed result is concatenated with the output of the SPPF layer and then processed sequentially into the C2fCIB layer and the P3 detector head for small target detection. The output of the third c2f layer is then processed into the P4 detector head for medium target detection, and the output of the second c2f layer is then processed into the P5 detector head for large target detection. The convolutional module includes a two-dimensional convolutional layer, a batch normalization layer, and a SiLU activation function connected sequentially.

[0053] The feature extraction UFSC layer consists of three 2D convolutional layers and a flattening layer. The input to the feature extraction UFSC layer is first processed in the first 2D convolutional layer and the flattening layer, then fed into the second and third 2D convolutional layers. The processed outputs are summed to obtain the output of the feature extraction UFSC layer. Furthermore, the 3×3 convolutional kernel in the first layer of the YOLOv10-N network needs to be replaced with a 7×7 convolutional kernel, with a stride of 2 and padding of 3.

[0054] The feature map from the previous layer of the UFSC layer is simultaneously input into two different branches. The left branch contains two convolutional layers. The first convolutional layer has a 3×3 kernel (c), a stride (s) of 2, and a padding value (p) of 1. After this layer, the output feature map's width and height are half the size of the input feature map, and the number of channels is the same as the input feature map. The second convolutional layer also has a 3×3 kernel (c), a stride (s) of 1, and a padding value (p) of 1. The right branch contains a unfolding operation and a convolutional layer. First, the input feature map is unfolded with a sliding window size (ws) of 2. After this operation, the output feature map's width and height are half the size of the input feature map, and the number of channels is four times that of the input feature map. The unfolded feature map then becomes the input to the convolutional layer. The third convolutional layer has a 1×1 kernel (c), a stride (s) of 1, and a padding value (p) of 0. The number of output channels is the same as the last convolutional layer in the left branch. The two feature maps output from the left and right branches are added together to obtain the final output of this layer.

[0055] The berth location localization algorithm in the vision processing module includes sequential adaptive binarization, light source center extraction, and potential outlier removal operations. An improved target detection network outputs a location-detected infrared image with the target bounding box of the active infrared light source array. This image is then processed through adaptive binarization to obtain the infrared location image. Next, light source center extraction is performed, followed by Canny edge detection and contour moment calculation to determine the center pixel coordinates of the four infrared LEDs in the active infrared light source array. Finally, potential outlier removal is performed. If the center pixel coordinates of the four infrared LEDs in the infrared location image do not meet the outlier criteria, the current infrared location image is discarded, and the next obtained infrared location image is processed. If the outlier criteria are met, the infrared location image is further processed through the multi-sensor fusion pose estimation module. The specific outlier criteria are as follows:

[0056] |cθ|≥1-ε(d)

[0057] ε(d)=ε min -k(dd min )

[0058] Where || represents the absolute value, cθ represents the cosine value between the two vertical line segments formed by the x and y coordinates of the centers of the four infrared LED light sources; ε(d) represents the outlier adaptive threshold, and d represents the distance between the unmanned surface vessel and the active infrared light source array; ε min d represents the minimum value of the adaptive threshold for outliers; k represents the linear variation coefficient of the adaptive threshold; d min This represents the theoretical minimum distance between the unmanned surface vessel and the active infrared light source array.

[0059] Adaptive binarization specifically employs the Maximum Between-Class Variance Method (OTSU), which automatically sets pixels outside the bounding box to 0 (black). Then, an adaptive threshold is selected within the bounding box, setting pixels exceeding the adaptive threshold to 255 (white) and pixels below the adaptive threshold to 0 (black).

[0060] The specific optimization model for factor graph pose estimation is as follows:

[0061]

[0062] X = [p, v, θ] T

[0063] X b =[p b ,θ b ] T

[0064] Among them, X and These are the attitude and velocity state variable nodes of the unmanned surface vessel and their optimal estimates, X. b and These are the berth pose state variable nodes and their optimal estimates; N is the length of the sliding window; f i IMU (), f i Bias (), f i GPS (), f i AHRS () and f light () represent the pre-integration factor node, inertial measurement unit bias factor node, global positioning system measurement factor node, attitude reference system measurement factor node, and infrared vision measurement geometric constraint factor node of the i-th inertial measurement unit, respectively; X i and X i-1 These are the pose and velocity state variable nodes for the i-th and (i-1)-th unmanned surface vessels, respectively. Let be the node for the bias error variable of the i-th inertial measurement unit; p is the pose state variable node for the i-th berth; lj Let be the coordinates of the j-th infrared LED light source in the active infrared light source array coordinate system; let p be the position of the unmanned surface vessel in the world three-dimensional coordinate system, p = [x, y, z]. T Let x, y, z be the three-dimensional coordinates of the unmanned surface vessel's position, and v be the velocity of the unmanned surface vessel in the world three-dimensional coordinate system, v = [u, v, w]. TLet u, v, and w represent the sway, roll, and heave velocities of the unmanned surface vessel (USV), respectively, and let θ represent the USV's attitude in the world's three-dimensional coordinate system, where θ = [ψ, θ, φ]. T ψ, θ, φ represent the unmanned surface vessel's roll, pitch, and yaw attitudes, respectively; p b p represents the position of the berth in the world's three-dimensional coordinate system. b =[x b ,y b ,z b ] T θ b θ represents the attitude of the berth in the world's three-dimensional coordinate system. b =[ψ b ,θ b ,φ b ] T .

[0065] The state variables of the unmanned surface vessel (USV) and the berthing position include the USV's pose and velocity state variable node X and the berthing position's pose state variable node X. b .

[0066] The optimal state estimate within the sliding window is obtained by minimizing the objective function. It is assumed that the state estimate outside the sliding window has already reached the optimal state estimate. The variable nodes within the sliding window are optimized by utilizing the factor nodes within the sliding window.

[0067] The specific nodes for the inertial measurement unit bias error variable are as follows:

[0068] X Bias =[b g ,b a ] T

[0069] Among them, b g and b a These represent the three-dimensional gyroscope deviation and the three-dimensional accelerometer deviation in the coordinate system of the inertial measurement unit itself.

[0070] The specific pre-integration factor nodes of the inertial measurement unit are as follows:

[0071]

[0072] Z gyro (t)=ω(t)+b g (t)+n g (t)

[0073]

[0074]

[0075] in, For the estimation error of the pre-integral factor node of the inertial measurement unit, || 2 The square of the absolute value. H is the covariance matrix of the pre-integral factor nodes of the inertial measurement unit. IMU () represents the measurement process of the inertial measurement unit. This represents the measurement data of the inertial measurement unit at all times from the (i-1)th node to the ith node. Z represents the measurement data of the inertial measurement unit at time t. acc (t) represents the accelerometer measurement data at the t-th node, Z gyro (t) represents the gyroscope measurement data at the t-th node; The result is the pre-integration calculation from the (i-1)th node to the ith node; a(t) and ω(t) are the true values ​​of the gyroscope and acceleration at the tth node, respectively; Let g be the rotation matrix from the world coordinate system to its own coordinate system; g is gravity; n is the rotation matrix from the world coordinate system to its own coordinate system. g (t) and n a (t) represents the noise measured by the gyroscope and accelerometer at time t, respectively; Δ t This is the update time of the inertial measurement unit.

[0076] The specific offset factor nodes for the inertial measurement unit are as follows:

[0077]

[0078] in, Let be the estimation error of the bias factor node of the i-th inertial measurement unit. Let H be the covariance matrix of the bias factor node of the i-th inertial measurement unit. Bias () represents the update process of the bias error of the inertial measurement unit, which can be defined as a random walk or an ordinary differential equation.

[0079] The specific measurement factor nodes of the Global Positioning System are as follows:

[0080]

[0081] H GPS (X i )=H GPS X i

[0082] in, Let be the estimation error of the i-th GPS measurement factor node. Let H be the covariance matrix of the factor node. GPS () represents measurement data from the Global Positioning System. For measurement data from the Global Positioning System, For the measurement noise of the Global Positioning System, H GPS H is the preset first matrix. GPS =[I 6×6 0 6×3 ], where I and 0 are the identity matrix and the 0 matrix, respectively.

[0083] The specific measurement factor nodes of the attitude and bearing reference system are as follows:

[0084]

[0085] H AHRS (X i )=H AHRS X i

[0086] in, Let be the estimation error of the measurement factor node of the i-th attitude reference system. Let H be the covariance matrix of the i-th attitude reference system measurement factor node. AHRS () represents the measurement process of the attitude reference system. For the measurement data of the attitude reference system, For the measurement noise of the attitude reference system, H AHRS For the preset second matrix, =[0 3×6 I 3 ×3 ].

[0087] The specific nodes of the geometric constraint factors for infrared vision measurement are as follows:

[0088]

[0089] in, Z represents the estimation error of the j-th infrared LED light source corresponding to the i-th infrared vision measurement geometric constraint factor node. cj Let K be the z-axis coordinate of the j-th infrared LED light source in the camera coordinate system; K and These are the standard and augmented camera intrinsic parameter matrices for submarine-mounted infrared cameras, respectively. and These are the transformation matrices from the UAV coordinate system to the world coordinate system, from the camera coordinate system to the UAV coordinate system, and from the light source array coordinate system to the camera coordinate system. The transformation matrices consist of the rotation matrix. Translation vector t u t c t lThe components are rotation matrices from the UAV coordinate system to the world coordinate system, from the camera coordinate system to the UAV coordinate system, and from the light source array coordinate system to the camera coordinate system; and translation vectors from the UAV coordinate system to the world coordinate system, from the camera coordinate system to the UAV coordinate system, and from the light source array coordinate system to the camera coordinate system. and The measurements of the center of the infrared LED light source and its augmented form are shown below. H is the covariance matrix of the j-th infrared LED light source corresponding to the i-th infrared vision measurement geometric constraint factor node; Pixel () represents the measurement process of the geometric constraint factor in infrared vision measurement; f represents the measurement noise of the j-th infrared LED light source corresponding to the i-th infrared vision measurement geometric constraint factor node; x and f y X is the focal length in the horizontal and vertical directions of the camera, u0 and v0 are the center focal point coordinates on the camera pixel plane, and X is the focal length in the horizontal and vertical directions of the camera. cj Y cj and Z cj These are the coordinates of the j-th light source in the camera coordinate system.

[0090] The autonomous docking and navigation method for unmanned surface vessels based on infrared vision tight coupling of the present invention is as follows:

[0091] Step 1) Establish an infrared visual perception module and install it in the berth area and on the unmanned surface vessel. Acquire several infrared images of the active infrared light source array in the berth area and add target box labels to construct a training set.

[0092] Step 2) Establish a visual processing module and install it in a computer device. Input the training set into the improved object detection network for training to obtain the trained improved object detection network.

[0093] Step 3) Establish a multi-sensor fusion pose estimation module;

[0094] Step 4) When the unmanned surface vessel (USV) autonomously berths to the berth area, it acquires an infrared image of the berth area to be berthed and inputs it into the vision processing module to obtain an infrared positioning image. Based on the infrared positioning image, it obtains infrared visual measurement geometric constraint factor nodes and inputs the infrared visual measurement geometric constraint factor nodes into the multi-sensor fusion pose estimation module. After processing, it outputs the state variables of the USV and the berth position to obtain the relative pose. Finally, it controls the USV to navigate until it reaches the position of the active infrared light source array in the berth area.

[0095] Finally, inference experiments were conducted on the visual processing module involved in this invention, and simulations were performed on the multi-sensor fusion pose estimation module to verify the effectiveness of the autonomous docking navigation system and method for unmanned surface vessels based on tight coupling of infrared and vision proposed in this invention.

[0096] During verification, the initial state of the unmanned surface vessel is 0, the initial speed is 0, and the initial berth state is:

[0097]

[0098] The coordinates of the four lights in the light source array coordinate system are [0 0.3 0.225]. T [0 -0.3 0.225] T [00.3 -0.225] T [0 -0.3 -0.225] T .

[0099] The camera intrinsic parameter matrix is:

[0100]

[0101] In the specific implementation process, the simulation time was set to 30 seconds. The unmanned surface vessel gradually approached the berth, and the pose estimation results were recorded at each sampling time. The target detection results of the infrared light source array of this invention are illustrated as follows. Figure 3 (b) and Figure 3 As shown in (f). The result of adaptive binarization is illustrated in Figure 1. Figure 3 (c) and Figure 3 As shown in (g). The result of extracting the center of the light source is illustrated in the diagram. Figure 3 (d) and Figure 3 As shown in (h).

[0102] like Figure 4 As shown in the figure, the trajectory estimation results under the action of the algorithm in the above embodiment are as follows. It can be seen from the figure that the algorithm has less jitter and error compared with the original noisy trajectory, and is closer to the true value data of the real trajectory. This proves the accuracy of the multi-sensor fusion pose estimation in the method of the present invention in the self-state estimation of unmanned surface vessels, and can provide reliable navigation information for the autonomous docking of unmanned surface vessels.

[0103] like Figure 5 and Figure 6As shown, the performance of the tightly coupled algorithm proposed in this invention for relative pose estimation is compared with that of the traditional n-point perspective algorithm, using the estimation error of each sampling step as the evaluation index. This comparison includes the distance estimation error in the xyz direction and the estimation errors of roll, pitch, and yaw angles. The results show that the proposed algorithm also exhibits good performance in relative pose estimation. This algorithm outperforms the traditional n-point perspective algorithm in relative pose estimation, resulting in a smaller root mean square error (RMSE) in pose estimation. This is because the n-point perspective algorithm is a one-step calculation and cannot consider prior information; therefore, its pose estimation accuracy decreases significantly over long distances, and as shown in the figure, there are obvious computational outliers.

[0104] Furthermore, this algorithm can infer the relative pose based solely on three coplanar points combined with prior information, which is impossible with traditional n-point perspective algorithms. This series of simulations indirectly demonstrates the effectiveness of the tightly coupled strategy. Simulation experiments on self-state estimation and relative pose estimation fully demonstrate the value of the multi-sensor fusion pose estimation module based on the factor graph optimization framework in this invention for autonomous docking and navigation of unmanned surface vessels.

[0105] The above content is merely a technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solution based on the technical concept proposed in this invention shall fall within the scope of protection of the claims of this invention.

Claims

1. An autonomous docking and navigation system for unmanned surface vessels based on infrared vision tight coupling, characterized in that, include: An infrared vision perception module is installed in the berth area and on the unmanned surface vessel (USV) and is used to acquire infrared images of the berth area when the USV autonomously berths to the berth area. The vision processing module, installed in the computer device, includes an improved target detection network and a berth location algorithm. It is used to process infrared images of the berth area and locate the unmanned surface vessel's berthing position in the berth area to obtain an infrared positioning image. The multi-sensor fusion pose estimation module is installed in the computer device and is a factor graph pose estimation optimization model that includes infrared visual measurement geometric constraint factor nodes. It is used to determine the state variables of the unmanned surface vessel and the berthing position based on the infrared positioning image, so as to control the unmanned surface vessel to navigate to the berthing position. The berth location algorithm of the aforementioned vision processing module includes sequential adaptive binarization, light source center extraction, and potential outlier removal operations. An improved target detection network outputs a location detection infrared image with the target bounding box of an active infrared light source array. This image is then processed through adaptive binarization to obtain an infrared location image. Next, light source center extraction is performed, followed by Canny edge detection and contour moment calculation to determine the center pixel coordinates of the four infrared LEDs in the active infrared light source array. Finally, potential outlier removal is performed. If the center pixel coordinates of the four infrared LEDs in the infrared location image do not meet the outlier condition, the current infrared location image is discarded, and the next obtained infrared location image is processed. If the outlier condition is met, the infrared location image is further processed through a multi-sensor fusion pose estimation module. The outlier conditions are as follows: in, Represents absolute value. The cosine value between the two vertical line segments formed by the x and y coordinates of the centers of the four infrared LED light sources; This represents the adaptive threshold for outliers. This indicates the distance between the unmanned surface vessel and the active infrared light source array; This represents the minimum value of the adaptive threshold for outliers; The linear variation coefficient of the adaptive threshold; This represents the minimum distance between the unmanned surface vessel and the active infrared light source array; The specific optimization model for factor graph pose estimation is as follows: in, and These are the attitude and velocity state variable nodes of the unmanned surface vessel and their optimal estimates. and These are the berth pose state variable nodes and their optimal estimated values, respectively; The length of the sliding window; , , , and The first The nodes include: inertial measurement unit pre-integration factor node, inertial measurement unit bias factor node, global positioning system measurement factor node, attitude reference system measurement factor node, and infrared vision measurement geometric constraint factor node. and The first The and the first The attitude and velocity state variable node of an unmanned surface vessel; For the first Each inertial measurement unit bias error variable node; For the first Each berth pose state variable node; For the first The coordinates of an infrared LED light source in the coordinate system of an active infrared light source array; This represents the position of the unmanned surface vessel in the world's three-dimensional coordinate system. The velocity of the unmanned surface vessel in the world's three-dimensional coordinate system. The attitude of the unmanned surface vessel in the world's three-dimensional coordinate system; This refers to the position of the berth in the world's three-dimensional coordinate system. The attitude of the berth in the world's three-dimensional coordinate system; The state variables of the unmanned surface vessel (USV) and its berthing position include the USV's pose and velocity state variable nodes. and berth pose state variable node .

2. The unmanned surface vessel autonomous berthing and navigation system based on infrared vision tight coupling according to claim 1, characterized in that: The infrared visual perception module includes an active infrared light source array and a shipborne infrared camera. The active infrared light source array includes four infrared LED light sources arranged in an array. The active infrared light source array is installed on the water surface at the berthing position in the berthing area. The shipborne infrared camera is installed at the front of the unmanned surface vessel and captures infrared images of the active infrared light source array while the unmanned surface vessel is navigating.

3. The unmanned surface vessel autonomous berthing and navigation system based on infrared vision tight coupling according to claim 1, characterized in that: The improved object detection network of the visual processing module is specifically an improved YOLOv10-N deep learning object detection network obtained by replacing the convolutional layers, convolutional neural network C2f layers, and lightweight downsampling SCDown layers in the YOLOv10-N deep learning object detection network with feature extraction UFSC layers. The feature extraction UFSC layer consists of three two-dimensional convolutional layers and a flattening layer. The input to the feature extraction UFSC layer is first processed in the first two-dimensional convolutional layer and the flattening layer, and then fed into the second and third two-dimensional convolutional layers. The processed outputs are summed and used as the output of the feature extraction UFSC layer.

4. The unmanned surface vessel autonomous berthing and navigation system based on infrared vision tight coupling according to claim 1, characterized in that: The specific nodes of the infrared vision measurement geometric constraint factor are as follows: in, Let be the z-axis coordinate of the j-th infrared LED light source in the camera coordinate system; and These are the standard and augmented camera intrinsic parameter matrices for submarine-mounted infrared cameras, respectively. , and These are the transformation matrices from the unmanned surface vessel coordinate system to the world coordinate system, from the camera coordinate system to the unmanned surface vessel coordinate system, and from the light source array coordinate system to the camera coordinate system, respectively. and The measurements of the center of the infrared LED light source and its augmented form are shown respectively. For the first The node corresponding to the geometric constraint factor of infrared vision measurement. The covariance matrix of an infrared LED light source; This refers to the measurement process of geometric constraint factors in infrared vision measurements. For the first The node corresponding to the geometric constraint factor of infrared vision measurement. Measurement noise of an infrared LED light source.

5. The navigation method of the unmanned surface vessel autonomous berthing navigation system based on infrared vision tight coupling according to any one of claims 1-4, characterized in that, include: Step 1) Establish an infrared vision perception module and install it in the berth area and on the unmanned surface vessel. Acquire several infrared images of the active infrared light source array in the berth area and add target box labels to construct a training set. Step 2) Establish a visual processing module and install it in a computer device. Input the training set into the improved object detection network for training to obtain the trained improved object detection network. Step 3) Establish a multi-sensor fusion pose estimation module; Step 4) When the unmanned surface vessel (USV) autonomously berths to the berth area, it acquires an infrared image of the berth area and inputs it into the vision processing module to obtain an infrared positioning image. Based on the infrared positioning image, it obtains infrared visual measurement geometric constraint factor nodes and inputs them into the multi-sensor fusion pose estimation module. After processing, it outputs the state variables of the USV and the berth position to obtain the relative pose. Finally, it controls the USV to navigate until it reaches the position of the active infrared light source array in the berth area.

6. An electronic device, characterized in that, include: A memory and a processor are coupled to each other, wherein the memory stores program data, and the processor invokes the program data to execute the method as described in claim 5.

7. A computer-readable storage medium storing program data thereon, characterized in that, When the program data is executed by the processor, the method described in claim 5 is implemented.

Images