Underwater navigation body back-to-dock guiding control method

Abstract

The invention relates to an underwater navigation body back-to-dock guide control method, which adopts self-adaptive guide control, considers the influence of relative movement and course deviation of a navigation body and a dock, adjusts control parameters in real time through lateral distance information of the navigation body and the dock, and controls the speed of the navigation body approaching a dock trajectory. The navigation body trajectory and the dock trajectory coincide and are merged into the dock. According to the method, the algorithm has real-time performance and flexibility, and the approaching speed of the navigation body to the extension line of the navigation trajectory of the dock can be adjusted in real time according to the lateral distance between the navigation body and the trajectory of the dock; wherein the dock motion information can be completely obtained based on active and passive search of sound signals, and the advantages of stability, small error, large communication distance and the like are achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater navigation system guidance control, and in particular to a docking guidance method for an underwater vehicle navigation system. Background Art

[0002] Underwater vehicles need to bring their own energy when performing tasks. Due to the limitations of size and mass, the energy they carry is very limited. For vehicles that operate for a long time, the area where the mission must be performed must be connected to a carrying platform or a support platform to deploy and recover the vehicle in order to replenish energy, read information, and provide maintenance. The use of surface ships or air support for deployment and recovery cannot guarantee the concealment of the recovery platform's activities. More importantly, due to the interference of surface waves, the deployment and recovery of the vehicle cannot be implemented in high sea conditions. Therefore, it is of great application value to release underwater dock platforms through surface ships and submarines, or to set up dock platforms on the bottom of the water to achieve autonomous docking and recovery of vehicles underwater. The process of autonomous underwater docking of the vehicle is shown in the attached figure. Figure 1 shown.

[0003] The existing docking control schemes mostly use segmented docking algorithms for navigation bodies, that is, according to the distance between the navigation body and the dock, the control parameters are adjusted in stages to achieve the process of the navigation body approaching the dock and returning to the dock smoothly. The algorithm has a certain adjustable space, but it is not real-time. In addition, the existing docking guidance algorithms mostly use visual signals to analyze the motion state. Visual signals have extremely poor stability in underwater environments, large errors, and are greatly restricted by the underwater environment. Summary of the invention

[0004] In order to improve the accuracy and success rate of docking between underwater vehicles and dock platforms, and ensure the normal operation and information transmission of the vehicle underwater for a long time, the present disclosure provides a method for guiding and controlling the underwater vehicle to return to dock. The method adopts adaptive guidance control, considers the influence of the relative motion and heading deviation between the vehicle and the dock, and adjusts the control parameters in real time through the lateral distance information between the vehicle and the dock to control the speed of the vehicle approaching the dock trajectory until the vehicle trajectory coincides with the dock trajectory and docks. In addition, the scheme only needs to use acoustic signals to analyze the motion state, and has the advantages of strong anti-interference, small error, and long signal transmission distance.

[0005] The underwater vehicle docking guidance control method provided by the present disclosure mainly includes the following steps:

[0006] S1, constructing the relative motion relationship between the underwater vehicle and the dock, and calculating the target heading angle of each guidance cycle;

[0007] S2, in each guidance cycle, calculates the lateral distance of the following navigation body relative to the dock trajectory, and calculates the desired target azimuth based on it;

[0008] S3, performing heading control according to the difference between the desired target azimuth and the current target azimuth;

[0009] S4, calculate the depth deviation of the navigation body relative to the dock and perform depth control.

[0010] Furthermore, the step S1 specifically includes:

[0011] Assume η M With η T are the angles between the velocity vectors of the navigation body and the dock and the sighting line, namely the target azimuth and target side angle, q is the target sight angle, ψ M , T are the heading angles of the navigation body and the dock respectively; v M 、v T are the speeds of the navigation body and the dock respectively;

[0012] Then the relative motion relationship equation between the navigation body and the dock is constructed as follows:

[0013]

[0014] Based on the motion parameters of the current cycle and the previous cycle as input, the dock heading angle ψ of each guidance cycle is calculated. T , the method is as follows:

[0015] Assume t 1 ,t 2 The heading angles of the vehicle at each moment are ψ Mt1 , Mt2 , the target azimuths 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 sight angle is calculated as follows:

[0017]

[0018] According to formula (1), at t 2 The moments are:

[0019]

[0020] Get the velocity component of the dock in the horizontal plane:

[0021]

[0022] if but

[0023]

[0024] if but

[0025]

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

[0027]

[0028] Furthermore, the step S2 specifically includes:

[0029] S21, in each guidance cycle, calculate the lateral distance r of the navigation body relative to the dock trajectory s ;

[0030] The calculation methods include:

[0031] Assume the guidance period is T g , the current cycle r s The value of r st1 , then the value of the next cycle r st2 for:

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

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

[0034] Δψ TM =ψ Tt1 -ψ Mt1 , represents the difference between the heading angle of the dock and the heading angle of the navigation body at the same moment;

[0035] S22, based on the lateral distance r of the navigation body relative to the dock trajectory s , calculate the expected target azimuth η gM : At any time t, the expected target azimuth η gMt At this moment r st The relationship is:

[0036]

[0037] Among them, σ 1 , σ 2 is a positive number, which is used to adjust the influence weight of the heading deviation between the navigation body and the dock and η M The rate of change of

[0038] η max It indicates the limit azimuth angle that the detection system can detect, which is selected according to the maximum opening angle of the docking guidance system;

[0039] r ft is the forward distance of the navigation body relative to the dock port on the dock heading at time t;

[0040] d min Minimum working distance for docking guidance control.

[0041] Furthermore, in step S22, in actual application, n gMt The sign of is determined according to the relative situation between the navigation body and the dock.

[0042] Furthermore, the step S3 specifically includes:

[0043] Assume the target azimuth of the current cycle is η Mt , then calculate:

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

[0045] As input command for heading control.

[0046] Furthermore, the step S4 specifically includes:

[0047] Assume that the heading angle of the navigation body is 0-360°, with the true north as the starting point and clockwise as positive; the platform coordinate system of the navigation body is the northeast celestial coordinate system. When the navigation body is placed horizontally 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;

[0048] ψ Mt -The heading angle output by the inertial navigation at time t;

[0049] ψ Tt -Dock heading angle at time t;

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

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

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

[0053] At time t, the forward distance r of the navigation body relative to the dock on the dock heading ft , the lateral distance r of the navigation body relative to the dock trajectory st , then the depth deviation of the navigation body relative to the dock is Δ Ht The calculation is as follows:

[0054]

[0055] r ft =Y T ' t |=|-X Tt sin(ψ Mt -ψ Tt )+Y Tt cos(ψ Mt -ψ Tt )

[0056] r st =|X T ' t |=|X Tt cos(ψ Mt -ψ Tt )+Z Tt sin(ψ Mt -ψ Tt )

[0057] Δ Ht =-Z T ' t =-Z T ' t

[0058] Δ Ht That is the depth control input instruction at time t.

[0059] Compared with the prior art, the beneficial effects of the present invention are: (1) compared with the previous docking algorithm, it is real-time and flexible, and the speed of the navigation body approaching the extension line of the dock navigation trajectory is adjusted in real time according to the lateral distance between the navigation body and the dock trajectory line; (2) the dock motion information can be fully acquired based on the active and passive search of acoustic signals, which has the advantages of stability, small error, and large communication distance; (3) it has good versatility. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] The above and other objects, features and advantages of the present disclosure will become more apparent through a more detailed description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present disclosure.

[0061] Figure 1 This is a schematic diagram of the guidance of underwater vehicle into docking;

[0062] Figure 2 is the relative motion relationship between the navigation body and the dock;

[0063] Figure 3 It is a schematic diagram of trajectory tracking;

[0064] Figure 4 It is the trajectory diagram of the navigation body and the dock head-on;

[0065] Figure 5 This is the trajectory diagram of the navigation body and the dock. DETAILED DESCRIPTION

[0066] The preferred embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to make the present disclosure more thorough and complete, and to fully convey the scope of the present disclosure to those skilled in the art.

[0067] The present disclosure provides a method for guiding and controlling an underwater vehicle to return to dock. The method adopts adaptive guidance control, considers the influence of the relative motion and heading deviation between the vehicle and the dock, adjusts the control parameters in real time through the lateral distance information between the vehicle and the dock, and controls the speed of the vehicle approaching the dock trajectory until the vehicle trajectory coincides with the dock trajectory and enters the dock.

[0068] In an exemplary embodiment, the following steps are mainly included:

[0069] Step 1:

[0070] The relative motion relationship between the navigation body and the dock is shown in the attached figure. Figure 2 As shown. M With η T are the angles between the velocity vectors of the navigation body and the dock and the sighting line, also known as the target azimuth (the advance angle of the navigation body’s velocity vector) and the target side angle (the advance angle of the dock’s velocity vector), q is the target sight angle, ψ M , T are the heading angles of the navigation body and the dock respectively. M 、v T are the speeds of the vehicle and the dock respectively.

[0071] according to Figure 1 The relative motion relationship between the navigation body and the dock is described by the following equation:

[0072]

[0073] Assume t 1 ,t 2 The heading angles of the vehicle at each moment are ψ Mt1 , Mt2 , the target azimuths are η Mt1 , η Mt2 , the target distances are r t1 、r t2 , where η Mt1 , η Mt2 、r t1 、r t2 Provided by the docking guidance system.

[0074] The target sight angle is calculated as follows:

[0075]

[0076] According to formula (1), at t 2 The moments are:

[0077]

[0078] Get the velocity component of the dock in the horizontal plane:

[0079]

[0080] if but

[0081]

[0082] if but

[0083]

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

[0085]

[0086] That is, η detected by the detection system Mt1 , η Mt2 、r t1 、r t2 and V measured by the inertial navigation Mt ,ψ Mt , the dock motion parameter ψ is calculated through the relative motion relationship equation of formula (1) Tt2 , as shown in formula (3).

[0087] Dock heading angle ψ at any time T The calculation needs to use the motion parameters of the current moment and the previous moment as input.

[0088] Step 2: Heading control

[0089] As attached Figure 3 As shown, r f is the forward distance of the navigation body relative to the dock on the dock heading, r s is the lateral distance of the navigation body relative to the dock track line. s When it is larger, a larger η should be taken M value, so that the navigation body quickly approaches the extension line of the dock navigation track. s When η decreases, M In order to make the navigation body's track line finally coincide with the dock's navigation track line, the influence of the deviation between the navigation body's heading and the dock's heading should also be considered.

[0090] by Figure 3 As shown in the figure, the dock navigation trajectory is on the left side of the navigation body, and the dock heading is on the right side of the navigation body heading. Assume that the guidance period is T g , the current cycle r st The value of r st1 , then the next cycle r st The values ​​are:

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

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

[0093] Δψ TM =ψ Tt1 -ψ Mt1 , represents the difference between the heading angle of the dock and the heading angle of the navigation body at the same moment;

[0094] In this formula, ψ Tt1 It is calculated by formula (3).

[0095] In summary, the expected target azimuth η at any time t is given by the exponential law gMt At this moment, the lateral distance r of the navigation body relative to the dock trajectory st Changing relationships:

[0096]

[0097] σ 1 , σ 2 is a positive number, which is used to adjust the influence weight of the heading deviation between the navigation body and the dock and η M The rate of change of

[0098] η max Select according to the maximum opening angle of the docking guidance system.

[0099] d min It is the minimum working distance of the docking guidance system.

[0100] In practical application, η gMt The sign should be determined according to the relative situation between the navigation body and the dock.

[0101] Assume the target azimuth of the current cycle is η Mt , then the guidance command input to the heading stability control system is:

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

[0103] Step 3: Depth Control

[0104] The heading angle of the navigation body is 0~360°, with true north as the starting point and clockwise as positive; the platform coordinate system of the navigation body is the northeast celestial coordinate system. When the navigation body is placed horizontally and the carrier coordinate system coincides with the platform coordinate system, the y-axis of the carrier coordinate system points forward and the z-axis points vertically upward.

[0105] ψ Mt - Heading angle output by inertial navigation;

[0106] ψ Tt - Dock heading angle;

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

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

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

[0110] The forward distance r of the navigation body relative to the dock on the dock heading ft , the lateral distance r of the navigation body relative to the dock trajectory st , the depth deviation of the navigation body relative to the dock Δ Ht The calculation is as follows:

[0111]

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

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

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

[0115] Δ Ht This is the input command of the depth stability control system.

[0116] Construct a simulation model to convert the guidance command Δψ of the heading stability control system t , the input command of the depth stability control system Δ Ht Input, generate the simulation image of the head-on docking trajectory of the navigation body and the dock, such as Figure 4 Generate a simulation image of the trajectory of the navigation body and the dock, as shown in Figure 5 As shown in the figure, it can be seen that the navigation body has excellent guidance effect in both relative motion states.

[0117] In this embodiment, based on Figure 2 The relative motion relationship between the navigation body and the dock is used to derive the target heading angle calculation of formula (3), and then based on the conclusion of formula (3), the guidance command Δψ of the heading stability control system of formula (5) is derived. t ;

[0118] The real-time performance and flexibility of the algorithm in this embodiment are mainly reflected in the parameter η M , whose value depends on the lateral distance r between the navigation body and the dock trajectory s Real-time adjustment is made to control the speed at which the navigation body approaches the extension line of the dock navigation track;

[0119] The dock motion information can all be obtained based on active and passive search of acoustic signals, which has the advantages of stability, small error, and long communication distance.

[0120] The above technical scheme is only an exemplary embodiment of the present invention. For those skilled in the art, it is easy to make various types of improvements or modifications based on the application methods and principles disclosed in the present invention, and it is not limited to the method described in the above specific embodiment of the present invention. Therefore, the method described above is only preferred and does not have a restrictive meaning.

Claims

1. A method for guiding and controlling an underwater vehicle to return to dock, comprising the following steps: S1, constructing the relative motion relationship between the underwater vehicle and the dock, and calculating the target heading angle of each guidance cycle; S2, in each guidance cycle, calculates the lateral distance of the following navigation body relative to the dock trajectory, and calculates the desired target azimuth based on it; S3, performing heading control according to the difference between the desired target azimuth and the current target azimuth; S4, calculate the depth deviation of the navigation body relative to the dock and perform depth control.

2. The method according to claim 1, characterized in that The step S1 specifically includes: Assume η M With η T are the angles between the velocity vectors of the navigation body and the dock and the sighting line, namely the target azimuth and target side angle, q is the target sight angle, ψ M , T are the heading angles of the navigation body and the dock respectively; v M 、v T are the speeds of the navigation body and the dock respectively; Then the relative motion relationship equation between the navigation body and the dock is constructed as follows: Based on the motion parameters of the current cycle and the previous cycle as input, the dock heading angle ψ of each guidance cycle is calculated. T , the method is as follows: Assume that the heading angles of the vehicle at time t1 and t2 are ψ Mt1 , Mt2 , the target azimuths are η 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 sight angle is calculated as follows: According to formula (1), at time t2: Get the velocity component of the dock in the horizontal plane: 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: The step S2 specifically includes: S21, in each guidance cycle, calculate the lateral distance r of the navigation body relative to the dock trajectory s ; The calculation methods include: Assume the guidance period is T g , the current cycle r s The value of r st1 , then the value of the next cycle r 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 , represents the difference between the heading angle of the dock and the heading angle of the navigation body at the same moment; S22, based on the lateral distance r of the navigation body relative to the dock trajectory s , calculate the expected target azimuth η gM : At any time t, the expected target azimuth η gMt At this moment r st The relationship is: Among them, σ1 and σ2 are positive numbers, which are used to adjust the influence weight of the heading deviation between the navigation body and the dock and η M The rate of change of η max It indicates the limit azimuth angle that the detection system can detect, which is selected according to the maximum opening angle of the docking guidance system; r ft is the forward distance of the navigation body relative to the dock port on the dock heading at time t; d min Minimum working distance for docking guidance control.

4. The method according to claim 3, characterized in that In the step S22, in actual application, n gMt The sign of is determined according to the relative situation between the navigation body and the dock.

5. The method according to claim 3, characterized in that: The step S3 specifically includes: Assume the target azimuth of the current cycle is η Mt , then calculate: Dp t =the gMt -or Mt As input command for heading control.

6. The method according to claim 1 or 5, characterized in that: The step S4 specifically includes: Assume that the heading angle of the navigation body is 0-360°, with the true north as the starting point and clockwise as positive; the platform coordinate system of the navigation body is the northeast celestial coordinate system. When the navigation body is placed horizontally 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 -The heading angle output by the inertial navigation at time t; ψ Tt -Dock heading angle at time t; 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 navigation body relative to the dock on the dock heading ft , the lateral distance r of the navigation body relative to the dock trajectory st , then the depth deviation of the navigation body relative to the dock is Δ 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 That is the depth control input instruction at time t.

Images