Underwater robot autonomous depth tracking control method and underwater robot system

Abstract

The invention discloses an autonomous depth tracking control method for an underwater robot. The method comprises the following steps: S1, constructing a hydrodynamic model and a control module of an underwater robot main body; s2, inputting geometric and physical parameters of the underwater robot main body remotely controlled by the umbilical cable; s3, calculating a stable solution of the underwater robot main body in a steady state; s4, introducing a control action of a control mechanism at a time step n; s5, the hydrodynamic load borne by the underwater robot body and the control force generated by the control mechanism are calculated in the CFD sub-module; s6, solving a main program sub-module of the underwater robot body hydrodynamic model; s7, outputting power parameters of the underwater robot main body and the umbilical cable in the time step n; s8, if the numerical simulation is not completed, enabling n to be equal to n + 1, sending an operation instruction to the control mechanism of the underwater robot in the control sub-module according to an operation task requirement, and returning to S4; otherwise, ending the numerical simulation, and outputting all calculation results. The invention is used for the underwater robot.

Description

Technical Field

[0001] The present invention relates to the field of marine observation and operation detection technology, and in particular to an underwater robot autonomous depth tracking control method and an underwater robot system. Background Art

[0002] Deep-sea operations involve strong winds and rough waves, so deep-sea inspection and maintenance operations need to be completed through a combination of surface support vessels, deployment and recovery systems, power umbilical cable management systems, and remote-controlled unmanned submersibles.

[0003] In the prior art, the control methods of some underwater robots only consider the fluid force, gravity and thrust generated by the propeller of the underwater robot itself, but do not fully consider the depth control of the underwater robot by the umbilical cable (such as patent publication number CN115657452A). Relying solely on the propeller, the adjustment speed is very slow, the depth tracking cannot be performed well, and the sudden change of the incoming flow cannot be well dealt with.

[0004] Therefore, based on the above problems, the existing control method needs to be improved to solve the above problems. Summary of the invention

[0005] In response to the problems existing in the above-mentioned prior art, the autonomous depth tracking control method of the underwater robot of the present invention can not only maintain the specified depth, but also realize dynamic depth tracking according to the control instructions, and can effectively solve the depth maintenance problem of the underwater robot caused by the sudden change of horizontal flow and vertical flow.

[0006] In order to solve the above technical problems, the technical solution of the present invention is:

[0007] An autonomous depth tracking control method for an underwater robot, comprising:

[0008] S1: Construct the hydrodynamic model and control module of the underwater robot body;

[0009] S2: Inputting geometric and physical parameters of the underwater robot body remotely controlled by the umbilical cable;

[0010] S3: Calculating the stable solution of the underwater robot body in a steady state;

[0011] S4: introducing the control action of the control mechanism at time step n;

[0012] S5: calculating the hydrodynamic load on the underwater robot body and the control force generated by the control mechanism in the CFD submodule;

[0013] S6: solving the main program submodule of the underwater robot body hydrodynamic model;

[0014] S7: Outputting the dynamic parameters of the underwater robot body and the umbilical cable in the time step n;

[0015] S8: If the numerical simulation has not been completed, set n=n+1, issue an operation instruction to the control mechanism of the underwater robot in the control submodule according to the operation task requirements, and return to S4; otherwise, the numerical simulation ends and all calculation results are output.

[0016] Preferably, the hydrodynamic model includes six-degree-of-freedom motion equations and a catenary equation;

[0017] The hydrodynamic load of the underwater robot body and the control force generated by the control mechanism on the underwater robot body are calculated by the CFD sub-module to serve as external force and external torque inputs of the right-hand side of the six-degree-of-freedom motion equation;

[0018] The tension effect of the umbilical cable on the robot body is determined by the catenary equation and is also superimposed on the right-hand side of the six-degree-of-freedom motion equation in the form of external force and external torque.

[0019] Preferably, the six-degree-of-freedom motion equation is

[0020]

[0021]

[0022] The left-hand side term of the six-degree-of-freedom motion equation is the inertia force and torque of the underwater robot body, and the right-hand side term is the external force and torque acting on the underwater robot body, and the symbols are marked according to the standard method;

[0023] The catenary equation is

[0024]

[0025] m0 is the mass of the umbilical cable per unit length, k is the stiffness of the umbilical cable, Lr is the slack length of the umbilical cable, α and β are integral constants related to the positions of the two ends of the umbilical cable and the total mass of the umbilical cable, and λ is the curve parameter.

[0026] Preferably, the external force F0 acting on the robot body is (X, Y, Z) T And external torque M0=(K,M,N) T Assume that the equation composed of the hydrostatic restoring force W, the umbilical cable tension T, the hydrodynamic force H acting on the underwater robot body caused by the underwater movement of the underwater robot body, and the thrust TH of the control duct propeller is:

[0027] F0=F W +F T+F H +F TH

[0028] M0=M W +M T +M H +M TH ;

[0029] The subscript W represents the hydrostatic restoring force, T represents the umbilical cable tension, H represents the hydrodynamic force acting on the underwater robot body including the ocean current factor, and TH represents the thrust of the duct propeller.

[0030] Preferably, the umbilical cable tension T on the underwater robot body is determined by the catenary equation, and the umbilical cable tension T is converted into the umbilical cable tension F on the local coordinate system of the underwater robot body. T and moment M T The equation is

[0031] F T =-[E][D]T

[0032] M T =r T ×F T ;

[0033] [E] is the conversion matrix between the relative coordinates and the absolute coordinates of the underwater robot body, and [D] is the conversion matrix between the relative coordinates and the absolute coordinates of the umbilical cable;

[0034] Hydrostatic recovery force F W and moment M W , the component expression on the right side of the six-degree-of-freedom motion equation is: W =-(WB)sinθ

[0035]

[0036] θ, ψ are the pitch angle, roll angle, and yaw angle of the underwater robot when it moves in a fixed inertial coordinate system; (x G ,y G ,z G ) and (x B ,y B ,z B ) are respectively the center of gravity coordinates and the center of buoyancy coordinates of the underwater robot body in its local coordinate system.

[0037] Preferably, the two ends of the umbilical cable are point P1 and point P2, and the forces at point P1 and point P2 of the umbilical cable are respectively:

[0038] F 1，x=c,F 1，y =csinh(λ1)

[0039] F 2，x =-c, F 2，y =-c sinh(λ2).

[0040] As a preference, F 1，x 、F 1，y is the force at the upper end of the umbilical cable, F 2，x 、F 2，y is the tension of the lower end point of the umbilical cable acting on the underwater robot body, and the resultant force T of the tension of the lower end point of the umbilical cable is

[0041]

[0042] T is the value of the umbilical cable local coordinate system.

[0043] Preferably, the control module comprises an umbilical cable given length equation and a stern duct propeller speed equation, and controls the umbilical cable given length and the stern duct propeller speed by an incremental PID method.

[0044] Preferably, the given length equation of the umbilical cable is:

[0045] L K =L0+K P ×[Ze(n)-Ze(n-1)]+K I ×Ze(n)+K D ×[Ze(n)-2Ze(n-1)+Ze(n-2)];

[0046] L K is the length of the umbilical cable, L0 is the initial length of the umbilical cable, Ze(n) is the difference between the actual vertical depth and the preset depth at the nth step, Ze(n-1) is the difference between the actual vertical depth and the preset depth at the n-1th step, and Ze(n-2) is the difference between the actual vertical depth and the preset depth at the n-2th step;

[0047] The stern duct propeller speed equation is:

[0048] R K =R0+K P [Ze(n)-Ze(n-1)]+K I Ze(n)+K D [Ze(n)-2Ze(n-1)+Ze(n-2)];

[0049] R Kis the speed of the stern duct propeller at each adjustment, R0 is the initial speed of the stern duct propeller, Ze(n) is the difference between the actual control amount and the target control amount in the nth step, Ze(n-1) is the difference between the actual control amount and the target control amount in the n-1th step, and Ze(n-2) is the difference between the actual control amount and the target control amount in the n-2th step.

[0050] An underwater robot system comprises: a controller, an umbilical cable retracting and releasing device, the umbilical cable and the underwater robot body, wherein the controller controls the umbilical cable retracting and releasing device, the umbilical cable and the underwater robot body by using the control method for autonomous depth tracking of the underwater robot according to any one of claims 1 to 9.

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

[0052] The depth control of the underwater robot is achieved by combining the retraction and deployment of the umbilical cable with the manipulation of the duct propeller. It can not only maintain the specified depth, but also realize dynamic depth tracking according to the control instructions, and can effectively solve the problem of depth maintenance of the underwater robot caused by sudden changes in horizontal and vertical flow. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 This is a schematic diagram of the structure of the autonomous depth tracking control system for an underwater robot;

[0054] Figure 2 It is a schematic diagram of the structure of the underwater robot body;

[0055] Figure 3 It is a structural diagram of the forces acting on the end points of the umbilical cable;

[0056] Figure 4 The schematic diagram of the framework of the solution module for the autonomous depth tracking control method of the underwater robot;

[0057] Figure 5 This is a schematic diagram of the computational analysis flow of the autonomous depth tracking control method for underwater robots;

[0058] Figure 6 This is a schematic diagram of the PID control algorithm embedded module;

[0059] Figure 7 This is a schematic diagram of the calculation results of the longitudinal angle;

[0060] Figure 8 It is a schematic diagram of the overall combined calculation domain;

[0061] Fig. 9 It is a schematic diagram of the motion combination calculation domain;

[0062] Fig.10 This is the principle framework diagram of the PID control system;

[0063] Fig.11 This is a schematic diagram of the surface mesh of the underwater robot body;

[0064] Fig.12 It is a cross-sectional schematic diagram of the grid near the main body of the underwater robot in the overlapping domain II;

[0065] Fig.13 Boundary condition diagram between computational domains. DETAILED DESCRIPTION

[0066] The following is combined with Figure 1-13 , the specific implementation methods of the present invention are further described in detail to make the technical solution of the present invention easier to understand and grasp.

[0067] In this embodiment, it should be understood that the terms "middle", "upper", "lower", "top", "right side", "end", "front", "back", "middle", etc. indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present invention.

[0068] In addition, in this specific embodiment, if the connection or fixing method between components is not specifically described, the connection or fixing method can be through bolt fixing or pin fixing, or pin shaft connection and the like commonly used in the prior art, and therefore, it is not described in detail in this embodiment.

[0069] like Figure 1-13 As shown, an autonomous depth tracking control method for an underwater robot comprises:

[0070] S1: Construct the hydrodynamic model and control module of the underwater robot body;

[0071] S2: Inputting geometric and physical parameters of the underwater robot body remotely controlled by the umbilical cable;

[0072] S3: Calculating the stable solution of the underwater robot body in a steady state;

[0073] S4: introducing the control action of the control mechanism at time step n;

[0074] S5: calculating the hydrodynamic load on the underwater robot body and the control force generated by the control mechanism in the CFD submodule;

[0075] S6: solving the main program submodule of the underwater robot body hydrodynamic model;

[0076] S7: Outputting the dynamic parameters of the underwater robot body and the umbilical cable in the time step n;

[0077] S8: If the numerical simulation has not been completed, set n=n+1, issue an operation instruction to the control mechanism of the underwater robot in the control submodule according to the operation task requirements, and return to S4; otherwise, the numerical simulation ends and all calculation results are output.

[0078] First, the hydrodynamic model can be expressed by the six-degree-of-freedom motion equation, which is:

[0079]

[0080]

[0081] The left-hand side term of the six-degree-of-freedom motion equation is the inertia force and torque of the underwater robot body, and the right-hand side term is the external force and external torque acting on the underwater robot body, and their symbols are marked according to the standard method.

[0082] Then, the control module includes the stern duct propeller speed equation, which is R K =R0+K P [Ae(n)-Ae(n-1)]+K I Ae(n)+K D [Ae(n)-2Ae(n-1)+Ae(n-2)].

[0083] Among them, R of the stern duct propeller speed equation is K is the rotation speed of the ducted propeller at each adjustment, R0 is the initial rotation speed of the ducted propeller, Ae(n) is the difference between the actual trim angle and the preset trim angle at the nth step, Ae(n-1) is the difference between the actual trim angle and the preset trim angle at the n-1th step, and Ae(n-2) is the difference between the actual trim angle and the preset trim angle at the n-2th step.

[0084] The control module also includes the equation for the given length of the umbilical cable:

[0085] L K =L0+K P ×[Ze(n)-Ze(n-1)]+K I ×Ze(n)+K D ×[Ze(n)-2Ze(n-1)+Ze(n-2)];

[0086] L K is the length of the umbilical cable, L0 is the initial length of the umbilical cable, Ze(n) is the difference between the actual vertical depth and the preset depth at the nth step, Ze(n-1) is the difference between the actual vertical depth and the preset depth at the n-1th step, and Ze(n-2) is the difference between the actual vertical depth and the preset depth at the n-2th step.

[0087] The above CFD submodules are computational domain construction and coupling. The corresponding computational domain is constructed by using overlapping grid nested sliding grid technology. The overall computational domain is constructed by coupling three types of sub-computational domains, namely background domain, overlapping domain, and cylindrical rotation domain. Figure 8-9 The coupling relationship between the computational domains is given. These sub-computational domains are divided according to the different computational functions and grid construction requirements to be achieved:

[0088] Background domain I: This computational domain is the background grid describing the flow field area outside the robot. The background domain is set as a rectangular computational domain that satisfies the requirement of providing a large enough background space for simulating the three-dimensional motion of the robot underwater.

[0089] Overlap domain II: It is a rectangular area that contains the entire space near the underwater robot. The free manipulation movement of the underwater robot is realized by overlapping the mesh nesting body. This computational domain contains the object surface mesh of the entire robot and the cylindrical rotation domains III-VI that describe the rotation of the propeller of the control mechanism duct; it is surrounded by the background domain I.

[0090] Cylindrical rotation domains III-VI: used to describe the rotational motion of the ducted propellers that serve as the robot trajectory motion control mechanism. They are: left ducted propeller rotation domain III, right ducted propeller rotation domain IV, front propeller rotation domain V, and stern ducted propeller rotation domain VI.

[0091] During the numerical simulation, the background domain I and the overlapping domain II exchange flow field information through the overlapping grid technology through the method of linear interpolation; while the calculation domain III-VI moves with the robot, it rotates around the axis of the calculation domain according to the speed required by the duct propeller at each time step. The flow field data is transmitted between the calculation domains III~VI and the overlapping domain II through the corresponding slip boundaries, and they together constitute a multi-motion combined calculation domain; all the surfaces contained in the overlapping domain II and the rotating domain III-VI constitute an underwater robot assembly that can perform multi-degree-of-freedom motion. And all the areas contained in the periphery of the overlapping domain II constitute Fig. 9 Finally, all the areas contained in the periphery of background domain I constitute Figure 8 The overall combined computational domain is shown. Figure 8-9 In the constructed computational domain, the background domain I, the overlapping domain II, and the cylindrical rotation domains III-VI are all discretized using cutting volume grids. The outer boundary of the area constructed in this way is the outer area of ​​the boundary conditions of the overall combined computational domain of the CFD sub-modules defined according to the robot's underwater environment and operating conditions. Figure 8 The coordinate system O-XYZ marked in the lower left corner is the fixed inertial coordinate system for describing the numerical simulation of robot motion.

[0092] According to the principle of relativity, the incoming flow is set to flow into the domain from the entrance of the overall combined calculation domain, and its size is equal to the forward movement speed of the underwater robot system. At the beginning of the calculation, the robot is roughly in the center of the combined calculation domain. In the calculation of this application, the flow rate of the inlet and the other four sides of the overall combined calculation domain is stipulated to be equal to the forward movement speed of the system; the outlet of the calculation domain is defined as the pressure outlet, and this is used as the boundary condition of the overall combined calculation domain.

[0093] Fig.11 and Fig.12 The generated robot surface and its surrounding area boundary layer and other mesh diagrams are shown in Figure 2. The boundary conditions of each sub-computational domain intersection boundary and the outer boundary of the overall combined computational domain (outer boundary of region I) are defined as follows: Fig.13 shown.

[0094] In each time step of the time domain simulation, according to the robot posture and duct propeller speed information of the previous time step determined by the main program module (module I), Figure 8 The overall combined calculation domain is used to calculate the hydrodynamic load acting on the underwater robot assembly and the control force generated by the ducted propeller at this time step.

[0095] The following is a supplementary description of the content of this control method:

[0096] First, it is necessary to build the hydrodynamic mathematical model and control model of the underwater robot body, and then couple the CFD module used to determine the hydrodynamic load acting on the underwater robot body and the operating force of the control mechanism into the overall framework of the model. The incremental PID technology is used as the core algorithm of the control module.

[0097] In the hydrodynamic model of the underwater robot body, the dynamic response caused by the underwater robot body during the manipulation movement is simulated by the submarine's six-degree-of-freedom motion equation, and the dynamic effect of the umbilical cable on the underwater robot body is described by the catenary equation. Among them, the submarine's six-degree-of-freedom motion equation constitutes the core module of the hydrodynamic model. At each time step n of the manipulation movement, the hydrodynamic load acting on the underwater robot body and the manipulation force generated by the control mechanism (duct propeller, robot center of gravity adjustment system, etc.) on the body are calculated by the CFD sub-module, which is used as the external force and external torque input of the right-hand side of the submarine's six-degree-of-freedom motion equation; the tension effect of the umbilical cable on the underwater robot body is determined by the catenary equation, and is also superimposed on the right-hand side of the submarine's six-degree-of-freedom motion equation in the form of external force and external torque.

[0098] In this application, the hydrodynamic model of the underwater robot body cable motion can be described by the six-degree-of-freedom motion equation of the submarine. In the local coordinate system of the underwater robot, the equation can be expressed as:

[0099]

[0100] Among them, the left-hand side terms of equations (1) to (6) are the inertial force and torque of the underwater robot body, and the right-hand side terms are the external force and torque acting on the underwater robot body, and their symbols are marked according to the standard method. The external force acting on the underwater robot body F0 = (X, Y, Z) T And external torque M0=(K,M,N) T Assuming that it is composed of the hydrostatic restoring force W, the umbilical cable tension T, the hydrodynamic force H acting on the robot body due to the ocean current and the robot's own underwater movement, the thrust TH of the control duct propeller and its corresponding torque

[24] , we have:

[0101] F0=F W +F T +F H +F TH (7)

[0102] M0=M W +M T +M H +M TH (8)

[0103] In equations (7) and (8), the subscript W represents the hydrostatic restoring force, T represents the umbilical cable tension, H represents the hydrodynamic force acting on the underwater robot body including the ocean current factor, and TH represents the thrust of the duct propeller.

[0104] In equation (7), the thrust resultant of the ducted propeller is TH The thrust F generated by the duct propeller is controlled by various postures on the underwater robot body. THi Superposition gives, F TH is a function of the ducted propeller speed, the ducted propeller advance speed after considering the influence of the underwater robot body on the flow field around the ducted propeller, and other factors.

[0105] F TH =∑F THi (9)

[0106] In equation (9), the subscript i represents the serial number of the ducted propeller, i = 1, 2, ..., n TH , n TH is the total number of operating ducted propellers installed on the robot, and F THi The value of is expressed in the local coordinate system fixed on the underwater robot body. THi The torque M generated THi It is given by the following equation:

[0107] M THi =ri ×F THi (10)

[0108] Wherein is the coordinate of the propeller propulsion force action point of the control duct expressed in the local coordinate system of the underwater robot body:

[0109] r i =(x i ,y i , z i )(i=1,2,…,N TH ) (11)

[0110] Therefore, M in equation (8) TH Determined by the following equation:

[0111]

[0112] The umbilical cable tension T acting on the robot body is determined by the catenary equation, which is converted into the umbilical cable tension F in the robot local coordinate system. T and moment M T for

[0113] F T =-[E][D]T (13)

[0114] M T =r T ×F T (14)

[0115] In equation (13), [E] is the transformation matrix between the relative coordinates and absolute coordinates of the underwater robot body, and [D] is the transformation matrix between the relative coordinates and absolute coordinates of the umbilical cable:

[0116] Substituting equations (9), (12) to (14) into (7) to (8), we can determine the tension of the umbilical cable at each time step, the hydrodynamic force acting on the underwater robot body due to the ocean current and the underwater movement of the underwater robot body, the thrust of the control duct propeller, and its corresponding torque in equations (1) to (6).

[0117] The static restoring force and moment F of the fluid in the right-hand side of equations (7) to (8) W 、M W The hydrostatic restoring force and torque composed of gravity W and buoyancy B in the fixed inertial coordinate system (X, Y, Z) are converted to the hydrostatic restoring force F in the local coordinate system (x, y, z) of the robot body through the transformation matrix [E]. W and moment M W , F W and M WThe component expressions of the right-hand side terms in equations (1) to (6) are:

[0118] X W = -(WB)sinθ (15)

[0119]

[0120] In equations (15) to (20), θ, ψ are the pitch angle, roll angle, and yaw angle of the underwater robot when it moves in a fixed inertial coordinate system; (x G ,y G ,z G ) and (x B ,y B ,z B ) are the center of gravity coordinates and center of buoyancy coordinates of the underwater robot body in its local coordinate system.

[0121] Substituting equations (15) to (20) into equations (7) to (8), the hydrostatic restoring force and torque generated by the weight W and buoyancy B of the underwater robot body at each time step can be determined.

[0122] The hydrodynamic load F acting on the underwater robot in equations (7) and (8) is H 、M H , umbilical cable tension load F T 、M T and the control force F of the ducted propeller TH 、M TH The numerical solutions are obtained by computational fluid dynamics.

[0123] In the study, the three-dimensional geometric model of the underwater robot including the ducted propeller was first established using modeling software. The model was then imported into computational fluid dynamics software, and the umbilical cable tension acting on the robot body was simplified and simulated in the form of a catenary. The fluid mechanics control equations were solved in the flow field where the robot, ducted propeller and umbilical cable were located by combining overlapping grids with sliding. The hydrodynamic load acting on the underwater robot body and the thrust and torque generated by the ducted propeller were obtained, and the umbilical cable tension acting on the robot was calculated by the catenary equation.

[0124] The treatment of the catenary ignores the direct effect of fluid force on the small diameter umbilical cable. The quasi-steady elastic umbilical cable equation is:

[0125]

[0126] In the equation: m0 is the mass of the umbilical cable per unit length, k is the stiffness of the umbilical cable, Lr is the slack length of the umbilical cable, α and β are integral constants related to the positions of the two ends of the umbilical cable and the total mass of the umbilical cable, and the curve parameter λ is related to the tangent angle of the point on the umbilical cable:

[0127] tanφ=sinhλ (23)

[0128] like Figure 3 As shown in the figure, the upper end point of the umbilical cable is defined in the working mother ship motion coordinate system, and the lower end point of the umbilical cable is defined in the underwater robot body motion coordinate system, that is, the two ends of the umbilical cable are P1 and P2 respectively. When there is no relative motion between the working mother ship and the underwater robot body, the forces at the two end points of the umbilical cable are:

[0129] F 1，x =c,F 1，y =c sinh(λ1) (24)

[0130] F 2，x =-c, F 2，y = -c sinh(λ2) (25)

[0131] In equations (24) and (25), F 1，x 、F 1，y is the force at the upper end of the umbilical cable, F 2，x 、F 2，y is the tension of the lower end of the umbilical cable acting on the underwater robot body, and the resultant force T of the tension at the lower end of the umbilical cable is:

[0132]

[0133] The tension resultant force T in equation (26) is the value in the local coordinate system of the umbilical cable.

[0134] Equation (26) can be used to determine the umbilical cable tension T expressed in the local coordinate system of the umbilical cable at each time step n. Substituting it into equations (13) to (14) can be converted into the umbilical cable tension F acting on the local coordinate system of the robot: T and moment M T , and finally substitute it into equations (7) to (8) to determine the tension and moment generated by the umbilical cable tension T at each time step.

[0135] The incremental PID method is used to control the given length L of the umbilical cable. K , the deviation value en between the instantaneous actual position of the vertical depth Z and the preset position is selected as the PID calculation input of the umbilical cable length, and the control expression of the umbilical cable is:

[0136] L K =L0+K P×[Ze(n)-ze(n-1)]+K I ×Ze(n)+K D ×[ze(n)-2Ze(n-1)+Ze(n-2)] (27)

[0137] In expression (27), L K is the length of the umbilical cable, L0 is the initial length of the umbilical cable, Ze(n) is the difference between the actual vertical depth and the preset depth at the nth step, Ze(n-1) is the difference between the actual vertical depth and the preset depth at the n-1th step, and Ze(n-2) is the difference between the actual vertical depth and the preset depth at the n-2th step.

[0138] The incremental PID method is used to control the speed R of the stern duct propeller. K , the deviation value en between the instantaneous actual position of the vertical depth Z and the preset position is selected as the PID calculation input of the stern duct propeller speed, and the control expression for the stern duct propeller is:

[0139] R K =R0+K P [Ze(n)-Ze(n-1)]+K I Ze(n)+K D [Ze(n)-2Ze(n-1)+Ze(n-2)] (28)

[0140] In expression (28), R K is the speed of the stern duct propeller at each adjustment, R0 is the initial speed of the stern duct propeller, Ze(n) is the difference between the actual control amount and the target control amount in the nth step, Ze(n-1) is the difference between the actual control amount and the target control amount in the n-1th step, and Ze(n-2) is the difference between the actual control amount and the target control amount in the n-2th step.

[0141] After completing the establishment of the hydrodynamic model of the cable-controlled underwater robot, the control module designed according to the manipulation requirements is added to the established hydrodynamic model of the underwater robot system using an appropriate control algorithm, and added to the established hydrodynamic model to form the hydrodynamic and control model of the cable-controlled underwater robot system. The algorithms used in the control module can include various control algorithms such as "incremental PID control", "sliding mode control", "neural network control" and "fuzzy logic control" that can be used for underwater robot trajectory and posture manipulation. This paper mainly uses the incremental PID control algorithm to construct the required control module.

[0142] After the hydrodynamic model and control module are constructed, a certain numerical method can be used to discretize the model, and the hydrodynamic and control response characteristics of the cable-controlled underwater robot system under certain control actions can be fully determined by numerical simulation. In such a hydrodynamic and control model, the input parameters are the motion parameters of the umbilical cable at the connection point between the upper end of the water surface and the surface workboat at each time step (speed, displacement value at the connection point, etc.), and the control module issues instructions to make the control mechanism perform the corresponding control action; the CFD module calculates and predicts the hydrodynamic force acting on the underwater robot body under such control actions and the control force issued by the control mechanism.

[0143] Based on the constructed hydrodynamic and control model, a time domain numerical solution module is established. Figure 4 The computational block diagram of the numerical solution module of the hydrodynamic and control model is given. The solution module of this paper mainly includes three sub-modules:

[0144] Submodule I: Main program submodule for solving the hydrodynamic model of the robot system constructed in Section 1.2 and discretizing it into a numerical calculation model;

[0145] Submodule II: A CFD submodule that uses CFD technology to determine the hydrodynamic load acting on the underwater robot body and the control force of the control mechanism in each time step;

[0146] Submodule III: A control submodule that issues manipulation instructions to the robot control mechanism according to the manipulation task requirements.

[0147] According to the constructed numerical solution module, the hydrodynamics and control characteristics of the cable-controlled underwater robot system under the control force issued by the control mechanism can be numerically simulated. The output robot system dynamic parameters include: the center of gravity of the underwater robot body, the three-dimensional space coordinates of the connection point between the underwater robot body and the umbilical cable, the mechanical parameters of the underwater robot such as the Euler angle, and the geometric parameters such as the spatial coordinates of the umbilical cable.

[0148] like Figure 5 As shown, the following is the calculation and analysis process:

[0149] By using the CFD sub-modules as a whole combined calculation domain and combining it with the main program sub-module, it is possible to numerically simulate the motion characteristics of the underwater robot system when the robot is driven by the umbilical cable, the left and right duct propellers performing periodic and constant speed alternating forward and reverse operations. The main calculation steps are as follows:

[0150] S1. Input the geometric and physical parameters of the underwater robot system such as the umbilical cable and the robot body;

[0151] S2. The main program module calculates the stable solution of the steady state of the underwater robot system;

[0152] S3. Introduce the movement speed of the workboat at time step n+1;

[0153] S4. Use the main program module to calculate the dynamic parameters of the robot system at the n+1 time step;

[0154] S5. If the numerical simulation has not been completed, set n=n+1 and proceed to the next step; otherwise, the numerical simulation ends and all calculation results are output;

[0155] S6. The main program submodule outputs the CFD submodule to calculate the required motion parameters of the robot in n time steps (the robot's three-dimensional velocity components and its posture (Euler angles), etc.);

[0156] S7. Introduce control actions of the robot at the time of n time steps, which actions may be the retraction and extension of the umbilical cable, the speed of the duct propeller, the steering, etc.;

[0157] S8. Based on the robot motion parameters and control actions in steps S6 and S7, use the CFD submodule to calculate the hydrodynamic loads acting on the robot as described by equations (13) and (14) in n time steps and the control force of the control mechanism (tension of the umbilical cable and thrust of the duct propeller) The next time step and The value can be obtained by extrapolating the values ​​of time steps n and n-1. The value obtained by this step of extrapolation calculation provides the required parameters for step S4 when using the main program module calculation;

[0158] S9. Return to step S3.

[0159] like Figure 1-Figure 2 As shown, an underwater robot system includes a controller (not shown), an umbilical cable retracting device (not shown), an umbilical cable 2 and an underwater robot body 1, wherein the underwater robot body 1 includes a plurality of ducted propellers 3, and the ducted propellers 3 include a front ducted propeller 31, a stern ducted propeller 32, a left ducted propeller 33 and a right ducted propeller 34.

[0160] The controller controls the umbilical cable retracting and releasing device, the umbilical cable 2 and the underwater robot body 1 through the underwater robot autonomous depth tracking control method to realize the depth control of the underwater robot. It can not only maintain the specified depth, but also realize dynamic depth tracking according to the control instructions, and can effectively solve the problem of underwater robot depth maintenance caused by sudden changes in horizontal and vertical flow.

[0161] The technical effects of the present invention are mainly reflected in the following aspects:

[0162] The depth control of the underwater robot is achieved by combining the retraction and deployment of the umbilical cable with the manipulation of the duct propeller. It can not only maintain the specified depth, but also realize dynamic depth tracking according to the control instructions, and can effectively solve the problem of depth maintenance of the underwater robot caused by sudden changes in horizontal and vertical flow.

[0163] Of course, the above are only typical examples of the present invention. In addition, the present invention may also have many other specific implementations. All technical solutions formed by equivalent replacement or equivalent transformation fall within the scope of protection required by the present invention.

Claims

1. An autonomous depth tracking control method for an underwater robot, characterized in that: include: S1: Construct the hydrodynamic model and control module of the underwater robot body; S2: Inputting geometric and physical parameters of the underwater robot body remotely controlled by the umbilical cable; S3: Calculating the stable solution of the underwater robot body in a steady state; S4: introducing the control action of the control mechanism at time step n; S5: calculating the hydrodynamic load on the underwater robot body and the control force generated by the control mechanism in the CFD submodule; S6: solving the main program submodule of the underwater robot body hydrodynamic model; S7: Outputting the dynamic parameters of the underwater robot body and the umbilical cable in the time step n; S8: If the numerical simulation has not been completed, set n=n+1, issue an operation instruction to the control mechanism of the underwater robot in the control submodule according to the operation task requirements, and return to S4; otherwise, the numerical simulation ends and all calculation results are output.

2. The autonomous depth tracking control method of an underwater robot according to claim 1, characterized in that: The hydrodynamic model includes six-degree-of-freedom motion equations and catenary equations; The hydrodynamic load of the underwater robot body and the control force generated by the control mechanism on the underwater robot body are calculated by the CFD sub-module to serve as external force and external torque inputs of the right-hand side of the six-degree-of-freedom motion equation; The tension effect of the umbilical cable on the robot body is determined by the catenary equation and is also superimposed on the right-hand side of the six-degree-of-freedom motion equation in the form of external force and external torque.

3. The autonomous depth tracking control method of an underwater robot as claimed in claim 2, characterized in that: The six-degree-of-freedom motion equation is The left-hand side term of the six-degree-of-freedom motion equation is the inertia force and torque of the underwater robot body, and the right-hand side term is the external force and torque acting on the underwater robot body, and the symbols are marked according to the standard method; The catenary equation is λ1≤λ≤λ2, m0 is the mass of the umbilical cable per unit length, k is the stiffness of the umbilical cable, Lr is the slack length of the umbilical cable, α and β are integral constants related to the positions of the two ends of the umbilical cable and the total mass of the umbilical cable, and λ is the curve parameter.

4. The autonomous depth tracking control method of an underwater robot as claimed in claim 3, characterized in that: The external force F0 acting on the robot body = (X, Y, Z) T And external torque M0=(K,M,N) T Assume that the equation composed of the hydrostatic restoring force W, the umbilical cable tension T, the hydrodynamic force H acting on the underwater robot body caused by the underwater movement of the underwater robot body, and the thrust TH of the control duct propeller is: F0=F W +F T +F H +F TH M0=M W +M T +M H +M TH ; The subscript W represents the hydrostatic restoring force, T represents the umbilical cable tension, H represents the hydrodynamic force acting on the underwater robot body including the ocean current factor, and TH represents the thrust of the duct propeller.

5. The autonomous depth tracking control method of an underwater robot as claimed in claim 4, characterized in that: The umbilical cable tension T on the underwater robot body is determined by the catenary equation, and the umbilical cable tension T is converted into the umbilical cable tension F on the local coordinate system of the underwater robot body. T and moment M T The equation is F T =-[E][D]T M T =r T ×F T ; [E] is the conversion matrix between the relative coordinates and the absolute coordinates of the underwater robot body, and [D] is the conversion matrix between the relative coordinates and the absolute coordinates of the umbilical cable; Hydrostatic recovery force F W and moment M W , the component expression on the right side of the six-degree-of-freedom motion equation is: W =-(WB)sinθ θ, ψ are the pitch angle, roll angle, and yaw angle of the underwater robot when it moves in a fixed inertial coordinate system; (x G ,y G ,z G ) and (x B ,y B ,z B ) are respectively the center of gravity coordinates and the center of buoyancy coordinates of the underwater robot body in its local coordinate system.

6. The autonomous depth tracking control method of an underwater robot as claimed in claim 3, characterized in that: The two ends of the umbilical cable are point P1 and point P2 respectively, and the forces at point P1 and point P2 of the umbilical cable are respectively: F 1，c =c,F 1，y =csinh(λ1) F 2，x =-c,F 2，y =-c sinh(λ2)。 7. The autonomous depth tracking control method of an underwater robot as claimed in claim 6, characterized in that: F 1，x 、F 1，y is the force at the upper end of the umbilical cable, F 2，x 、F 2，y is the tension of the lower end point of the umbilical cable acting on the underwater robot body, and the resultant force T of the tension of the lower end point of the umbilical cable is T is the value of the umbilical cable local coordinate system.

8. The autonomous depth tracking control method of an underwater robot as claimed in claim 1, characterized in that: The control module includes an umbilical cable given length equation and a stern duct propeller rotation speed equation, and controls the umbilical cable given length and the stern duct propeller rotation speed by an incremental PID method.

9. The autonomous depth tracking control method of an underwater robot as claimed in claim 8, characterized in that: The given length equation of the umbilical cable is: L K =L0+K P ×[Ze(n)-Ze(n-1)]+K I ×Ze(n)+K D ×[Ze(n)-2Ze(n-1)+Ze(n-2)]; L K is the length of the umbilical cable, L0 is the initial length of the umbilical cable, Ze(n) is the difference between the actual vertical depth and the preset depth at the nth step, Ze(n-1) is the difference between the actual vertical depth and the preset depth at the n-1th step, and Ze(n-2) is the difference between the actual vertical depth and the preset depth at the n-2th step; The stern duct propeller speed equation is: R K =R0+K P [Ze(n)-Ze(n-1)]+K I Ze(n)+K D [Ze(n)-2Ze(n-1)+Ze(n-2)]; R K is the speed of the stern duct propeller at each adjustment, R0 is the initial speed of the stern duct propeller, Ze(n) is the difference between the actual control amount and the target control amount in the nth step, Ze(n-1) is the difference between the actual control amount and the target control amount in the n-1th step, and Ze(n-2) is the difference between the actual control amount and the target control amount in the n-2th step.

10. An underwater robot system, characterized in that: include: A controller, an umbilical cable retracting and releasing device, the umbilical cable and the underwater robot body, wherein the controller controls the umbilical cable retracting and releasing device, the umbilical cable and the underwater robot body through the control method for autonomous depth tracking of the underwater robot according to any one of claims 1 to 9.

Images