A method for guiding and controlling underwater vehicles back to dock

Abstract

A method for guiding and controlling an underwater vehicle back to its dock is disclosed. This method employs adaptive guidance control, considering the influence of the relative motion and heading deviation between the vehicle and the dock. By using lateral distance information between the vehicle and the dock, control parameters are adjusted in real time to control the speed at which the vehicle approaches the dock's trajectory line until the vehicle's trajectory line coincides with the dock's trajectory line and the vehicle enters the dock. This method and its algorithm are real-time and flexible, capable of adjusting the speed at which the vehicle approaches the extended dock trajectory line in real time based on the lateral distance between the vehicle and the dock's trajectory line. Furthermore, the dock's motion information can be entirely acquired through active and passive acoustic signal search, offering advantages such as stability, low error, and long communication range.

Description

Technical Field

[0001] This invention relates to the field of underwater navigation system guidance and control technology, and in particular to a docking guidance method for an underwater navigation system. Background Technology

[0002] Underwater vehicles require their own energy sources to perform missions. However, due to size and mass limitations, the energy they can carry is very limited. For vehicles operating for extended periods, the mission area must be served by a launch platform or by connecting with a support platform for deployment and recovery to replenish energy, retrieve information, and perform maintenance. Using surface ships or aerial support for deployment and recovery cannot guarantee the stealth of the recovery platform's activities. More importantly, it is susceptible to interference from surface waves, making deployment and recovery impossible in high sea states. Therefore, deploying underwater dock platforms from surface ships or submarines, or setting up dock platforms underwater, to achieve autonomous docking and recovery of underwater vehicles has significant application value. The process of autonomous underwater docking of a vehicle is shown in the attached figure. Figure 1 As shown.

[0003] Existing docking control schemes mostly employ segmented docking algorithms, which adjust control parameters in stages based on the distance between the vehicle and the dock to achieve the process of the vehicle approaching the dock and successfully docking. While these algorithms offer some adjustability, they lack real-time capability. Furthermore, existing docking guidance algorithms often rely on visual signal analysis of motion states, which suffers from extremely poor stability, significant errors, and is heavily constrained by the underwater environment. Summary of the Invention

[0004] To improve the accuracy and success rate of docking between underwater vehicles and dock platforms, and to ensure the long-term normal operation and information transmission of the vehicles underwater, this disclosure provides a dock return guidance control method for underwater vehicles. This method employs adaptive guidance control, considering the influence of the relative motion and heading deviation between the vehicle and the dock. By using lateral distance information between the vehicle and the dock, control parameters are adjusted in real time to control the speed at which the vehicle approaches the dock's trajectory line until the vehicle's trajectory line coincides with the dock's trajectory line and the vehicle enters the dock. Furthermore, this solution only requires acoustic signal analysis of the motion state, and has advantages such as strong anti-interference capability, small error, and long signal transmission distance.

[0005] The underwater vehicle docking guidance and control method disclosed herein mainly includes the following steps:

[0006] S1, establish the relative motion relationship between the underwater vehicle and the dock, and calculate the target heading angle for each guidance cycle;

[0007] S2, in each guidance cycle, calculate the lateral distance of the following vehicle relative to the dock trajectory line, and calculate the desired target azimuth angle accordingly;

[0008] S3, perform heading control based on the difference between the desired target azimuth and the current target azimuth;

[0009] S4 calculates the depth deviation of the vessel relative to the dock opening and performs depth control.

[0010] Furthermore, step S1 specifically includes:

[0011] Let η M With η T Here, θ represents the angle between the velocity vectors of the vehicle and the dock and the line of sight, i.e., the target azimuth and the target hull angle, respectively; q represents the target line-of-sight angle; and ψ represents the target line-of-sight angle. M ψ T These are the heading angles of the vehicle and the dock, respectively; v M v T These are the speeds of the vehicle and the dock, respectively.

[0012] The equations relating the relative motion between the ship and the dock are then constructed as follows:

[0013]

[0014] Using the motion parameters of the current cycle and the previous cycle as inputs, the dock heading angle ψ for each guidance cycle is calculated. T The method is as follows:

[0015] Let the heading angles of the vehicle at times t1 and t2 be ψ. Mt1 ψ Mt2 The target azimuth angles are η Mt1 η Mt2 The target distances are r t1 r t2 , where η Mt1 η Mt2 r t1 r t2 Provided by the docking guidance system;

[0016] The target line-of-sight angle is calculated as follows:

[0017]

[0018] According to equation (1), at time t2 we have:

[0019]

[0020] The velocity component of the dock on the horizontal plane is obtained:

[0021]

[0022] if but

[0023]

[0024] if but

[0025]

[0026] The target heading angle at time t2 is calculated as follows:

[0027]

[0028] Furthermore, step S2 specifically includes:

[0029] S21, in each guidance cycle, calculate the lateral distance r of the vehicle relative to the dock trajectory line. s The calculation methods include:

[0030] Let the guidance period be T. g Current period r s The value is r st1 Then the value r in the next period st2 for:

[0031] r st2 =r st1 -v Mt1 T g sin(ψ Tt1 -ψ Mt1 ) = r st1 -v Mt1 T g sinΔψ TM

[0032] When Δψ TM When the angle is small, r st2 =r st1 -v Mt1 T g Δψ TM ;

[0033] Δψ TM =ψ Tt1 -ψ Mt1 , which represents the difference between the dock's heading angle and the ship's heading angle at the same moment;

[0034] S22, based on the lateral distance r of the vehicle relative to the dock trajectory line. s Calculate the desired target azimuth angle η gM At any given time t, the desired target azimuth angle η gMt With that time r st The relationship is:

[0035]

[0036] Where σ1 and σ2 are positive numbers, used to adjust the influence weights of the deviation between the sailing body and the dock, and η M The rate of change;

[0037] η max This indicates the extreme azimuth angle that the detection system can detect, which is selected based on the maximum opening angle of the docking guidance system;

[0038] r ft Let t be the forward distance of the vehicle relative to the dock entrance in the dock heading;

[0039] d min This is the minimum working distance for docking guidance control.

[0040] Furthermore, in step S22, in practical applications, η gMt The symbol is determined based on the relative position of the navigating body and the dock.

[0041] Furthermore, step S3 specifically includes:

[0042] Let the target azimuth angle of the current period be η. Mt Then calculate:

[0043] Δψ t =η gMt -η Mt (5)

[0044] As input commands for heading control.

[0045] Furthermore, step S4 specifically includes:

[0046] Assume the heading angle of the vehicle body is 0 to 360°, with true north as the starting point and clockwise as positive; the platform coordinate system of the vehicle body is the northeast-sky coordinate system. When the vehicle body is laid flat and the carrier coordinate system coincides with the platform coordinate system, the y-axis of the carrier coordinate system is forward and the z-axis is vertically upward.

[0047] ψ Mt -t is the heading angle output by the inertial navigation system.

[0048] ψ Tt -t time dock heading angle;

[0049] X Tt -The component of the relative distance in the x-direction of the platform coordinate system at time t;

[0050] Y Tt -The component of the relative distance in the y-direction of the platform coordinate system at time t;

[0051] Z Tt -The component of the relative distance in the z-direction of the platform coordinate system at time t;

[0052] At time t, the forward distance r of the ship relative to the dock entrance on the dock heading. ft The lateral distance r of the vehicle relative to the dock trajectory line st The depth deviation Δ of the ship relative to the dock is then... Ht The calculation is as follows:

[0053]

[0054] r ft =|Y′ Tt |=|-X Tt sin(ψ Mt -ψ Tt )+Y Tt cos(ψ Mt -ψ Tt )|

[0055] r st =|X′ Tt |=|X Tt cos(ψ Mt -ψ Tt )+Z Tt sin(ψ Mt -ψ Tt )|

[0056] Δ Ht =-Z′ Tt =-Z′ Tt

[0057] Δ Ht This refers to the depth control input command at time t.

[0058] Compared with the prior art, the beneficial effects of this disclosure are: (1) Compared with the previous docking algorithm, it has real-time performance and flexibility, and adjusts the speed at which the vehicle approaches the extension line of the docking trajectory in real time according to the lateral distance between the vehicle and the dock trajectory line; (2) The docking motion information can be obtained entirely based on the active and passive search of the acoustic signal, which has advantages such as stability, small error and large communication distance; (3) It has good versatility. Attached Figure Description

[0059] The above and other objects, features and advantages of this disclosure will become more apparent from the more detailed description of exemplary embodiments of this disclosure taken in conjunction with the accompanying drawings, in which the same reference numerals generally represent the same components.

[0060] Figure 1 A schematic diagram illustrating the docking guidance for an underwater vehicle.

[0061] Figure 2 The relative motion between the ship and the dock;

[0062] Figure 3 This is a schematic diagram of trajectory tracking;

[0063] Figure 4 A diagram showing the trajectory of the ship's approaching dock.

[0064] Figure 5 This is a diagram showing the trajectory of the vehicle's tail-to-tail docking with the ship. Detailed Implementation

[0065] Preferred embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While preferred embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure will be thorough and complete, and will fully convey the scope of the present disclosure to those skilled in the art.

[0066] This disclosure provides a method for guiding and controlling an underwater vehicle back to dock. The method employs adaptive guidance control, considering the effects of relative motion and heading deviation between the vehicle and the dock. Using lateral distance information between the vehicle and the dock, control parameters are adjusted in real time to control the speed at which the vehicle approaches the dock's trajectory line until the vehicle's trajectory line coincides with the dock's trajectory line and the vehicle enters the dock.

[0067] In one exemplary implementation, the main steps include:

[0068] Step 1:

[0069] The relative motion between the ship and the dock is shown in the attached figure. Figure 2 As shown. η M With η T These are the angles between the velocity vectors of the vehicle and the dock and the line of sight, respectively, also known as the target azimuth (the lead angle of the vehicle's velocity vector) and the target hull angle (the lead angle of the dock's velocity vector), where q is the target line-of-sight angle and ψ is the target line-of-sight angle. M ψ T These are the heading angles of the vehicle and the dock, respectively. M v T These are the speeds of the vehicle and the dock, respectively.

[0070] according to Figure 1 The relative motion between the ship and the dock shown is described by the following equations:

[0071]

[0072] Let the heading angles of the vehicle at times t1 and t2 be ψ. Mt1 ψ Mt2 The target azimuth angles are η Mt1 η Mt2 The target distances are rt1 r t2 , where η Mt1 η Mt2 r t1 r t2 Provided by the docking guidance system.

[0073] The target line-of-sight angle is calculated as follows:

[0074]

[0075] According to formula (1), at time t2 we have:

[0076]

[0077] The velocity component of the dock on the horizontal plane is obtained:

[0078]

[0079] if but

[0080]

[0081] if but

[0082]

[0083] The target heading angle is calculated as follows:

[0084]

[0085] That is, η detected by the detection system Mt1 η Mt2 r t1 r t2 and V measured by inertial navigation Mt ,ψ Mt By using the relative motion equation in equation (1), the dock motion parameter ψ can be calculated. Tt2 As shown in equation (3).

[0086] The dock heading angle ψ at any time T The calculation requires the motion parameters of the current moment and the previous moment as inputs.

[0087] Step 2, Heading Control

[0088] As attached Figure 3 As shown, r f r is the forward distance of the navigable body relative to the dock entrance in the dock heading. s This is the lateral distance of the vehicle relative to the dock's trajectory line. When r s When the value is large, a larger value η should be taken.M The value causes the vehicle to rapidly approach the extended trajectory line of the dock, when r s When η decreases, M The value should decrease accordingly. To ensure that the track of the vehicle eventually coincides with the track of the dock, the impact of the deviation between the course of the vehicle and the course of the dock should also be considered.

[0089] by Figure 3 As shown, taking the example where the dock's navigation trajectory is to the left of the ship and the dock's heading is to the right of the ship's heading, let the guidance period be T. g Current period r st The value is r st1 Then the next period r st The value is:

[0090] r st2 =r st1 -v Mt1 T g sin(ψ Tt1 -ψ Mt1 ) = r st1 -v Mt1 T g sinΔψ TM

[0091] When Δψ TM When the angle is small, r st2 =r st1 -v Mt1 T g Δψ TM

[0092] Δψ TM =ψ Tt1 -ψ Mt1 , which represents the difference between the dock's heading angle and the ship's heading angle at the same moment;

[0093] ψ in the formula Tt1 It can be calculated using equation (3).

[0094] In summary, the expected target azimuth angle η at any time t is given according to the exponential law. gMt The lateral distance r of the vehicle relative to the dock trajectory line at that moment st The changing relationship:

[0095]

[0096] σ1 and σ2 are positive numbers used to adjust the influence weights of the deviation between the sailing body and the dock's heading, as well as η. M The rate of change;

[0097] η max Selected based on the maximum opening angle of the docking guidance system.

[0098] d min This is the minimum working distance for the docking guidance system.

[0099] In practical applications, η gMt The symbol should be determined based on the relative position of the navigating body and the dock.

[0100] Let the target azimuth angle of the current period be η. Mt Then the guidance command input to the heading stability control system is:

[0101] Δψ t =η gMt -η Mt (5)

[0102] Step 3, Depth Control

[0103] The heading angle of the vehicle is 0 to 360°, with true north as the starting point and clockwise as positive. The platform coordinate system of the vehicle is the northeast-sky coordinate system. When the vehicle is laid flat and the carrier coordinate system coincides with the platform coordinate system, the y-axis of the carrier coordinate system is forward and the z-axis is vertically upward.

[0104] ψ Mt - The heading angle output by the inertial navigation system;

[0105] ψ Tt -Dock heading angle;

[0106] X Tt - The component of the relative distance in the x-direction of the platform coordinate system;

[0107] Y Tt - The component of the relative distance in the y-direction of the platform coordinate system;

[0108] Z Tt - The component of the relative distance in the z-direction of the platform coordinate system.

[0109] The forward distance r of the hull relative to the dock entrance in the dock heading ft lateral distance r of the vehicle relative to the dock trajectory line st Depth deviation of the vessel relative to the dock opening Δ Ht The calculation is as follows:

[0110]

[0111] r ft =|Y′ Tt |=|-X Tt sin(ψ Mt -ψ Tt )+Y Tt cos(ψ Mt -ψ Tt )|

[0112] r st =|X′ Tt |=|X Tt cos(ψ Mt -ψ Tt )+Z Tt sin(ψ Mt -ψ Tt )|

[0113] Δ Ht =-Z′ Tt =-Z′ Tt

[0114] Δ Ht This refers to the input commands for a deep stability control system.

[0115] A simulation model is constructed to implement the guidance command Δψ of the heading stability control system. t The input command Δ of the deep stability control system Ht Input, generate a simulation image of the head-on collision trajectory between the vehicle and the dock, such as Figure 4 As shown. A simulated image of the trajectory of the vehicle and the docking dock is generated, as shown. Figure 5 As shown in the image, the vehicle exhibits excellent guidance performance under both relative motion conditions.

[0116] In this embodiment, based on Figure 2 Based on the relative motion between the vehicle and the dock, the target heading angle calculation formula (3) is derived. Then, based on the conclusion of formula (3), the guidance command Δψ of the heading stability control system formula (5) is derived. t ;

[0117] The real-time performance and flexibility of the algorithm in this embodiment are mainly reflected in the parameter η. M Its value depends on the lateral distance r between the navigating body and the dock's trajectory line. s Real-time adjustments are made to control how quickly the vehicle approaches the extended trajectory line of the dock.

[0118] The dock movement information can be obtained entirely through active and passive search based on acoustic signals, which has advantages such as stability, small error, and long communication distance.

[0119] The above technical solutions are merely exemplary embodiments of the present invention. For those skilled in the art, based on the application methods and principles disclosed in the present invention, it is easy to make various types of improvements or modifications, and not limited to the methods described in the specific embodiments of the present invention. Therefore, the methods described above are merely preferred and not restrictive.

Claims

1. A method for guiding and controlling an underwater vehicle back to dock, comprising the following steps: S1, establish the relative motion relationship between the underwater vehicle and the dock, and calculate the target heading angle for each guidance cycle; S2, in each guidance cycle, calculate the lateral distance of the following vehicle relative to the dock trajectory line, and calculate the desired target azimuth angle accordingly; S3, perform heading control based on the difference between the desired target azimuth and the current target azimuth; S4, calculate the depth deviation of the navigable body relative to the dock opening, and perform depth control; Step S2 specifically includes: Let v M v T ψ represents the speed of the vehicle and the dock, respectively. M ψ T η represents the heading angles of the vehicle and the dock, respectively; M With η T Let ψ1 and ψ2 be the angles between the velocity vectors of the vehicle and the dock and the line of sight, respectively, i.e., the target azimuth and the target hull angle. The heading angles of the vehicle at times t1 and t2 are ψ1 and ψ2, respectively. Mt1 ψ Mt2 ; S21, in each guidance cycle, calculate the lateral distance r of the vehicle relative to the dock trajectory line. s ; The calculation methods include: Let the guidance period be T. g Current period r s The value is r st1 Then the value r in the next period st2 for: r st2 =r st1 -v Mt1 T g sin(ψ Tt1 -ψ Mt1 )=r st1 -v Mt1 T g sinΔψ TM When Δψ TM When the angle is small, r st2 =r st1 -v Mt1 T g Δψ TM ; Δψ TM =ψ Tt1 -ψ Mt1 , which represents the difference between the dock's heading angle and the ship's heading angle at the same moment; S22, based on the lateral distance r of the vehicle relative to the dock trajectory line. s Calculate the desired target azimuth angle η gM : At any given time t, the desired target azimuth angle η gMt With that time r st The relationship is: Where σ1 and σ2 are positive numbers, used to adjust the influence weights of the deviation between the sailing body and the dock, and η M The rate of change; η max This indicates the extreme azimuth angle that the detection system can detect, which is selected based on the maximum opening angle of the docking guidance system; r ft Let t be the forward distance of the vehicle relative to the dock entrance in the dock heading; d min This is the minimum working distance for docking guidance control; Step S3 specifically includes: Let the target azimuth angle of the current period be η. Mt Then calculate: Dp t =the gMt -or Mt As input commands for heading control.

2. The method according to claim 1, characterized in that, Step S1 specifically includes: Let η M With η T These are the angles between the velocity vectors of the vehicle and the dock and the aiming line, i.e., the target azimuth and the target hull angle, respectively, and q is the target line-of-sight angle; The equations relating the relative motion between the ship and the dock are then constructed as follows: Using the motion parameters of the current cycle and the previous cycle as inputs, the dock heading angle ψ for each guidance cycle is calculated. T The method is as follows: Let the target azimuth angles at times t1 and t2 be η respectively. Mt1 η Mt2 The target distances are r t1 r t2 , where η Mt1 η Mt2 r t1 r t2 Provided by the docking guidance system; The target line-of-sight angle is calculated as follows: According to equation (1), at time t2 we have: The velocity component of the dock on the horizontal plane is obtained: if but if but The target heading angle at time t2 is calculated as follows:

3. The method according to claim 1, characterized in that, In step S22, in practical applications, η gMt The symbol is determined based on the relative position of the navigating body and the dock.

4. The method according to claim 1, characterized in that, Step S4 specifically includes: Assume the heading angle of the vehicle body is 0 to 360°, with true north as the starting point and clockwise as positive; the platform coordinate system of the vehicle body is the northeast-sky coordinate system. When the vehicle body is laid flat and the carrier coordinate system coincides with the platform coordinate system, the y-axis of the carrier coordinate system is forward and the z-axis is vertically upward. ψ Mt -t is the heading angle output by the inertial navigation system. ψ Tt -t time dock heading angle; X Tt -The component of the relative distance in the x-direction of the platform coordinate system at time t; Y Tt -The component of the relative distance in the y-direction of the platform coordinate system at time t; Z Tt -The component of the relative distance in the z-direction of the platform coordinate system at time t; At time t, the forward distance r of the ship relative to the dock entrance on the dock heading. ft The lateral distance r of the vehicle relative to the dock trajectory line st The depth deviation Δ of the ship relative to the dock is then... Ht The calculation is as follows: r ft =|Y′ Tt |=|-X Tt sin(ψ Mt -ψ Tt )+Y Tt cos(ψ Mt -ψ Tt )| r st =|X′ Tt |=|X Tt cos(ψ Mt -ψ Tt )+Z Tt sin(ψ Mt -ψ Tt )| Δ Ht =-Z′ Tt =-Z′ Tt Δ Ht This refers to the depth control input command at time t.

Images