A Method for Positioning and Docking of a Laser-Guided Underwater Autonomous Vehicle

Abstract

The present invention discloses a method for positioning and docking an underwater autonomous vehicle guided by laser. Based on the light intensity information received by the receiver prism group of the RSS light intensity detector, an error equation for positioning is constructed, and a method for analytically calculating the current AUV target positioning value; by iteratively updating the deflection angle φ corresponding to the offset radius r in the new coordinate system of the optical path direction, a new error equation is constructed again, thereby improving the positioning accuracy of the AUV and reducing the positioning error. Based on the independently constructed positioning algorithm, the present invention designs a node guidance docking scheme based on the received light intensity. By utilizing the characteristics of strong laser directivity, while improving the concealment of the docking station to a certain extent, it eliminates the dependence of the AUV docking on optical images, instead uses the signal intensity of the receiver for navigation docking, and still has a good docking guidance success rate in the case of long distance and weak turbulence, improving the autonomy of the AUV.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater vehicle positioning, and particularly relates to a laser-guided underwater autonomous navigation positioning and docking method. Background Art

[0002] With the increasing demand for ocean exploration, autonomous underwater vehicles (AUVs) have gradually become a research hotspot. However, the dual effects of the low transmission rate of underwater acoustic communication and the limited battery capacity carried by AUVs have severely restricted the development and application of AUVs. The many advantages of underwater wireless optical communication (UWOC) undoubtedly make it the most promising alternative for underwater communication.

[0003] Positioning is one of the basic functions that AUVs should possess. Traditional underwater positioning mostly uses sonars, including long baseline (LBL), short baseline (SBL), ultra-short baseline (USBL), and depth sounders, etc. Sonars have good performance in large-scale obstacle avoidance, but have poor accuracy in small-scale areas, low data update rates, are vulnerable to interference from the water surface, the bottom, or other sound reflection sources, may have ambiguities caused by multi-path propagation in shallow and medium-depth waters, and there are limitations on the minimum measurement distance, etc. Although there are also many that use optical positioning, most of them use cameras and other devices as signal receiving components. This solution has high requirements for the resolution of the camera. At the same time, light attenuates rapidly underwater, and the underwater light transmittance is very poor. Using a camera as the receiving component, it is very difficult to distinguish the specific position of the AUV.

[0004] After positioning, what needs to be done is to dock the AUV for the next mission. In the early days, AUV recovery was carried out using a recovery mother ship and corresponding recovery equipment to recover the AUV, and its operation required manual instructions and the manipulation of relevant equipment. However, with the increasing number of AUVs put into use and the demand for exploration scope, this method with too much manual operation is cumbersome and inefficient, which has led to the idea of autonomous docking of AUVs. Currently, AUV docking can be roughly divided into the following two categories according to the design method of its docking station: one is contact-guided docking, such as the Odyssey IIB AUV system in the United States uses physical structure design for capture docking; the other is non-contact docking, which is commonly a fairing-guided docking. And because the fairing-guided docking has higher fault tolerance, better practicability, and relatively simple layout, it is the commonly used solution for guiding the autonomous docking of AUVs currently.

[0005] At present, the actual application of AUV autonomous docking mainly still relies on acoustics for AUV navigation. However, the accuracy of acoustic docking is poor and the error rate is high. With the development of underwater optics, some achievements have been made in AUV docking navigation based on optics. Some researchers have gradually adopted optics for AUV docking, but mainly based on vision to complete docking. This requires some additional lighting equipment to provide visual information for the AUV, resulting in poor concealment performance of the docking port. There is also a problem that whether the camera can capture the docking port will have a serious impact on the guidance, and strict requirements are imposed on the initial pose of the AUV. Summary of the Invention

[0006] In view of the above deficiencies in the prior art, a laser-guided underwater autonomous navigation positioning and docking method provided by the present invention solves the problems of poor accuracy and high error rate of existing AUV docking relying on acoustics, as well as poor concealment of the docking port and strict requirements for the initial pose of the AUV in vision-based AUV docking.

[0007] To achieve the above invention objective, the technical solution adopted by the present invention is: a laser-guided underwater autonomous vehicle positioning and docking method, including the following steps:

[0008] S1. Estimate the optical path direction distance according to the laser sent by SOBS received by the AUV.

[0009] S2. Estimate the deflection angle based on the maximum light intensity detected by the AUV.

[0010] S3. Based on the optical path direction distance and the deflection angle, construct an error equation for system positioning.

[0011] S4. Solve the error equation to obtain the preliminary positioning of the AUV, and correct its angular error to obtain the current AUV positioning.

[0012] S5. During the navigation of the AUV, at intervals, make the AUV continuously re-determine the AUV heading according to the current AUV positioning until it is recovered by the fairing of the docking port.

[0013] Wherein, SOBS is arranged at the docking port, and the laser generators at the center of each plane thereof emit several non-overlapping lasers, which are received by the prisms in the prism group of the AUV receiver and transmit the received light intensity information to the AUV.

[0014] The deflection angle is the included angle between the line connecting the intersection point of the plane of the AUV receiving prism group and the center of the AUV receiving prism group and the center of the AUV receiving prism group and the line connecting the center of the 5th prism in the AUV receiving prism group and the center of the AUV receiving prism group, where the laser source direction sent by SOBS is towards.

[0015] Furthermore, the optical path direction distance L in the step S1 is as follows:

[0016]

[0017] In the formula, L 1 is the distance when the AUV first receives the laser scanning coverage of the SOBS node, and L 2 is the distance when the AUV receives the laser scanning coverage of the SOBS node for the last time. is the angle between the plane where the AUV receiver prism group is located and the normal line of the initial scanning direction, and α 1 and α 2 are respectively the azimuth angles of the laser optical paths of L 1 and L 2 respectively, and R 0 is the radius of the AUV receiver prism group.

[0018] Furthermore, the step S2 is specifically as follows:

[0019] S21. Construct a quadratic function relationship between the light intensity near the prism that receives the maximum light intensity and the prism position angle;

[0020]

[0021] In the formula, α′ x is the deflection angle of the x-th prism, and a, b, and c are all parameters in the quadratic function RSS i (α′ x );

[0022] S22. Substitute the prism that receives the maximum light intensity and the adjacent prisms into the quadratic function relationship for fitting and solve to obtain an approximate value estimate of the deflection angle α′.

[0023]

[0024] Furthermore, the step S3 is specifically as follows:

[0025] S31. Rotate the coordinate system where the AUV receiver prism group is located so that the positive direction of the rotated x-axis is consistent with the laser source orientation;

[0026] S32. In the rotated new coordinate system, determine the relationship between the optical path direction distance L i and the offset radius r pi of the center of the i-th prism in the AUV receiver prism group as:

[0027] L i = L + x pi

[0028]

[0029] In the formula, L is the distance in the optical path direction, and φ is the included angle formed by the offset radius r in the new coordinate system and the y-axis, and is the vector connecting the center O of the SOBS node and the center P of the prism group rx0 , and α′ is the deflection angle; p i is the coordinate in the new coordinate system, expressed as [x pi , y pi , z pi ;

[0030] S33. Take the light intensity detected by the AUV receiver prism group as the result value of the light field distribution function Ω(r, L), perform resolution on it to obtain (r, L) that satisfies this value, obtain several (r, L) combinations, perform linear fitting on them, and construct a linear relationship f Ii (L) with r as the dependent variable and L as the independent variable, and then obtain the offset radius r of the light intensity detected by the AUV receiver prism group Ii ;

[0031] r Ii = f Ii (L i )

[0032] In the formula, r is the offset radius and L is the distance in the optical path direction;

[0033] S34. Based on the offset radius r Ii and r pi , determine the error value of a single prism, and then obtain the error equation e(r, L) for system positioning:

[0034]

[0035] In the formula, M 1 is the set of prism serial numbers in the receiver prism group that can detect light intensity, and the current positioning value of the AUV is the combination of independent variables (r, L) when the error equation e(r, L) takes the minimum value.

[0036] Furthermore, the specific steps of step S4 are as follows:

[0037] S41. Calculate the error equation, and take the combination of independent variables (r, L) when the error equation e(r, L) takes the minimum value as the preliminary positioning of the AUV;

[0038] S42. Fit the received light intensity of the AUV receiver prism group corresponding to different φ at (r, L) through Monte Carlo, and compare it with the actual received light intensity to determine the angle when the absolute error between the received light intensity of the AUV receiver prism group obtained by Monte Carlo fitting and the actual received light intensity is minimized.

[0039]

[0040] In the formula, P i is the actual received optical power of the i-th prism in the AUV receiver prism group. corresponds to φ j The optical power of the i-th prism in the AUV receiver prism group at this time is a function of the included angle φ, and φ j is the included angle corresponding to the j-th prism.

[0041] S43. Based on the included angle Update the error equation and solve it to obtain the current AUV positioning.

[0042] Furthermore, the method for determining the AUV heading in step S5 is as follows:

[0043] A1. Determine the positional relationship between the AUV positions obtained in the previous and subsequent acquisitions.

[0044]

[0045] In the formula, is the position of the AUV relative to the SOBS node at time t 1 , is the position of the AUV relative to the coordinate origin at time t 0 ; Δt is the time interval between the previous and subsequent positionings; v A is the normal speed of the AUV; is the AUV heading calculated after the positioning ends at time t 0 ; is the ocean current at the position of the AUV at time t 0 , and is regarded as a uniform motion within the time of Δt;

[0046] A2. Based on the determined positional relationship, judge whether the AUV is traveling along the optical path direction or going to the middle;

[0047] If traveling along the optical path, go to step A3;

[0048] If going to the middle, go to step A4;

[0049] A3. According to the offset radius r at time t 0 ​t0 , determine the heading information of the AUV;

[0050] A4. Based on the current AUV positioning and the position of the axis of the fairing, determine the heading information of the AUV.

[0051] Further, in step A1, the ocean current intensity v at the position of the AUV at time t 0 is: hl as follows:

[0052]

[0053] Further, in step A3, when the offset radius r at time t 0 makes it impossible to eliminate the optical path offset before the next AUV positioning is obtained, the AUV heading t0 is: as follows:

[0054]

[0055] where φ t0 is the declination of r in the new coordinate system solved at time t 0 , and A′ is the transpose matrix of the coordinate transformation matrix A; t0 0

[0056] When the offset radius r at time t 0 makes it possible to eliminate the optical path offset before the next AUV positioning is obtained, the AUV heading t0 is: as follows:

[0057]

[0058] where O is the origin of coordinates, that is, the center of the upper circle of the fairing, and P t0 is the AUV positioning solved at time t 0 , OP′ t0 is the projection of P t0 in the optical path direction, and O′ is a point on the line segment OP′ t0 and satisfies |O′P t0 | = Δt * v A , is the direction vector of the current communication optical path.

[0059] Further, in step A4, the AUV travels towards the axis of the fairing, and the interval time Δt for the next AUV timing is determined according to the distance between the position P t0 solved at the current moment and the axis of the fairing. The determined heading information is:

[0060]

[0061]

[0062] In the formula, is the displacement that the AUV needs to travel currently.

[0063] The beneficial effects of the present invention are as follows:

[0064] (1) The present invention proposes a scheme for constructing an error equation based on the light intensity information received by a circular receiving prism group RSS light intensity detector on an AUV, analytically calculating the current positioning value of the AUV, and then obtaining the heading of the AUV by directly removing the middle based on the current positioning value, and finally completing the docking of the AUV.

[0065] (2) Compared with the traditional positioning through underwater acoustic communication, the present invention uses UWOC for positioning, overcoming the problem of low transmission rate of underwater acoustic communication; compared with positioning schemes based on optics but using cameras as signal receiving devices, etc., this scheme uses an RSS light intensity detector, which overcomes problems such as high requirements for the resolution of cameras, and at the same time, the RRS light intensity detector has a stronger connection with the UWOC network.

[0066] (3) The present invention also iteratively updates the deflection angle φ corresponding to the offset radius r in the new coordinate system of the optical path direction, and reconstructs the error equation using the updated deflection angle φ, thereby improving the positioning accuracy.

[0067] (4) Compared with the AUV autonomous docking based on vision, the docking process in this method is based on a self-constructed positioning algorithm, and a node guidance docking scheme based on received light intensity is designed, overcoming problems such as poor concealment of the docking port and strict requirements for the initial attitude of the AUV caused by vision-based docking, and still having a good docking guidance success rate at relatively long distances and in weak turbulence conditions, improving the autonomy of the AUV. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 is a flowchart of an underwater self-navigation positioning and docking method based on guidance.

[0069] Figure 2 is a schematic diagram of the relative position relationship between the AUV receiving prism group and the SOBS from a top view in an ideal situation.

[0070] Figure 3 is a schematic diagram of three relative position relationships between the AUV receiving prism group and the SOBS from a top view in a real situation.

[0071] Figure 4When performing declination estimation, the plane point O' on the circular receiving prism group of the AUV where the laser source in the current orientation strikes. Schematic diagram of this plane.

[0072] Figure 5 P is the center of the AUV receiver prism group rx0 , the intersection point O' of the laser source in the current orientation on the plane S where the AUV receiver prism group is located rx , the center P of the No. 5 prism A , the projection O' of the center of the receiving group on the optical path L , tetrahedron structure diagram formed by the four points.

[0073] Figure 6 Internal relationship diagram of the AUV receiver prism group.

[0074] Figure 7 For t 0 After the AUV completes the positioning and solution at time t, the relationship diagram formed by its solution result and the coordinate origin. Specific implementation manner

[0075] The following describes the specific implementation manner of the present invention to facilitate those skilled in the art of the present technology to understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation manner. For those of ordinary skill in the art of the present technology, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions and creations using the concept of the present invention are within the scope of protection.

[0076] Example 1:

[0077] The embodiment of the present invention provides a method for positioning and docking a laser-guided underwater autonomous vehicle, as Figure 1 shown, including the following steps:

[0078] S1. Estimate the optical path direction distance according to the laser sent by the SOBS received by the AUV;

[0079] S2. Estimate the declination based on the maximum light intensity detected by the AUV;

[0080] S3. Based on the optical path direction distance and the declination, construct an error equation for system positioning;

[0081] S4. Solve the error equation to obtain the preliminary positioning of the AUV, and correct its angular error to obtain the current AUV positioning;

[0082] S5. During the navigation of the AUV, within the interval time, make the AUV continuously re-determine the AUV heading according to the current AUV positioning until it is recovered by the fairing of the docking port;

[0083] Among them, the SOBS is set at the docking port. The laser generators at the center of each plane of the SOBS emit several non-overlapping lasers, which are received by the prisms in the AUV receiver prism group and transmit the received light intensity information to the AUV.

[0084] The deflection angle is the included angle between the line connecting the intersection point of the laser source direction sent by the SOBS and the plane of the AUV receiving prism group and the center of the AUV receiving prism group and the line connecting the center of the No. 5 prism in the AUV receiving prism group and the center of the AUV receiving prism group.

[0085] In the embodiment of the present invention, the system structure for realizing the positioning and docking method of the underwater autonomous vehicle includes a spherical-like optical base station (SOBS) with multi-laser scanning collaboration located at the docking port and the AUV that needs to be positioned and docked. The SOBS is composed of 14 planes. The upper and lower planes are both regular hexagons, and the middle two layers are respectively composed of six isosceles trapezoids. Each plane has a laser emitter at its center, and emits multiple non-overlapping lasers that are received by the AUV. There is a circular prism group composed of 8 receiving prisms on the AUV to receive the light intensity signal of the SOBS, and it will transmit the received light intensity information to the AUV.

[0086] In step S1 of the embodiment of the present invention, in an ideal situation, as Figure 2 shown, the AUV receiver prism group is directly facing the SOBS node. At this time, both ends of the AUV receiver prism group are exactly on a circle with the center of a SOBS node as the center and the scanning distance L as the radius. The optical paths of the first scan and the last scan form a triangular-like structure with the AUV receiver prism group. Based on this, a system of equations related to the scanning distance L at this time can be obtained:

[0087]

[0088]

[0089] In the formula, is the included angle formed by the optical paths of the light intensity that can be detected first and the light intensity that can be detected last by the AUV receiver prism group, and its value can be calculated from the difference in the azimuth angles α of the two scans; R 0 is the radius of the AUV receiving prism group.

[0090] However, due to the interference of the underwater environment (such as water flow, etc.), there is an offset between the AUV and the ideal position. Therefore, when the actual SOBS node scans and captures the AUV, the AUV receiver prism group is most likely not exactly aligned with the concentric circles with the SOBS node as the center. As Figure 3 shown, there are three position situations. By calculating the geometric relationships of these three positions, it can be obtained that:

[0091]

[0092]

[0093] In the formula, L 1 is the distance when the AUV first receives the laser scanning coverage of the SOBS node; L 2 is the distance when the AUV receives the laser scanning coverage of the SOBS node for the last time, is the included angle between the prism group of the AUV receiver and the plane where the initial scanning direction is located. In this system, since is the YOZ plane and its normal is in the x direction, that is, the meridian direction where the system is located. Therefore, the AUV can obtain an approximate value of, α 1 and α 2 are respectively the azimuth angles of the laser optical paths of L 1 and L 2 respectively. Therefore, the optical path direction distance L in step S1 of the embodiment of the present invention is:

[0094]

[0095] In the formula, L 1 is the distance when the AUV first receives the laser scanning coverage of the SOBS node, L 2 is the distance when the AUV receives the laser scanning coverage of the SOBS node for the last time, is the included angle between the prism group of the AUV receiver and the plane where the initial scanning direction is located, α 1 and α 2 are respectively the azimuth angles of the laser optical paths of L 1 and L 2 respectively, and R 0 is the radius of the prism group of the AUV receiver.

[0096] In step S2 of the embodiment of the present invention, as Figure 4 shown, denote the deflection angle α′ = ∠O′P rx0 P A . Among them, O′ represents the intersection point of the laser source in the current direction on the plane S rx where the AUV receiver prism group is located, and P rx0 is the midpoint of the AUV receiver prism group. Since when the AUV receives scanning, its receiver prism group is respectively parallel to the meridian and gravity where it is located, it can be considered that the AUV receiver prism group is parallel to the XOZ plane. P A is a point on the receiver, and it is related to P rx0The unit vectors of the connection line and the positive direction of the x-axis are the same. Since the system is set to have M = 8 receiver prisms, at this time P A happens to be at the center of the 5th prism set by the system.

[0097] In the embodiment of the present invention, since the receiver prism group has a circular structure and the light field intensity I of the laser is negatively correlated with the offset radius r when L is determined, based on this, step S2 is specifically as follows:

[0098] S21. Construct a quadratic function relationship between the light intensity near the prism receiving the maximum light intensity and the prism position angle;

[0099]

[0100] In the formula, α′ x is the deflection angle of the x-th prism, and a, b, and c are all parameters in the quadratic function RSS i (α′ x );

[0101] Substitute the prism detecting the maximum light intensity and its adjacent prisms in the AUV receiver prism group into the fitting:

[0102]

[0103] In the formula, j is the serial number of the prism detecting the maximum / minimum light intensity in the receiver prism group.

[0104] S22. Substitute the prism receiving the maximum light intensity and its adjacent prisms into the quadratic function relationship for fitting and solve to obtain an approximate value estimate of the deflection angle α′;

[0105]

[0106] In the embodiment of the present invention, the reason for using the prism detecting the maximum light intensity and its adjacent prisms in the AUV receiver prism group to solve instead of using the prism detecting the minimum light intensity and its adjacent prisms to solve is that there is a minimum threshold for the receiving prisms of the system. If the minimum value is used, it is possible that the detection values of the minimum value prism and its adjacent prisms are both 0, and the situation where the estimated value of α′ cannot be obtained may occur.

[0107] In step S3 of the embodiment of the present invention, the core idea of constructing the error equation is to calculate an offset radius r of the field intensity using the detected light intensity of the prism Ii , and at the same time obtain r about the center of the prism group using the geometric relationship of the prism group pi , and the two obtain the error value of a single prism, and then obtain the error equation e(r, L):

[0108]

[0109] To obtain the above error equation, step S3 of the embodiment of the present invention is specifically as follows:

[0110] S31. Rotate the coordinate system where the AUV receiver prism group is located so that the positive direction of the rotated x-axis is consistent with the direction of the laser source;

[0111] S32. In the rotated new coordinate system, determine the optical path direction distance L i and the offset radius r pi The relationship is:

[0112] L i = L + x pi

[0113]

[0114] In the formula, L is the optical path direction distance, and φ is The included angle formed by the offset radius r and the y-axis in the new coordinate system, and Is the vector connecting the center O of the SOBS node and the center P of the prism group rx0 , and α' is the deflection angle; p i Is the coordinate in the new coordinate system, expressed as [x pi , y pi , z pi ;

[0115] S33. Take the light intensity detected by the AUV receiver prism group as the result value of the light field distribution function Ω(r, L), solve it to obtain (r, L) that satisfies this value, get several (r, L) combinations, and perform linear fitting on them to construct a linear relationship f Ii (L), and then obtain the offset radius r of the light intensity detected by the AUV receiver prism group Ii ;

[0116] r Ii = f Ii (L i )

[0117] In the formula, r is the offset radius and L is the optical path direction distance;

[0118] S34. Based on the offset radius r Ii and r pi , determine the error value of a single prism, and then obtain the error equation e(r, L) of the system positioning:

[0119]

[0120] In the formula, M 1is the set of prism serial numbers for which the light intensity can be detected in the receiver prism group, and the current positioning value of the AUV is the combination of the independent variables (r, L) when the error equation e(r, L) takes the minimum value.

[0121] Specifically, in this embodiment, the specific process of constructing the error equation in step S3 is provided:

[0122] Since the subsequent parameters are all related to the laser optical path and the offset radius, for the convenience of solution, the coordinate axes of the AUV receiver prism group are rotated. The overall x-axis is rotated around the SOBS node to the laser orientation, and the positive direction of the x-axis is consistent with the laser source orientation. Before rotation, the direction vector of the laser optical path is:

[0123]

[0124] In the formula, α and θ are respectively the azimuth angle and elevation angle of the current laser orientation.

[0125] Thus, the coordinate transformation matrix A is obtained as:

[0126]

[0127] In the new coordinate system, determine the relationship between the coordinates of the center P of the AUV receiver prism group, the distance L of the optical path direction, and the optical path offset r, as rx0 shown. Combining with the positional relationship in the α′ estimation, it can be obtained that O′, P Figure 5 rx0 A and the projection O′ of the receiving group center on the optical path L form a tetrahedral structure. Among them, the plane where O′, P rx0 A are located is the plane where the receiving prism group is located ∠O′P rx0 P A is the previously estimated α′, O′O′ L is the laser optical path direction, and O′ L P rx0 is the distance from the prism group center to the laser optical path, that is, the offset radius r. At this time, as Figure 6 shown, there is the following vector relationship in the new coordinate system:

[0128]

[0129] According to the positional relationship between O′ and the prism group in the α′ estimation, in the original coordinate system:

[0130]

[0131] Convert it to the coordinates in the new coordinate system: ​​​

[0132]

[0133] Meanwhile, in the new coordinate system, only the x-axis has values for the coordinates. Therefore,

[0134]

[0135] From the above two equations, it can be seen that in the new coordinate system, the angle φ formed by the offset radius r and the y-axis is:

[0136]

[0137] Thus, in the new coordinate system, the coordinates are:

[0138]

[0139] Next, consider the positional relationship of the center P rxi of the i-th prism:

[0140]

[0141] According to the geometric relationship, it can be obtained that:

[0142]

[0143] Among them, is the connection line between the center of the AUV receiver prism group and the center of the i-th prism of the receiver, and the angle with the positive x-axis direction of the original coordinate system.

[0144] Therefore, use p i to represent its transformation into the coordinates of the new coordinate system:

[0145]

[0146] In summary, corresponding to the center of the AUV receiver prism group, the relationship between the optical path direction distance L i and the offset radius r pi is:

[0147] L i = L + x pi

[0148]

[0149] Next, for r IiSolve it by using the light intensity detected by the receiving prism as the result value of the light field distribution function Ω(r, L). By calculating (r, L) that satisfies this value, multiple groups of (r, L) combinations can be obtained. Performing linear fitting on these values can obtain a linear relationship f with r as the dependent variable and L as the independent variable Ii (L), at this time:

[0150] r Ii = f Ii (L i )

[0151] Substituting it can obtain the expression of the error equation for system positioning:

[0152]

[0153] And the current positioning value of the AUV is the combination of independent variables (r, L) when this error equation obtains the minimum value. Therefore, the positioning value of the AUV can be obtained by solving the minimum value of the error equation

[0154] In step S4 of the embodiment of the present invention, the value of φ used in the construction of the error equation in step S3 adopts α′, and α′ is estimated according to the light intensity distribution received by the AUV receiving prism group, and there is a large deviation between its estimated value and the actual value. At the same time, it is not effectively corrected in the subsequent construction of the error equation, so there is a certain deviation in the subsequent positioning. Therefore, to correct the positioning value, the principle is mainly to calculate and correct the deflection angle φ when calculating the optical path distance L and offset radius r of the AUV by using the error equation

[0155] Based on this, step S4 of the embodiment of the present invention is specifically:

[0156] S41. Calculate the error equation, and use the combination of independent variables (r, L) when the error equation e(r, L) obtains the minimum value as the preliminary positioning of the AUV

[0157] S42. Fit the received light intensity of the AUV receiver prism group corresponding to different φ at (r, L) through Monte Carlo, and compare it with the actual received light intensity to determine the angle when the absolute error between the received light intensity of the AUV receiver prism group obtained by Monte Carlo fitting and the actual received light intensity is the smallest

[0158]

[0159] In the formula, P i is the actual received optical power of the i-th prism in the AUV receiver prism group, corresponds to φ jThe optical power of the i-th prism in the AUV receiver prism group at a certain time, which is a function of the included angle φ, φ j is the included angle corresponding to the j-th prism;

[0160] S43. Based on the included angle Update the error equation and solve it to obtain the current AUV positioning;

[0161] Specifically, use Monte Carlo to simulate and fit the calculation for the interval of φ j ∈[0, 2π), with an interval of 1°. Finally, solve for φ that minimizes the expression value, and then substitute it into the following formula to reconstruct the error equation:

[0162]

[0163] As before, obtain a combination of independent variables (r, L) by calculating the minimum value of the error equation, and this (r, L) combination is the positioning value of the current AUV.

[0164] In step S5 of the embodiment of the present invention, the method for guiding the AUV to return to the dock is mainly optical path guidance to return to the dock. This method mainly uses the underwater SOBS node to construct an optical path for the AUV. After that, the directional direction information carried by the communication optical path, combined with the position information of the AUV relative to the fairing, the AUV obtains its own heading information, thereby completing the docking operation.

[0165] The method for determining the AUV heading in step S5 of the embodiment of the present invention is as follows:

[0166] A1. Determine the positional relationship between the AUV positions obtained in two consecutive acquisitions of AUV positioning;

[0167]

[0168] In the formula, is the position of the AUV relative to the SOBS node at time t 1 ; is the position of the AUV relative to the coordinate origin at time t 0 ; Δt is the time interval between two consecutive positionings; v A is the conventional speed of the AUV; is the AUV heading calculated after the positioning ends at time t 0 ; is the ocean current at the position where the AUV is located at time t 0 , and is regarded as a uniform motion within the time of Δt;

[0169] A2. Based on the determined positional relationship, judge whether the AUV heading is to travel along the optical path direction or to travel to the middle;

[0170] If traveling along the optical path, proceed to step A3;

[0171] If traveling towards the center, proceed to step A4;

[0172] A3. Based on the offset radius r 0 at time t t0 , determine the AUV heading information;

[0173] A4. Based on the current AUV position and the position of the fairing central axis, determine the AUV heading information.

[0174] In step A1 of this embodiment, as Figure 7 shown, since the AUV is in the process of calculating its position and there is an offset effect caused by ocean currents, time-consuming iterations cannot be used; at the same time, the AUV's return to the dock is a continuous positioning and traveling process. As a series of subsequent errors accumulate, the initial distance estimate will no longer be applicable to the optical path distance estimate, and at this time, the error equation will have multiple solutions; and because the AUV's return to the dock is a continuous positioning process and there is a certain correlation between its front and back, in the docking guidance, the positioning information before the previous voyage is used to select and correct the result obtained from this positioning.

[0175] Therefore, the positional relationship between the AUV's positions when obtaining AUV positioning twice before and after is:

[0176]

[0177] From this relationship, it can be seen that when the errors that occur when the system uses the front and back positioning information mainly stem from the rigidity of the ocean current, the ocean current intensity v 0 at the position where the AUV is located at time t hl is expressed as:

[0178]

[0179] The AUV heading in the formula is obtained by directly going to the center.

[0180] In this embodiment, the main steps of going to the center are as follows: When the AUV is at a relatively long distance, its heading is to sail along the optical path. When the AUV travels within a certain distance from the fairing, if the corresponding fairing opening angle of the AUV is less than the maximum collision-free opening angle θ' at this time, it continues to sail along the current optical path; if the corresponding fairing opening angle of the AUV is greater than the maximum collision-free opening angle θ' at this time, that is, if it continues to sail along the current communication optical path, it will collide with the fairing, then the heading of the AUV is no longer to sail along the optical path at this time, but is adjusted to sail towards the central axis of the fairing, that is, to go to the center. The maximum collision-free opening angle θ' is the angle between the center P' of the receiving prism group of the AUV, the center O of the upper cross-section of the fairing (i.e., the line segment OP'), and the central axis of the fairing (i.e., the line segment OO'). After calculation, it can be known that θ'≈37.6610°.

[0181] In step A3 of this embodiment, during the stage of sailing along the optical path, at this time, since it is very difficult for the communication optical path to exactly irradiate the center of the AUV receiver prism group. If it sails simply along the direction of the optical path L at this time, it is easy for the AUV to get out of the detectable range of the light field, resulting in rescan and increasing the system time consumption and instability. Therefore, at this time, the heading should give priority to eliminating or reducing the offset radius of the current communication optical path, so that the optical path of the SOBS node after operation is basically at the center of the AUV receiver prism group.

[0182] When t 0 the offset radius r t0 is too large and the optical path offset cannot be eliminated before the next AUV positioning is obtained, the heading of the AUV is:

[0183]

[0184] where φ t0 is the deflection angle of r 0 in the new coordinate system solved at time t t0 and A' is the transpose matrix of the coordinate transformation matrix A;

[0185] When t 0 the offset radius r t0 makes it possible to eliminate the optical path offset before the next AUV positioning is obtained. At this time, the heading should consider adding a component in the direction of the optical path. At this time, the heading of the AUV is:

[0186]

[0187] where O is the origin of coordinates, that is, the center of the upper circle of the fairing frustum, and P t0 is the AUV positioning solved at time t 0 which is a solved result and not the actual position of the AUV at time t 0 the line segment OP't0 The projection in the optical path direction, at this time P' t0 is the projection of P in the optical path direction. At this time, P' t0 corresponds to the offset radius r calculated by the AUV at time t t0 corresponding to time t 0 of the AUV at time t. The offset radius r t0 , and O' is a point on the line segment OP' t0 and satisfies |O'P t0 | = Δt * v A , is the direction vector of the current communication optical path.

[0188] In step A4 of this embodiment, when the AUV is traveling to the middle, it no longer sails along the optical path direction, but towards the central axis of the fairing. Moreover, the next positioning interval time Δt is no longer a fixed time, which is determined by the distance between the position P t0 calculated at the current moment and the central axis of the fairing. Since the system sets the center of the fairing as the coordinate origin and its orientation as the latitude direction of the system, it can be known that in the old coordinate system, the direction vector of the central axis of the fairing is:

[0189]

[0190] From the above formula, the displacement that needs to be traveled currently is

[0191]

[0192] Thus, the heading information is determined as:

[0193]

[0194]

[0195] wherein, is the displacement that the AUV needs to travel currently.

[0196] In the description of the present invention, it should be understood that the orientation or positional relationship indicated by the terms "center", "thickness", "upper", "lower", "horizontal", "top", "bottom", "inner", "outer", "radial", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus cannot be construed as a limitation to the present invention. In addition, the terms "first", "second", "third" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly specifying the quantity of technical features. Therefore, the features defined by "first", "second", "third" may explicitly or implicitly include one or more of such features.

Claims

1. A method for positioning and docking of a laser-guided underwater autonomous vehicle, characterized in that, it includes the following steps: S1. Estimate the optical path direction distance according to the laser sent by SOBS received by the AUV; S2. Estimate the deflection angle based on the maximum light intensity detected by the AUV; S3. Based on the optical path direction distance and the deflection angle, construct an error equation for system positioning; S4. Solve the error equation to obtain the preliminary positioning of the AUV, and correct the angle error thereof to obtain the current AUV positioning; S5. During the navigation of the AUV, within an interval time, enable the AUV to continuously re-determine the AUV heading according to the current AUV positioning until it is recovered by the fairing of the docking port; wherein, SOBS is arranged at the docking port, and the laser generators at the center of each plane thereof emit a number of non-overlapping lasers that are received by the prisms in the prism group of the AUV receiver, and transmit the received light intensity information to the AUV; the deflection angle is the included angle between the line connecting the intersection point of the laser source orientation sent by SOBS and the plane of the AUV receiving prism group and the center of the AUV receiving prism group and the line connecting the center of the 5th prism in the AUV receiving prism group and the center of the AUV receiving prism group.

2. The method for positioning and docking of a laser-guided underwater autonomous vehicle according to claim 1, characterized in that, the optical path direction distance L in the step S1 is: ； Wherein, is the distance when the AUV first receives the laser scan coverage of the SOBS node, is the distance when the AUV receives the laser scan coverage of the SOBS node for the last time, is the included angle between the prism group of the AUV receiver and the plane where the initial scan direction is located and the normal line, and are respectively and the azimuth angles of the laser optical paths of the two, is the radius of the prism group of the AUV receiver.

3. The method for positioning and docking of a laser-guided underwater autonomous vehicle according to claim 1, characterized in that, the step S2 is specifically: S21. Construct a quadratic function relationship between the light intensity near the prism receiving the maximum light intensity and the prism position angle; ； In the formula, is the deflection angle of the x-th prism, are all quadratic functions parameters in; S22. Substitute the prism that receives the maximum light intensity and the adjacent prism into the quadratic function relationship for fitting, and solve to obtain an approximate value estimate of the deflection angle. ​ 。 4. The method for positioning and docking of a laser-guided underwater autonomous vehicle according to claim 1, characterized in that, the step S3 is specifically: S31. Rotate the coordinate system where the AUV receiver prism group is located so that the positive direction of the rotated x-axis is consistent with the laser source orientation; S32. In the rotated new coordinate system, determine the optical path direction distance of the center of the th prism in the AUV receiver prism group and the offset radius The relationship is: ； ； Wherein, is the optical path direction distance, is the offset radius in the new coordinate system and the included angle formed with the axis, and is the center of the SOBS node and the center of the prism group the connected vector, is the deflection angle; is the coordinate in the new coordinate system, expressed as ; S33. Use the light intensity detected by the AUV receiver prism group as the result value of the light field distribution function to solve the light field distribution function to obtain that satisfies this result value, and get several combinations. After linearly fitting several , construct a linear relationship with the dependent variable being and the independent variable being , and then obtain the offset radius of the light intensity detected by the AUV receiver prism group ; ； In the formula, is the offset radius, is the optical path direction distance; S34. Based on the offset radius and , determine the error value of a single prism, and then obtain the error equation for system positioning : ； In the formula, is the set of prism numbers in the receiver prism group that can detect the light intensity, and the current positioning value of the AUV is the combination of independent variables when the minimum value of is taken.

5. The method for positioning and docking of a laser-guided underwater autonomous vehicle according to claim 4, characterized in that, the step S4 is specifically: S41. Calculate the error equation and use the combination of the independent variables when the error equation takes the minimum value as the preliminary positioning of the AUV; S42. By performing Monte Carlo fitting on the received light intensity of the AUV receiver prism group corresponding to different at and comparing the received light intensity of the AUV receiver prism group corresponding to different at with the actual received light intensity, determine the angle when the absolute error between the received light intensity of the AUV receiver prism group obtained by Monte Carlo fitting and the actual received light intensity is minimized; ； In the formula, is the actual received optical power of the i-th prism in the AUV receiver prism group, corresponds to when the optical power of the -th prism in the AUV receiver prism group, and its value is a function of the included angle , is the -th prism corresponding included angle; S43. Based on the included angle Update the error equation and solve it to obtain the current AUV positioning.

6. The method for positioning and docking of a laser-guided underwater autonomous vehicle according to claim 5, characterized in that, the method for determining the AUV heading in the step S5 is: A1. Determine the positional relationship between the positions when the AUV positioning is obtained twice before and after; ； Wherein, is the position of the AUV relative to the SOBS node at time is the position of the AUV relative to the origin of coordinates at time is the time interval between two consecutive positionings; is the normal speed of the AUV; is the heading of the AUV calculated after the positioning ends at time is the ocean current at the position of the AUV at time , and is considered to be in uniform motion within the time period. A2. Based on the determined positional relationship, judge whether the AUV heading is to travel along the optical path direction or to travel to the middle; If traveling along the optical path, go to step A3; If traveling to the middle, go to step A4; A3. According to the offset radius at a moment , determine the AUV heading information; A4. Based on the current AUV positioning and the position of the fairing central axis, determine the AUV heading information.

7. The method for positioning and docking of a laser-guided underwater autonomous vehicle according to claim 6, characterized in that, In the said step A1, the sea current intensity at the position where the AUV is located at the moment is: 。 8. The method for positioning and docking of a laser-guided underwater autonomous vehicle according to claim 6, characterized in that, In step A3, when the offset radius at the moment makes it impossible to eliminate the optical path offset before the next AUV positioning, the AUV heading is: ； In the formula, is the declination angle in the new coordinate system solved at time , is the transpose matrix of the coordinate transformation matrix A; When Offset radius at a moment enables the elimination of the optical path offset before the next acquisition of AUV positioning, the AUV heading is as follows: ； In the formula, is the AUV positioning solution at a certain moment, is the projection in the optical path direction, is a point on the line segment and satisfies , is the direction vector of the current communication optical path.

9. The method for positioning and docking of a laser-guided underwater autonomous vehicle according to claim 7, characterized in that, In the step A4, the AUV travels towards the central axis of the fairing and within the time interval between the next acquisitions of the AUV timing Based on the position calculated according to the current moment Determined by the distance from the central axis of the fairing, the determined course information is: ； ； Wherein, is the displacement that the AUV needs to travel currently.

Images