A water surface and underwater heterogeneous cooperative control method and device based on a preset time disturbance observer

Abstract

The application provides a water surface and underwater heterogeneous collaborative control method and device based on a preset time disturbance observer, and the control method mainly comprises the following steps: a water surface unmanned ship controller calculates an expected control torque according to a specified expected trajectory, actual state information of the unmanned ship body and a compensation value obtained by an observer; a underwater unmanned vehicle controller calculates an expected control torque according to an expected trajectory obtained by a guidance method, actual state information of the water surface unmanned ship, actual state information of the underwater unmanned vehicle body and the compensation value obtained by the observer; and the water surface unmanned ship and the underwater unmanned vehicle are collaboratively controlled in a heterogeneous mode. The application combines vertical projection and line-of-sight method to complete the guidance of the water surface and underwater heterogeneous collaborative control; the preset time disturbance observer is used to estimate external disturbance, and the disturbance is compensated in the controller to improve the anti-interference ability of the system; and the improved adaptive integral terminal sliding mode control method improves the robustness and rapidity of the system while increasing the tracking accuracy of the system.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of trajectory tracking control of unmanned systems, and particularly relates to a water-surface and underwater heterogeneous collaborative control method and device based on a preset time disturbance observer. BACKGROUND

[0002] In recent decades, ocean technology has attracted much attention due to the increasing investment in ocean exploration. When humans face high-risk tasks such as seabed mapping, ocean measurement, mine clearance, sea rescue, and environmental monitoring, they often use surface unmanned ships and underwater unmanned vehicles to perform these tasks. Therefore, water-surface and underwater collaborative control technology has gradually become a research hotspot in the current field of ocean engineering.

[0003] In the complexity and variability of the marine environment, the motion system of the surface unmanned ship and the underwater unmanned vehicle exhibits strong nonlinear characteristics when they are collaboratively performing tasks, and is affected by uncertain disturbances such as wind, waves, and currents. How to effectively deal with these disturbances and ensure safe navigation is a key problem that needs to be solved. In the face of these disturbances, sliding mode control is of great concern due to its strong robustness, but reducing system chattering while improving 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 is not ideal in collaborative control, and PID technology may produce overshoot, which cannot meet the safety of collaborative control. Therefore, it is necessary to study a method that can meet the real-time performance and safety of water-surface and underwater unmanned system collaborative control, which is crucial for improving the ability of ocean target detection.

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

[0005] (1) By applying distributed model predictive control technology, water-surface and underwater system collaborative control is achieved. However, this technology needs to process a large amount of data during calculation, and there are certain limitations in the computing power of ship equipment. Therefore, additional calculation time is required for the implementation of the algorithm, which inevitably affects the real-time response and accuracy of collaborative control.

[0006] (2) Traditional PID control technology is used to achieve water-surface and underwater system collaborative control, although this method is effective in some cases, but it needs to frequently adjust the control parameters to meet the desired value of the collaborative effect. In addition, when encountering adverse external environmental conditions, the adaptive ability of PID control is insufficient, which has a negative impact on the safety of collaborative control.

[0007] In view of this, the application provides a water surface and underwater heterogeneous collaborative control method and device based on a preset time disturbance observer. SUMMARY

[0008] The application aims to provide a water surface and underwater heterogeneous collaborative control method and device based on a preset time disturbance observer, a guidance strategy based on vertical projection fusion line-of-sight method is used to realize efficient collaboration of the water surface and underwater systems; a preset time disturbance observer is designed to quickly estimate and compensate for unknown disturbances, and the process is not affected by the initial state; at the same time, through improved approach law design, the response speed of the system is significantly improved, so that the water surface unmanned ship and underwater unmanned vehicle can safely, accurately and stably perform the collaborative tracking task.

[0009] To achieve the above-mentioned purpose, the technical scheme of the application is as follows:

[0010] A water surface and underwater heterogeneous collaborative control method based on a preset time disturbance observer, comprising the following steps:

[0011] Step 1, the shore-based equipment sends the expected trajectory to the water surface unmanned ship according to the task target;

[0012] Step 2, the water surface unmanned ship obtains the expected trajectory through the radio module, and obtains the unmanned ship body state information through the positioning system;

[0013] Step 3, the underwater unmanned vehicle obtains the water surface unmanned ship state information through the underwater acoustic communication machine, and obtains the underwater unmanned vehicle body state information through the optical fiber inertial navigation device;

[0014] Step 4, the water 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 disturbance 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 water surface unmanned ship, the actual state information of the underwater unmanned vehicle body, and the compensation value obtained by the preset time disturbance observer, to obtain the expected control torque;

[0015] Step 5, the water surface unmanned ship controller and the underwater unmanned vehicle controller respectively issue the calculated control torque command to the water surface unmanned ship controller and the propeller of the underwater unmanned vehicle, to realize the heterogeneous collaborative control of the water surface unmanned ship and the underwater unmanned vehicle;

[0016] Step 6, when the water surface unmanned ship and the underwater unmanned vehicle cooperatively navigate to the task position, the side-scan sonar and the downward-looking multi-beam sonar are started to detect the target in the task area.

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

[0018] Preferably, the obtaining of the underwater unmanned vehicle body state information further comprises: the underwater unmanned vehicle obtains the body state information through a fiber-optic inertial navigation and a Doppler log, and corrects the obtained body state information through the positioning function of an underwater acoustic communication machine.

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

[0020] Step 4.1, establishing a mixed motion model of the water surface unmanned ship and the underwater unmanned vehicle; the mixed motion model of the water surface unmanned ship and the underwater unmanned vehicle comprises a nonlinear kinematic model and a nonlinear dynamic model of a mixed system of the water surface unmanned ship and the underwater unmanned vehicle;

[0021] Step 4.2, constructing the trajectory deviation of the water surface unmanned ship relative to the reference trajectory and the reference trajectory deviation of the underwater unmanned vehicle relative to the water surface unmanned ship through the line-of-sight method fusion vertical projection;

[0022] Step 4.3, constructing a preset time disturbance observer to estimate the disturbance factors of the external environment to the water surface and underwater heterogeneous collaborative system operation;

[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, designing the control law to calculate the expected control moment.

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

[0025]

[0026] wherein x L , y L represent the position of the water surface unmanned ship, ψ L represents the heading angle of the water surface unmanned ship, u L , v L , r L represent the pitch speed, roll speed and yaw angle speed of the water surface unmanned ship respectively; x F , y F , z F 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 Frespectively represent the pitch velocity, roll velocity, vertical velocity and yaw angular velocity of the underwater unmanned vehicle.

[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 the sea current and wave 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 the sea current and wave for the underwater unmanned vehicle; m L = [m L11 , m L22 , m L33 ] T represents the added mass of the surface unmanned ship, m F = [m F11 , m F22 , m F33 , m F44 ] T represents the added mass of the underwater unmanned vehicle.

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

[0031] S4.2.1, projecting the actual navigation trajectory of the surface unmanned ship to the underwater by vertical projection, the state information of the surface unmanned ship being transmitted to the underwater unmanned vehicle through the underwater acoustic communication machine;

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

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

[0034] wherein

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

[0036]

[0037] Derivation of the above position error is obtained by tracking error dynamics:

[0038]

[0039] wherein, v represents the resultant velocity of the underwater unmanned vehicle; β = arctan (v / u) is the side slip angle of the underwater unmanned vehicle; is the total velocity of the target point;

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

[0041]

[0042]

[0043] wherein, k is a parameter greater than 0; Δ is the forward distance, taking 2-5 times the length of the ship.

[0044] Preferably, the following method is used to construct the predetermined time disturbance observer of the surface unmanned ship or the underwater unmanned vehicle in step S4.3:

[0045]

[0046] wherein, M is used to represent the matrix of the weight inertia of the surface unmanned ship or the underwater unmanned vehicle and the inertia of the water power addition, and v 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 appendage coordinate system; K a and K b are both positive definite parameter diagonal matrices to be designed, denotes the estimate of the disturbance; K b The choice of β(t) is a time-varying function for the design of the timing controller:

[0047]

[0048] where, T>0 is a positive design parameter, T denotes the maximum allowed settling time; a e R 3 is the auxiliary state vector of the disturbance observer, which is updated by the following adaptive law:

[0049]

[0050] where p is a 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 moment generated by gravity / buoyancy, τ is the control vector of the surface unmanned ship thruster or underwater unmanned vehicle thruster control input.

[0051] Preferably, the step S4.4 specifically comprises 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] where, e Lu = u Ld - u L denotes the error between the expected forward speed of the unmanned ship and the actual forward speed, e Lψ = ψ Ld - ψ L denotes the error between the expected heading angle of the unmanned ship and the actual heading angle, e Fu = u Fd - u F denotes the error between the expected forward speed of the unmanned underwater vehicle and the actual forward speed, e Fψ = ψ Fd - ψ F denotes the error between the expected heading angle of the unmanned underwater vehicle and the actual heading angle, e Fw = w Fd - w F denotes the error between the expected heave speed of the unmanned underwater vehicle and the actual heave speed; a Lu , a Lψ , a Fu , a Fψ , β Lψ , βFψ ,λ Fw are positive parameters to be designed; e LI,u , e LI,ψ , e FI,u , e FI,ψ , e FI,w are calculated as follows:

[0055]

[0056] where parameters p and q are chosen as positive odd numbers, and satisfy the condition p>q>0;

[0057] S4.4.2, let the first derivative of the sliding mode variable s i equal to 0, i.e. Without considering all uncertain terms 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 constant parameters, and c1>0, c2>0, σ>1, 0<γ<1;

[0062] The following dead zone technique is used to adjust the adaptive law;

[0063]

[0064] And the following adaptive law is updated:

[0065]

[0066] where k1 is a dead zone gain parameter, ε is a dead zone threshold parameter, μ>0 is a constant parameter, k min is an adaptive law threshold parameter, k δ >0 is a constant parameter; if s i is greater than μ, the adaptive controller adjusts k1 to increase; if s i is lower than μ, k1 decreases until k min is reached to ensure no zero gain and reduce unnecessary control gain;

[0067] S4.4.3, combined with the external disturbance estimation value of the preset time disturbance observer The final control law is designed as follows

[0068]

[0069] A water surface and underwater heterogeneous collaborative control system based on a preset time disturbance observer, which adopts the water surface and underwater heterogeneous collaborative control method based on a preset time disturbance observer to realize control, and comprises a shore-based device, a water surface unmanned ship and an underwater unmanned vehicle;

[0070] The water surface unmanned ship comprises a radio module, a controller, a GPS positioning system, a water acoustic communication machine, a millimeter wave radar and a propeller;

[0071] The underwater unmanned vehicle comprises a controller, a water acoustic communication machine, a Doppler log, a fiber-optic inertial navigation system, a side-scan sonar, a downward-looking multi-beam sonar and a propeller.

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

[0073] The present application firstly establishes a mixed motion model of the water surface unmanned ship and the underwater unmanned vehicle, adopts a vertical projection and line-of-sight method combined guidance method to complete collaborative guidance, and realizes effective estimation and compensation of external disturbances to the controller by constructing a preset time disturbance observer, thereby solving the adverse effects of external disturbances on the water surface and underwater heterogeneous collaborative control system; the controller proposed in the present application combines the advantages of fast non-singular terminal sliding mode, integral sliding mode and double main item reaching law, and can effectively realize expected tracking performance and suppress control signal jitter.

[0074] Compared with model predictive control, the control method of the present application has smaller calculation amount and can complete collaborative tasks in real time; compared with PID control, the control method of the present application does not need frequent adjustment of parameters and is more convenient to apply; compared with integral sliding mode control, terminal sliding mode control and other methods, the control method of the present application has higher speed, more accurate and more robust tracking performance when facing disturbances caused by wind, waves and currents in the ocean environment. While improving the accuracy and safety of the water surface and underwater collaborative system in marine target detection operations, the overall operation efficiency is also improved. BRIEF DESCRIPTION OF DRAWINGS

[0075] Figure 1 It is a structure schematic diagram of the water surface and underwater heterogeneous collaborative control overall device based on a preset time disturbance observer of the present application;

[0076] Figure 2 It is a water surface and underwater collaborative control communication device diagram of the present application;

[0077] Figure 3 It is a flow chart of the water surface and underwater heterogeneous collaborative control method based on a preset time disturbance observer of the present application;

[0078] Figure 4 It is a water surface and underwater collaborative vertical projection fusion line-of-sight method guidance schematic diagram of the present application;

[0079] Figure 5 The flow chart of the water surface and underwater heterogeneous collaborative control is provided for the application.

[0080] Figure 6 The three-dimensional collaborative trajectory tracking simulation diagram of the water surface unmanned ship and underwater unmanned vehicle in an embodiment of the application is provided. DETAILED DESCRIPTION

[0081] The technical solutions of the application will be specifically described below with reference to the accompanying drawings. Figures 1-6 The technical solutions of the application will be specifically described below with reference to the accompanying drawings.

[0082] As shown in the figure, the application provides a water surface and underwater heterogeneous collaborative control system based on a preset time disturbance observer, which comprises a shore-based device, a water surface unmanned ship and an underwater unmanned vehicle. Figure 1 Specifically, as shown in the figure, the shore-based device is connected with the water surface unmanned ship through radio, and sends the expected trajectory to the water surface unmanned ship and obtains the state information of the water surface unmanned ship according to the task requirement. Figure 2 Specifically, the water surface unmanned ship obtains the state information of the body through the GPS positioning system, and sends it back to the shore-based device after being packaged. Further, the underwater unmanned vehicle obtains the state information of the water surface unmanned ship through the underwater acoustic communication machine, and sends the state information of the body to the water surface unmanned ship. Specifically, the water surface unmanned ship is equipped with a dry end of the underwater acoustic communication machine, and the underwater unmanned vehicle is equipped with a wet end of the underwater acoustic communication machine. The underwater unmanned vehicle obtains the state information of the body through the fiber-optic inertial navigation and Doppler log, corrects the obtained state information of the body through the positioning function of the underwater acoustic communication machine, and sends it to the water surface unmanned ship after being packaged. Further, the shore-based device can obtain the state information of the water surface unmanned ship and the underwater unmanned vehicle in real time. Further, the main control converts the calculated control torque into a control instruction and sends it to the propeller, and the propeller executes the instruction to complete the collaborative tracking. Further, when the water surface and underwater heterogeneous collaborative system sails to the designated task area, the shore-based device issues the operation side-scan sonar and downward-looking multi-beam sonar instruction, which is transmitted to the underwater unmanned vehicle through the water surface unmanned ship as a communication gateway, and the underwater unmanned vehicle executes the instruction to carry out the marine target detection operation.

[0083] The application also provides a water surface and underwater heterogeneous collaborative control method based on a preset time disturbance observer, which comprises the following steps:

[0084] Step 1: The shore-based device sends the expected trajectory to the water surface unmanned ship according to the task target.

[0085] Step 2: The water surface unmanned ship obtains the expected trajectory through the radio module, and obtains the state information of the body through the GPS positioning system.

[0086] Step 3, the underwater unmanned vehicle obtains the state information of the surface unmanned ship through the underwater acoustic communication machine, and obtains the body state information through the optical fiber inertial navigation device;

[0087] Step 4, the surface unmanned ship controller analyzes and calculates according to the specified desired trajectory, the actual state information of the unmanned ship body and the compensation value obtained by the observer to obtain the desired control torque; the underwater unmanned vehicle analyzes and calculates according to the desired 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 desired control torque;

[0088] Step 5, the control torque command calculated by the main control is sent to the propeller to realize the heterogeneous collaborative control of the surface unmanned ship and the underwater unmanned vehicle;

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

[0090] For step 1, the sailing trajectory is formulated according to the demand of marine target detection, and the sailing trajectory is sent to the surface unmanned ship by the shore-based device 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 latitude and longitude into the position vector of the surface unmanned ship in the inertial coordinate through coordinate conversion;

[0092] For step 3, the present application proposes a surface and underwater collaborative control communication device, which is referred to Figure 2 , the surface unmanned ship acts as a communication gateway between the underwater unmanned vehicle and the shore-based device, the underwater unmanned vehicle obtains the state information of the surface unmanned ship through the underwater acoustic communication machine, and obtains the body state information through the optical fiber inertial navigation device, and at the same time, the positioning function of the underwater acoustic communication machine is used to correct the body position estimation of the optical fiber inertial navigation, so as to improve the accuracy of the body state information;

[0093] For step 4, the improved adaptive integral terminal sliding mode method is applied in the surface and underwater heterogeneous collaborative control, and combined with the preset time disturbance observer, the overshoot and chattering are greatly reduced. The safety and robustness of the surface and underwater heterogeneous collaborative system in the marine target detection operation are improved, and the process is referred to Figure 3 , the specific process includes the following steps:

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

[0095] Step 4.2, a guidance method combining vertical projection and line-of-sight method is used, so that the underwater unmanned vehicle obtains the actual sailing trajectory of the surface unmanned ship, and its guidance chart is shown in Figure 4 ;

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

[0097] Step 4.4, the surface unmanned ship controller analyzes and calculates the expected control torque according to the specified expected trajectory, the actual state information of the unmanned ship body, and the compensation value obtained by the observer; the underwater unmanned vehicle controller analyzes and calculates the expected control torque 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, and 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 collaborative control, and combined with the preset time disturbance observer, the overshoot and chattering are greatly reduced. The safety and robustness of the surface and underwater heterogeneous collaborative system in the marine target detection operation are improved.

[0099] For step 5, the control torque calculated by the main control is issued to the propeller to realize the 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 are collaboratively sailing to the task position, the side-scan sonar and the downward-looking multi-beam sonar are started to detect targets in the task 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] Wherein, x L , y L represent the position of the surface unmanned ship, ψ L represents the heading angle of the surface unmanned ship, u L , v L , r L represent the pitch speed, roll speed and yaw angle speed of the surface unmanned ship respectively. x F , y F , z F represent the position of the underwater unmanned vehicle. ψ F represents the heading angle of the underwater unmanned vehicle, u F , v F , wF F denote the pitch, roll, vertical velocity and yaw rate 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] where x L , y L , z L denote the position of the surface unmanned ship, ψ L denote the yaw angle of the surface unmanned ship, u L , v L , r F denote the pitch, roll and yaw rate of the surface unmanned ship, respectively. x F , y F , z F denote the position of the underwater unmanned vehicle. ψ F denote the yaw angle of the underwater unmanned vehicle, u F , v F , w F , r L denote the pitch, roll, vertical velocity and yaw rate of the underwater unmanned vehicle, respectively. τ Lu = [τ Lr , 0, τ T ] L denote the control force and torque acting on the surface unmanned ship, d Lu = [d Lv , d Lr , d T ] F denote the unknown time-varying environmental disturbance caused by the sea current and wave. τ Fu = [τ Fw , 0, 0, 0, τ Fr , τ T ] F denote the control force and torque acting on the underwater unmanned vehicle, d Fu = [d Fv , d Fw , d Fr , d T ] L denote the unknown time-varying environmental disturbance caused by the sea current and wave. m L11 = [m L22 , m L33 , m T denote the added mass of the surface unmanned ship, m F = [m F11 ​m F22 ,m F33 ,m F44 ] T denotes the added mass of the underwater unmanned vehicle.

[0108] The application proposes an improved adaptive integral terminal sliding mode control method based on a preset time disturbance observer, considering external disturbances caused by wind waves and other factors, to realize safe and accurate stable cooperative tracking of the surface unmanned ship and the underwater unmanned vehicle.

[0109] In this embodiment, the guidance algorithm is used to guide one underwater unmanned vehicle to track the trajectory of one surface unmanned ship for underwater detection in 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 the vertical projection through the line-of-sight method.

[0110] First, the actual navigation trajectory of the surface unmanned ship is projected to the underwater through the vertical projection, and the state information of the surface unmanned ship is transmitted to the underwater unmanned vehicle through the underwater acoustic communication machine;

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

[0112]

[0113] wherein

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

[0115]

[0116] At the same time, the derivative of the above position error is obtained, and the tracking error dynamic is obtained:

[0117]

[0118] wherein, denotes the resultant velocity 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 yaw guidance law is designed as:

[0120]

[0121] The desired velocity guidance law is:

[0122]

[0123] where, k is a parameter greater than 0; and Δ is a look-ahead distance, generally 2-5 times the length of the ship.

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

[0125]

[0126] The present application considers the disturbance of wind, wave and current in the marine environment and proposes an improved adaptive integral terminal sliding mode control method based on a preset time disturbance observer, which can effectively solve the above problems and realize safe and accurate stable cooperative tracking of the surface unmanned ship and the underwater unmanned vehicle.

[0127] In the embodiment, the preset time disturbance observer is designed as follows:

[0128] Through the design of the observer, the external disturbance can be effectively observed, and for the underwater unmanned vehicle, the observer design of the surface unmanned ship is the same, and the process is as follows

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

[0130]

[0131] wherein, η=[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, and ψ 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 body 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 propeller control input, τ u is the forward force, τ w is the vertical force, and τ r is the yaw force; d=[d u ,d v ,d w ,d r ] TM is the matrix of the weight inertia and the additional inertia of the underwater vehicle caused by the water dynamics; J(ψ) is the coordinate transformation matrix; C(v) is the Coriolis-centripetal matrix; D(v) is the water dynamic damping parameter matrix; and g(η) is the restoring force and moment caused by the gravity / buoyancy.

[0132] In order to eliminate the negative influence of the unknown disturbance in a predetermined time, the predetermined time disturbance observer is constructed as

[0133]

[0134] wherein K a and K b are positive definite parameter diagonal matrices to be designed, represents the estimation of the disturbance, and the estimation error can be defined as K b is selected to satisfy β(t) is a time-varying function used for the design of the timing controller, and its mathematical form can be expressed as

[0135]

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

[0137]

[0138] wherein ρ is a design parameter, and satisfies I 3×1 is the identity vector. Since the external disturbance is slow time-varying, a conservative value can be selected to meet this requirement.

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

[0140] Firstly, the following adaptive integral terminal sliding mode variable of the mixed system of the surface unmanned ship and the underwater vehicle is constructed

[0141]

[0142] wherein, 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 and the actual heading angle of the unmanned ship, and e Fu =uFd -u F represents the error of the desired forward velocity of the unmanned underwater vehicle and the actual forward velocity, e Fψ = ψ Fd - ψ F represents the error of the desired heading angle of the unmanned underwater vehicle and the actual heading angle, e Fw = w Fd - w F represents the error of the desired heave velocity of the unmanned underwater vehicle and the actual heave velocity, e

[0143] α Lu , α Lψ , α Fu , α Fψ , β Lψ , β Fψ , λ Fw are positive parameters to be designed; e LI,u is designed as e LI,ψ , e FI,u , e FI,ψ , e FI,w The design of p and q is the same as that of p and q, and p and q are selected as positive odd numbers, and the condition p>q>0 is met.

[0144] Then the first derivative of the sliding mode variable s i is equal to 0, that is Without considering all the uncertain terms in the system, the equivalent control moment is obtained:

[0145]

[0146] In order to achieve the ideal stable convergence requirement, improve the robustness of the system to uncertainty, and speed up the response speed of the system, the switching control moment 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, sigma, gamma are constant parameters, and c1>0, c2>0, sigma>1, 0<gamma<1; in practical application, the sliding variable often shakes due to measurement noise, which leads to overestimation of k1. In order to alleviate this problem, the following dead zone technology is used to adjust the adaptive law.

[0149]

[0150] And the following adaptive law is used to update

[0151]

[0152] where k1 is a dead-zone gain parameter, ε is a dead-zone threshold parameter, μ>0 is a constant parameter, k min is an adaptive law threshold parameter, k δ >0 is a constant parameter. If s i is greater than μ, the adaptive controller adjusts k1 to increase; if s i is lower than μ, it decreases until k min , so as to ensure no zero gain and reduce unnecessary control gain. Considering the existence of unknown disturbance, the disturbance estimation value of the preset time disturbance observer is needed to be fed back in the following controller design to realize the stable operation of the surface-underwater collaborative system.

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

[0154]

[0155] Through the above description, the specific process of a surface-underwater heterogeneous collaborative control method based on a preset time disturbance observer is described. First, the guidance of the surface-underwater heterogeneous collaborative control is completed by fusing the vertical projection and the line-of-sight method; then, the external disturbance is estimated by using the preset time disturbance observer, and is compensated in the controller, so as to improve the anti-interference ability of the system and make the system run more smoothly; finally, a new improved adaptive integral terminal sliding mode control method suitable for surface-underwater collaborative control is proposed, which increases the tracking accuracy of the system, improves the robustness and rapidity of the system, and is more in line with the requirements of high tracking accuracy, strong anti-interference ability, safety, fast response, stability and reliability of the surface-underwater collaborative system in the marine target detection operation.

[0156] The above is the preferred embodiment of the present application, and any changes made according to the technical solutions of the present application, as long as the generated function does not exceed the scope of the technical solutions of the present application, belong to the protection scope of the present application.

Claims

1. A water surface and underwater heterogeneous collaborative control method based on a preset time disturbance observer, characterized in that, The method comprises the following steps: Step 1, the shore-based equipment sends a desired trajectory to the surface unmanned ship according to a task target; Step 2, the surface unmanned ship obtains the desired trajectory through a radio module and obtains the state information of the unmanned ship body through a positioning system; Step 3, the underwater unmanned vehicle obtains the state information of the surface unmanned ship through a hydrophone communication machine and obtains the state information of the underwater unmanned vehicle body through a fiber-optic inertial navigation device; Step 4, the surface unmanned ship controller analyzes and calculates according to the specified desired trajectory, the actual state information of the unmanned ship body and the compensation value obtained by the preset time disturbance observer, and obtains a desired control torque; The underwater unmanned vehicle controller analyzes and calculates according to the desired 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 disturbance observer, and obtains a desired control torque; Step 5, the surface unmanned ship controller and the underwater unmanned vehicle controller respectively issue the control torque instructions calculated to the surface unmanned ship controller and the propeller of the underwater unmanned vehicle, so as to realize the 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 cooperatively sail to the task position, a side-scan sonar and a downward-looking multi-beam sonar are started to detect targets in the task area; The step 4 comprises the following specific steps: Step 4.1, a mixed motion model of the surface unmanned ship and the underwater unmanned vehicle is established; the mixed motion model of the surface unmanned ship and the underwater unmanned vehicle comprises a nonlinear kinematic model and a nonlinear dynamic model of a mixed system of the surface unmanned ship and the underwater unmanned vehicle; Step 4.2, a vertical projection is used to fuse a line-of-sight method to construct a trajectory deviation of the surface unmanned ship relative to a reference trajectory and a reference trajectory deviation of the underwater unmanned vehicle relative to the surface unmanned ship; Step 4.3, a preset time disturbance observer is constructed to estimate disturbance factors of the outside world to the surface-underwater heterogeneous collaborative system during operation; Step 4.4, based on the disturbance estimation value of the outside world of the preset time disturbance observer and an improved adaptive integral terminal sliding mode control method, a control law is designed to calculate a desired control torque; The following method is used to construct the preset time disturbance observer of the surface unmanned ship or the underwater unmanned vehicle in the step 4.3: （11） wherein, a matrix for representing the inertial composition of the weight inertia of the surface unmanned ship or underwater unmanned vehicle and the hydrodynamic added inertia, a velocity vector of the surface unmanned ship in the inertial coordinate system or a velocity vector of the underwater unmanned vehicle in the body coordinate system; and are positive definite parameter diagonal matrices to be designed, represents an estimate of the disturbance; the choice of ; is a time-varying function for the design of the timed controller: （12） where is a positive design parameter, denotes the maximum allowed settling time; is the auxiliary state vector of the disturbance observer, updated by the following adaptive law: （13） wherein is a design parameter, satisfying , is the identity vector; is the Coriolis-Centrifugal matrix; is the hydrodynamic damping parameter matrix, is the restoring force and moment due to gravity / buoyancy, is the control vector of the surface unmanned ship propeller or underwater unmanned vehicle propeller control input.

2. The water surface and underwater heterogeneous collaborative control method based on preset time disturbance observer according to claim 1, characterized in that, The state information of the unmanned ship body is obtained through a GPS positioning system, wherein the position latitude and longitude are converted into a position vector of the surface unmanned ship in an inertial coordinate through coordinate conversion.

3. The water surface and underwater heterogeneous collaborative control method based on preset time disturbance observer according to claim 1, characterized in that, The state information of the underwater unmanned vehicle body is obtained through fiber-optic inertial navigation and Doppler log calculation, and the obtained state information is corrected through the positioning function of the hydrophone communication machine.

4. The water surface and underwater heterogeneous collaborative control method based on preset time disturbance observer according to claim 1, characterized in that, The nonlinear kinematic model is specifically as follows: （1） wherein , denotes the position of the surface unmanned ship, denotes the heading angle of the surface unmanned ship, , , denote the pitch velocity, the roll velocity and the yaw angular velocity of the surface unmanned ship, respectively; , , denote the position of the underwater unmanned vehicle, denotes the heading angle of the underwater unmanned vehicle, , , , denote the pitch velocity, the roll velocity, the vertical velocity and the yaw angular velocity of the underwater unmanned vehicle, respectively.

5. The method of claim 1, wherein the preset time disturbance observer is based on a water surface and underwater heterogeneous collaborative control method. The nonlinear dynamic model is specifically as follows: （2） wherein represents the control force and torque acting on the surface UUV, represents unknown time-varying environmental disturbances caused by currents and waves to the surface UUV; represents the control force and torque acting on the underwater AUV, represents unknown time-varying environmental disturbances caused by currents and waves to the underwater AUV; represents the added mass of the surface UUV, represents the added mass of the underwater AUV.

6. The water surface and underwater heterogeneous collaborative control method based on preset time disturbance observer according to claim 1, characterized in that, The step 4.2 specifically comprises the following steps: Step 4.2.1, the actual sailing trajectory of the surface unmanned ship is projected to the underwater through vertical projection, and the state information of the surface unmanned ship is transmitted to the underwater unmanned vehicle through a hydrophone communication machine; Step 4.2.2, underwater unmanned vehicle obtains a navigation trajectory of the unmanned ship , and a tangential angle of the technical trajectory : （3） wherein , ; Step 4.2.

3. Calculate the position error of the underwater unmanned vehicle and the surface unmanned ship actual positions in the trajectory reference coordinate system under: （4） Derivation of the position error in the above formula, the tracking error dynamic is obtained: （5） wherein, represents the resultant velocity of the underwater unmanned vehicle; is the sideslip angle of the underwater unmanned vehicle; is the total velocity of the target point; Step 4.2.

4. Deriving the desired yaw guidance law and the desired velocity guidance law to derive the velocity error of the underwater vehicle and the yaw error : （8） （6） （7） wherein ; is a parameter greater than 0; is the forward looking distance, taken as 2-5 times the length of the ship.

7. The method of claim 1, wherein the preset time disturbance observer is based on a water surface and underwater heterogeneous collaborative control method. The step 4.4 specifically includes the following steps: Step 4.4.1, construct the following water unmanned ship-underwater unmanned vehicle hybrid system adaptive integral terminal sliding mode variable （14） wherein, represents an error between the desired forward velocity and the actual forward velocity of the unmanned ship, represents an error between the desired heading angle and the actual heading angle of the unmanned ship, represents an error between the desired forward velocity and the actual forward velocity of the unmanned underwater vehicle, represents an error between the desired heading angle and the actual heading angle of the unmanned underwater vehicle, represents an error between the desired heave velocity and the actual heave velocity of the unmanned underwater vehicle; are positive parameters to be designed; is calculated as follows: wherein the parameters and are chosen to be positive odd integers, while satisfying the condition ; Step 4.4.2, let the sliding mode variable the first derivative of the sliding mode variable is equal to 0, i.e. and without considering all the uncertainties in the system, the equivalent control moment is obtained: （15） The switching control torque is designed as follows: （16） wherein is a constant parameter, is an adaptive gain parameter, ; , , , are constant parameters, and > 0, > 0, > 1, 0 < 1 The following dead zone technology is used to adjust the adaptive law; （17） And the following adaptive law is updated （18） wherein is a dead-zone gain parameter, is a dead-zone threshold parameter, > 0 is a constant parameter, is an adaptive law threshold parameter, > 0 is a constant parameter; if is greater than , the adaptive controller adjusts to increase; if is less than , is decreased until is reached to ensure no zero gain and to reduce unnecessary control gain; Step 4.4.

3. Estimation of external disturbance value in combination with preset time disturbance observer The final control law is designed as follows （19）。 8. A surface-underwater heterogeneous cooperative control system based on a preset time disturbance observer, characterized in that, The system uses the water-surface and underwater heterogeneous collaborative control method based on the preset time disturbance observer in any one of claims 1-7 to realize control, including a shore-based device, a water unmanned ship and an underwater unmanned vehicle; The water unmanned ship includes a radio module, a controller, a GPS positioning system, a water acoustic communication machine, a millimeter wave radar and a propeller; The underwater unmanned vehicle includes a controller, a water acoustic communication machine, a Doppler log, an optical fiber inertial navigation system, a side-scan sonar, a downward-looking multi-beam sonar and a propeller.

Images