A multi-stage AUV end recovery guidance method based on machine vision

Abstract

This invention discloses a multi-stage AUV end-of-life navigation and positioning method based on machine vision. The invention includes the following steps: calibrating underwater camera parameters: obtaining the intrinsic parameters and distortion coefficients of the underwater camera; active light source navigation: calculating the AUV pose using light source information; finding the entry point: switching the AUV from active light source navigation to passive pattern navigation; passive pattern navigation: calculating the pose using a chessboard when the distance is still far, and calculating the pose using the fusion of multiple QR code information when the chessboard cannot be fully observed. This invention, tailored to the characteristics of a six-degree-of-freedom all-wheel-drive AUV, designs a landing docking method that conforms to its kinematic characteristics, employing a combined navigation method of mid-to-long-range detection of active light sources and short-range detection of passive pattern markers. This method integrates the advantages of light source navigation and pattern marker navigation, providing highly reliable positioning for the AUV throughout the end-of-life navigation phase.

Description

Technical Field

[0001] This invention relates to a multi-stage AUV end-of-life navigation and positioning method based on machine vision, belonging to the field of AUV autonomous recovery and guidance technology. Background Technology

[0002] Currently, most AUV end-of-life retrieval guidance methods employ single-light source positioning, which can only determine the orientation of two degrees of freedom relative to the base station and requires strong coupling with the control system. Alternatively, multi-light source positioning can be used to achieve six degrees of freedom positioning, but this method is prone to problems such as incomplete observation of the light source and excessive halo when the end-of-life guidance approaches the base station, resulting in large deviations in the extracted light source feature pixels, leading to positioning failures and inaccuracies, and consequently affecting the overall docking accuracy of the AUV. Most current AUVs are underactuated, using a horn-mouth mechanical structure to enter the dock via collision, providing the necessary redundancy for docking. Six-degree-of-freedom fully driven AUVs can perform translation and rotation in more dimensions, and the horn-mouth collision-dock end-of-life guidance method cannot effectively utilize the kinematic characteristics of six-degree-of-freedom fully driven AUVs.

[0003] Therefore, researching a multi-stage AUV terminal navigation and positioning method based on machine vision, tailored to the characteristics of six-degree-of-freedom all-wheel-drive AUVs, and subdividing AUV terminal navigation into different stages, employing different positioning strategies to ensure positioning accuracy and stability throughout the terminal navigation process, is of significant research importance and practical value for long-term and stable ocean observation of AUVs. Summary of the Invention

[0004] The purpose of this invention is to provide a machine vision-based landing-type multi-stage AUV end-of-life recovery guidance method for the characteristics of six-degree-of-freedom all-drive AUVs.

[0005] This invention includes the following steps:

[0006] The first stage is acoustic navigation: the AUV is guided to the top of the base station using acoustic navigation; and a pentagram-shaped fixed-point and fixed-depth navigation mission is performed within the designated area to find the active light source of the base station. After finding the active light source, the second stage begins.

[0007] The second stage is active light source navigation: For images containing active light sources, image processing operations are performed to extract feature pixels, and the image feature pixels are matched with the positions of active light sources in the world coordinate system; the pose is then solved.

[0008] The third stage is passive pattern navigation: the chessboard detection algorithm is called to detect the chessboard on the base station. If the chessboard exists, the corner points of the chessboard grid are extracted to calculate the pose; if the chessboard is not detected, the QR code information in the image is extracted, and the pose is output by fusing multiple QR code information.

[0009] Furthermore, the base station is equipped with active light source markers and passive pattern markers for positioning;

[0010] The active light source comprises four in total, used for guidance in the second stage;

[0011] The passive pattern marker is in the form of a chessboard nested QR code, used for guidance in the third stage;

[0012] The four active light sources are located at the four corners of the passive pattern mark.

[0013] To elaborate further, the second phase specifically includes:

[0014] Images of the light source mounted on the base station are captured by a camera carried by an AUV;

[0015] The pixel coordinates of the four active light sources are obtained by using a binary-based heuristic thresholding algorithm and by calculating the first moment of the image.

[0016] The convex hull sequence of the image feature pixel sequence is calculated using the convex hull algorithm. The sequence is then matched with known world coordinate system coordinates, with the coordinates of one of the active light sources as the starting point of the sequence.

[0017] Determine the pose of the camera relative to the base station.

[0018] Furthermore, the period between the second and third phases also includes:

[0019] If the AUV turns on its searchlight but cannot obtain the chessboard and QR code features, it turns off the searchlight and proceeds to the second stage. It then uses the active light source to navigate and maintain a fixed depth before repeating this step.

[0020] Furthermore, it also includes camera calibration, specifically: fixing the camera parameters, using the camera at different positions underwater to take pictures of the calibration board to obtain chessboard images, using Zhang Zhengyou calibration method to calibrate the camera's intrinsic distortion coefficients, and using reprojection error to determine whether the camera calibration results are correct.

[0021] Furthermore, the AP3P algorithm is used in the process of solving the pose of the camera relative to the base station, and the pose is selected by using the reprojection error. The pose solution is optimized by using the Newton-Gauss method to utilize the reprojection error.

[0022] Furthermore, the specific steps of fusing and processing multiple QR code information to output the pose are as follows:

[0023] Calculate the normalized standard point;

[0024] By performing image processing and pose calculation on each QR code in the image, the corresponding QR code number and the six-degree-of-freedom pose of each QR code relative to the camera are obtained.

[0025] The position of the camera relative to the QR code is obtained through matrix transformation;

[0026] Normalize the coordinates of each QR code to a normalized standard point according to its QR code number; thus obtaining the normalized sequence.

[0027] The normalized sequence is fused using the K-Means algorithm or the mean algorithm to output the pose.

[0028] Compared with the prior art, the present invention has the following advantages:

[0029] 1. This invention is designed for the characteristics of a six-degree-of-freedom all-drive AUV, and adopts a landing-type multi-stage docking method that conforms to its kinematic characteristics. It uses a combined navigation method of detecting active light sources at medium and long distances and detecting passive pattern marks at close distances. It combines the advantages of light source navigation and pattern mark navigation, and can provide highly reliable positioning for the AUV throughout the entire process during the terminal navigation phase.

[0030] 2. This invention employs passive pattern marking at the near end, solving the problem of large pose calculation errors caused by inaccurate pixel extraction at close range during light source navigation. The passive pattern marking is designed as a chessboard nested within AprilTags, breaking down a large marker (chessboard) into many smaller markers (QR codes), thus resolving the issue of incomplete pattern marking observation at close range.

[0031] 3. This invention utilizes a binary-based heuristic thresholding algorithm to better segment the light source foreground.

[0032] 4. This invention uses blue light as preset information and determines the unique matching relationship between feature pixels and feature points in the world coordinate system through convex hull calculation, thus simplifying the matching process.

[0033] 5. This invention utilizes the redundant information of multiple AprilTags observed at close range and uses the K-means algorithm or mean algorithm for data fusion processing, thereby enhancing the data reliability and robustness under close range conditions. Attached Figure Description

[0034] Figure 1 This invention relates to an underwater base station suitable for six-degree-of-freedom all-drive AUV descent docking, equipped with active light source markers and passive pattern markers. 1 and 2 represent four active light sources, with 1 being a blue light and 2 being three green lights; 3 is a passive pattern marker.

[0035] Figure 2 This is a passive pattern design in the present invention. The example uses a chessboard nested AprilTag.

[0036] Figure 3This is a schematic diagram of the process of a multi-stage AUV end-of-life retrieval guidance method based on machine vision according to the present invention.

[0037] Figure 4 This is a flowchart of a multi-stage AUV end-of-life retrieval guidance method based on machine vision according to the present invention.

[0038] Figure 5 This is a flowchart illustrating the process of calculating camera pose using a binary-based heuristic thresholding algorithm, a convex hull-based feature matching algorithm, and a PNP algorithm for active light source navigation in this invention.

[0039] Figure 6 This is a flowchart of the passive pattern navigation process for calculating the pose of a passive light source marker. Detailed Implementation

[0040] The embodiments of the present invention are described in detail below. These embodiments are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0041] This invention is primarily used to provide a reliable and accurate relative position for AUVs during autonomous underwater recovery, when approaching a base station. This process includes the following steps: calibrating underwater camera parameters: obtaining the intrinsic parameters and distortion coefficients of the underwater camera; active light source navigation: calculating the AUV pose using light source information; finding the entry point: switching the AUV from active light source navigation to passive pattern navigation; passive pattern navigation: calculating the pose using a chessboard when the distance is still far, and calculating the pose using the fusion of multiple AprilTag information when the chessboard cannot be fully observed.

[0042] like Figure 1 As shown, the base station's mechanical structure is designed using a six-degree-of-freedom all-drive AUV descent docking method. It is equipped with four active light sources and a passive pattern marker with a checkerboard-like nested QR code, which can be used for both active light source navigation and passive pattern marker navigation. In the diagram, 1 represents a blue light, 2 a green light, and they are numbered 0-3 counter-clockwise starting from the blue light. The blue-green light source is chosen to increase the visibility range of the underwater light source. 3 represents the passive pattern marker in the form of a checkerboard-like nested QR code. A world coordinate system is established with the normalized standard point in the multi-QR code navigation as the origin. The coordinates of the four light sources on the base station in the world coordinate system are known quantities, and the pose obtained by the algorithm is the pose of the AUV in the world coordinate system.

[0043] like Figure 2 As shown, the passive pattern marker is designed as a chessboard nested QR code, which retains both chessboard and QR code features. When viewed from a distance, it can be treated as a chessboard feature, and when viewed up close, it can be treated as multiple QR code features.

[0044] The specific method involves adding a corresponding numbered QR code pattern to the white squares of a chessboard with alternating black and white squares. The spacing between the QR code pattern and the edge of the white square is the same. The QR code can be, but is not limited to, AprilTag, Aruco, or other location-based QR codes. For a passive pattern marker with n*m black and white squares, the chessboard can be represented as {b0, b1, b2, ..., b...} according to the square order. n*m-1 The sequence, whose index sequence record ChessboardIndex is defined as:

[0045] ChessboardIndex={0,1,2,...,(n*m)-1}

[0046] The chessboard grid sequence with odd indices {b1, b3, b5, ..., b} n*m-1 The white squares contain QR code information; each QR code has a unique number, which is uniquely linked to the QR code pattern. There are a total of (n*m) / 2 QR code markers on the pattern markers. The QR code number sequence QRCodeNO can be represented as:

[0047] QRCodeNO = {1, 2, ..., (n*m) / 2}

[0048] like Figure 3 As shown, this invention divides the overall docking process into three stages in a fine-grained manner. The specific steps of the overall docking process are as follows:

[0049] a) Pentagram traversal to find the light source: Using acoustic navigation methods such as USBL, the AUV is navigated to a circular plane area (radius 10M) 10M above the base station, and a pentagram fixed-point and fixed-depth navigation mission is performed within this circular area to find the light source. By performing this mission, the base station light source is found in the fastest time, and the active light source navigation phase begins.

[0050] b) Active Light Source Navigation to Find the Switching Point: In the active light source navigation phase, image processing operations are performed on the image containing the light source to extract feature pixels. These feature pixels are then matched with the light source positions in the world coordinate system, and the pose is solved using the AP3P algorithm. The AUV uses active light source navigation to find the switching point between active and passive navigation.

[0051] c) Passive pattern navigation: In the passive pattern navigation stage, the chessboard detection algorithm is first called to detect the chessboard. If the chessboard exists, the corner points of the chessboard are extracted to calculate the pose. If the chessboard is not detected, the QR code information in the image is extracted and the pose is output by fusion processing based on multiple AprilTag information.

[0052] like Figure 4As shown, end-of-pipe retrieval guidance can be divided into four key operations: active light source navigation, finding the entry point, chessboard navigation, and multi-QR code navigation. Among them, chessboard navigation and multi-QR code navigation are collectively referred to as passive pattern navigation.

[0053] The switching point between active light source navigation and passive pattern navigation is called the entry point. The switching process is called finding the entry point. The strategy for finding the entry point is as follows: Since active light source navigation can accurately calculate the Z-axis distance, determining the active / passive navigation switching point based on the distance between the AUV and the base station on the Z-axis can further improve positioning accuracy. In this embodiment, the active light source navigation is first used to navigate to the point (0,0,6) in the world coordinate system, and the AUV light source is turned on. If the chessboard feature and QR code feature cannot be obtained at this time, the above operation is rolled back, and the light source navigation is used to descend 1M to (0,0,5) and the above operation is repeated. The number of retries is 2 (up to 4M). If the chessboard feature or QR code feature can be obtained, the passive pattern stage is entered.

[0054] In addition, before performing the above operations, the underwater camera parameters need to be calibrated offline using the Zhang Zhengyou calibration method. Since deep-sea cameras are mostly wide-angle to expand the field of view, and the introduction of pressure-sealing measures will introduce large lens installation errors, the distortion model of the camera is represented by the third-order radial distortion coefficient and the second-order tangential distortion coefficient. The average reprojection error is considered to be below 0.3px as a good result for the camera intrinsic parameters and distortion coefficients.

[0055] D = [k1,k2,k3,t1,t2]

[0056] Step 1: Active Light Source Navigation

[0057] The algorithm flowchart for active light source navigation is as follows: Figure 5 As shown. Active light source navigation obtains the pixel coordinates of the four light sources through image processing algorithms, performs feature matching using known blue light information, and uses a pose estimation algorithm to solve the camera pose relative to the base station.

[0058] Step 1.1: Image Processing Algorithm

[0059] The image processing algorithm includes steps such as grayscale processing, thresholding, image noise reduction, edge detection, morphological filtering, and extraction of light source feature pixels. The input to the image processing algorithm is the camera image acquired by the AUV camera, and the output is the pixel coordinates of the four light sources. The thresholding algorithm adopts a binary heuristic thresholding algorithm, and the extraction of light source feature pixels is achieved by calculating the first moment of the image to obtain the pixel coordinates of the four light sources.

[0060] The process of the binary search-based heuristic thresholding algorithm is as follows:

[0061] The upper and lower bound threshold intervals of the algorithm are denoted as t. land t h The number of active light sources is denoted as n. For a given upper and lower bound threshold interval, a threshold t is determined for threshold processing. m for:

[0062] t m =floor((t) l +t h ) / 2)

[0063] Where floor is the floor function.

[0064] Threshold t used for threshold processing m Perform a thresholding operation on the grayscale image src to obtain the thresholded image dst:

[0065]

[0066] For the threshold image, edge detection and extraction of light source feature pixels are performed to obtain the number of features m. This number m is then compared with the number of active light sources n to determine the new upper and lower bound threshold intervals.

[0067]

[0068] By repeating the above algorithm, the optimal threshold for image background removal can be obtained.

[0069] Image denoising is performed using a Gaussian filter. After necessary edge extraction and morphological filtering, adjacent pixels are grouped into blob features. Blob feature pixels are extracted at the sub-pixel level by calculating the first moment of the image.

[0070]

[0071] Pixel coordinates corresponding to active light source features for:

[0072]

[0073]

[0074] Step 1.2: Feature Matching Algorithm

[0075] The feature matching algorithm uses known blue light information to match the pixel coordinates of the four light sources obtained by the image processing algorithm with the known world coordinate system coordinates.

[0076] The feature matching algorithm process is as follows:

[0077] After obtaining the feature pixels of the image, the convex hull algorithm is used to calculate the convex hull sequence I of the image feature pixel sequence:

[0078] I = [i0, i1, i2, i3]

[0079] Where it is assumed that i k Given a known blue light; perform a cyclic shift operation on the original convex hull sequence I, placing the blue light at the beginning, to obtain a new rearranged convex hull sequence N:

[0080] N = [i k i (k+1)％n i (k+2)％n i (k+3)％n ]

[0081] Since the four lights are on the same horizontal plane during base station feature design, the three-dimensional features in the world coordinate system can be simplified to two-dimensional features. The convex hull algorithm described above is then repeated in the corresponding area to obtain the feature convex hull sequence W in the world coordinate system.

[0082] W = [w0, w1, w2, w3]

[0083] Where w0 is the blue light coordinate in the world coordinate system. Since the AUV docking strategy is a descent-type docking, there is no situation where the AUV observes from the bottom. Therefore, the image convex hull sequence N can be matched with the known feature convex hull sequence W to form a one-to-one correspondence.

[0084] Step 1.3: Pose Estimation Algorithm

[0085] The pose estimation algorithm uses the matching relationship of four known point pairs to perform AP3P operation to obtain four possible poses for the P3P problem. The pose selection is based on reprojection error. The pose solution is optimized using the Newton-Gaussian method with the reprojection error. Then, SVD is used to obtain the output pose matrix R and t (R represents the rotation matrix, t represents the translation matrix), which represents the pose of the target object in the camera coordinate system. The camera position t in the world coordinate system is obtained through matrix transformation. camera :

[0086] t camera =-R -1 t

[0087] Step 2: Passive Pattern Navigation

[0088] The algorithm flowchart for passive pattern navigation is as follows: Figure 6 As shown, since the passive pattern marker is a specially designed chessboard, it retains both chessboard and QR code features. It can be treated as a chessboard from a distance and as multiple QR code features up close. Therefore, based on whether the complete chessboard can be detected, the process is divided into two stages: chessboard navigation and multi-QR code navigation.

[0089] Step 2.1: Chessboard Navigation

[0090] Chessboard navigation primarily utilizes chessboard information from passive patterns for navigation. Since the chessboard features are the corner points where black and white blocks intersect, the addition of QR codes does not affect these features. A standard chessboard extraction algorithm can be used to process the passive pattern markings to accurately and systematically locate sub-pixel-level feature corner points. After obtaining the chessboard corner point feature sequence, it is directly matched one-to-one with the feature sequence in the known world coordinate system. The iterative PNP algorithm is used to solve for the pose R and t, and the pose of the AUV relative to the base station is obtained through coordinate system transformation, which is then passed to the control system for asymptotic control. The pixel feature coordinates and world coordinate system feature coordinates obtained in this step are more accurate than those obtained through active light source navigation, and there are more point-to-point matching relationships, thus providing more precise positioning for the AUV.

[0091] Step 2.2: Multi-QR code navigation

[0092] When the distance is close, the chessboard navigation fails because it cannot be detected. Therefore, a multi-QR code navigation stage is introduced when the distance is close. The multi-QR code navigation stage detects all QR code information contained in the image and fuses and calculates their pose.

[0093] For a passive pattern marker with n*m black and white squares (n rows, m columns), the normalized standard point for different QR codes is the center point of the chessboard {row}. mid ,col mid The calculation process is as follows:

[0094] row mid =floor(n / 2)

[0095] col mid =floor(m / 2)

[0096] By performing image processing and pose calculation on each QR code in the image, we can obtain the corresponding QR code number i and the six-degree-of-freedom pose R of each QR code relative to the camera. i and t i (where R) i Let t represent the rotation matrix. i (Represents the translation matrix).

[0097] The position t′ of the camera relative to each QR code can be determined through matrix transformation. i :

[0098] t′ i =-R i -1 t i

[0099] The above poses represent the camera's six degrees of freedom pose relative to each QR code, and they need to be normalized. The position {row,col} of each QR code on the chessboard can be quickly determined using its QR code number i.

[0100] row=(i*2-1) / m

[0101] col=(i*2-1)%m

[0102] Where row represents the row number (row∈[0,n) and col represents the column number (col∈[0,m)).

[0103] Assuming the chessboard grid width is h, the offset of each QR code's corresponding position {row,col} on the chessboard from the normalized standard point is... It can be represented as:

[0104]

[0105] in,

[0106] Each QR code at its position t′ i Based on this, the corresponding offset is added to obtain the normalized coordinates t″ of the camera relative to the normalized standard point. i :

[0107]

[0108] For multiple observable QR codes {tag i ,tag i′ ,tag i″ ...}, therefore we have the normalized coordinate sequence T:

[0109] T={t″ i ,t″ i ,t″ i″ ...,}

[0110] When the length of the above sequence is less than 6, the K-means algorithm is not used for calculation; instead, the mean algorithm is used to obtain the output pose t. out :

[0111]

[0112] In other cases, the K-means algorithm is used to fuse multiple pose data. If n QR code coordinates are observed (n>5), the normalized coordinate sequence T can be expressed as:

[0113] T = {x1, x2, ..., x} n}

[0114] Where xi ∈R 3 Let i = 1, 2, 3, ..., n. Divide it into K groups (k <n / 2){S1,S2,...,S k The K-means algorithm optimizes by minimizing the number of elements x within the group. i With group heart μ c Euclidean distance:

[0115]

[0116] (1) Randomly initialize k group centers

[0117]

[0118] (2) Calculate the Euclidean distance from each element to each group.

[0119]

[0120] (3) Update the group heart:

[0121]

[0122] (4) Repeat steps (2)-(3) above until the group center no longer changes, then:

[0123]

[0124] The coordinates of the group center of the group containing the most elements are selected as the output for the control system:

[0125]

[0126] The embodiments of the present invention have been described in detail above with reference to the accompanying drawings. However, the present invention is not limited to the embodiments described above. Various changes can be made within the scope of knowledge possessed by those skilled in the art without departing from the spirit of the present invention.

Claims

1. A multi-stage AUV end recovery guidance method based on machine vision, characterized in that This method is a landing-type multi-stage AUV end-of-life recovery guidance method applicable to six-degree-of-freedom all-wheel drive AUVs. It provides high-precision positioning throughout the entire end-of-life navigation phase and includes the following steps: The first stage is acoustic navigation: the AUV is guided to the top of the base station using acoustic navigation; and a pentagram-shaped fixed-point and fixed-depth navigation mission is performed within the designated area to find the active light source of the base station. After finding the active light source, the second stage begins. The second stage is active light source navigation: For images containing active light sources, image processing operations are performed to extract feature pixels, and the image feature pixels are matched with the positions of active light sources in the world coordinate system; the pose is then solved. The third stage, passive pattern navigation: A chessboard detection algorithm is invoked to detect the chessboard on the base station. If the chessboard exists, the corner points of the chessboard grid are extracted to calculate the pose; if no chessboard is detected, QR code information is extracted from the image, and the pose is output by fusing multiple QR code information. Specifically, the fusing of multiple QR code information to output the pose involves: Calculate the normalized standard point; By performing image processing and pose calculation on each QR code in the image, the corresponding QR code number and the six-degree-of-freedom pose of each QR code relative to the camera are obtained. The position of the camera relative to the QR code is obtained through matrix transformation; Normalize the coordinates of each QR code to a normalized standard point according to its QR code number; thus obtaining the normalized sequence. The normalized sequence is fused using the K-Means algorithm or the mean algorithm to output the pose.

2. The multi-stage AUV end recovery guidance method based on machine vision according to claim 1, characterized in that: The base station is equipped with active light source markers and passive pattern markers for positioning; There are four active light sources, which are used for guidance in the second stage; The passive pattern marker is in the form of a chessboard nested QR code, used for guidance in the third stage; The four active light sources are located at the four corners of the passive pattern mark.

3. The multi-stage AUV end recovery guidance method based on machine vision according to claim 2, characterized in that: The second phase specifically involves: Images of the light source mounted on the base station are captured by a camera carried by an AUV; The pixel coordinates of the four active light sources are obtained by using a binary-based heuristic thresholding algorithm and by calculating the first moment of the image. The convex hull sequence of the image feature pixel sequence is calculated using the convex hull algorithm. The sequence is then matched with known world coordinate system coordinates, with the coordinates of one of the active light sources as the starting point of the sequence. Determine the pose of the camera relative to the base station.

4. The multi-stage AUV end-effector retrieval guidance method based on machine vision according to claim 2, characterized in that: Between the second and third phases, there is also: If the AUV turns on its searchlight but cannot obtain the chessboard and QR code features, it turns off the searchlight and proceeds to the second stage. It then uses the active light source to navigate and maintain a fixed depth before repeating this step.

5. The multi-stage AUV end-effector retrieval guidance method based on machine vision according to claim 1, characterized in that: It also includes camera calibration, specifically: fixing the camera's optical parameters, using the camera at different positions underwater to take pictures of the calibration board to obtain chessboard images, using Zhang Zhengyou's calibration method to calibrate the camera's intrinsic distortion coefficients, and using reprojection error to determine whether the camera calibration results are correct.

6. The multi-stage AUV end-effector retrieval guidance method based on machine vision according to claim 3, characterized in that: The AP3P algorithm is used to solve the pose of the camera relative to the base station, and the pose is selected by using the reprojection error. The pose solution is optimized by using the Newton-Gauss method to utilize the reprojection error.

Images