AUV pose real-time sensing method and system based on light source array and ins

Abstract

The present application relates to AUV pose perception method and system, specifically relates to a kind of AUV pose real-time perception method and system based on light source array and INS;Solve the prior art underwater AUV autonomous guidance system all need to be pre-offline calibration, however, offline calibration value cannot reflect the current real state Problem, specifically includes: step 1, install four light sources on the recovery station, and the four light sources constitute a square array;Step 2, camera, INS and information processing unit are carried on AUV;Step 3, at k time and k+1 time, the first frame image and the second frame image are collected;Step 4, the four sub-pixel coordinates of the first frame image are sequentially numbered, and are inherited to the second frame image according to the principle of close distance, to obtain the ordered light spot coordinates at k+1 time;Step 5, the pose of recovery station relative to AUV at k time and k+1 time is calculated by Levenberg-Marquardt algorithm.The present application improves the efficiency and accuracy of AUV pose perception.

Description

Technical Field

[0001] This invention relates to an AUV (Autonomous Underwater Vehicle) pose perception method and system, specifically to an AUV pose real-time perception method and system based on a light source array and an INS (Inertial Navigation System). Background Technology

[0002] After completing tasks such as deep-sea exploration and resource discovery, Autonomous Underwater Vehicles (AUVs) need to periodically return to a recovery station (dock) for refueling, data exchange, and equipment maintenance. Because the deep-sea environment cannot receive GPS signals, the autonomous recovery and docking of AUVs heavily relies on their onboard navigation and guidance systems. Typically, the long-range guidance phase uses acoustic navigation (such as long baseline (LBL) or short baseline (SBL) navigation combined with other methods to guide the AUV to the vicinity of the recovery station. During the close-range docking phase, high-precision relative position and attitude between the AUV and the recovery station are required; this phase primarily relies on acoustic guidance, electromagnetic guidance, and visual guidance.

[0003] With the continuous development of computer vision technology, guidance methods based on optical vision have been increasingly studied and applied in the autonomous docking of underwater terminals. This type of method involves deploying actively emitting marker lights (LED light arrays) on the recovery station, with underwater cameras mounted on the AUV capturing images of the marker lights, and using the prior spatial coordinate information of the marker lights to calculate the pose of the recovery station relative to the AUV.

[0004] To address the aforementioned application scenarios, Chinese invention patent CN113525631A discloses an "Underwater Terminal Docking System and Method Based on Optical Vision Guidance." The technical solution involves installing six manually labeled marker light sources on an underwater docking station to construct a three-dimensional guide light array; an AUV equipped with an underwater camera captures images of the light array; image processing extracts the centroid coordinates of the marker light sources, and a light source consistency matching algorithm is used to match image points with physical points; the EPnP algorithm is used to calculate the attitude rotation matrix and position translation vector of the camera coordinate system relative to the world coordinate system, thereby obtaining the pose of the recovery station relative to the AUV; simultaneously, a phased guidance strategy for long-distance and short-distance operations is designed based on whether the camera can capture the complete light array. However, this solution has the following technical drawbacks: First, the six marker light sources need to be manually labeled one by one, and the light source consistency matching algorithm relies on pre-defined numbering and sorting logic, which increases the complexity of the hardware design; second, in scenarios where underwater light signal attenuation and scattering cause some light sources to be unrecognizable, the matching algorithm is prone to failure. Second, the proposed solution explicitly requires "image distortion correction based on camera intrinsic parameters and distortion coefficients obtained from offline camera calibration" in the image processing steps. This means that offline calibration of camera intrinsic parameters must be completed before system deployment, making online self-calibration impossible. Third, the proposed solution only utilizes a single frame image for pose calculation, without introducing inter-frame constraints. Independent calculation for each frame results in low information utilization and is prone to calculation failure or pose jumps when the water is turbid or the LEDs are partially obscured.

[0005] Furthermore, Chinese invention patent CN112611376B discloses an RGI-Lidar / SINS tightly coupled AUV underwater navigation and positioning method and system. Its technical solution involves: pre-integrating the output information of the Inertial Navigation System (INS) to obtain the INS pose constraints of the AUV at different times; performing ORB feature extraction and feature matching on the gated image to obtain visual odometry constraints; obtaining loop closure constraints through loop closure detection; and adding the above three constraints to the pose graph for joint optimization to output the optimized pose of the AUV. However, this solution also has significant shortcomings: on the one hand, its visual front-end uses ORB feature extraction and matching, relying on the natural texture features of the underwater scene, while underwater environments generally have weak textures and low contrast, making it difficult to guarantee the stability of feature extraction and matching; on the other hand, this solution focuses on the global navigation and positioning of the AUV itself, outputting the AUV's position and attitude in the world coordinate system, while the core requirement of the AUV autonomous recovery and docking task is to obtain the pose of the recovery station relative to the AUV, resulting in a low degree of matching between the output of this solution and the actual requirements of the docking task.

[0006] In summary, existing underwater AUV guidance and positioning technologies still have the following overall shortcomings:

[0007] First, the vision solution based on LED arrays lacks inter-frame information coupling, resulting in low data utilization. Existing solutions calculate pose independently frame by frame, failing to utilize motion information between adjacent frames. In underwater environments, the quality of a single frame image may degrade due to water turbidity, partial LED occlusion, or sudden changes in illumination. When calculating independently frame by frame, these degraded frames directly lead to pose jumps or solution failures. Currently, no solution incorporates the inter-frame rigid constraints provided by INS into the LED vision calculation framework—that is, utilizing the relative rotation and translation relationships between adjacent moments given by INS pre-integration to couple multi-frame observations into a joint system of equations for a one-time solution, thereby enhancing resistance to single-frame quality degradation.

[0008] Secondly, existing solutions all require pre-calibration offline, making them unable to adapt to changes in the underwater environment. LED-based vision methods require pre-calibration of camera intrinsic parameters offline. While vision-inertial navigation combined methods can estimate some parameters online, they typically still assume that the camera intrinsic parameters are known and fixed. However, AUVs experience varying water pressures at different depths, and the deformation of the camera's sealing window under pressure can cause slight drifts in the intrinsic parameters, making offline calibration values ​​unable to reflect the current true state. Currently, there is no solution that can achieve synchronous online identification of both camera intrinsic and INS-camera extrinsic parameters for each recovery mission. Summary of the Invention

[0009] This invention aims to solve the technical problems in the prior art, such as the reliance on individual coding and offline intrinsic parameter calibration in the visual guidance scheme based on LED light arrays and the lack of inter-frame constraints, and the requirement for pre-calibration in existing underwater AUV autonomous guidance systems, where the offline calibration values ​​cannot reflect the current real state. Therefore, this invention proposes a real-time AUV pose perception method and system based on light source arrays and INS.

[0010] To achieve the above-mentioned objectives, the present invention adopts the following technical solution:

[0011] A real-time AUV pose sensing method based on a light source array and INS is disclosed for sensing the pose of the AUV recovery station relative to the AUV. Its key feature is that it includes the following steps:

[0012] Step 1: Install four light sources on the recycling bin, forming a square array;

[0013] Step 2: Mount a camera, INS, and information processing unit on the AUV; the camera and INS are respectively connected to the information processing unit;

[0014] Step 3: At time k and time k+1, acquire the first frame image I of the square array using the camera. K and next frame image I K+1The data is then sent to the information processing unit; simultaneously, the relative attitude rotation matrix of the AUV from time k to time k+1 is obtained via the INS. and relative position translation vector And send it to the information processing unit;

[0015] Step 4: The information processing unit calculates the first frame image I respectively. K and next frame image I K+1 Sub-pixel coordinates of the four light spots ( , ), ( , ), and the first frame image I K The four sub-pixel coordinates ( , The light spots are numbered sequentially to obtain the ordered coordinates of the light spots at time k. Then, the numbering at time k is inherited to the next frame image I according to the proximity principle. K+1 The four sub-pixel coordinates ( , ), to obtain the ordered light spot coordinates at time k+1;

[0016] Step 5: The information processing unit uses the ordered spot coordinates at time k, the ordered spot coordinates at time k+1, and the attitude rotation matrix. Position translation vector Relative attitude rotation matrix and relative position translation vector The attitude rotation matrix R of the recovery station relative to the AUV at time k is calculated using the Levenburg-Marquardt algorithm and the following formula. k Position translation vector T k The attitude rotation matrix R of the recovery station relative to the AUV at time k+1 k+1 Position translation vector T k+1 :

[0017] ;

[0018] Among them, f x f y Let f be the normalized focal length along the x-axis and y-axis of the camera, respectively; (u0, v0) be the coordinates of the principal point of the camera; in the Levenberg-Marquardt algorithm, f x f y u0 and v0 are merely letters and do not need to be known in advance; represents the world coordinates of the four light sources; TT represents the transpose; the subscripts x, y, and z represent the components of the column vector on the corresponding axes of the camera coordinate system.

[0019] Furthermore, step 1 specifically includes:

[0020] Four light sources are installed on the recycling bin, forming a square array with a side length of 2a. The center of the square array is located on the central axis of the recycling bin, where a > 0.

[0021] Furthermore, in step 4, the specific processing steps of the information processing unit include:

[0022] Step 4.1: Calculate the first frame image I using the gray-scale centroid method. K Sub-pixel coordinates of the four light spots ( , ):

[0023] ; ;

[0024] Among them, I mn,i For the first frame image I K The pixel grayscale value of the i-th spot region, m i and n i For the first frame image I K The column and row numbers of the pixels in the i-th spot region, i = 1, 2, 3, 4; the summation range corresponds to the spot region covering the i-th light source;

[0025] Step 4.2: Calculate the first frame image I K The geometric center coordinates of the four sub-pixel coordinates ( ):

[0026] ; ;

[0027] Step 4.3, using the coordinates of the geometric center point ( Establish a polar coordinate system with the origin as the origin, and calculate the first frame image I using the following formula. K Each subpixel coordinate ( , polar angle :

[0028] ;

[0029] Step 4.4, according to the four polar angles Assign coordinates to each subpixel in ascending order ( , If we number the light spots 1, 2, 3, 4, then the order of the numbers 1, 2, 3, 4 is clockwise, thus obtaining the ordered light spot coordinates at time k;

[0030] Step 4.5: Obtain the next frame image I using the method described in Step 4.1. K+1 The sub-pixel coordinates of the four corresponding light spots ( , ); using the nearest neighbor as the inheritance criterion, the first frame image I K The four sub-pixel coordinates ( , The number of each frame is inherited by the next frame image I. K+1 The four sub-pixel coordinates ( , ), and obtain the ordered light spot coordinates at time k+1.

[0031] Furthermore, in step 4.5, the first frame image I... K The four sub-pixel coordinates ( , The number of each frame is inherited by the next frame image I. K+1 The four sub-pixel coordinates ( , This is followed by a step to verify whether the number inheritance is correct:

[0032] Based on a clockwise sequence, verify whether the numbers corresponding to the ordered light spot coordinates at time k+1 are 1, 2, 3, and 4:

[0033] If not, return to step 3, update the first frame image and the second frame image, until the numbering is inherited correctly;

[0034] If so, it means the numbering inheritance is correct.

[0035] Furthermore, in step 5, the specific processing steps of the information processing unit include:

[0036] Step 5.1, change the unknown quantity f x f y u0, v0, α k ,β k γ k , t xk , t yk , t zk α IC ,β IC γ IC , t xIC , t yIC , t zIC Arranged into the state column vector X at the Wth iteration of the Levenberg-Marquardt algorithm w W≥1:

[0037] X w =[f x f y u0, v0, α k ,β k γ k , t xk , t yk , t zkα IC ,β IC γ IC , t xIC , t yIC , t zIC ] TT ;

[0038] Where, α k β k γ k Let t be the attitude angle of the recycling station relative to the camera at time k. xk t yk t zk Let α be the translation of the recycling station relative to the camera at time k. IC β IC γ IC Let t be the Euler angle from the INS coordinate system to the camera coordinate system. xIC t yIC t zIC This represents the translation from the INS coordinate system to the camera coordinate system.

[0039] Step 5.2, from the first frame image I K Extract the observations of sub-pixel coordinates of each light source. , and from the next frame image I K+1 Extract the observations of sub-pixel coordinates for each light source. , Simultaneously, the following formula is used to calculate the state column vector X. w The first frame image I K The theoretical value of the sub-pixel coordinates of the i-th light source , and based on state column vectors The next frame image I K+1 The theoretical value of the sub-pixel coordinates of the i-th light source , :

[0040] ;

[0041] in, ; ; ; ;

[0042] Step 5.3: Through observation values , , and theoretical value , , , Calculate the residual of the i-th light source at time k. , and the residual at time k+1 , :

[0043] ;

[0044] ;

[0045] ;

[0046] ;

[0047] Step 5.4: Stack all residuals into a column vector r( ):

[0048] r( )=[ ] TT ;

[0049] Step 5.5: Using the numerical difference method, through the column vector r( Calculate the 16*16 Jacobian matrix J. Each of its elements for:

[0050] ;

[0051] Where g represents row number 1 to 16, and h represents column number 1 to 16; The perturbation step size; It is a 16*1 column vector. The h-th element is 1, and the rest are 0; For column vector r( The g-th element;

[0052] Step 5.6: Based on the 16*16 Jacobian matrix J Construct a regularized system of linear equations:

[0053] ;

[0054] in, Let W be the damping factor in the Wth iteration. Indicates taking the matrix A diagonal matrix composed of the diagonal elements. This is the state update amount;

[0055] Step 5.7: Solve the regularized linear equation system to obtain the state update quantity. ;

[0056] Step 5.8: Using the state column vector X w and state update amount Calculate candidate updated state vector :

[0057] ;

[0058] Step 5.9, using column vector r( Calculate the sum of squared residuals for the current state. Update the state vector through candidates Calculate the sum of squared residuals of candidate states And compare the sum of squared residuals in the current state. Sum of squared residuals of candidate states :

[0059] like Then the state column vector at the (W+1)th iteration Updated to And the damping factor λ at the (W+1)th iteration w+1 Reduced to λ w / ρ, where ρ is the damping factor scaling factor, proceed to step 5.10;

[0060] like Then the state column vector at the (W+1)th iteration Updated to The damping factor λ at the (W+1)th iteration w+1 Increase to λ w ·ρ, return to step 5.6;

[0061] Step 5.10: Determine the state update amount Sum of squared residuals of candidate states Sum of squared residuals with current state Whether the absolute value of the difference and the current iteration number W satisfy the corresponding preset convergence conditions:

[0062] If any state satisfies its corresponding preset convergence condition, then output the state column vector obtained in the last iteration, denoted as the optimal state column vector. =[f x f y u0 v0 α k ,β k γ k , t xk , t yk , t zk α IC ,β IC γ IC , t xIC , t yIC , t zIC ] TT Proceed to step 5.11;

[0063] If none of them satisfy their respective preset convergence conditions, then the state column vector X will be... w Update to candidate update state vector Then return to step 5.2;

[0064] Step 5.11: Using the optimal state column vector Calculate the attitude rotation matrix R of the recovery station relative to the AUV at time k. k and position translation vector T k :

[0065] ;

[0066] ;

[0067] Wherein, the attitude rotation matrix R k Using the zyx sequence of rotation, first rotate around the z-axis. Then rotate around the y-axis Finally, rotate around the x-axis ;

[0068] Step 5.12: Based on the attitude rotation matrix R of the recovery station relative to the AUV at time k. k and position translation vector T k Relative attitude rotation matrix and relative position translation vector The attitude rotation matrix from the INS coordinate system to the camera coordinate system The translation vector T from the INS coordinate system to the camera coordinate system IC The attitude rotation matrix R of the recovery station relative to the AUV at time k+1 is calculated using the following formula. k+1 and position translation vector T k+1 :

[0069] R k+1 = ·R k ;

[0070] .

[0071] Furthermore, in step 5.9:

[0072] The value of ρ is either 2 or 10.

[0073] Furthermore, in step 3:

[0074] The time interval between time k and time k+1 ranges from 1 / 28 to 1 / 32 of a second.

[0075] Furthermore, in step 5.5:

[0076] The perturbation step size The range is 10 -6 Up to 10 -7 ;

[0077] In step 5.10:

[0078] The state update amount The corresponding convergence condition is: ;

[0079] The sum of squared residuals of the candidate states Sum of squared residuals with current state The convergence condition corresponding to the absolute value of the difference is: ;

[0080] The convergence condition corresponding to the current iteration number W is: W> , This represents the maximum number of iterations.

[0081] Furthermore, in step 1, the light source is an LED;

[0082] In step 2, the camera is a monocular camera.

[0083] A real-time AUV pose sensing system based on a light source array and an INS is provided to implement the aforementioned real-time AUV pose sensing method based on a light source array and an INS. Its distinctive feature is that:

[0084] It includes four light sources set on the recycling station and a camera, INS and information processing unit mounted on the AUV; the four light sources form a square array.

[0085] The beneficial effects of this invention are:

[0086] 1. In traditional LED-based visual localization methods, the AUV pose (6 unknowns) for each frame is solved independently. If there is only one frame, the 4 LEDs provide 8 pixel coordinate equations, but the unknowns include camera intrinsics (4) and the AUV pose for that frame (6), totaling 10 unknowns. The number of equations (8) is less than the number of unknowns, resulting in an underdetermined system of equations that cannot be solved. Therefore, traditional methods must pre-calibrate the intrinsics offline to reduce the unknowns to 6 poses before using a single frame image to solve the problem using the PnP algorithm. This invention introduces rigid inter-frame constraints through INS, so the AUV poses of adjacent frames are no longer independent—the AUV pose at time k+1 is obtained from the AUV pose at time k through rigid transformations of ΔR and ΔP, both derived from the same INS-camera extrinsic parameter (R). IC , T IC Therefore, although the two frames of images provide 2×8=16 pixel coordinate measurements, the newly added unknowns are only the AUV pose at time k (6) and INS-camera extrinsic parameters (6), plus the camera intrinsic parameters (4), for a total of 16 unknowns; 16 equations and 16 unknowns constitute a mathematically complete set of equations, which solves the camera intrinsic parameters, INS-camera extrinsic parameters and the pose of the recycle bin relative to the AUV in one go, without any offline calibration steps or individual LED coding, which greatly improves the efficiency and accuracy of AUV pose perception.

[0087] 2. The present invention provides a real-time AUV pose sensing method based on a light source array and INS. Since the underwater motion of the AUV is continuous and the time interval between two frames is extremely short, the imaging position between frames will not change drastically. Compared with traditional methods such as calculating optical flow or extracting feature descriptors, the present invention adopts a method with lower computational load: directly inheriting the coordinate number at time k and supplementing it with polar angle order verification, the number at time k+1 can be accurately obtained. This method only involves coordinate subtraction, division and arctangent operations, and can be run in real time on the AUV information processing unit.

[0088] 3. This invention utilizes the rigid inter-frame constraints provided by INS to provide robustness. The camera poses of two frames are rigidly bound through INS. Even if the detection of individual light sources in one frame is affected by water turbidity, the observation of the other frame and the rigid inter-frame constraints provided by INS can still provide sufficient constraints, and the overall solution is not interrupted. Attached Figure Description

[0089] Figure 1 This is a schematic diagram of the recycling station and AUV structure corresponding to steps 1 and 2 in this embodiment of the invention.

[0090] The attached figures are labeled as follows:

[0091] 1. Recycle bin; 2. Square array; 3. AUV; 4. Camera; 5. INS; 6. Information processing unit. Detailed Implementation

[0092] The technical solution of the present invention will be clearly and completely described below with reference to the accompanying drawings and embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0093] This invention provides a real-time AUV pose sensing method based on a light source array and INS, used to sense the pose of the AUV recovery station relative to the AUV.

[0094] like Figure 1 As shown, the method includes the following steps:

[0095] Step 1: Install four light sources on recycling bin 1, forming a square array 2; specifically:

[0096] Four light sources are installed on recycling station 1. The four light sources form a square array 2 with a side length of 2a (a>0). The horizontal direction of the square array 2 is horizontal and the vertical direction is vertical. The center of the square array 2 is located on the central axis of recycling station 1. The light sources are LEDs. The four LEDs are of the same specification, without individual numbering or any additional coding or modulation.

[0097] Step 2: Mount a camera 4, an INS 5, and an information processing unit 6 on the AUV 3. The relative relationship between the camera 4 and the INS 5 is fixed, and precise measurement of their extrinsic parameters is not required during installation. The camera 4 and the INS 5 are connected to the information processing unit 6. The information processing unit 6 includes an image acquisition and processing module, an INS data acquisition and pre-integration module, and a joint optimization solution module. The camera 4 is a monocular camera.

[0098] The hardware parameter settings for this embodiment are shown in Table 1:

[0099] Table 1

[0100]

[0101] Step 3: At time k and time k+1, acquire the first frame image I of the square array 2 using camera 4. K and next frame image I K+1 The data is then sent to the information processing unit 6; simultaneously, the relative attitude rotation matrix of AUV 3 from time k to time k+1 is obtained via INS 5. (3x3 matrix) and relative position translation vector (3*1 vector), and send it to information processing unit 6; the time interval between time k and time k+1 is between 1 / 28 and 1 / 32 of a second.

[0102] Note: In this embodiment, and The known quantities directly given by INS 5, and their specific calculation methods (such as pre-integration) are well-known techniques in the field of INS navigation.

[0103] In this embodiment, the INS output is:

[0104] ΔR: .

[0105] ΔP: [2.5, -0.8, 15.2] TT (Unit: mm)

[0106] Step 4: Information processing unit 6 calculates the first frame image I respectively. K and next frame image I K+1 Sub-pixel coordinates of the four light spots ( , ), ( , ), and the first frame image I K The four sub-pixel coordinates are ordered to obtain the ordered spot coordinates at time k. Then, the numbering at time k is inherited to the next frame image I according to the proximity principle. K+1 The four sub-pixel coordinates ( , The ordered light spot coordinates at time k+1 are obtained. The specific processing steps of information processing unit 6 include:

[0107] Step 4.1: Calculate the first frame image I using the gray-scale centroid method. K Sub-pixel coordinates of the four light spots ( , ):

[0108] ; ;

[0109] Among them, I mn,i For the first frame image I K The pixel grayscale value of the i-th spot region, m i and n i For the first frame image I K The column and row numbers of the pixels in the i-th spot region, i = 1, 2, 3, 4; the summation range corresponds to the spot region covering the i-th light source.

[0110] The sub-pixel coordinates of the four light spots (A, B, C, D) calculated in this embodiment are as follows:

[0111] A: (982.5, 485.1), B: (1065.7, 486.5), C: (1064.0, 569.8), D: (981.0,568.5).

[0112] Step 4.2: Calculate the first frame image I K The geometric center coordinates of the four sub-pixel coordinates ( ):

[0113] ;

[0114] .

[0115] The calculation results of this embodiment are as follows:

[0116] ;

[0117] .

[0118] Step 4.3, using the above-mentioned geometric center point coordinates ( Establish a polar coordinate system with the origin as the origin, and calculate the first frame image I using the following formula. K Each sub-pixel coordinate ( , polar angle :

[0119] ;

[0120] The calculation results of this embodiment are shown in Table 2:

[0121] Table 2

[0122]

[0123] Step 4.4, according to the four polar angles Assign coordinates to each subpixel in ascending order ( , If we number the light spots 1, 2, 3, and 4, then the order of the numbers 1, 2, 3, and 4 is clockwise (top left → top right → bottom right → bottom left), thus obtaining the ordered light spot coordinates at time k.

[0124] In this embodiment, the physical LEDs are numbered according to their polar angles from smallest to largest (-133.9°→-44.0°→46.1°→135.9°) as shown in Table 3.

[0125] Table 3

[0126]

[0127] Step 4.5: Obtain the next frame image I using the method described in Step 4.1. K+1 The sub-pixel coordinates of the four corresponding light spots ( , ); using the nearest neighbor as the inheritance criterion, the first frame image I K The four sub-pixel coordinates ( , The number of each frame is inherited by the next frame image I. K+1 The four sub-pixel coordinates ( , Based on a clockwise order (top left → top right → bottom right → bottom left), verify whether the numbers corresponding to the ordered light spot coordinates at time k+1 are 1, 2, 3, and 4.

[0128] If not, return to step 3 and update the first frame image and the second frame image until the numbers are 1, 2, 3, 4, that is, until the numbering is inherited correctly;

[0129] If so, it means the numbering inheritance is correct.

[0130] In this embodiment, the number at time k is directly inherited and corresponding according to the principle of nearest position, as shown in Table 4:

[0131] Table 4

[0132]

[0133] Polar angle sequence consistency check: At time k+1, the polar angle sequence of the four points with the geometric center as the origin is: point A (approximately -133.9°) → point B (approximately -43.9°) → point C (approximately 46.1°) → point D (approximately 135.9°), which is consistent with the sequence at time k, so the check passes; the numbering inheritance is correct.

[0134] Step 5: Information processing unit 6 uses the ordered spot coordinates at time k, the ordered spot coordinates at time k+1, and the attitude rotation matrix. Position translation vector Relative attitude rotation matrix and relative position translation vector The attitude rotation matrix R of recovery station 1 relative to AUV 3 at time k is calculated using the Levenburg-Marquardt algorithm and the following formula. k Position translation vector T k The attitude rotation matrix R of recycling station 1 relative to AUV 3 at time k+1. k+1 Position translation vector T k+1 :

[0135] ;

[0136] Among them, f x fy u0, v0 are the four intrinsic parameters of the camera. In the Levenberg-Marquardt algorithm, f x f y u0 and v0 are merely letters and do not need to be known in advance; Here are the world coordinates for the four light sources; TT is the transpose; the subscripts x, y, and z represent the components of the column vectors on the corresponding axes of the camera coordinate system. The origin of the world coordinate system corresponding to the above world coordinates is the center point of square array 2. Its x-axis is the horizontal direction of square array 2, and the positive x-axis direction is to the right facing square array 2. The y-axis is the vertical direction of square array 2, and the positive y-axis direction is downward facing square array 2. The z-axis is the direction of the central axis of recycling station 1, and the positive z-axis direction is perpendicular to the plane where square array 2 is located and points towards AUV 3.

[0137] The 16 unknowns are shown in Table 5:

[0138] Table 5

[0139]

[0140] The known quantities are as follows:

[0141] The world coordinates of the four LEDs are P1, P2, P3, and P4 (determined by the half-side length 'a' of the square, a known design value); the first frame image I at time k. K The 4 pixel coordinates; the next frame image I at time k+1. K+1 The 4 pixel coordinates; inter-frame rigid constraints obtained from INS: (Relative attitude rotation matrix) (Relative position translation vector).

[0142] The pose of Recycling Station 1 relative to AUV 3 at time k+1 is obtained through inter-frame rigid constraint transformation.

[0143] The attitude rotation matrix R of recycling station 1 relative to AUV 3 at time k. k and position translation vector T k The transformation from the INS coordinate system to the camera coordinate system is (R IC T IC Then, at time k+1, the attitude rotation matrix R of recovery station 1 relative to AUV 3 is... k+1 and position translation vector T k+1 satisfy:

[0144] R k+1 = ·R k ;

[0145]

[0146] Physical meaning: The poses of recovery station 1 relative to AUV 3 at two different times are not independent unknowns, but are rigidly bound together by the relative motion measured by the INS. That is, knowing the pose of recovery station 1 relative to AUV 3 at time k and the INS-camera extrinsic parameters, the pose of recovery station 1 relative to AUV 3 at time k+1 is uniquely determined. This is the core of the invention's ability to solve 16 unknowns using 16 equations.

[0147] In step 5, the specific processing steps of the information processing unit 6 include:

[0148] Step 5.1, change the unknown quantity f x f y u0, v0, α k ,β k γ k , t xk , t yk , t zk α IC ,β IC γ IC , t xIC , t yIC , t zIC Arranged into the state column vector X at the Wth iteration of the Levenberg-Marquardt algorithm w W≥1:

[0149] X w =[f x f y u0, v0, α k ,β k γ k , t xk , t yk , t zk α IC ,β IC γ IC , t xIC , t yIC , t zIC ] TT ;

[0150] Where, α k β k γ k Let t be the attitude angle of the recycling station relative to the camera at time k. xk t yk t zk Let α be the translation of the recycling station relative to the camera at time k. IC β IC γ IC Let t be the Euler angle from the INS coordinate system to the camera coordinate system. xIC t yIC tzIC This represents the translation from the INS coordinate system to the camera coordinate system.

[0151] Step 5.2, from the first frame image I K Extract the observations of sub-pixel coordinates of each light source. , and from the next frame image I K+1 Extract the observations of sub-pixel coordinates for each light source. , Simultaneously, the following formula is used to calculate the state column vector X. w The first frame image I K The theoretical value of the sub-pixel coordinates of the i-th light source , and based on state column vectors The next frame image I K+1 The theoretical value of the sub-pixel coordinates of the i-th light source , :

[0152] ;

[0153] in, ; ; ; .

[0154] In this embodiment, , , , , unit mm.

[0155] Step 5.3: Through observation values , , and theoretical value , , , Calculate the residual of the i-th light source at time k. , and the residual at time k+1 , :

[0156] ;

[0157] ;

[0158] ;

[0159] .

[0160] Step 5.4: Stack all residuals into a column vector r( ):

[0161] r( )=[ ] TT .

[0162] Step 5.5: Using the numerical difference method, through the column vector r( Calculate the 16*16 Jacobian matrix J. Each of its elements for:

[0163] ;

[0164] Where g represents row number 1 to 16, and h represents column number 1 to 16; The perturbation step size has a range of 10. -6 Up to 10 -7 ; It is a 16*1 column vector. The h-th element is 1, and the rest are 0; For column vector r( The g-th element.

[0165] Step 5.6: Based on the 16*16 Jacobian matrix J Construct a regularized system of linear equations:

[0166] ;

[0167] in, Let W be the damping factor in the Wth iteration. Indicates taking the matrix A diagonal matrix composed of the diagonal elements. This represents the state update amount.

[0168] Step 5.7: Solve the regularized linear equation system to obtain the state update quantities. .

[0169] Step 5.8: Using the state column vector X w and state update amount Calculate candidate updated state vector :

[0170] .

[0171] Step 5.9, using column vector r( Calculate the sum of squared residuals for the current state. Update the state vector through candidates Calculate the sum of squared residuals of candidate states And compare the sum of squared residuals in the current state. Sum of squared residuals of candidate states :

[0172] like Then the state column vector at the (W+1)th iteration Updated to And the damping factor λ at the (W+1)th iteration w+1 Reduced to λ w / ρ, proceed to step 5.10, where ρ is the damping factor scaling factor; the value of ρ is 2 or 10.

[0173] like Then the state column vector at the (W+1)th iteration Updated to The damping factor λ at the (W+1)th iteration w+1 Increase to λ w ·ρ, return to step 5.6.

[0174] Step 5.10: Determine the state update amount Sum of squared residuals of candidate states Sum of squared residuals with current state Whether the absolute value of the difference and the current iteration number W satisfy the corresponding preset convergence conditions:

[0175] If any state satisfies its corresponding preset convergence condition, then output the state column vector obtained in the last iteration, denoted as the optimal state column vector. : =[f x f y u0 v0 α k ,β k γ k , t xk , t yk , t zk α IC ,β IC γ IC , t xIC , t yIC , t zIC ] TT Then proceed to step 5.11.

[0176] If none of them satisfy their respective preset convergence conditions, then the state column vector X will be... w Update to candidate update state vector Then return to step 5.2.

[0177] In this embodiment, the state update amount The corresponding convergence condition is: Sum of squared residuals of candidate states Sum of squared residuals with current state The convergence condition corresponding to the absolute value of the difference is: The convergence condition for the current iteration number W is: W > , This represents the maximum number of iterations.

[0178] Step 5.11: Using the optimal state column vector Calculate the attitude rotation matrix R of recovery station 1 relative to AUV 3 at time k. k and position translation vector T k :

[0179] ;

[0180] T k =[t xk , t yk , t zk ] TT ;

[0181] Wherein, the attitude rotation matrix R k Using the zyx sequence of rotation, first rotate around the z-axis. Then rotate around the y-axis Finally, rotate around the x-axis .

[0182] Step 5.12: Based on the attitude rotation matrix R of recovery station 1 relative to AUV 3 at time k. k and position translation vector T k Relative attitude rotation matrix and relative position translation vector The attitude rotation matrix from the INS coordinate system to the camera coordinate system The translation vector T from the INS coordinate system to the camera coordinate system IC The attitude rotation matrix R of recovery station 1 relative to AUV3 at time k+1 is calculated using the following formula. k+1and position translation vector T k+1 :

[0183] R k+1 = ·R k ;

[0184] .

[0185] The calculation results in this embodiment are shown in Tables 6, 7, and 8:

[0186] Table 6 Camera Internal Parameters

[0187]

[0188] Table 7 Relative pose at time k (recovery station relative to AUV)

[0189]

[0190] Table 8 INS-Camera Extrinsic Parameters (INS Coordinate System → Camera Coordinate System)

[0191]

[0192] Output the solution directly:

[0193] At time k, the pose of the recovery station 1 relative to the AUV 3 is: position (150.2, -32.5, 2985.7) mm, attitude (2.3°, -1.5°, 0.8°) → output to the AUV control system.

[0194] The pose of Recycling Station 1 relative to AUV 3 at time k+1:

[0195] Location ≈ (128.93, -34.57, 3016.28) mm;

[0196] Posture ≈ (2.07°, -2.29°, 1.03°).

[0197] It should be noted that when applied to this method, the relative attitude rotation matrix ΔR and the relative position translation vector ΔP cannot be simultaneously identity matrix and zero vector, respectively. Therefore, in this embodiment, the absolute value of the change in at least one of the pitch angle, yaw angle, and roll angle of the AUV is not less than 0.3 degrees, and the magnitude of the relative translation vector between time k and time k+1 is not less than 8 mm.

[0198] The AUV pose real-time sensing system based on a light source array and INS provided in this embodiment of the invention is used in the aforementioned AUV pose real-time sensing method based on a light source array and INS. The system includes four light sources disposed on a recycling station 1 and a camera 4, an INS 5, and an information processing unit 6 mounted on an AUV 3; the four light sources form a square array 2. The camera 4 is a monocular camera, and the light source is an LED.

[0199] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions within the technical scope disclosed in the present invention should be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A real-time AUV pose sensing method based on a light source array and INS, used to sense the pose of a recovery station relative to the AUV; characterized in that, Includes the following steps: Step 1: Install four light sources on the recycling bin, forming a square array; Step 2: Mount a camera, INS, and information processing unit on the AUV; the camera and INS are respectively connected to the information processing unit; Step 3: At time k and time k+1, acquire the first frame image I of the square array using the camera. K and next frame image I K+1 The data is then sent to the information processing unit; simultaneously, the relative attitude rotation matrix of the AUV from time k to time k+1 is obtained via the INS. and relative position translation vector And send it to the information processing unit; Step 4: The information processing unit calculates the first frame image I respectively. K and next frame image I K+1 Sub-pixel coordinates of the four light spots ( , ), ( , ), and the first frame image I K The four sub-pixel coordinates ( , The light spots are numbered sequentially to obtain the ordered coordinates of the light spots at time k. Then, the numbering at time k is inherited to the next frame image I according to the proximity principle. K+1 The four sub-pixel coordinates ( , ), to obtain the ordered light spot coordinates at time k+1; Step 5: The information processing unit uses the ordered spot coordinates at time k, the ordered spot coordinates at time k+1, and the attitude rotation matrix. Position translation vector Relative attitude rotation matrix and relative position translation vector The attitude rotation matrix R of the recovery station relative to the AUV at time k is calculated using the Levenburg-Marquardt algorithm and the following formula. k Position translation vector T k The attitude rotation matrix R of the recovery station relative to the AUV at time k+1 k+1 Position translation vector T k+1 : ； Among them, f x f y Let f be the normalized focal length along the x-axis and y-axis of the camera, respectively; (u0, v0) be the coordinates of the principal point of the camera; in the Levenberg-Marquardt algorithm, f x f y u0 and v0 are merely letters and do not need to be known in advance; represents the world coordinates of the four light sources; TT represents the transpose; the subscripts x, y, and z represent the components of the column vector on the corresponding axes of the camera coordinate system.

2. The AUV pose real-time sensing method based on light source array and INS according to claim 1, characterized in that, Step 1 is as follows: Four light sources are installed on the recycling bin, forming a square array with a side length of 2a. The center of the square array is located on the central axis of the recycling bin, where a > 0.

3. The AUV pose real-time sensing method based on light source array and INS according to claim 2, characterized in that, In step 4, the specific processing steps of the information processing unit include: Step 4.1: Calculate the first frame image I using the gray-scale centroid method. K Sub-pixel coordinates of the four light spots ( , ): ； ； Among them, I mn,i For the first frame image I K The pixel grayscale value of the i-th spot region, m i and n i For the first frame image I K The column and row numbers of the pixels in the i-th spot region, i = 1, 2, 3, 4; the summation range corresponds to the spot region covering the i-th light source; Step 4.2: Calculate the first frame image I K The geometric center coordinates of the four sub-pixel coordinates ( ): ； ； Step 4.3, using the coordinates of the geometric center point ( Establish a polar coordinate system with the origin as the origin, and calculate the first frame image I using the following formula. K Each subpixel coordinate ( , polar angle : ； Step 4.4, according to the four polar angles Assign coordinates to each subpixel in ascending order ( , If we number the light spots 1, 2, 3, 4, then the order of the numbers 1, 2, 3, 4 is clockwise, and we can obtain the ordered light spot coordinates at time k. Step 4.5: Obtain the next frame image I using the method described in Step 4.

1. K+1 The sub-pixel coordinates of the four corresponding light spots ( , ); using the nearest neighbor as the inheritance criterion, the first frame image I K The four sub-pixel coordinates ( , The number of each frame is inherited by the next frame image I. K+1 The four sub-pixel coordinates ( , ), and obtain the ordered light spot coordinates at time k+1.

4. The AUV pose real-time sensing method based on light source array and INS according to claim 3, characterized in that, In step 4.5, the first frame image I... K The four sub-pixel coordinates ( , The number of each frame is inherited by the next frame image I. K+1 The four sub-pixel coordinates ( , This is followed by a step to verify whether the number inheritance is correct: Based on a clockwise sequence, verify whether the numbering corresponding to the ordered light spot coordinates at time k+1 is 1, 2, 3, or 4: If not, return to step 3, update the first frame image and the second frame image, until the numbering is inherited correctly; If so, it means the numbering inheritance is correct.

5. The AUV pose real-time sensing method based on light source array and INS according to claim 4, characterized in that, In step 5, the specific processing steps of the information processing unit include: Step 5.1, change the unknown quantity f x f y u0, v0, α k ,β k γ k , t xk , t yk , t zk α IC ,β IC γ IC , t xIC , t yIC , t zIC Arranged into the state column vector X at the Wth iteration of the Levenberg-Marquardt algorithm w W≥1: X w =[f x ,f y ,u0,v0,α k ,b k ,c k ,t xk ,t yk ,t zk ,a IC ,b IC ,c IC ,t xIC ,t yIC ,t zIC ] TT ; Where, α k β k γ k Let t be the attitude angle of the recycling station relative to the camera at time k. xk t yk t zk Let α be the translation of the recycling station relative to the camera at time k. IC β IC γ IC Let t be the Euler angle from the INS coordinate system to the camera coordinate system. xIC t yIC t zIC This represents the translation from the INS coordinate system to the camera coordinate system. Step 5.2, from the first frame image I K Extract the observations of sub-pixel coordinates of each light source. , and from the next frame image I K+1 Extract the observations of sub-pixel coordinates for each light source. , Simultaneously, the following formula is used to calculate the state column vector X. w The first frame image I K The theoretical value of the sub-pixel coordinates of the i-th light source , and based on state column vectors The next frame image I K+1 The theoretical value of the sub-pixel coordinates of the i-th light source , : ； in, ; ; ; ; Step 5.3: Through observation values , , and theoretical value , , , Calculate the residual of the i-th light source at time k. , and the residual at time k+1 , : ； ； ； ； Step 5.4: Stack all residuals into a column vector r( ): ； Step 5.5: Using the numerical difference method, through the column vector r( Calculate the 16*16 Jacobian matrix J. Each of its elements for: ； Where g represents row number 1 to 16, and h represents column number 1 to 16; The perturbation step size; It is a 16*1 column vector. The h-th element is 1, and the rest are 0; For column vector r( The g-th element; Step 5.6: Based on the 16*16 Jacobian matrix J Construct a regularized system of linear equations: ； in, Let W be the damping factor in the Wth iteration. Indicates taking the matrix A diagonal matrix composed of the diagonal elements. This is the state update amount; Step 5.7: Solve the regularized linear equation system to obtain the state update quantity. ; Step 5.8: Using the state column vector X w and state update amount Calculate candidate updated state vector : ； Step 5.9, using column vector r( Calculate the sum of squared residuals for the current state. Update the state vector through candidates Calculate the sum of squared residuals of candidate states And compare the sum of squared residuals in the current state. Sum of squared residuals of candidate states : like Then the state column vector at the (W+1)th iteration Updated to And the damping factor λ at the (W+1)th iteration w+1 Reduced to λ w / ρ, where ρ is the damping factor scaling factor, proceed to step 5.10; like Then the state column vector at the (W+1)th iteration Updated to The damping factor λ at the (W+1)th iteration w+1 Increase to λ w ·ρ, return to step 5.6; Step 5.10: Determine the state update amount Sum of squared residuals of candidate states Sum of squared residuals with current state Whether the absolute value of the difference and the current iteration number W satisfy the corresponding preset convergence conditions: If any state satisfies its corresponding preset convergence condition, then output the state column vector obtained in the last iteration, denoted as the optimal state column vector. =[f x f y u0 v0 α k ,β k γ k , t xk , t yk , t zk α IC ,β IC γ IC , t xIC , t yIC , t zIC ] TT Proceed to step 5.11; If none of them satisfy their respective preset convergence conditions, then the state column vector X will be... w Update to candidate update state vector Then return to step 5.2; Step 5.11: Using the optimal state column vector Calculate the attitude rotation matrix R of the recovery station relative to the AUV at time k. k and position translation vector T k : ； ； Wherein, the attitude rotation matrix R k Using the zyx sequence of rotation, first rotate around the z-axis. Then rotate around the y-axis Finally, rotate around the x-axis ; Step 5.12: Based on the attitude rotation matrix R of the recovery station relative to the AUV at time k. k and position translation vector T k Relative attitude rotation matrix and relative position translation vector The attitude rotation matrix from the INS coordinate system to the camera coordinate system The translation vector T from the INS coordinate system to the camera coordinate system IC The attitude rotation matrix R of the recovery station relative to the AUV at time k+1 is calculated using the following formula. k+1 and position translation vector T k+1 : ； 。 6. The AUV pose real-time sensing method based on light source array and INS according to claim 5, characterized in that, In step 5.9: The value of ρ is either 2 or 10.

7. The AUV pose real-time sensing method based on light source array and INS according to claim 6, characterized in that, In step 3: The time interval between time k and time k+1 ranges from 1 / 28 to 1 / 32 of a second.

8. The AUV pose real-time sensing method based on light source array and INS according to claim 7, characterized in that: In step 5.5: The perturbation step size The range is 10 -6 Up to 10 -7 ; In step 5.10: The state update amount The corresponding convergence condition is: ; The sum of squared residuals of the candidate states Sum of squared residuals with current state The convergence condition corresponding to the absolute value of the difference is: ; The convergence condition corresponding to the current iteration number W is: W> , This represents the maximum number of iterations.

9. The AUV pose real-time sensing method based on light source array and INS according to any one of claims 1-8, characterized in that: In step 1, the light source is an LED; In step 2, the camera is a monocular camera.

10. A real-time AUV pose sensing system based on a light source array and an INS, used to implement the real-time AUV pose sensing method based on a light source array and an INS as described in any one of claims 1-9, characterized in that: It includes four light sources set on the recycling station and a camera, INS and information processing unit mounted on the AUV; the four light sources form a square array.

Images