A towed underwater robot and its navigation depth control method

Abstract

A towed underwater robot and its navigation depth control method belong to the technical field of underwater robot control. To solve the problem of precise depth control of underwater robots. In the sealed cabin of the present invention, there are a communication module and an industrial control computer. Outside the sealed cabin, there are a detection device, an underwater depth sensor, a thruster, a lifting mechanism, and a power supply; the industrial control computer is connected to the communication module, the detection device, the underwater depth sensor, the thruster, the lifting mechanism, and the power supply; the power supply is connected to the detection device, the underwater depth sensor, the thruster, and the lifting mechanism; the thruster is a vertical plane thruster for realizing the heaving motion of a towed underwater robot; the detection device is used for receiving and sending signals; the underwater depth sensor is used for detecting underwater depth data. The present invention realizes depth control in a complex environment by installing a thruster on the vertical plane of the towed underwater robot to achieve up and down adjustment, gives a dynamic model, designs an active disturbance rejection depth controller, and realizes the depth control function.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater robot control, and particularly relates to a towed underwater robot and a method for controlling its navigation depth. Background Art

[0002] Due to the danger and complexity of the seabed environment, manned underwater operations are very dangerous, which urgently requires the research of a high-tech unmanned underwater detector. As a high-tech ocean exploration carrier, the towed underwater robot has emerged as the times require. The research and development of the towed underwater robot are the products of the times and an organic combination of technology and information. Currently, many different models of towed underwater robots have been successively put into use, playing a huge role in the development and utilization of the ocean by humans.

[0003] Currently, the research on the depth control of towed underwater robots at home and abroad mainly focuses on three directions: underwater suspended depth traction or tow cable stage classification, retracting and releasing the tow cable, and controlling the rudder or thruster of the towed underwater robot. Anirban Nag et al. achieved the simultaneous control of the heading angle, pitch angle, and depth of an AUV through a fuzzy logic controller; scholars such as Wang Wei designed a fuzzy neural network to optimize and adjust the parameters of a sliding mode variable structure controller, achieving the control of the depth of an underwater robot.

[0004] However, the towed underwater robot is disturbed by both the tow cable and the environment during movement. The tow cable interference includes flow field interference, self-vibration interference, and tow ship interference, resulting in a very complex force analysis of the tow cable and the tow body of the towed underwater robot. In addition, its movement is a real-time changing process and the motion system of the towed underwater robot is highly nonlinear, making it difficult to model and analyze, resulting in difficult to accurately achieve depth control. Summary of the Invention

[0005] The problem to be solved by the present invention is the accurate depth control of an underwater robot, and a towed underwater robot and a method for controlling its navigation depth are proposed.

[0006] To achieve the above object, the present invention is realized through the following technical solutions:

[0007] Solution 1: A towed underwater robot, comprising a sealed cabin, a communication module, an industrial control computer, a detection device, an underwater depth sensor, a thruster, a lifting mechanism, and a power supply;

[0008] The communication module and the industrial control computer are arranged inside the sealed cabin, and the detection device, the underwater depth sensor, the thruster, the lifting mechanism, and the power supply are installed outside the sealed cabin;

[0009] The industrial control computer is respectively connected to the communication module, the detection device, the underwater depth sensor, the thruster, the lifting mechanism, and the power supply;

[0010] The power supply is respectively connected to the detection device, the underwater depth sensor, the thruster, and the hoisting mechanism;

[0011] The thruster is a vertical plane thruster, which is used to realize the heaving motion of a towed underwater robot;

[0012] The detection device is used to receive and send signals;

[0013] The underwater depth sensor is used to detect underwater depth data.

[0014] Further, the motor of the thruster is a DC three-phase brushless motor, and the rotation of the motor of the thruster drives the propeller to rotate.

[0015] Further, the calculation formula for the underwater depth H of the underwater depth sensor is:

[0016] P = ρgH + P0

[0017] Wherein, P is the current pressure value, ρ is the fluid density, and P0 is the atmospheric pressure.

[0018] Solution 2: A navigation depth control method for a towed underwater robot, including the following steps:

[0019] S1. The underwater depth sensor collects the underwater depth data of the underwater robot, and the underwater robot is a towed underwater robot described in Technical Solution 1;

[0020] S2. Construct a mathematical model of the underwater robot, and then conduct an underwater mechanical analysis on the underwater robot to construct a vertical plane dynamic mathematical model of the underwater robot;

[0021] S3. Based on the vertical plane dynamic mathematical model of the underwater robot constructed in step S2, design a depth-keeping motion controller based on the active disturbance rejection control of the immune genetic algorithm to achieve the stable depth-keeping motion of the underwater robot within a speed of 2 knots and under interference conditions;

[0022] S4. Based on the underwater depth data collected in step S1, determine whether the underwater robot meets the expected underwater depth. If not, use the collected underwater depth data as feedback and restart a new round of control of the underwater robot until the underwater robot meets the underwater depth requirements.

[0023] Further, the implementation method of step S2 includes the following steps:

[0024] S2.1. Set the six-degree-of-freedom kinematic equation of the underwater robot as:

[0025]

[0026] Among them, x g , y g , z g are the center-of-gravity positions of the underwater robot respectively, I xx , I yy , I zz are the moments of inertia of the underwater robot about the X, Y, and Z axes respectively. The forces and torques acting on the underwater robot about the x, y, and z axes are X, Y, Z, K, M, and N respectively. The linear velocities and angular velocities about the x, y, and z axes are u, v, w, p, q, and r respectively;

[0027] S2.2. Analyze the forces acting on the underwater robot underwater. The calculation formula for the resultant force on the underwater robot is:

[0028] F = F T + W + F L + f + B

[0029] where F is the resultant force on the underwater robot, F T is the towing force of the tow cable, W is the gravity, B is the buoyancy, F L is the hydrodynamic force, and f is the thrust of the thruster;

[0030] The calculation formula for the water resistance received by the antenna carried by the underwater robot on the oncoming flow surface is:

[0031]

[0032] where F LX is the component force of the water resistance received by the antenna in the axial direction, F LY is the component force of the water resistance received by the antenna in the radial direction, A is the cross-sectional area of the antenna, v is the water velocity on the oncoming flow surface, and γ is the angle formed by the water flow direction and the axial direction of the underwater robot;

[0033] The calculation formula for the restoring force vector g(η) generated by gravity and buoyancy is:

[0034]

[0035] The forces on the underwater robot body in three directions by the tow cable, the calculation formula is:

[0036]

[0037] S2.3. Construct the mathematical model of the thruster thrust T as:

[0038] T = ρn 2 D 4 K T

[0039] where K Tis the thrust coefficient, ρ is the seawater density, D is the propeller diameter, and n is the rotational speed of the propeller;

[0040] S2.4. Based on the above steps, the vertical-plane dynamic mathematical model of the underwater robot is constructed as follows:

[0041]

[0042] where m is the mass of the underwater robot, x g , y g , z g are the center-of-gravity positions of the underwater robot respectively, X T is the end tension of the tow cable, X hs , Z hs , M hs are the static forces and pitching moment in the lateral and vertical directions respectively; Z uw is the lift coefficient, M uw is the lift moment; X u|u| is the axial drag coefficient, Z w|w| , Z q|q| , M w|w| , M q|q| are the lateral drag coefficients, is the axial inertial added force, is the lateral flow added force, X wq , Z uq , Z uw , M uq , M uw are the cross-coupling inertial forces, T is the thruster thrust, F LX F LY are the components of the water resistance forces received by the detection device in the axial and radial directions respectively, Z T , X T are the components of the towing force received by the underwater robot in the axial and vertical directions.

[0043] Further, the implementation method of step S3 includes the following steps:

[0044] S3.1. Design a differential tracker with the set depth z0 as the desired depth to track the desired depth and obtain the target differential to eliminate the differential error. The mathematical model of the differential tracker is:

[0045]

[0046] where x1 is the approximate differential signal of the tracking input, r is the fast factor, h is the filtering factor, and fhan is the fastest control synthesis function;

[0047] And there are calculation formulas as:

[0048]

[0049] S3.2. Design an extended state observer using the output z and input u of the underwater robot. The mathematical model of the extended state observer is as follows:

[0050]

[0051] Among them, β 01 , β 02 , β 03 are gain coefficients, z1 is the estimated value of the current actual depth z, b is the correction coefficient of the input signal δ s , δ is an adjustable parameter, and the sign function in the fal function is replaced by the sigmoid function;

[0052] S3.3. Design the nonlinear state error feedback law and disturbance compensation for the underwater robot. The expression is as follows:

[0053]

[0054] Among them, e1 is the depth value deviation, e2 is the depth value deviation change rate, and δ s is the input signal, that is, the controller output value; β1 and β2 are nonlinear error feedbacks;

[0055] S3.4. Design a depth-holding motion controller based on the active disturbance rejection control using the immune genetic algorithm.

[0056] Furthermore, the implementation method of step S3.4 includes the following steps:

[0057] S3.4.1. Set the calculation formula for probability selection based on antibody concentration as follows:

[0058]

[0059] Among them, ρ(x i ) is the distance of the specified antibody f(x i ) on a non-empty immune set X;

[0060] Encode the antibodies for the parameters to be optimized, select the parameter benchmark using the empirical method, and design using the immune genetic method;

[0061] S3.4.2. Based on the parameter benchmark selected in step S3.4.1, design the objective function. The expression is as follows:

[0062]

[0063] Among them, e(t) is the error value of the system, u(t) is the controller output value, and t uis the rise time, w1, w2, w3, and w4 are weights, y(t) is the output value of the underwater robot system, and |ey(t)| is the overshoot;

[0064] The fitness function is designed as:

[0065]

[0066] where A is a constant greater than 0 to prevent the algorithm from overflowing due to the denominator approaching 0;

[0067] S3.4.3. Use the probability selection formula of the antibody concentration constructed in step S3.4.1 to perform diversity preservation and population update on the objective function obtained in step S3.4.2;

[0068] S3.4.4. Perform crossover and mutation on the objective function, and use an adaptively adjusted mutation rate p m for mutation. The calculation formula is:

[0069]

[0070] where: f max is the maximum fitness value in the population, f avg is the average fitness value of each generation of the population, f min is the minimum fitness value in the population, and f is the fitness value of the active disturbance rejection control parameter group to be mutated;

[0071] The crossover rate p c adopted is calculated as:

[0072]

[0073] where f' is the larger one of the fitness values of the two groups of parameters for crossover.

[0074] Advantages of the present invention:

[0075] For the depth control method of a towed underwater robot described in the present invention, depth control is achieved by controlling the thruster of the towed underwater robot, and a more advanced and reliable control strategy is adopted to achieve precise control means. In view of the problem that the active disturbance rejection control parameters are numerous and difficult to adjust, the present invention uses an immune genetic algorithm to optimize some of the parameters in the active disturbance rejection controller to improve the adjustment efficiency of the active disturbance rejection controller.

[0076] For the towed underwater robot described in the present invention, it effectively solves the problem of resisting ocean current interference under large interference conditions in water. By controlling the thruster, the underwater robot can reach the expected depth position and maintain it unchanged, achieving the goal of depth control;

[0077] A towed underwater robot according to the present invention has a compact structure and reasonable space utilization. It makes full use of the characteristics of the underwater robot's structure to minimize the disturbance of the body, and has a strong ability to resist external interference in its external structure;

[0078] A depth control method for a towed underwater robot according to the present invention. Active disturbance rejection control is applicable to model control under large disturbances. The purpose of the present invention is to solve the depth control under large underwater disturbances, which is more suitable for combination with active disturbance rejection control. And aiming at some deficiencies in active disturbance rejection control, an immune genetic algorithm is used to make up for it to achieve precise depth control, providing an effective reference solution for the depth control of underwater robots, and having important practical engineering significance and theoretical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 It is a structural schematic block diagram of a towed underwater robot according to the present invention;

[0080] Figure 2 It is a design drawing of a towed underwater robot according to the present invention;

[0081] Figure 3 It is a propeller drive circuit diagram of a towed underwater robot according to the present invention;

[0082] Figure 4 It is a hydrodynamic simulation analysis diagram of a towed underwater robot according to the present invention;

[0083] Figure 5 It is a flow chart of a depth control method for a towed underwater robot according to the present invention;

[0084] Figure 6 It is a flow chart of the immune genetic algorithm in the depth control method for a towed underwater robot according to the present invention;

[0085] Figure 7 It is a fitting curve of the force on the upstream-facing surface in the depth control method for a towed underwater robot according to the present invention;

[0086] Figure 8 It is a flow chart of the active disturbance rejection depth control in the depth control method for a towed underwater robot according to the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0087] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention, that is, the specific embodiments described are only a part of the embodiments of the present invention, rather than all of the specific embodiments. The components of the specific embodiments of the present invention usually described and shown in the accompanying drawings herein can be arranged and designed in various different configurations, and the present invention can also have other embodiments.

[0088] Therefore, the following detailed description of the specific embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed present invention, but merely represents the selected specific embodiments of the present invention. All other specific embodiments obtained by those skilled in the art based on the specific embodiments of the present invention without creative efforts fall within the scope of protection of the present invention.

[0089] In order to further understand the content, features and effects of the present invention, the following specific embodiments are exemplified and described in detail in conjunction with the accompanying drawings as follows: Specific Embodiment 1:

[0091] A towed underwater robot, comprising a sealed cabin 1, a communication module 2, an industrial control computer 3, a detection device 4, an underwater depth sensor 5, a thruster 6, a lifting mechanism 7, and a power supply 8;

[0092] The communication module 2 and the industrial control computer 3 are arranged inside the sealed cabin 1, and the detection device 4, the underwater depth sensor 5, the thruster 6, the lifting mechanism 7, and the power supply 8 are installed outside the sealed cabin 1;

[0093] The industrial control computer 3 is respectively connected to the communication module 2, the detection device 4, the underwater depth sensor 5, the thruster 6, the lifting mechanism 7, and the power supply 8;

[0094] The power supply 8 is respectively connected to the detection device 4, the underwater depth sensor 5, the thruster 6, and the lifting mechanism 7;

[0095] The thruster 6 is a vertical plane thruster for realizing the heave motion of a towed underwater robot;

[0096] The detection device 4 is used for receiving and sending signals;

[0097] The underwater depth sensor 5 is used for detecting underwater depth data.

[0098] Further, the motor of the thruster 6 is a DC three-phase brushless motor, and the rotation of the motor of the thruster 6 drives the propeller to rotate.

[0099] Further, the calculation formula for the underwater depth H of the underwater depth sensor 5 is:

[0100] P = ρgH + P0

[0101] where P is the current pressure value, ρ is the fluid density, and P0 is the atmospheric pressure. Specific Embodiment 2:

[0103] A navigation depth control method for a towed underwater robot, implemented relying on the towed underwater robot described in Specific Embodiment 1, includes the following steps:

[0104] S1. The underwater depth sensor collects the underwater depth data of the underwater robot;

[0105] S2. Construct a mathematical model of the underwater robot, and then conduct an underwater mechanical analysis of the underwater robot to construct a vertical plane dynamic mathematical model of the underwater robot;

[0106] Further, the implementation method of step S2 includes the following steps:

[0107] S2.1. Set the six-degree-of-freedom kinematic equation of the underwater robot as:

[0108]

[0109] where x g , y g , z g are respectively the center-of-gravity positions of the underwater robot, I xx , I yy , I zz are respectively the moments of inertia of the underwater robot about the X, Y, and Z axes, the forces and torques acting on the x, y, and z axes are X, Y, Z, K, M, N respectively, and the linear velocities and angular velocities about the x, y, and z axes are u, v, w, p, q, r respectively;

[0110] S2.2. Analyze the forces acting on the underwater robot underwater. The calculation formula for the resultant force received by the underwater robot is:

[0111] F = F T + W + F L + f + B

[0112] where F is the resultant force received by the underwater robot, F T is the towing force of the tow cable, W is the gravity, B is the buoyancy, F L is the hydrodynamic force, and f is the thrust of the thruster;

[0113] The calculation formula for the water resistance received by the antenna carried by the underwater robot on the upstream side is:

[0114]

[0115] Among them, F LX is the component force of the water resistance acting on the antenna in the axial direction, F LY is the component force of the water resistance acting on the antenna in the radial direction, A is the cross-sectional area of the antenna, v is the water velocity on the upstream surface, and γ is the angle between the water flow direction and the axial direction of the underwater robot;

[0116] The calculation formula for the restoring force vector g(η) generated by gravity and buoyancy is:

[0117]

[0118] The calculation formulas for the forces exerted by the towing cable on the underwater robot body in three directions are:

[0119]

[0120] S2.3. Construct the mathematical model of the thruster thrust T as:

[0121] T = ρn 2 D 4 K T

[0122] Among them, K T is the thrust coefficient, ρ is the seawater density, D is the propeller diameter, and n is the propeller rotation speed;

[0123] S2.4. Based on the above steps, construct the vertical plane dynamic mathematical model of the underwater robot as:

[0124]

[0125] Among them, m is the mass of the underwater robot, x g , y g , z g are the center of gravity positions of the underwater robot respectively, X T is the end tension of the towing cable, X hs , Z hs , M hs are the static forces and pitching moments in the lateral and vertical directions respectively; Z uw is the lift coefficient, M uw is the lift moment; X u|u| is the axial resistance coefficient, Z w|w| , Z q|q| , M w|w| , M q|q| are the lateral resistance coefficients, is the axial inertial added force, is the lateral flow added force, X wq , Z uq , Zuw , M uq , M uw is the cross - coupled inertial force, T is the thruster thrust, F LX F LY are respectively the component forces of the water resistance received by the detection device in the axial and radial directions, Z T , X T are the component forces of the drag force on the underwater robot in the axial and vertical directions;

[0126] S3. Based on the vertical - plane dynamic mathematical model of the underwater robot constructed in step S2, design a depth - keeping motion controller based on the active disturbance rejection control of the immune genetic algorithm to achieve a stable depth - keeping motion of the underwater robot with a course speed within 2 knots under disturbance conditions;

[0127] Furthermore, the implementation method of step S3 includes the following steps:

[0128] S3.1. Design a differential tracker with the set depth z0 as the desired depth to track the desired depth and obtain the target differential to eliminate the differential error. The mathematical model of the differential tracker is:

[0129]

[0130] where x1 is the approximate differential signal of the tracking input, r is the fast factor, h is the filtering factor, and fhan is the fastest control synthesis function;

[0131] And there is a calculation formula as:

[0132]

[0133] S3.2. Design an extended state observer with the output z and input u of the underwater robot. The mathematical model of the extended state observer is:

[0134]

[0135] where β 01 , β 02 , β 03 is the gain coefficient, z1 is the estimated value of the current actual depth z, b is the correction coefficient of the input signal δ s and δ is the adjustable parameter. The sign function in the function fal is replaced by the sigmoid function;

[0136] S3.3. Design the nonlinear state error feedback law and disturbance compensation for the underwater robot, and the expression is:

[0137]

[0138] Among them, e1 is the depth value deviation, e2 is the change rate of the depth value deviation, and δ s is the input signal, i.e., the output value of the controller; β1 and β2 are non-linear error feedbacks;

[0139] S3.4. Design a depth-keeping motion controller based on the active disturbance rejection control using the immune genetic algorithm;

[0140] Furthermore, the implementation method of step S3.4 includes the following steps:

[0141] S3.4.1. Set the calculation formula for probability selection based on antibody concentration as:

[0142]

[0143] where ρ(x i ) is the distance of the specified antibody f(x i ) on a non-empty immune set X;

[0144] Encode the parameters to be optimized into antibodies, select the parameter benchmark using the empirical method, and design using the immune genetic method;

[0145] S3.4.2. Based on the parameter benchmark selected in step S3.4.1, design the objective function, and the expression is:

[0146]

[0147] where e(t) is the error value of the system, u(t) is the output value of the controller, t u is the rise time, w1, w2, w3, and w4 are weights, y(t) is the output value of the underwater robot system, and |ey(t)| is the overshoot;

[0148] The fitness function is designed as:

[0149]

[0150] where A is a constant greater than 0 to prevent the algorithm from overflowing due to the denominator approaching 0;

[0151] S3.4.3. Use the probability selection formula of antibody concentration constructed in step S3.4.1 to perform diversity preservation and population update on the objective function obtained in step S3.4.2;

[0152] S3.4.4. Perform crossover and mutation on the objective function, and use the self-adaptive adjusted mutation rate p m to perform mutation, and the calculation formula is:

[0153]

[0154] where: fmax is the maximum fitness value in the population, f avg is the average fitness value of each generation of the population, f min is the minimum fitness value in the population, and f is the fitness value of the active disturbance rejection control parameter group to be mutated;

[0155] The crossover rate p adopted c The calculation formula of is as follows:

[0156]

[0157] where f' is the one with the maximum fitness value among the two groups of parameters for crossover.

[0158] S4. Based on the underwater depth data collected in step S1, determine whether the underwater robot meets the expected underwater depth. If not, use the collected underwater depth data as feedback and restart a new round of control of the underwater robot until the underwater robot meets the underwater depth requirement.

[0159] Such as Figure 7 As shown: By fitting the axial force analysis of the underwater robot at different speeds in the hydrodynamic simulation software, it can be seen that the axial force is approximately proportional to the square term of the flow velocity. This shows that the design structure of the underwater robot has a certain rationality.

[0160] It should be noted that relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variant thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not expressly listed, or also includes elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "including a..." does not exclude the existence of additional identical elements in the process, method, article or device including the said element.

[0161] Although the present application has been described above with reference to specific embodiments, various improvements can be made to it and its components can be replaced with equivalents without departing from the scope of the present application. In particular, as long as there is no structural conflict, the various features in the specific embodiments disclosed in the present application can be combined with each other in any way, and the cases of these combinations are not exhaustively described in this specification only for the sake of saving space and resources. Therefore, the present application is not limited to the specific embodiments disclosed herein, but includes all technical solutions falling within the scope of the claims.

Claims

1. A method for controlling the depth of a towed underwater robot, which is realized based on a towed underwater robot, including a sealed cabin (1), a communication module (2), an industrial control computer (3), a detection device (4), an underwater depth sensor (5), a thruster (6), a lifting mechanism (7), and a power supply (8); The communication module (2) and the industrial control computer (3) are arranged inside the sealed cabin (1), and the detection device (4), the underwater depth sensor (5), the thruster (6), the lifting mechanism (7), and the power supply (8) are installed outside the sealed cabin (1); The industrial control computer (3) is respectively connected to the communication module (2), the detection device (4), the underwater depth sensor (5), the thruster (6), the lifting mechanism (7), and the power supply (8); The power supply (8) is respectively connected to the detection device (4), the underwater depth sensor (5), the thruster (6), and the lifting mechanism (7); The thruster (6) is a vertical plane thruster, which is used to realize the heaving motion of a towed underwater robot; The detection device (4) is used to receive and send signals; The underwater depth sensor (5) is used to detect underwater depth data; Characterized in that, It includes the following steps: S1. The underwater depth sensor collects the underwater depth data of the underwater robot; S2. Build a mathematical model of the underwater robot, and then conduct underwater mechanical analysis on the underwater robot to build a vertical plane dynamic mathematical model of the underwater robot; S3. Based on the vertical plane dynamic mathematical model of the underwater robot constructed in step S2, design a depth-keeping motion controller based on an immune genetic algorithm-based active disturbance rejection control to achieve stable depth-keeping motion of the underwater robot at a speed within 2 knots under interference conditions; The implementation method of step S3 includes the following steps: S3.

1. Design a differential tracker, with the set depth z0 as the desired depth, to track the desired depth and obtain the target differential to eliminate the differential error. The mathematical model of the differential tracker is: where x1 is the approximate differential signal of the tracking input, r is the fast factor, h is the filtering factor, fhan is the fastest control synthesis function, e0 is the deviation between the approximate differential signal of the tracking input and the set depth; fh is the fastest control synthesis function based on e, z2, r, h; z3 is the estimated acceleration value of tracking the desired depth, x2 is the estimated value of the speed during the tracking process, e is the deviation between the estimated value of the current actual depth z and the current actual depth, and z2 is the estimated disturbance value; And there is a calculation formula: where d is the product of the fast factor and the filtering factor, d0 is the product of the fast factor and the square of the filtering factor; y is the synthesis of the position estimate and the speed prediction; a0 is the dynamic adjustment parameter; a is the intermediate calculation result of the acceleration, which is used to generate the fastest control signal; sign is the sign function; S3.

2. Design an extended state observer with the output z and input u of the underwater robot. The mathematical model of the extended state observer is: Among them, β 01 , β 02 , β 03 is the gain coefficient, z1 is the estimated value of the current actual depth z, b is the correction coefficient of the input signal δ s , δ is an adjustable parameter, and the sign function in the function fal is replaced by the sigmoid function; f e is the non - linear function in the extended state observer; β 01 , β 02 , β 03 are the first gain coefficient, the second gain coefficient, and the third gain coefficient respectively; a2 is the correction coefficient of the input signal; e1 is the depth value deviation; S3.

3. Design the nonlinear state error feedback law and disturbance compensation for the underwater robot, and the expression is: where, e1 is the depth value deviation, e2 is the change rate of the depth value deviation, and δ s is the input signal, i.e., the controller output value; β1 and β2 are the non-linear error feedbacks; u0 is the preliminary control signal, which is the non-linear feedback of the depth value deviation e1 and the change rate of the depth value deviation e2; S3.

4. Design a depth-keeping motion controller based on an immune genetic algorithm-based active disturbance rejection control; The implementation method of step S3.4 includes the following steps: S3.4.

1. Set the calculation formula for probability selection based on antibody concentration as: Among them, ρ(x i ) is the distance of the specified antibody f(x i ) on a non-empty immune set X; f(x i ) is the i-th specified antibody, ρ c (x i ) is the probability based on antibody concentration, f(x j ) is the j-th specified antibody, and N is the total number of specified antibodies; Encode the parameters to be optimized with antibodies, select the parameter benchmark using the empirical method, and design using the immune genetic method; S3.4.

2. Based on the parameter benchmark selected in step S3.4.1, design the objective function, and the expression is: where, e(t) is the error value of the system, u(t) is the output value of the controller, t u is the rise time, w1, w2, w3, w4 are the weights, y(t) is the output value of the underwater robot system, |ey(t)| is the overshoot, and J is the objective function; The fitness function is designed as: where A is a constant greater than 0 to prevent the algorithm from overflowing due to the denominator approaching 0; S3.4.

3. Use the probability selection formula of antibody concentration constructed in step S3.4.1 to perform diversity maintenance and population update on the objective function obtained in step S3.4.2; S3.4.

4. Cross and mutate the objective function, and use an adaptively adjusted mutation rate P m for mutation. The calculation formula is as follows: Among them, f max is the maximum fitness value in the population, f avg is the average fitness value of each generation of the population, f min is the minimum fitness value in the population, P m1 is the basic mutation rate parameter when the individual fitness is lower than the average fitness of the population, P m2 is the mutation rate parameter when the individual fitness is close to or equal to the maximum fitness of the population, P m3 is the dynamic adjustment parameter when the individual fitness is between the average fitness and the maximum fitness; The adopted crossover rate P c has the following calculation formula: Among them, f' is the one with the largest fitness value among the two groups of crossed parameters; P c1 is the basic crossover rate when the individual fitness is lower than the average fitness, P c2 is the basic crossover rate when the individual fitness is higher than the average fitness, P c3 is the crossover rate adjustment parameter related to the maximum fitness; S4. Based on the underwater depth data collected in step S1, determine whether the underwater robot meets the expected underwater depth. If not, use the collected underwater depth data as feedback and restart a new round of control of the underwater robot until the underwater robot meets the underwater depth requirement.

2. The navigation depth control method of a towed underwater robot according to claim 1, characterized in that The implementation method of step S2 includes the following steps: S2.

1. Set the six-degree-of-freedom kinematic equation of the underwater robot as: where x g , y g , z g are the center-of-gravity positions of the underwater robot, I xx , I yy , I zz are the moments of inertia of the underwater robot about the X, Y, and Z axes, respectively. The forces and torques acting on the underwater robot about the x, y, and z axes are X, Y, Z, K, M, and N, respectively. The linear and angular velocities of the underwater robot about the x, y, and z axes are u, v, w, p, q, and r, respectively; are the first-order derivatives of u, v, w, p, q, and r, respectively; m is the mass of the underwater robot; S2.

2. Analyze the forces acting on the underwater robot underwater. The calculation formula for the resultant force on the underwater robot is: F = F T +W + F L +f + B Among them, F is the resultant force acting on the underwater robot, F T is the towing force of the tow cable, W is the gravity, B is the buoyancy, F L is the hydrodynamic force, and f is the thrust of the thruster; The calculation formula for the water resistance received by the antenna carried by the underwater robot on the upstream side is: Among them, F LX is the component force of the water resistance acting on the antenna in the axial direction, F LY is the component force of the water resistance acting on the antenna in the radial direction, A is the cross-sectional area of the antenna, v w is the water velocity on the upstream surface, γ is the angle between the water flow direction and the axial direction of the underwater robot, Δv w is the Laplace operator of v w ; ρ is the density of seawater; The calculation formula for the restoring force vector g(η) generated by gravity and buoyancy is: where θ is the pitch angle; φ is the roll angle; The forces on the underwater robot body in three directions by the tow cable, the calculation formula is: where T is the thruster thrust; ψ is the yaw angle; S2.

3. Construct the mathematical model of the thruster thrust T as: T = ρn 2 D 4 K T where K T is the thrust coefficient, ρ is the seawater density, D is the propeller diameter, and n is the rotational speed of the propeller; S2.

4. Based on the above steps, construct the vertical plane dynamic mathematical model of the underwater robot as: Among them, m is the mass of the underwater robot, and x g , y g , z g are the center-of-gravity positions of the underwater robot in the x, y, and z directions respectively. X hs , Z hs , M hs are the static forces and pitching moments in the lateral and vertical directions respectively. Z uw is the lift coefficient, and M uw is the lift moment. X u|u| is the axial drag coefficient, and Z w|w| , Z q|q| , M w|w| , M q|q| are the lateral drag coefficients. is the axial inertial added force. is the lateral flow added force. X wq , Z uq , Z uw , M uq , M uw are the cross-coupling inertial forces. T is the thruster thrust, and F LX F LY are the components of the water resistance forces received by the detection device in the axial and radial directions respectively. Z T , X T are the components of the towing force received by the underwater robot in the axial and vertical directions respectively. I yy is the pitching moment inertia coefficient. X q|q| is the pitching water damping coefficient generated in the X-axis direction. M T is the thruster moment.

3. A navigation depth control method for a towed underwater robot according to claim 2, characterized in that The motor of the thruster (6) is a DC three-phase brushless motor, and the rotation of the motor of the thruster (6) drives the propeller to rotate.

4. A navigation depth control method for a towed underwater robot according to claim 3, characterized in that, The calculation formula for the underwater depth H of the underwater depth sensor (5) is: P = ρgH + P0 where P is the current pressure value, ρ is the fluid density, and P0 is the atmospheric pressure.

Images