Water surface and underwater heterogeneous cooperative control method and device based on preset time interference observer

Abstract

The invention provides a water surface and underwater heterogeneous cooperative control method and device based on a preset time disturbance observer. The control method mainly comprises the steps that a water surface unmanned ship controller calculates an expected control torque according to a specified expected trajectory, the actual state information of an unmanned ship body and a compensation value obtained by the observer; the underwater unmanned vehicle controller calculates an expected control moment according to the expected trajectory obtained by the guidance method, the actual state information of the water surface unmanned ship, the actual state information of the underwater unmanned vehicle body and a compensation value obtained by the observer; and heterogeneous cooperative control of the water surface unmanned ship and the underwater unmanned vehicle is realized. According to the method, guidance of water surface and underwater heterogeneous cooperative control is completed by fusing vertical projection and a sight line method; according to the improved self-adaptive integral terminal sliding mode control method, the external interference is estimated by using the preset time interference observer and is compensated in the controller to improve the anti-interference capability of the system, and the robustness and rapidity of the system are improved while the tracking precision of the system is improved by the provided improved self-adaptive integral terminal sliding mode control method.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned system trajectory tracking control, and in particular to a surface and underwater heterogeneous collaborative control method and device based on a preset time interference observer. Background Art

[0002] In recent decades, marine technology has attracted much attention due to the increase in investment in ocean exploration. When faced with high-risk tasks such as seabed mapping, ocean measurement, minesweeping, maritime rescue and environmental monitoring, humans often use surface unmanned ships and underwater unmanned vehicles to perform. Therefore, surface and underwater collaborative control technology has gradually become a research hotspot in the current marine engineering field.

[0003] In the complexity and variability of the marine environment, when surface unmanned ships and underwater unmanned vehicles perform tasks in a collaborative manner, their motion systems exhibit strong nonlinear characteristics and are affected by uncertain interference such as wind, waves, and currents. How to effectively deal with these interferences and ensure navigation safety is a key issue that needs to be solved urgently. In the face of these interferences, sliding mode control has attracted much attention due to its strong robustness, but reducing system jitter while increasing response speed is a challenge faced by sliding mode control systems. In addition, model predictive control is a commonly used technology, but due to its large amount of calculation, its real-time performance in collaborative control is not ideal. PID technology may produce overshoot and cannot meet the safety of collaborative control. Therefore, it is necessary to study methods that can meet the real-time and safety of collaborative control of surface and underwater unmanned systems, which is crucial to improving marine target detection capabilities.

[0004] Specifically, in existing work, most of the research on collaborative control focuses on homogeneous systems. As a cross-domain operation method, surface and underwater collaborative control has the advantages of high work efficiency and the ability to perform complex tasks. There are currently two more commonly used technical methods:

[0005] (1) By applying distributed model predictive control technology, the coordinated control of surface and underwater systems is achieved. However, this technology requires processing a large amount of data during the calculation process, and the ship equipment has certain limitations in computing power. Therefore, the implementation of the algorithm requires additional computing time, which inevitably affects the real-time response and accuracy of the coordinated control.

[0006] (2) The traditional PID control technology is used to achieve the coordinated control of the surface and underwater systems. Although this method is effective in some cases, it requires frequent adjustment of the control parameters to meet the expected value of the coordinated effect. In addition, when encountering harsh external environmental conditions, the adaptive ability of PID control is insufficient, which has an adverse impact on the safety of coordinated control.

[0007] In view of this, the present invention proposes a surface and underwater heterogeneous collaborative control method and device based on a preset time interference observer. Summary of the invention

[0008] The purpose of the present invention is to propose a surface and underwater heterogeneous collaborative control method and device based on a preset time interference observer, and to achieve efficient collaboration between surface and underwater systems based on a guidance strategy based on a vertical projection fusion line of sight method; by designing a preset time interference observer, unknown interference can be quickly estimated and compensated, and the process is not affected by the initial state; at the same time, through an improved convergence law design, the response speed of the system is significantly improved, so that surface unmanned ships and underwater unmanned vehicles can perform collaborative tracking tasks safely, accurately and stably.

[0009] To achieve the above object, the technical solution of the present invention is as follows:

[0010] A surface and underwater heterogeneous cooperative control method based on a preset time interference observer comprises the following steps:

[0011] Step 1: The shore-based equipment sends the expected trajectory to the surface unmanned vessel according to the mission objectives;

[0012] Step 2: The surface unmanned ship obtains the desired trajectory through the radio module and obtains the state information of the unmanned ship through the positioning system;

[0013] Step 3: The underwater unmanned vehicle obtains the status information of the surface unmanned ship through the hydroacoustic communication device, and obtains the status information of the underwater unmanned vehicle body through the fiber optic inertial navigation device;

[0014] Step 4, the surface unmanned ship controller analyzes and calculates according to the specified expected trajectory, the actual state information of the unmanned ship body, and the compensation value obtained by the preset time interference observer to obtain the expected control torque; the underwater unmanned vehicle controller analyzes and calculates according to the expected trajectory obtained by the guidance method, the actual state information of the surface unmanned ship, the actual state information of the underwater unmanned vehicle body, and the compensation value obtained by the preset time interference observer to obtain the expected control torque;

[0015] Step 5: The surface unmanned ship controller and the underwater unmanned vehicle controller respectively send the calculated control torque instructions to the surface unmanned ship controller and the underwater unmanned vehicle thrusters to achieve heterogeneous collaborative control of the surface unmanned ship and the underwater unmanned vehicle;

[0016] Step 6: When the surface unmanned ship and the underwater unmanned vehicle navigate to the mission location in coordination, they activate the side-scan sonar and downward-looking multi-beam sonar to detect targets in the mission area.

[0017] Preferably, the obtaining of the status information of the unmanned ship body through the positioning system is specifically: obtaining the status information of the unmanned ship body through the GPS positioning system, wherein the position longitude and latitude are converted into the position vector of the surface unmanned ship in inertial coordinates through coordinate conversion.

[0018] Preferably, the acquisition of the underwater unmanned vehicle body state information also includes: the underwater unmanned vehicle calculates and obtains the body state information through fiber optic inertial navigation and a Doppler odometer, and corrects the calculated body state information through the positioning function of the underwater acoustic communicator.

[0019] Preferably, step S4 includes the following specific steps:

[0020] Step 4.1, establishing a hybrid motion model of a surface unmanned ship and an underwater unmanned vehicle; the hybrid motion model of the surface unmanned ship and the underwater unmanned vehicle includes a nonlinear kinematic model and a nonlinear dynamic model of a surface unmanned ship-underwater unmanned vehicle hybrid system;

[0021] Step 4.2, constructing the trajectory deviation of the surface unmanned ship relative to the reference trajectory and the reference trajectory deviation of the underwater unmanned vehicle relative to the surface unmanned ship by fusing the vertical projections through the line of sight method;

[0022] Step 4.3, construct a preset time interference observer to estimate the external interference factors on the operation of the surface and underwater heterogeneous collaborative system;

[0023] Step 4.4: Based on the external disturbance estimation value of the preset time disturbance observer and the improved adaptive integral terminal sliding mode control method, the control law is designed to calculate the expected control torque.

[0024] Preferably, the nonlinear kinematic model is specifically:

[0025]

[0026] Among them, x L ,y L represents the position of the unmanned surface ship, ψ L Indicates the heading angle of the surface unmanned ship, u L 、v L 、r L They represent the pitch velocity, roll velocity and yaw angular velocity of the surface unmanned ship respectively; x F ,y F 、z F They represent the position of the underwater unmanned vehicle, ψ F represents the heading angle of the underwater unmanned vehicle, u F 、v F 、w F 、r FThey represent the pitch velocity, roll velocity, vertical velocity and yaw angular velocity of the underwater unmanned vehicle respectively.

[0027] Preferably, the nonlinear dynamic model is specifically:

[0028]

[0029] where τ L =[τ Lu ,0,τ Lr ] T represents the control force and torque acting on the surface unmanned ship, d L =[d Lu ,d Lv ,d Lr ] T represents the unknown time-varying environmental disturbance caused by ocean currents and waves for the surface unmanned ship; τ F =[τ Fu ,0,0,0,τ Fw ,τ Fr ] T represents the control force and torque acting on the underwater unmanned vehicle, d F =[d Fu ,d Fv ,d Fw ,d Fr ] T represents the unknown time-varying environmental disturbance caused by ocean currents and waves for underwater unmanned vehicles; m L =[m L11 ,m L22 ,m L33 ] T Represents the additional mass of the surface unmanned ship, m F =[m F11 ,m F22 ,m F33 ,m F44 ] T Represents the additional mass of the underwater unmanned vehicle.

[0030] Preferably, the step S4.2 specifically includes the following steps:

[0031] S4.2.1. Use vertical projection to project the actual navigation track of the surface unmanned ship underwater, and transmit the status information of the surface unmanned ship to the underwater unmanned vehicle through an underwater acoustic communication device;

[0032] S4.2.2. The underwater unmanned vehicle obtains the navigation track of the unmanned ship (x p (t),y p (t)), and calculate the tangent angle φ of the trajectory p :

[0033] φ p =arctan(y' p (t) / x' p (t)) (3)

[0034] in

[0035] S4.2.3. Calculate the actual position of the underwater unmanned vehicle and the surface unmanned ship in the trajectory reference coordinate system O p X p Y p The position error is:

[0036]

[0037] Derivative the position error of the above equation, and we get the tracking error dynamics:

[0038]

[0039] in, represents the total speed of the underwater unmanned vehicle; β = arctan (v / u) is the sideslip angle of the underwater unmanned vehicle; is the total speed of the target point;

[0040] S4.2.4, according to the desired heading guidance law ψ d and the desired velocity guidance law u d The velocity error e of the underwater unmanned vehicle is obtained Fu and heading error e Fψ :

[0041]

[0042]

[0043] in, k is a parameter greater than 0; Δ is the foresight distance, which is 2 to 5 times the length of the ship.

[0044] Preferably, in step S4.3, the following methods are used to construct the predetermined time interference observer of the surface unmanned ship or the underwater unmanned vehicle respectively:

[0045]

[0046] Wherein, M is used to represent the matrix composed of the weight inertia of the surface unmanned ship or the underwater unmanned vehicle and the inertia attached by the hydrodynamic force, ν is used to represent the velocity vector of the surface unmanned ship in the inertial coordinate system or the velocity vector of the underwater unmanned vehicle in the attached coordinate system; K a and K b are all positive definite parameter diagonal matrices to be designed, represents the estimation of disturbance; K b The choice satisfies β(t) is a time-varying function used in the design of the timing controller:

[0047]

[0048] in, T>0 is a positive design parameter, T represents the maximum allowable stabilization time; α∈R 3 is the auxiliary state vector of the disturbance observer, which is updated by the following adaptive law:

[0049]

[0050] Where ρ is the design parameter, satisfying I 3×1 is the identity vector; C(v) is the Coriolis-centripetal matrix; D(v) is the hydrodynamic damping parameter matrix, g(η) is the restoring force and torque generated by gravity / buoyancy, and τ is the control vector of the control input of the surface unmanned ship propulsion system or the underwater unmanned vehicle propulsion system.

[0051] Preferably, the step S4.4 specifically includes the following steps:

[0052] S4.4.1. Construct the following surface unmanned ship-underwater unmanned vehicle hybrid system adaptive integral terminal sliding mode variable

[0053]

[0054] Among them, e Lu =u Ld -u L represents the error between the expected forward speed and the actual forward speed of the unmanned ship, e Lψ =ψ Ld -ψ L represents the error between the expected heading angle of the unmanned ship and the actual heading angle, e Fu =u Fd -u F represents the error between the expected forward speed and the actual forward speed of the unmanned underwater vehicle, e Fψ =ψ Fd -ψ F represents the error between the desired heading angle of the unmanned underwater vehicle and the actual heading angle, e Fw =w Fd -w F represents the error between the expected heave speed and the actual heave speed of the unmanned underwater vehicle; α Lu ,α Lψ ,α Fu ,α Fψ ,β Lψ ,βFψ ,λ Fw are all positive parameters that need to be designed; e LI,u ,e LI,ψ ,e FI,u ,e FI,ψ ,e FI,w The calculation of is as follows:

[0055]

[0056] Among them, the parameters p and q are selected as positive odd numbers, and the condition p>q>0 is satisfied;

[0057] S4.4.2. Let the sliding mode variable s i The first-order derivative of is equal to 0, that is, At the same time, ignoring all uncertainties in the system, the equivalent control torque is obtained:

[0058]

[0059] The switching control torque is designed as follows:

[0060]

[0061] Where k2>0 is a constant parameter, K1 is an adaptive gain parameter, i=Lu,Lψ,Fu,Fψ,Fw; c1, c2, σ, γ are all constant parameters, and c1>0, c2>0, σ>1, 0<γ<1;

[0062] The following dead zone techniques are used to adjust the adaptive law;

[0063]

[0064] And the following adaptive law is used for update:

[0065]

[0066] Where k1 is the dead zone gain parameter, ε is the dead zone threshold parameter, μ>0 is a constant parameter, k min is the adaptive law threshold parameter, k δ >0 is a constant parameter; if s i If s is greater than μ, the adaptive controller adjusts k1 to increase it; i Below μ, k1 decreases until it reaches k min , to ensure that there is no zero gain and reduce unnecessary control gain;

[0067] S4.4.3. External interference estimation value combined with preset time interference observer The final control law is designed as follows

[0068]

[0069] A surface and underwater heterogeneous cooperative control system based on a preset time interference observer, the system adopts the surface and underwater heterogeneous cooperative control method based on the preset time interference observer to realize control, and includes shore-based equipment, surface unmanned ships and underwater unmanned vehicles;

[0070] The unmanned surface ship includes a radio module, a controller, a GPS positioning system, an underwater acoustic communication device, a millimeter wave radar and a propeller;

[0071] The underwater unmanned vehicle comprises a controller, an underwater acoustic communication device, a Doppler odometer, an optical fiber inertial navigation system, a side-scan sonar, a downward-looking multi-beam sonar, and a thruster.

[0072] Compared with the prior art, the present invention has the following beneficial effects:

[0073] The present invention firstly establishes a hybrid motion model of a surface unmanned ship and an underwater unmanned vehicle, and adopts a guidance method combining vertical projection and line of sight to complete collaborative guidance; and by constructing a preset time interference observer, it realizes effective estimation of external interference and compensates it in the controller, thereby solving the adverse effects of external interference on the surface and underwater heterogeneous collaborative control system; the proposed controller combines the advantages of fast non-singular terminal sliding mode, integral sliding mode, and dual-dominant reaching law, and can effectively achieve the desired tracking performance and suppress control signal jitter.

[0074] Compared with model predictive control, the control method of the present invention has a smaller amount of calculation and can complete collaborative tasks in real time; compared with PID control, it does not require frequent parameter adjustment and is more convenient to use; compared with integral sliding mode control, terminal sliding mode control and other methods, it has a higher speed, more accurate and more robust tracking performance when facing interference caused by wind, waves and currents in the marine environment. While improving the accuracy and safety of the surface and underwater collaborative system in marine target detection operations, it also improves the overall operating efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 It is a schematic diagram of the structure of the overall device for surface and underwater heterogeneous cooperative control based on a preset time interference observer of the present invention;

[0076] Figure 2 This is a diagram of the surface and underwater collaborative control communication device of the present invention;

[0077] Figure 3 This is a flow chart of the surface and underwater heterogeneous collaborative control method based on a preset time interference observer of the present invention;

[0078] Figure 4 This is a schematic diagram of the surface and underwater collaborative vertical projection fusion line of sight guidance of the present invention;

[0079] Figure 5 This is a flow chart of the surface and underwater heterogeneous collaborative control of the present invention;

[0080] Figure 6 This is a simulation diagram of three-dimensional collaborative trajectory tracking between a surface unmanned ship and an underwater unmanned vehicle in one embodiment of the present invention. DETAILED DESCRIPTION

[0081] The following is combined with Figure 1-6 , the technical solution of the present invention is specifically described.

[0082] like Figure 1 As shown, the present invention proposes a surface and underwater heterogeneous cooperative control system based on a preset time interference observer, including shore-based equipment, a surface unmanned ship and an underwater unmanned vehicle; the surface unmanned ship serves as a leader, and the underwater unmanned vehicle serves as a follower; specifically, as Figure 2 As shown, the shore-based equipment establishes a connection with the surface unmanned ship through radio, sends the expected trajectory to the surface unmanned ship and obtains the status information of the surface unmanned ship according to the mission requirements; specifically, the surface unmanned ship obtains the status information of the body through the GPS positioning system, and packages it and sends it back to the shore-based equipment; further, the underwater unmanned vehicle obtains the status information of the surface unmanned ship through the hydroacoustic communicator and sends the body status information to the surface unmanned ship; specifically, the surface unmanned ship is equipped with a hydroacoustic communicator dry end, and the underwater unmanned vehicle is equipped with a hydroacoustic communicator wet end. The underwater unmanned vehicle obtains the body status information through fiber optic inertial navigation and Doppler odometer calculations, and sends the body status information through The positioning function of the hydroacoustic communicator corrects the calculated body status information, packages it and sends it to the surface unmanned ship; further, the shore-based equipment can obtain the status information of the surface manned ship and the underwater unmanned vehicle in real time; further, the main control converts the calculated control torque into a control command and sends it to the thruster, and the thruster executes the command to complete collaborative tracking; further, when the surface and underwater heterogeneous collaborative system navigates to the designated mission area, the shore-based equipment issues side-scan sonar and downward-looking multi-beam sonar commands, which are transmitted to the underwater unmanned vehicle through the surface unmanned ship, a communication gateway, and the underwater unmanned vehicle executes the command to carry out ocean target detection operations.

[0083] The present invention also proposes a surface and underwater heterogeneous cooperative control method based on a preset time interference observer, comprising the following steps:

[0084] Step 1: The shore-based equipment sends the expected trajectory to the surface unmanned vessel according to the mission objectives;

[0085] Step 2: The surface unmanned ship obtains the desired trajectory through the radio module and obtains the body status information through the GPS positioning system;

[0086] Step 3, the underwater unmanned vehicle obtains the status information of the surface unmanned ship through the hydroacoustic communication device, and obtains the status information of the body through the fiber optic inertial navigation device;

[0087] Step 4, the surface unmanned ship controller analyzes and calculates according to the specified expected trajectory, the actual state information of the unmanned ship body and the compensation value obtained by the observer to obtain the expected control torque; the underwater unmanned vehicle analyzes and calculates according to the expected trajectory obtained by the guidance method, the actual state information of the surface unmanned ship, the actual state information of the underwater unmanned vehicle body and the compensation value obtained by the observer to obtain the expected control torque;

[0088] Step 5: The control torque command calculated by the master control is sent to the thruster to realize heterogeneous collaborative control of the surface unmanned ship and the underwater unmanned vehicle;

[0089] Step 6: The underwater unmanned vehicle detects the target through side-scan sonar and downward-looking multi-beam sonar.

[0090] For step 1, a navigation track is formulated according to the requirements of ocean target detection, and the shore-based equipment sends the navigation track to the surface unmanned ship through radio transmission;

[0091] For step 2, the surface unmanned ship obtains the state information of the body through the GPS positioning system, and converts the position longitude and latitude into the position vector of the surface unmanned ship in the inertial coordinates through coordinate conversion;

[0092] For step 3, the present invention proposes a surface and underwater collaborative control communication device, referring to Figure 2 The surface unmanned ship acts as a communication gateway between the underwater unmanned vehicle and the shore-based equipment. The underwater unmanned vehicle obtains the status information of the surface unmanned ship through the hydroacoustic communicator and obtains the status information of the body through the fiber-optic inertial navigation device. At the same time, the positioning function of the hydroacoustic communicator is used to correct the body position estimation of the fiber-optic inertial navigation to improve the accuracy of the body status information;

[0093] For step 4, this design applies the improved adaptive integral terminal sliding mode method to the surface and underwater heterogeneous cooperative control, and combines it with the preset time disturbance observer to greatly reduce overshoot and chattering. It improves the safety and robustness of the surface and underwater heterogeneous cooperative system in the ocean target detection operation. The process is referenced Figure 3 The specific process is divided into the following steps:

[0094] Step 4.1, establishing a mixed motion model of the surface unmanned ship and the underwater unmanned vehicle;

[0095] Step 4.2, using the guidance method combining vertical projection and line of sight, the underwater unmanned vehicle obtains the actual navigation track of the surface unmanned ship. The guidance map is shown in Figure 4 ;

[0096] Step 4.3, in the face of disturbances in the marine environment, a preset time interference observer is constructed to accurately estimate the external interference factors on the operation of the surface and underwater heterogeneous collaborative system, and then realize interference compensation, reduce the impact of external interference on the system, and improve the robustness of the system.

[0097] Step 4.4, the surface unmanned ship controller analyzes and calculates according to the specified expected trajectory, the actual state information of the unmanned ship body and the compensation value obtained by the observer to obtain the expected control torque; the underwater unmanned vehicle controller analyzes and calculates according to the expected trajectory obtained by the guidance method, the actual position state information of the surface unmanned ship, the actual position state information of the underwater unmanned vehicle body and the compensation value obtained by the observer to obtain the expected control torque. The control flow chart is referred to Figure 5 .

[0098] The improved adaptive integral terminal sliding mode method is applied to the surface and underwater heterogeneous cooperative control, and combined with the preset time disturbance observer, the overshoot and chattering are greatly reduced, which improves the safety and robustness of the surface and underwater heterogeneous cooperative system in the marine target detection operation.

[0099] For step 5, the control torque calculated by the main control is sent to the thruster to achieve heterogeneous collaborative control of the surface unmanned ship and the underwater unmanned vehicle.

[0100] For step 6, when the surface unmanned ship and the underwater unmanned vehicle navigate to the mission location in a coordinated manner, they activate the side-scan sonar and downward-looking multi-beam sonar to detect targets in the mission area.

[0101] In this embodiment, the mathematical model of the surface unmanned ship-underwater unmanned vehicle hybrid system is as follows:

[0102] The nonlinear kinematic model of the surface unmanned ship-underwater unmanned vehicle hybrid system is as follows:

[0103]

[0104] Among them, x L ,y L represents the position of the unmanned surface ship, ψ L Indicates the heading angle of the surface unmanned ship, u L 、v L 、r L They represent the pitch velocity, roll velocity and yaw angular velocity of the surface unmanned ship respectively. F ,y F 、z F Respectively represent the position of the underwater unmanned vehicle. F represents the heading angle of the underwater unmanned vehicle, u F 、v F 、wF 、r F They represent the pitch velocity, roll velocity, vertical velocity and yaw angular velocity of the underwater unmanned vehicle respectively.

[0105] The nonlinear dynamic model of the surface unmanned ship-underwater unmanned vehicle hybrid system is as follows:

[0106]

[0107] Among them, x L ,y L represents the position of the unmanned surface ship, ψ L Indicates the heading angle of the surface unmanned ship, u L 、v L 、r L They represent the pitch velocity, roll velocity and yaw angular velocity of the surface unmanned ship respectively. F ,y F 、z F Respectively represent the position of the underwater unmanned vehicle. F represents the heading angle of the underwater unmanned vehicle, u F 、v F 、w F 、r F They represent the pitch velocity, roll velocity, vertical velocity and yaw angular velocity of the underwater unmanned vehicle respectively. L =[τ Lu ,0,τ Lr ] T represents the control force and torque acting on the surface unmanned ship, d L =[d Lu ,d Lv ,d Lr ] T represents the unknown time-varying environmental disturbance caused by ocean currents and waves. F =[τ Fu ,0,0,0,τ Fw ,τ Fr ] T represents the control force and torque acting on the underwater unmanned vehicle, d F =[d Fu ,d Fv ,d Fw ,d Fr ] T Represents the unknown time-varying environmental disturbance caused by ocean currents and waves. L =[m L11 ,m L22 ,m L33 ] T Represents the additional mass of the surface unmanned ship, m F =[m F11,m F22 ,m F33 ,m F44 ] T Represents the additional mass of the underwater unmanned vehicle.

[0108] Taking into account factors such as external interference caused by wind, waves and currents, the present invention proposes an improved adaptive integral terminal sliding mode control method based on a preset time interference observer to achieve safe, accurate and stable collaborative tracking of surface unmanned ships and underwater unmanned vehicles.

[0109] In this embodiment, the guidance algorithm uses a surface unmanned ship to guide an underwater unmanned vehicle to track the trajectory and complete the underwater detection of the target area. The trajectory deviation of the surface unmanned ship relative to the reference trajectory and the reference trajectory deviation of the underwater unmanned vehicle relative to the surface unmanned ship are constructed by fusing vertical projections through the line of sight method.

[0110] Firstly, the actual navigation track of the surface unmanned ship is projected underwater by vertical projection, and the status information of the surface unmanned ship is transmitted to the underwater unmanned vehicle through the hydroacoustic communicator;

[0111] Next, the underwater unmanned vehicle obtains the navigation track of the unmanned ship (x p (t),y p (t)), and gives the tangent angle φ of the trajectory p :

[0112]

[0113] in

[0114] Next, the actual positions of the underwater unmanned vehicle and the surface unmanned ship in the trajectory reference coordinate system O are given. p X p Y p The position error is:

[0115]

[0116] At the same time, the position error is derived from the above equation to obtain the tracking error dynamics:

[0117]

[0118] in, represents the total speed of the underwater unmanned vehicle; β = arctan (v / u) is the sideslip angle of the underwater unmanned vehicle; is the total velocity of the target point.

[0119] The desired heading guidance law is designed as:

[0120]

[0121] The desired velocity guidance law is:

[0122]

[0123] in, k is a parameter greater than 0; Δ is the foresight distance, which is generally 2 to 5 times the length of the ship.

[0124] Finally, the speed error and heading error of the underwater unmanned vehicle are obtained:

[0125]

[0126] The present invention takes into account the disturbance of wind, waves and currents in the marine environment, and proposes an improved adaptive integral terminal sliding mode control method based on a preset time interference observer. The method can effectively solve the above-mentioned problems and realize safe, accurate and stable collaborative tracking of surface unmanned ships and underwater unmanned vehicles.

[0127] In this embodiment, the design of the preset time interference observer is as follows:

[0128] Through the design of this observer, external interference can be effectively observed. Taking the underwater unmanned vehicle as an example, the observer design of the surface unmanned ship is similar. The process is as follows:

[0129] First, the mathematical model of underwater unmanned vehicle proposed by Fossen is adopted, and its mathematical expression is as follows

[0130]

[0131] where η = [x, y, ψ, z] T is the position vector of the underwater unmanned vehicle in the inertial coordinate system, (x, y, z) is the actual position of the underwater unmanned vehicle, ψ is the heading angle of the underwater unmanned vehicle; v = [u, v, w, r] T is the velocity vector of the underwater unmanned vehicle in the attached coordinate system, u is the pitch velocity, v is the roll velocity, w is the vertical velocity, and r is the yaw angular velocity; τ=[τ u ,τ w ,τ r ] T is the control vector of the thruster control input, τ u is the forward force, τ w is the vertical force, τ r is the yaw force; d=[d u ,d v ,d w ,d r ] Tis the external disturbance of the underwater unmanned vehicle caused by wind, waves, etc. in the attached coordinate system; M is the matrix composed of the weight inertia of the underwater unmanned vehicle and the inertia attached by hydrodynamics; J(ψ) is the coordinate system transformation matrix; C(v) is the Coriolis-centripetal matrix; D(v) is the hydrodynamic damping parameter matrix, and g(η) is the restoring force and torque generated by gravity / buoyancy.

[0132] In order to eliminate the negative impact of unknown disturbances within the predetermined time, the predetermined time disturbance observer is constructed as

[0133]

[0134] Where K a and K b are all positive definite parameter diagonal matrices to be designed, Represents the estimation of the disturbance, and the estimation error can be defined as K b The choice satisfies β(t) is a time-varying function used in the design of timing controllers. Its mathematical form can be expressed as

[0135]

[0136] in T>0 is a positive design parameter, and T represents the maximum allowable stabilization time. α∈R 3 is the auxiliary state vector of the disturbance observer, which is updated by the following adaptive law:

[0137]

[0138] Where ρ is the design parameter, satisfying I 3×1 is the identity vector. Since the external disturbance is slowly time-varying, this requirement can be met by selecting a conservative value.

[0139] In this embodiment, the control law design is specifically as follows:

[0140] The first step is to construct the following surface unmanned ship-underwater unmanned vehicle hybrid system adaptive integral terminal sliding mode variable

[0141]

[0142] Among them, e Lu =u Ld -u L represents the error between the expected forward speed and the actual forward speed of the unmanned ship, e Lψ =ψ Ld -ψ L represents the error between the expected heading angle of the unmanned ship and the actual heading angle, e Fu =uFd -u F represents the error between the expected forward speed and the actual forward speed of the unmanned underwater vehicle, e Fψ =ψ Fd -ψ F represents the error between the desired heading angle of the unmanned underwater vehicle and the actual heading angle, e Fw =w Fd -w F It represents the error between the expected heave speed and the actual heave speed of the unmanned underwater vehicle;

[0143] α Lu ,α Lψ ,α Fu ,α Fψ ,β Lψ ,β Fψ ,λ Fw are all positive parameters that need to be designed; e LI,u Designed for e LI,ψ ,e FI,u ,e FI,ψ ,e FI,w The design of is similar, with parameters p and q chosen to be positive odd numbers, satisfying the condition p>q>0.

[0144] Then let the sliding mode variable s i The first-order derivative of is equal to 0, that is At the same time, ignoring all uncertainties in the system, the equivalent control torque is obtained:

[0145]

[0146] In order to achieve the ideal stable convergence requirements, improve the robustness of the system in the face of uncertainty, and speed up the response of the system, the switching control torque is designed as follows:

[0147]

[0148] Where i = Lu, Lψ, Fu, Fψ, Fw. k2>0 is a constant parameter, K1 is an adaptive gain parameter, c1, c2, σ, γ are all constant parameters, and c1>0, c2>0, σ>1, 0<γ<1; In practical applications, the sliding variable often jitters due to measurement noise, resulting in an overestimation of k1. In order to alleviate this problem, the present invention adopts the following dead zone technology to adjust the adaptive law.

[0149]

[0150] And use the following adaptive law to update

[0151]

[0152] Where k1 is the dead zone gain parameter, ε is the dead zone threshold parameter, μ>0 is a constant parameter, k min is the adaptive law threshold parameter, k δ >0 is a constant parameter. If s i If s is greater than μ, the adaptive controller will adjust k1 to increase it; i Below μ, it decreases until it reaches k min , thus ensuring that there is no zero gain and reducing unnecessary control gain. Considering the existence of unknown disturbances, it is necessary to use the external disturbance estimation value of the preset time disturbance observer And provide feedback in the subsequent controller design to achieve stable operation of the surface and underwater collaborative system.

[0153] Finally, the control law is designed as follows

[0154]

[0155] Through the above description, the present invention has described the specific process of a surface and underwater heterogeneous cooperative control method based on a preset time interference observer. First, the guidance of the surface and underwater heterogeneous cooperative control is completed by integrating the vertical projection and the line of sight method; then, the preset time interference observer is used to estimate the external interference and compensate it in the controller, thereby improving the anti-interference ability of the system and making the system run more smoothly; finally, a new improved adaptive integral terminal sliding mode control method suitable for surface and underwater cooperative control is proposed, which increases the system tracking accuracy while improving the robustness and rapidity of the system, and is more in line with the requirements of high tracking accuracy, strong anti-interference ability, safety, rapid response, stability and reliability for the surface and underwater cooperative system in marine target detection operations.

[0156] The above are preferred embodiments of the present invention. Any changes made according to the technical solution of the present invention, as long as the resulting functions do not exceed the scope of the technical solution of the present invention, belong to the protection scope of the present invention.

Claims

1. A method for surface and underwater heterogeneous cooperative control based on a preset time interference observer, characterized in that: The following steps are involved: Step 1: The shore-based equipment sends the expected trajectory to the surface unmanned vessel according to the mission objectives; Step 2: The surface unmanned ship obtains the desired trajectory through the radio module and obtains the state information of the unmanned ship through the positioning system; Step 3: The underwater unmanned vehicle obtains the status information of the surface unmanned ship through the hydroacoustic communication device, and obtains the status information of the underwater unmanned vehicle body through the fiber optic inertial navigation device; Step 4: The surface unmanned ship controller analyzes and calculates the compensation value obtained by the preset time disturbance observer according to the specified expected trajectory, the actual state information of the unmanned ship body, and obtains the expected control torque; The underwater unmanned vehicle controller analyzes and calculates the expected trajectory obtained by the guidance method, the actual state information of the surface unmanned ship, the actual state information of the underwater unmanned vehicle body, and the compensation value obtained by the preset time interference observer to obtain the expected control torque; Step 5: The surface unmanned ship controller and the underwater unmanned vehicle controller respectively send the calculated control torque instructions to the surface unmanned ship controller and the underwater unmanned vehicle thrusters to achieve heterogeneous collaborative control of the surface unmanned ship and the underwater unmanned vehicle; Step 6: When the surface unmanned ship and the underwater unmanned vehicle navigate to the mission location in coordination, they activate the side-scan sonar and downward-looking multi-beam sonar to detect targets in the mission area.

2. The method for surface and underwater heterogeneous cooperative control based on a preset time interference observer according to claim 1 is characterized in that: The obtaining of the state information of the unmanned ship body through the positioning system specifically includes: obtaining the state information of the unmanned ship body through the GPS positioning system, wherein the position longitude and latitude are converted into the position vector of the surface unmanned ship in the inertial coordinates through coordinate conversion.

3. The method for surface and underwater heterogeneous collaborative control based on a preset time interference observer according to claim 1 is characterized in that: The acquisition of the underwater unmanned vehicle body state information also includes: the underwater unmanned vehicle calculates and obtains the body state information through fiber optic inertial navigation and Doppler odometer, and corrects the calculated body state information through the positioning function of the underwater acoustic communication device.

4. The method for surface and underwater heterogeneous cooperative control based on a preset time interference observer according to claim 1 is characterized in that: The step S4 comprises the following specific steps: Step 4.1, establishing a hybrid motion model of a surface unmanned ship and an underwater unmanned vehicle; the hybrid motion model of the surface unmanned ship and the underwater unmanned vehicle includes a nonlinear kinematic model and a nonlinear dynamic model of a surface unmanned ship-underwater unmanned vehicle hybrid system; Step 4.2, constructing the trajectory deviation of the surface unmanned ship relative to the reference trajectory and the reference trajectory deviation of the underwater unmanned vehicle relative to the surface unmanned ship by fusing the vertical projections through the line of sight method; Step 4.3, construct a preset time interference observer to estimate the external interference factors on the operation of the surface and underwater heterogeneous collaborative system; Step 4.4: Based on the external disturbance estimation value of the preset time disturbance observer and the improved adaptive integral terminal sliding mode control method, the control law is designed to calculate the expected control torque.

5. The method for surface and underwater heterogeneous cooperative control based on a preset time interference observer according to claim 3 is characterized in that: The nonlinear kinematic model is specifically: Among them, x L ,y L represents the position of the unmanned surface ship, ψ L Indicates the heading angle of the surface unmanned ship, u L 、v L 、r L They represent the pitch velocity, roll velocity and yaw angular velocity of the surface unmanned ship respectively; x F ,y F 、z F They represent the position of the underwater unmanned vehicle, ψ F represents the heading angle of the underwater unmanned vehicle, u F 、v F 、w F 、r F They represent the pitch velocity, roll velocity, vertical velocity and yaw angular velocity of the underwater unmanned vehicle respectively.

6. The method for surface and underwater heterogeneous cooperative control based on a preset time interference observer according to claim 4 is characterized in that: The nonlinear dynamics model is specifically: where τ L =[τ Lu ,0,τ Lr ] T represents the control force and torque acting on the surface unmanned ship, d L =[d Lu ,d Lv ,d Lr ] T represents the unknown time-varying environmental disturbance caused by ocean currents and waves for the surface unmanned ship; τ F =[τ Fu ,0,0,0,τ Fw ,τ Fr ] T represents the control force and torque acting on the underwater unmanned vehicle, d F =[d Fu ,d Fv ,d Fw ,d Fr ] T represents the unknown time-varying environmental disturbance caused by ocean currents and waves for underwater unmanned vehicles; m L =[m L11 ,m L22 ,m L33 ] T Represents the additional mass of the surface unmanned ship, m F =[m F11 ,m F22 ,m F33 ,m F44 ] T Represents the additional mass of the underwater unmanned vehicle.

7. The method for surface and underwater heterogeneous cooperative control based on a preset time interference observer according to claim 3 is characterized in that: The step S4.2 specifically includes the following steps: S4.2.

1. Use vertical projection to project the actual navigation track of the surface unmanned ship underwater, and transmit the status information of the surface unmanned ship to the underwater unmanned vehicle through an underwater acoustic communication device; S4.2.

2. The underwater unmanned vehicle obtains the navigation track of the unmanned ship (x p (t),y p (t)), and the tangent angle φ of the technical trajectory p : φ p =arctan(y' p (t) / x' p (t)) (3) in S4.2.

3. Calculate the actual position of the underwater unmanned vehicle and the surface unmanned ship in the trajectory reference coordinate system O p X p Y p The position error is: Derivative the position error of the above equation, and we get the tracking error dynamics: in, represents the total speed of the underwater unmanned vehicle; β = arctan (v / u) is the sideslip angle of the underwater unmanned vehicle; is the total speed of the target point; S4.2.4, according to the desired heading guidance law ψ d and the desired velocity guidance law u d The velocity error e of the underwater unmanned vehicle is obtained Fu and heading error e Fψ : in, k is a parameter greater than 0; Δ is the foresight distance, which is 2 to 5 times the length of the ship.

8. The method for surface and underwater heterogeneous cooperative control based on a preset time interference observer according to claim 3 is characterized in that: In step S4.3, the following method is used to construct a predetermined time interference observer for the surface unmanned ship or the underwater unmanned vehicle respectively: Wherein, M is used to represent the matrix composed of the weight inertia of the surface unmanned ship or the underwater unmanned vehicle and the inertia attached by the hydrodynamic force, ν is used to represent the velocity vector of the surface unmanned ship in the inertial coordinate system or the velocity vector of the underwater unmanned vehicle in the attached coordinate system; K a and K b are all positive definite parameter diagonal matrices to be designed, represents the estimation of disturbance; K b The choice satisfies β(t) is a time-varying function used in the design of the timing controller: in, is a positive design parameter, T represents the maximum allowable stabilization time; α∈R 3 is the auxiliary state vector of the disturbance observer, which is updated by the following adaptive law: Where ρ is the design parameter, satisfying I 3×1 is the identity vector; C(v) is the Coriolis-centripetal matrix; D(v) is the hydrodynamic damping parameter matrix, g(η) is the restoring force and torque generated by gravity / buoyancy, and τ is the control vector of the control input of the surface unmanned ship propulsion system or the underwater unmanned vehicle propulsion system.

9. The method for surface and underwater heterogeneous cooperative control based on a preset time interference observer according to claim 3 is characterized in that: The step S4.4 specifically includes the following steps: S4.4.

1. Construct the following surface unmanned ship-underwater unmanned vehicle hybrid system adaptive integral terminal sliding mode variable Among them, e Lu =u Ld -u L represents the error between the expected forward speed and the actual forward speed of the unmanned ship, e Lψ =ψ Ld -ψ L represents the error between the expected heading angle of the unmanned ship and the actual heading angle, e Fu =u Fd -u F represents the error between the expected forward speed and the actual forward speed of the unmanned underwater vehicle, e Fψ =ψ Fd -ψ F represents the error between the desired heading angle of the unmanned underwater vehicle and the actual heading angle, e Fw =w Fd -w F represents the error between the expected heave speed and the actual heave speed of the unmanned underwater vehicle; α Lu ,α Lψ ,α Fu ,α Fψ ,β Lψ ,β Fψ ,λ Fw are all positive parameters that need to be designed; e LI,u ,e LI,ψ ,e FI,u ,e FI,ψ ,e FI,w The calculation of is as follows: Among them, the parameters p and q are selected as positive odd numbers, and the condition p>q>0 is satisfied; S4.4.

2. Let the sliding mode variable s i The first-order derivative of is equal to 0, that is, At the same time, ignoring all uncertainties in the system, the equivalent control torque is obtained: The switching control torque is designed as follows: Where k2>0 is a constant parameter, K1 is an adaptive gain parameter, i=Lu,Lψ,Fu,Fψ,Fw; c1, c2, σ, γ are all constant parameters, c 1 and>0,c2>0,σ>1,0<γ<1 The following dead zone techniques are used to adjust the adaptive law; And use the following adaptive law to update Where k1 is the dead zone gain parameter, ε is the dead zone threshold parameter, μ>0 is a constant parameter, k min is the adaptive law threshold parameter, k δ >0 is a constant parameter; if s i If s is greater than μ, the adaptive controller adjusts k1 to increase it; i Below μ, k1 decreases until it reaches k min , to ensure that there is no zero gain and reduce unnecessary control gain; S4.4.

3. External interference estimation value combined with preset time interference observer The final control law is designed as follows 10. A surface and underwater heterogeneous cooperative control system based on a preset time interference observer, characterized in that: The system is controlled by using the surface and underwater heterogeneous collaborative control method based on a preset time interference observer as described in any one of claims 1 to 9, including shore-based equipment, surface unmanned ships and underwater unmanned vehicles; The unmanned surface ship includes a radio module, a controller, a GPS positioning system, an underwater acoustic communication device, a millimeter wave radar and a propeller; The underwater unmanned vehicle comprises a controller, an underwater acoustic communication device, a Doppler odometer, an optical fiber inertial navigation system, a side-scan sonar, a downward-looking multi-beam sonar, and a thruster.

Images