An AUV autonomous docking positioning method based on optical direction finding and ranging

Abstract

The present invention discloses an AUV autonomous docking positioning method based on optical direction finding and ranging. The method includes: emitting a guiding light source from a dock towards an autonomous underwater vehicle (AUV); obtaining the relative azimuth angle between the AUV and the guiding light source by using a spot imaging method; obtaining the relative position between the AUV and the guiding light source by using an optical direction finding and ranging method according to the relative azimuth angle obtained by an optical detection component; performing data fusion by using a Kalman filtering algorithm according to the relative position to obtain the final relative position between the AUV and the guiding light source, and performing positioning to achieve autonomous docking. The underwater optical guiding and positioning technology of the method of the present invention aims to ensure measurement accuracy and real-time performance, provides position information for underwater robots in real time for the recovery and utilization of the AUV, can realize AUV positioning and autonomous docking within a short distance, and has important application value.

Description

Technical Field

[0001] The present invention relates to a method for autonomous docking and positioning of an AUV, in particular to a method for autonomous docking and positioning of an AUV based on optical direction finding and ranging. Background Art

[0002] An autonomous underwater vehicle (AUV) works autonomously in the marine environment. The recovery and reuse of the AUV is one of the important research contents for the research and facilitation of the AUV. In recent years, rich research results have been achieved in underwater vision. However, due to the troublesome calibration of underwater cameras and the large amount of calculation for image processing, the accuracy of image processing is not very high, and the data update frequency is low. These all have a great impact on the description of underwater light sources and the real-time positioning of targets, thus affecting the operation tasks of autonomous underwater vehicles and the recovery work of autonomous underwater vehicles. Summary of the Invention

[0003] In order to solve the problems existing in the background art, the present invention provides a method for autonomous docking and positioning of an AUV based on optical direction finding and ranging.

[0004] The technical solution adopted by the present invention is as follows:

[0005] The method for autonomous docking and positioning of the AUV of the present invention includes the following steps:

[0006] Step 1: A guiding light source is emitted from the center of the docking port of the docking station installed on the ship on the sea surface towards the autonomous underwater vehicle AUV in the sea water, and the guiding light source is emitted to two optical detection components installed on the head of the autonomous underwater vehicle AUV.

[0007] Step 2: For each optical detection component, the relative azimuth angle between the autonomous underwater vehicle AUV and the guiding light source is calculated by using the spot imaging method.

[0008] Step 3: For each optical detection component, according to the relative azimuth angle obtained by the optical detection component in Step 2, the relative position between the autonomous underwater vehicle AUV and the guiding light source is calculated by using the optical direction finding and ranging method.

[0009] Step 4: According to the relative positions obtained by the two optical detection components respectively in Step 3, the Kalman filtering algorithm is used for data fusion to obtain the final relative position between the autonomous underwater vehicle AUV and the guiding light source. The autonomous underwater vehicle AUV performs positioning according to the final relative position and sails towards the ship installed with the docking station to finally achieve autonomous docking.

[0010] In the first step described above, two optical detection components are hermetically installed inside the head of an autonomous underwater vehicle (AUV) through waterproof and pressure-resistant glass. There is a gap between the two optical detection components. Each optical detection component includes a semi-cylindrical convex lens and a quadrant photodetector. The photosensitive surfaces of the two quadrant photodetectors are on the same plane and both face the navigation direction of the AUV. The semi-cylindrical convex lens of each optical detection component is installed between its own quadrant photodetector and the waterproof and pressure-resistant glass, and there is no contact among the semi-cylindrical convex lens, the waterproof and pressure-resistant glass, and the quadrant photodetector. The spherical surface of the semi-cylindrical convex lens faces the waterproof and pressure-resistant glass, and the plane of the semi-cylindrical convex lens is parallel to the photosensitive surface of the quadrant photodetector. The connection line between the center of the photosensitive surface of each quadrant photodetector and the center of its own semi-cylindrical convex lens is perpendicular to the photosensitive surface of the quadrant photodetector.

[0011] The distance between the semi-cylindrical convex lens and the quadrant photodetector of each optical detection component is the focal length of the semi-cylindrical convex lens. The light of the guiding light source passes through the waterproof and pressure-resistant glass and converges on the photosensitive surface S1 of the two quadrant photodetectors respectively through the two semi-cylindrical convex lenses to form circular light spots. Due to different incident angles of the light, the positions of the light spots formed by the light incident at different angles on the photosensitive surface will also be different. The incident angle of the guiding light can be calculated according to the position of the center of the light spot on the photosensitive surface.

[0012] In the second step described above, for each optical detection component, the relative azimuth angle between the AUV and the guiding light source is calculated using the light spot imaging method. The relative azimuth angle includes the relative heading angle and the relative pitch angle, specifically as follows:

[0013]

[0014]

[0015] Among them, ψ represents the relative heading angle between the AUV and the guiding light source; n a and n w respectively represent the refractive indices of air and water; ψ′ represents the horizontal exit angle of the guiding light source emitted to the waterproof and pressure-resistant glass; θ represents the relative pitch angle between the AUV and the guiding light source; θ′ represents the vertical exit angle of the guiding light source emitted to the waterproof and pressure-resistant glass.

[0016] The horizontal exit angle ψ′ of the guiding light source emitted to the waterproof and pressure-resistant glass is specifically as follows:

[0017]

[0018]

[0019]

[0020] Among them, α1 and α2 respectively represent the horizontal incident angle and the horizontal exit angle at which the guiding light source is emitted to the semi-circular convex lens; n g represents the refractive index of the waterproof and pressure-resistant glass; l1 represents the distance between the curvature center of the semi-circular convex lens and the plane of the semi-circular convex lens; l2 represents the radius of curvature of the semi-circular convex lens; Δm represents the offset distance of the center of the circular light spot on the four-quadrant photodetector in the plane where the photosensitive surface of the four-quadrant photodetector is located; f represents the focal length of the semi-circular convex lens.

[0021] The vertical exit angle θ′ at which the guiding light source is emitted to the waterproof and pressure-resistant glass is specifically as follows:

[0022]

[0023]

[0024]

[0025] Among them, β1 and β2 respectively represent the vertical incident angle and the vertical exit angle at which the guiding light source is emitted to the semi-circular convex lens; n g represents the refractive index of the waterproof and pressure-resistant glass; l1 represents the distance between the curvature center of the semi-circular convex lens and the plane of the semi-circular convex lens; l2 represents the radius of curvature of the semi-circular convex lens; Δm represents the offset distance of the center of the circular light spot on the four-quadrant photodetector in the plane where the photosensitive surface of the four-quadrant photodetector is located; Δn represents the offset distance of the center of the circular light spot on the four-quadrant photodetector in the plane perpendicular to the plane where the photosensitive surface of the four-quadrant photodetector is located; f represents the focal length of the semi-circular convex lens. A plane coordinate system is created on the photosensitive surface S1. With the center point of the photosensitive surface of the four-quadrant photodetector as the center, the direction of the connection line of the center points of the two photosensitive surfaces of the four-quadrant photodetector as the m-axis, and the vertical direction as the n-axis to determine the offset distance; it is stipulated that the distance from the center of the circular light spot to the coordinate axis m is Δn, and the distance to the coordinate axis n is Δm. When the guiding light source emits light to the optical detection component, since the distance between the light source and the convex lens in the detection component is generally very far (greater than 300 cm), the light rays reaching the convex lens are regarded as parallel light rays, and the refraction of the light rays needs to be considered when the light rays are converged by the convex lens.

[0026] In the third step described above, for each optical detection component, according to the relative azimuth angle obtained by the optical detection component in the second step, the relative position between the autonomous underwater vehicle (AUV) and the guiding light source is calculated using the optical direction and ranging method. Specifically, first, the midpoint of the line connecting the centers of the photosensitive surfaces of the two quadrant photodetectors is taken as the reference point. The top end face of the head of the AUV is perpendicular to its own forward direction and parallel to the photosensitive surface of the quadrant photodetector. The intersection point between the straight line perpendicular to the photosensitive surface of the quadrant photodetector where the reference point is located and the head end face of the AUV is taken as the origin. The direction of the line connecting the reference point to the origin is taken as the positive direction of the x-axis. The intersection point between the straight line perpendicular to the photosensitive surface of the quadrant photodetector where the center point of the photosensitive surface of the first quadrant photodetector is located and the head end face of the AUV is taken as the first intersection point. The direction of the line connecting the origin to the first intersection point is taken as the positive direction of the y-axis. The straight line perpendicular to the x-axis and the y-axis is taken as the z-axis, thus establishing the AUV vehicle coordinate system.

[0027] The first relative position Z1(x1, y1, z1) between the AUV and the guiding light source is calculated based on the first quadrant photodetector as follows:

[0028] x1 = d

[0029]

[0030] z1 = d tan(θ1)

[0031] Where x1, y1, and z1 respectively represent the x-axis, y-axis, and z-axis coordinates of the first relative position; d represents the distance between the photosensitive surface of the quadrant photodetector and the guiding light source; ψ1 represents the relative bow angle between the AUV and the guiding light source calculated by the first quadrant photodetector; L represents the distance between the centers of the photosensitive surfaces of the two quadrant photodetectors; θ1 represents the relative pitch angle between the AUV and the guiding light source calculated by the first quadrant photodetector.

[0032] The second relative position Z2(x2, y2, z2) between the AUV and the guiding light source is calculated based on the second quadrant photodetector as follows:

[0033] x2 = d

[0034]

[0035] z2 = d tan(θ2)

[0036] Among them, x2, y2, and z2 respectively represent the x-axis, y-axis, and z-axis coordinates of the second relative position; ψ2 represents the relative bow angle between the autonomous underwater vehicle (AUV) and the guiding light source calculated by the second quadrant photodetector; θ2 represents the relative pitch angle between the AUV and the guiding light source calculated by the second quadrant photodetector.

[0037] The distance d between the photosensitive surface of the quadrant photodetector and the guiding light source is specifically as follows:

[0038]

[0039] In step 4, according to the relative positions obtained by the two optical detection components in step 3, the Kalman filtering algorithm is used for data fusion, that is, according to the first relative position Z1(x1, y1, z1) and the second relative position Z2(x2, y2, z2), the Kalman filtering algorithm is used for data fusion to obtain the final relative position between the AUV and the guiding light source.

[0040] In step 4, the state quantity in the Kalman filtering algorithm is X = [x, y, z, u, v, w] T , where x, y, and z are respectively the x-axis, y-axis, and z-axis coordinates of the guiding light source center in the AUV body coordinate system, and u, v, and w are the linear velocities of the guiding light source center in the x-axis, y-axis, and z-axis of the AUV body coordinate system; the observed quantity in the Kalman filtering algorithm is Z = [Z1, Z2] T ; the state transition matrix F and the observation matrix H in the Kalman filtering algorithm are specifically as follows:

[0041]

[0042]

[0043] Among them, T represents the update period of the final relative position between the AUV and the guiding light source. The beneficial effects of the present invention are:

[0044] The underwater optical guiding and positioning technology of the method of the present invention aims to ensure measurement accuracy and real-time performance, provides position information for underwater robots in real time for the recovery and utilization of the AUV, and can achieve AUV positioning and autonomous docking within a short distance, which has important application value. Description of the Drawings

[0045] Figure 1 is a schematic diagram of the spot imaging of the quadrant photodetector in the present invention;

[0046] Figure 2 is a schematic diagram of the direction measurement principle of the optical detection component in the present invention;

[0047] Figure 3 Schematic diagram of the arrangement of the dual optical detection components in the present invention;

[0048] Figure 4 Schematic diagram of the positioning principle of the dual optical detection components in the present invention. Specific embodiments

[0049] To make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be further described in detail with reference to the accompanying drawings. Such description introduces specific embodiments consistent with the principles of the present invention by way of example rather than limitation. The descriptions of these embodiments are detailed enough to enable those skilled in the art to practice the present invention. Other embodiments may be used and the structures of the elements may be changed and / or replaced without departing from the scope and spirit of the present invention. Therefore, the following detailed description should not be construed in a limiting sense.

[0050] The AUV autonomous docking and positioning method of the present invention includes the following steps:

[0051] Step 1: A guiding light source is emitted from the center of the docking port of the docking station installed on the ship on the sea surface towards the autonomous underwater vehicle (AUV) in the sea water, and the guiding light source is emitted onto two optical detection components installed on the head of the autonomous underwater vehicle (AUV).

[0052] In Step 1, the two optical detection components are hermetically installed in the head of the autonomous underwater vehicle (AUV) through waterproof and pressure-resistant glass. There is a gap between the two optical detection components. Each optical detection component includes a semi-cylindrical convex lens and a quadrant photodetector; the photosensitive surfaces of the two quadrant photodetectors are on the same plane and both face the navigation direction of the autonomous underwater vehicle (AUV). The semi-cylindrical convex lens of each optical detection component is installed between its respective quadrant photodetector and the waterproof and pressure-resistant glass, and there is no contact between the semi-cylindrical convex lens, the waterproof and pressure-resistant glass, and the quadrant photodetector; the spherical surface of the semi-cylindrical convex lens faces the waterproof and pressure-resistant glass, the plane of the semi-cylindrical convex lens is parallel to the photosensitive surface of the quadrant photodetector, and the connection line between the center of the photosensitive surface of each quadrant photodetector and the center of one of its semi-cylindrical convex lenses is perpendicular to the photosensitive surface of the quadrant photodetector.

[0053] The distance between the semi-cylindrical convex lens and the quadrant photodetector of each optical detection component is the focal length of the semi-cylindrical convex lens; the light of the guiding light source passes through the waterproof and pressure-resistant glass and converges on the photosensitive surface S1 of the two quadrant photodetectors through the two semi-cylindrical convex lenses respectively to form circular light spots. Due to the different incident angles of the light, the positions of the light spots formed by the light incident at different angles on the photosensitive surface will also be different. According to the position of the center of the light spot on the photosensitive surface, the incident angle of the guiding light can be calculated.

[0054] Step 2: For each optical detection component, use the spot imaging method to calculate the relative azimuth angle between the autonomous underwater vehicle (AUV) and the guiding light source.

[0055] In Step 2, for each optical detection component, use the spot imaging method to calculate the relative azimuth angle between the AUV and the guiding light source. The relative azimuth angle includes the relative heading angle and the relative pitch angle, which are specifically as follows:

[0056]

[0057]

[0058] Among them, ψ represents the relative heading angle between the AUV and the guiding light source; n a and n w respectively represent the refractive indices of air and water; ψ′ represents the horizontal exit angle of the guiding light source emitted to the waterproof and pressure-resistant glass; θ represents the relative pitch angle between the AUV and the guiding light source; θ′ represents the vertical exit angle of the guiding light source emitted to the waterproof and pressure-resistant glass.

[0059] The horizontal exit angle ψ′ of the guiding light source emitted to the waterproof and pressure-resistant glass is specifically as follows:

[0060]

[0061]

[0062]

[0063] Among them, α1 and α2 respectively represent the horizontal incident angle and the horizontal exit angle of the guiding light source emitted to the semi-circular convex lens; n g represents the refractive index of the waterproof and pressure-resistant glass; l1 represents the distance between the curvature center of the semi-circular convex lens and the plane of the semi-circular convex lens; l2 represents the radius of curvature of the semi-circular convex lens; Δm represents the offset distance of the center of the circular spot on the quadrant photodetector in the plane of the photosensitive surface of the quadrant photodetector; f represents the focal length of the semi-circular convex lens.

[0064] The vertical exit angle θ′ of the guiding light source emitted to the waterproof and pressure-resistant glass is specifically as follows:

[0065]

[0066]

[0067]

[0068] Among them, β1 and β2 respectively represent the vertical incident angle and the vertical exit angle at which the guiding light source emits light to the semi-circular convex lens; n g represents the refractive index of the waterproof and pressure-resistant glass; l1 represents the distance between the center of curvature of the semi-circular convex lens and the plane of the semi-circular convex lens; l2 represents the radius of curvature of the semi-circular convex lens; Δm represents the offset distance of the center of the circular light spot on the four-quadrant photodetector in the plane where the photosensitive surface of the four-quadrant photodetector is located; Δn represents the offset distance of the center of the circular light spot on the four-quadrant photodetector in the plane perpendicular to the plane where the photosensitive surface of the four-quadrant photodetector is located; f represents the focal length of the semi-circular convex lens. A plane coordinate system is created on the photosensitive surface S1. Taking the center point of the photosensitive surface of the four-quadrant photodetector as the center of the circle, the direction of the connection line of the center points of the photosensitive surfaces of the two four-quadrant photodetectors as the m-axis, and the vertical direction as the n-axis to determine the offset distance; it is stipulated that the distance from the center of the circular light spot to the coordinate axis m is Δn, and the distance to the coordinate axis n is Δm. When the guiding light source emits light to the optical detection component, since the distance between the light source and the convex lens in the detection component is generally very far (greater than 300 cm), the light reaching the convex lens is regarded as parallel light, and the refraction of the light needs to be considered when the light is converged by the convex lens.

[0069] Step 3: For each optical detection component, according to the relative azimuth angle obtained by the optical detection component in Step 2, use the optical direction finding and ranging method to calculate the relative position between the autonomous underwater vehicle AUV and the guiding light source.

[0070] In Step 3, for each optical detection component, according to the relative azimuth angle obtained by the optical detection component in Step 2, use the optical direction finding and ranging method to calculate the relative position between the autonomous underwater vehicle AUV and the guiding light source. Specifically, first take the midpoint of the connection line of the center points of the photosensitive surfaces of the two four-quadrant photodetectors as the reference point. The top end surface of the head of the autonomous underwater vehicle AUV is perpendicular to its own forward direction and parallel to the photosensitive surface of the four-quadrant photodetector. The intersection point between the straight line perpendicular to the photosensitive surface of the four-quadrant photodetector where the reference point is located and the head end surface of the autonomous underwater vehicle AUV is used as the origin. The direction of the connection line of the reference point towards the origin is used as the positive direction of the x-axis. The intersection point between the straight line perpendicular to the photosensitive surface of the four-quadrant photodetector where the center point of the photosensitive surface of the first four-quadrant photodetector is located and the head end surface of the autonomous underwater vehicle AUV is used as the first intersection point. The direction of the connection line of the origin towards the first intersection point is used as the positive direction of the y-axis. The straight line perpendicular to the x-axis and the y-axis is used as the z-axis to establish the AUV carrier coordinate system.

[0071] Calculate the first relative position Z1(x1, y1, z1) between the autonomous underwater vehicle AUV and the guiding light source according to the first four-quadrant photodetector, specifically as follows:

[0072] x1 = d

[0073]

[0074] z1 = d tan(θ1)

[0075] Wherein, x1, y1, and z1 respectively represent the x-axis, y-axis, and z-axis coordinates of the first relative position; d represents the distance between the photosensitive surface of the quadrant photodetector and the guiding light source; ψ1 represents the relative bow angle between the autonomous underwater vehicle AUV and the guiding light source obtained by calculation of the first quadrant photodetector; L represents the distance between the center points of the photosensitive surfaces of the two quadrant photodetectors; θ1 represents the relative pitch angle between the autonomous underwater vehicle AUV and the guiding light source obtained by calculation of the first quadrant photodetector.

[0076] According to the second quadrant photodetector, the second relative position Z2(x2, y2, z2) between the autonomous underwater vehicle AUV and the guiding light source is obtained by calculation, specifically as follows:

[0077] x2 = d

[0078]

[0079] z2 = d tan(θ2)

[0080] Wherein, x2, y2, and z2 respectively represent the x-axis, y-axis, and z-axis coordinates of the second relative position; ψ2 represents the relative bow angle between the autonomous underwater vehicle AUV and the guiding light source obtained by calculation of the second quadrant photodetector; θ2 represents the relative pitch angle between the autonomous underwater vehicle AUV and the guiding light source obtained by calculation of the second quadrant photodetector.

[0081] The distance d between the photosensitive surface of the quadrant photodetector and the guiding light source is specifically as follows:

[0082]

[0083] Step Four: According to the relative positions respectively obtained by the two optical detection components in Step Three, use the Kalman filtering algorithm for data fusion to obtain the final relative position between the autonomous underwater vehicle AUV and the guiding light source. The autonomous underwater vehicle AUV positions according to the final relative position and sails towards the ship at the installation dock to finally achieve autonomous docking.

[0084] In Step 4, based on the relative positions obtained by the two optical detection components in Step 3, the Kalman filtering algorithm is used for data fusion, that is, based on the first relative position Z1(x1, y1, z1) and the second relative position Z2(x2, y2, z2), the Kalman filtering algorithm is used for data fusion to obtain the final relative position between the autonomous underwater vehicle (AUV) and the guiding light source.

[0085] In Step 4, the state variables in the Kalman filtering algorithm are X = [x, y, z, u, v, w] T , where x, y, and z are the x-axis, y-axis, and z-axis coordinates of the guiding light source center in the AUV vehicle coordinate system, and u, v, and w are the linear velocities of the guiding light source center along the x-axis, y-axis, and z-axis in the AUV vehicle coordinate system; the observed variables in the Kalman filtering algorithm are Z = [Z1, Z2] T ; the state transition matrix F and the observation matrix H in the Kalman filtering algorithm are specifically as follows:

[0086]

[0087]

[0088] where T represents the update period of the final relative position between the autonomous underwater vehicle (AUV) and the guiding light source.

[0089] As Figure 1 shown, it is a schematic diagram of the spot imaging of the quadrant photodetector. After the light of the guiding light source passes through the waterproof and pressure-resistant glass, it converges on the photosensitive surface of the quadrant photodetector through the semi-cylindrical convex lens to form a spot. A plane coordinate system is created on the photosensitive surface S1, with the center of the quadrant photodetector as the origin. It is stipulated that the distance from the spot center to the coordinate axis m is Δn, and the distance to the coordinate axis n is Δm.

[0090] As Figure 2 shown, it is a schematic diagram of the direction-finding principle of the optical detection component. In the figure, taking the calculation of the relative heading angle in the horizontal direction as an example, the calculation of the relative pitch angle in the vertical direction is similar. The incident light at the spot center is taken as the calculation object. This incident light has experienced four refractions before reaching the photosensitive surface S1. First, it enters the waterproof and pressure-resistant glass from the water, and the incident angle at this time is the relative heading angle ψ; second, it enters the interior of the optical detection component from the waterproof and pressure-resistant glass, and the exit angle of the light at this time is ψ'; third, it enters the convex lens from the air, and the incident angle of the light at this time is α1; finally, it exits from the convex lens into the air, and the exit angle at this time is α2.

[0091] As Figure 3As shown in the figure, it is a schematic diagram of the arrangement of dual optical detection components. The optical detection component 1 and the optical detection component 2 are radially arranged at a certain distance L in the horizontal direction and are arranged in the head window of the AUV, serving as the main sensors for the optical guidance and positioning of the AUV.

[0092] As Figure 4 shown in the figure, it is a schematic diagram of the positioning principle of the dual optical detection components. By using the optical detection component 1 and the optical detection component 2, the relative bow angles ψ1 and ψ2 can be calculated respectively. Combining the distance L between the two components, the distance d from the photosensitive surface of the quadrant photodetector to the center P of the guiding light source can be calculated. Finally, the relative pitch angle θ between the AUV and the center P of the guiding light source can be calculated, and the relative position between the AUV and the center P of the guiding light source can be calculated. l Regarding the calculated relative position information as two observation values, Z1 = [x1, y1, z1] l and Z2 = [x2, y2, z2], l and perform linear Kalman filtering on these two observation values to obtain a relative position estimate value closer to the true value. T T ​

Claims

1. An AUV autonomous docking positioning method based on optical direction finding and ranging, characterized in that: The method includes the following steps: Step 1: A guiding light source is emitted from the center of the docking port of the docking station installed on a ship on the sea surface towards an autonomous underwater vehicle (AUV) in the sea water, and the guiding light source is emitted onto two optical detection components installed on the head of the AUV; Step 2: For each optical detection component, the relative azimuth angle between the AUV and the guiding light source is calculated using the spot imaging method; Step 3: For each optical detection component, according to the relative azimuth angle obtained by the optical detection component in Step 2, the relative position between the AUV and the guiding light source is calculated using the optical direction finding and ranging method; Step 4: According to the relative positions obtained through the two optical detection components respectively in Step 3, the Kalman filtering algorithm is used for data fusion to obtain the final relative position between the AUV and the guiding light source, and the AUV performs positioning according to the final relative position and sails towards the ship with the docking station installed, and finally realizes autonomous docking; In the said Step 1, there is a gap between the two optical detection components. Each optical detection component includes a semi-cylindrical convex lens and a quadrant photodetector. The photosensitive surfaces of the two quadrant photodetectors are on the same plane and both face the sailing direction of the AUV.

2. The AUV autonomous docking and positioning method based on optical direction finding and ranging according to claim 1, wherein: In the said Step 1, the two optical detection components are hermetically installed in the head of the AUV through waterproof and pressure-resistant glass; the semi-cylindrical convex lens of each optical detection component is installed between its respective quadrant photodetector and the waterproof and pressure-resistant glass, and there is no contact between the semi-cylindrical convex lens, the waterproof and pressure-resistant glass and the quadrant photodetector; the spherical surface of the semi-cylindrical convex lens faces the waterproof and pressure-resistant glass, the plane of the semi-cylindrical convex lens is parallel to the photosensitive surface of the quadrant photodetector, and the connection line between the center of the photosensitive surface of each quadrant photodetector and the center of its respective semi-cylindrical convex lens is perpendicular to the photosensitive surface of the quadrant photodetector.

3. A method for autonomous docking and positioning of an AUV based on optical direction finding and ranging according to claim 2, characterized in that: The distance between the semi-cylindrical convex lens and the quadrant photodetector of each optical detection component is the focal length of the semi-cylindrical convex lens; the light of the guiding light source passes through the waterproof and pressure-resistant glass and converges on the photosensitive surfaces of the two quadrant photodetectors respectively through the two semi-cylindrical convex lenses to form circular spots.

4. The AUV autonomous docking and positioning method based on optical direction finding and ranging according to claim 2, wherein: In the said Step 2, for each optical detection component, the relative azimuth angle between the AUV and the guiding light source is calculated using the spot imaging method. The relative azimuth angle includes the relative heading angle and the relative pitch angle, specifically as follows: where, ψ represents the relative bow angle between the autonomous underwater vehicle (AUV) and the guiding light source; n a and n w respectively represent the refractive indices of air and water; ψ′ represents the horizontal exit angle of the guiding light source emitted to the waterproof and pressure-resistant glass; θ represents the relative pitch angle between the AUV and the guiding light source; θ′ represents the vertical exit angle of the guiding light source emitted to the waterproof and pressure-resistant glass.

5. A method for autonomous docking and positioning of an AUV based on optical direction finding and ranging according to claim 4, characterized in that: The horizontal exit angle ψ′ of the guiding light source emitted to the waterproof and pressure-resistant glass is specifically as follows: Wherein, α1 and α2 respectively represent the horizontal incident angle and the horizontal exit angle at which the guiding light source emits light to the semi-circular convex lens; n g represents the refractive index of the waterproof and pressure-resistant glass; l1 represents the distance between the curvature center of the semi-circular convex lens and the plane of the semi-circular convex lens; l2 represents the radius of curvature of the semi-circular convex lens; Δm represents the offset distance of the center of the circular light spot on the quadrant photodetector in the plane where the photosensitive surface of the quadrant photodetector is located; f represents the focal length of the semi-circular convex lens.

6. The AUV autonomous docking and positioning method based on optical direction finding and ranging according to claim 4, characterized in that: The vertical exit angle θ′ of the guiding light source emitted to the waterproof and pressure-resistant glass is specifically as follows: Among them, β1 and β2 respectively represent the vertical incident angle and the vertical exit angle at which the guiding light source emits light to the semi-circular convex lens; n g represents the refractive index of the waterproof and pressure-resistant glass; l1 represents the distance between the curvature center of the semi-circular convex lens and the plane of the semi-circular convex lens; l2 represents the curvature radius of the semi-circular convex lens; Δm represents the offset distance of the center of the circular light spot on the four-quadrant photodetector in the plane where the photosensitive surface of the four-quadrant photodetector is located; Δn represents the offset distance of the center of the circular light spot on the four-quadrant photodetector in the plane perpendicular to the plane where the photosensitive surface of the four-quadrant photodetector is located; f represents the focal length of the semi-circular convex lens.

7. A method for autonomous docking positioning of an AUV based on optical direction finding and ranging according to claim 2, characterized in that: In the third step described above, for each optical detection component, according to the relative azimuth angle obtained by the optical detection component in the second step, the relative position between the autonomous underwater vehicle (AUV) and the guiding light source is calculated using the optical direction and ranging method. Specifically, first, the midpoint of the line connecting the centers of the photosensitive surfaces of two quadrant photodetectors is taken as the reference point. The top end surface of the head of the AUV is perpendicular to its own forward direction and parallel to the photosensitive surface of the quadrant photodetector. The intersection point between the line perpendicular to the photosensitive surface of the quadrant photodetector where the reference point is located and the head end surface of the AUV is taken as the origin. The direction of the line connecting the reference point towards the origin is taken as the positive direction of the x-axis. The intersection point between the line perpendicular to the photosensitive surface of the quadrant photodetector where the center point of the photosensitive surface of the first quadrant photodetector is located and the head end surface of the AUV is taken as the first intersection point. The direction of the line connecting the origin towards the first intersection point is taken as the positive direction of the y-axis. The line perpendicular to the x-axis and y-axis is taken as the z-axis, thus establishing the AUV vehicle coordinate system. The first relative position Z1(x1, y1, z1) between the AUV and the guiding light source is calculated based on the first quadrant photodetector as follows: x1 = d z1 = dtan(θ1) where x1, y1, and z1 respectively represent the x-axis, y-axis, and z-axis coordinates of the first relative position; d represents the distance between the photosensitive surface of the quadrant photodetector and the guiding light source; ψ1 represents the relative heading angle between the AUV and the guiding light source calculated by the first quadrant photodetector; L represents the distance between the centers of the photosensitive surfaces of the two quadrant photodetectors; θ1 represents the relative pitch angle between the AUV and the guiding light source calculated by the first quadrant photodetector. The second relative position Z2(x2, y2, z2) between the AUV and the guiding light source is calculated based on the second quadrant photodetector as follows: x2 = d z2 = dtan(θ2) where x2, y2, and z2 respectively represent the x-axis, y-axis, and z-axis coordinates of the second relative position; ψ2 represents the relative heading angle between the AUV and the guiding light source calculated by the second quadrant photodetector; θ2 represents the relative pitch angle between the AUV and the guiding light source calculated by the second quadrant photodetector.

8. A method for autonomous docking positioning of an AUV based on optical direction finding and ranging according to claim 7, characterized in that: The distance d between the photosensitive surface of the quadrant photodetector and the guiding light source is specifically as follows:

9. A method for autonomous docking and positioning of an AUV based on optical direction finding and ranging according to claim 7, characterized in that: In the fourth step described above, according to the relative positions obtained through the two optical detection components in the third step, the Kalman filtering algorithm is used for data fusion, that is, based on the first relative position Z1(x1, y1, z1) and the second relative position Z2(x2, y2, z2), the Kalman filtering algorithm is used for data fusion to obtain the final relative position between the AUV and the guiding light source.

10. A method for autonomous docking and positioning of an AUV based on optical direction finding and ranging according to claim 1, characterized in that: In the fourth step described above, the state variables in the Kalman filter algorithm are \(X = [x, y, z, u, v, w]\), T , where \(x\), \(y\), and \(z\) are the coordinates of the center of the guiding light source on the \(x\)-axis, \(y\)-axis, and \(z\)-axis in the AUV body coordinate system respectively, and \(u\), \(v\), and \(w\) are the linear velocities of the center of the guiding light source on the \(x\)-axis, \(y\)-axis, and \(z\)-axis in the AUV body coordinate system; the observed variables in the Kalman filter algorithm are \(Z = [Z1, Z2]\). T The state transition matrix \(F\) and the observation matrix \(H\) in the Kalman filter algorithm are specifically as follows: where T represents the update period of the final relative position between the AUV and the guiding light source.

Images