Underwater submersible motion control method and controller based on new S-surface controller

Abstract

The present invention discloses a motion control method and controller for an underwater submersible based on a novel S-surface controller, the method comprising: constructing a first motion control method to realize the control of the longitudinal, transverse, vertical and bow directions of the underwater submersible, constructing a second motion control method to control the pitch and roll of the underwater submersible; after the control solution, thrust distribution is required, and a thrust distribution strategy with bow priority is adopted. The present invention adopts an improved sigmoid function to replace the classical sigmoid function to improve the control response, considers the adaptive adjustment of the residual buoyancy by the deformation of the hull, considers the influence of the pitch and roll on the control when distributing the thrust, realizes precise control of the pitch and roll by adjusting the position of the "ballast water" through the opening and closing of the pump-valve linkage system, realizes six-degree-of-freedom motion control, and is suitable for engineering applications, and can be applied to motion control during static / dynamic submarine rescue with high control accuracy requirements.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater submersible motion control, and in particular relates to an underwater submersible motion control method and a controller based on a novel S-plane controller. Background Art

[0002] With the continuous development of computer simulation, big data, intelligent control, intelligent optimization, artificial intelligence and other technologies, underwater submersible technology has also made significant progress. Underwater submersibles have played an increasingly prominent role in many working environments that are difficult for personnel and equipment to access, such as shipwreck detection and salvage, dam inspection, pipeline renewal, cable maintenance, etc. Underwater submersibles have shown broad application prospects in civil and military fields and have become an important tool for completing underwater operations. Due to the small size and light weight of underwater submersibles, they need to carry different sensors and tools according to the operation tasks, so good control performance has always been the primary goal of controller design. As a highly coupled and strongly nonlinear system, the design of the controller of an underwater submersible needs to focus on control quality.

[0003] After years of research, PD controller, classic PID controller, fuzzy controller, sliding mode controller, robust controller, robust predictive controller, adaptive fuzzy controller, fuzzy neural network controller, etc. have been proposed. These control methods meet the control accuracy requirements of the operation tasks to a certain extent, but some control structures are too complex, some have more control parameters to be adjusted, and some are highly dependent on the motion model, which have certain limitations in promotion and application. The classic S-surface control method can better meet the control accuracy requirements of the conventional operation tasks of underwater submersibles, but does not consider the influence of variable static load. The improved S-surface controller takes into account the influence of residual buoyancy, but does not take into account the deformation of the hull, etc. The residual buoyancy is not adaptively adjusted according to the changes in the operation tasks, six-degree-of-freedom motion control is not achieved, and the influence of longitudinal and transverse inclination is not considered when distributing thrust. There is still room for further improvement in control performance. Summary of the invention

[0004] In view of the problem that conventional control methods for underwater submersibles are complex in structure and require many control parameters to be adjusted and are not suitable for engineering applications, the present invention proposes an underwater submersible motion control method and controller based on a novel S-surface controller, adopts an improved sigmoid function instead of the classic sigmoid function to improve control response, takes into account the adaptive adjustment of residual buoyancy due to hull deformation, takes into account the influence of pitch and roll on control when distributing thrust, realizes precise control of pitch and roll by opening and closing the position of "ballast water" through a pump-valve linkage system, realizes six-degree-of-freedom motion control, and is suitable for engineering applications.

[0005] The technical solution adopted by the present invention is:

[0006] According to a first aspect of the present invention, there is provided a method for controlling underwater vehicle motion based on a novel S-plane controller, the method comprising:

[0007] The motion control method shown in equation (1) is constructed to realize the longitudinal, lateral, vertical and bow control of the underwater submersible:

[0008] (1)

[0009] In the formula, is the output information of the controller, indicating The thrust or torque required for the degree of freedom; =1,2,3,6, representing longitudinal, transverse, vertical and bow directions respectively; is an improved sigmoid function. is the symbolic function, and is the input information of the controller, representing the Deviation and rate of change of deviation in degrees of freedom; and They are and The control parameters, is an exponential function, is the fixed disturbance force for a certain period of time calculated by the adaptive method; For the The thrust or torque required for the degree of freedom; For the The maximum thrust or torque that a degree of freedom can provide, is the estimated static load including forces and moments;

[0010] The motion control method shown in equation (3) is constructed to control the pitch and roll of the underwater submersible:

[0011] (3)

[0012] In the formula, =4, 5, representing heel and pitch respectively, Indicates the controller The time for the pump valve to open or close for each output. When it is greater than the first set threshold, the pump valve is opened or closed; and Indicates The deviation and rate of change of the input trim or heel; is the intelligent integral item, that is, when When it is less than the second set threshold, an integral term is introduced. When it is greater than the second set threshold, the integral item is cancelled; , and Respectively represent the proportional, differential and integral adjustment coefficients;

[0013] After the control solution is completed, thrust distribution is required, and a thrust distribution strategy with heading priority is adopted.

[0014] Furthermore, define As shown in formula (2):

[0015] (2)

[0016] In the formula, is the static load of the vehicle’s force and moment, for depth; , and is the preset depth and satisfies ;in, is the maximum diving depth; this formula reflects the adaptability of single mission state changes.

[0017] Furthermore, the underwater submersible includes a submersible body, a sensor and an actuator;

[0018] Sensors include Doppler speedometer, gyrocompass, inclinometer, depth gauge and water leak detector;

[0019] The actuator includes thrusters and pump valves; the thrusters include left main thruster and right main thruster, bow thruster and tail thruster, left vertical thruster and right vertical thruster;

[0020] The underwater submersible is also provided with at least four internal regulating water tanks, at least one each on the port and starboard sides of the submersible body for heel control, and at least one each on the bow and stern of the submersible body for pitch control;

[0021] The water inside the regulating water tank is moved by opening and closing the pump valve, thereby changing the position of the water inside the regulating water tank and changing the longitudinal and transverse inclination angles of the submersible.

[0022] Furthermore, the underwater submersible includes a CPU board, a serial port board, an AD board, a DA board and a DIO board, and each board transmits information through a bus;

[0023] The CPU board contains an intelligent planning system, a navigation system and a motion control system; the intelligent planning system is used to solve and issue target instructions; the navigation system is connected to the inertial navigation device and GPS to provide high-precision navigation data; the motion control system is used to collect sensor data and decode it, solve the control algorithm and send thrust instructions to the thruster, and send the opening and closing instructions of the pump valve control switch to the pump valve;

[0024] The serial port board is connected to the Doppler speed meter, gyrocompass and inclination sensor; the Doppler speed meter is used to measure the speed information of the relative water flow, the gyrocompass is used to measure the heading angle, and the inclination sensor is used to measure the longitudinal and transverse inclination information;

[0025] The AD board is connected to a depth meter; the depth meter is used to measure depth information;

[0026] The DA plate is connected to the thruster;

[0027] The DIO board is connected to a water leakage detector and a pump valve control switch; the water leakage detector is used to measure whether water has entered the cabin, and the pump valve control switch is used to control the opening and closing of the pump valve.

[0028] Further, the working process of the underwater submersible is as follows:

[0029] The mother ship monitors the underwater submersible and issues tasks;

[0030] Calculate target instructions and issue them according to mission requirements;

[0031] Initialize the control target, including selecting the motion control mode, type and parameters of motion control; the above information is then sent to the motion control algorithm module, and the data collected, preprocessed, filtered and fused by different sensors are also sent to the motion control algorithm module; the motion control mode includes automatic control mode and remote control mode; in the automatic control mode, the target is solved by the control algorithm and sent to the thruster and pump valve, the rotation of the thruster and the opening and closing of the pump valve control switch make the underwater submersible move according to the expected target position and attitude angle; at the same time, the information of the sensor and actuator is obtained and stored.

[0032] Further, the automatic control method is as follows:

[0033] When performing longitudinal position control, the set value is the target longitudinal position, the measured value is the actual longitudinal position measured by the navigation system, the deviation is the difference between the target longitudinal position and the actual longitudinal position, the controller is the longitudinal position controller, the control signal is the thrust voltage signal that the corresponding thruster should send out obtained by solving the new S-plane controller of formula (1), the actuators are the left main thruster and the right main thruster, the controlled medium is the thrust of the thruster, the external disturbance refers to the increased current disturbance, the controlled parameter is the longitudinal position, and the sensor is the navigation system; the control of the lateral position, depth, and heading angle is similar;

[0034] When performing heel control, the set value is the target heel angle, the measured value is the actual heel angle measured by the inclination sensor, the deviation is the difference between the target heel angle and the actual heel angle, the controller is the heel angle controller, the control signal is the time when the pump valve control switch should be opened and closed obtained by solving equation (3), the actuator is the pump valve, the controlled medium is the opening and closing time of the pump valve control switch, the external disturbance refers to the increased current disturbance, the controlled parameter is the heel angle, and the sensor is the inclination sensor; the control of the pitch angle is similar.

[0035] Furthermore, the automatic control process is as follows:

[0036] When performing automatic control of longitudinal, lateral, depth and heading angle, the target value is set first, and then the control parameters of the controller are initialized. Then the controller is solved to obtain the thrust or torque that should be provided by each degree of freedom. Then, the thrust distribution is carried out according to the heading priority strategy to obtain the thrust that each thruster should issue, and the thrust command is sent to the corresponding thruster in the form of analog voltage. The rotation of the thruster drives the underwater submersible to move, and the sensor data is updated accordingly. The current value is compared with the target value to determine whether the closed-loop control needs to be continued.

[0037] When performing automatic control of pitch and roll, first set the target value, then initialize the control parameters of the controller, and then solve the controller to obtain the time when the pitch and roll freedom pump and valve control switch should be opened and closed, and drive the pump and valve control switch to open or close in the form of digital switch quantity. The water inside the regulating water tank is moved by opening and closing the pump and valve, changing the position of the water inside the regulating water tank, thereby changing the pitch and roll angles of the submersible. The sensor data is updated accordingly, and the current value is compared with the target value to determine whether closed-loop control needs to continue.

[0038] Furthermore, the thrust distribution is as follows:

[0039] Before thrust distribution, assuming that the thrusts of the left and right main thrusters are T1 and T2, the force arm to the center of the hull is and The thrust of the bow thruster and the stern thruster are T3 and T4, and the force arm to the center of the hull is and The thrusts of the left and right vertical thrusts are T5 and T6, and the force arm to the center of the hull is and ;

[0040] Formula (4) is the thrust distribution formula without inclination state:

[0041] (4)

[0042] in is the force in the fixed coordinate system, is the force in the motion coordinate system; X is the longitudinal force in the motion coordinate system, Y is the lateral force in the motion coordinate system, Z is the vertical force in the motion coordinate system, and N is the bow moment in the motion coordinate system;

[0043] Formula (5) and Formula (6) are the thrust distribution formulas with inclination state:

[0044] (5)

[0045] (6)

[0046] in is the heel angle at a certain moment, is the pitch angle at a certain moment.

[0047] Furthermore, the control parameters are selected as: =1.5, =2.5, =3.0, =3.5, =1.0, =1.5, =1.0, =1.3, = -1.6, =0.8, = -0.2, = -1.0, =0.6, =0.1.

[0048] According to a second aspect of the present invention, there is provided a controller which adopts any one of the above-mentioned methods for controlling underwater vehicle motion based on a novel S-plane controller.

[0049] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0050] 1. The improved sigmoid function is used instead of the classic sigmoid function to improve the control response; 2. Through the control item , taking into account the adaptive adjustment of the residual buoyancy due to the deformation of the hull, no manual intervention is required; 3. When distributing thrust, the influence of the longitudinal and transverse inclination on the control is taken into account. The position of the "ballast water" is adjusted by the opening and closing of the pump-valve linkage system to achieve longitudinal and transverse inclination control. Even under the interference of ocean currents, high-precision motion control can be achieved; 4. The present invention can be applied to motion control during static / dynamic submarine rescue with high control accuracy requirements. The longitudinal control accuracy can reach within 0.05m, the lateral control accuracy can reach within 0.1m, the depth control accuracy can reach within 0.05m, the bow control accuracy can reach within 0.5°, the longitudinal inclination control accuracy can reach within 0.2°, and the roll control accuracy can reach within 0.3°. High-precision motion control is achieved to meet the needs of actual engineering applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 It is a schematic diagram of an underwater submersible;

[0052] Figure 2 This is the hardware architecture diagram;

[0053] Figure 3 It is the software information flow diagram;

[0054] Figure 4 It is the automatic control principle diagram;

[0055] Figure 5 It is an automatic control flow chart;

[0056] Figure 6 This is a schematic diagram of the simulation experiment platform;

[0057] Figure 7 is a motion control simulation curve diagram; among them, Figure 7 (a) is the simulation curve diagram of longitudinal position control. Figure 7 (b) is the lateral position control simulation curve diagram. Figure 7 (c) is the depth control simulation curve. Figure 7 (d) in the figure is the simulation curve of heading control. Figure 7 (e) is the simulation curve of pitch control. Figure 7 (f) in the figure is the roll control simulation curve diagram.

[0058] In the figure: 1-left main thruster, 2-right main thruster, 3-fore thruster, 4-aft thruster, 5-left vertical thruster, 6-right vertical thruster. DETAILED DESCRIPTION

[0059] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0060] The present invention provides an underwater submersible motion control method and a controller based on a novel S-plane controller, belonging to the technical field of depth / attitude control of underwater rescue equipment.

[0061] The underwater submersible motion control method based on the novel S-plane controller of the present invention has the following technical solutions:

[0062] The motion control method shown in equation (1) is constructed to achieve precise control in the longitudinal, lateral, vertical and bow directions. is the output information of the controller, indicating The thrust or torque required for each degree of freedom is normalized; =1,2,3,6, respectively representing longitudinal, transverse, vertical and bow directions; is an improved sigmoid function. is the symbolic function, and is the input information of the controller, indicating The deviation and the rate of change of the deviation of the degrees of freedom have been normalized; and They are and The control parameters, is an exponential function, is the fixed disturbance force for a certain period of time calculated by the adaptive method, For the The thrust or torque required for the degree of freedom; For the The maximum thrust or torque that a degree of freedom can provide, The estimated static loads include forces and moments, reflecting the adaptability of single task state changes.

[0063] (1)

[0064] In formula (1), we define As shown in formula (2), is the depth, because the maximum diving depth of the submersible is designed to be 200 meters, so The value is between 0 and 200. is the static load of the submersible force and moment.

[0065] (2)

[0066] The motion control method shown in equation (3) is constructed to accurately control the pitch and roll, where =4, 5, representing heel and pitch respectively, Indicates the controller The time for the pump valve to open or close for each output. When it is greater than the set threshold, the pump valve is opened or closed; , Indicates The deviation and rate of change of the input trim or heel; is the intelligent integral item, that is, when When it is less than the set threshold, an integral term is introduced. When it is greater than the threshold, the integral term is cancelled. This method can effectively solve the integral saturation problem and reduce the overshoot while eliminating the steady-state error. , , Represent the proportional, differential, and integral adjustment coefficients respectively.

[0067] (3)

[0068] After the control solution is completed, thrust distribution is required, and a thrust distribution strategy with heading priority is adopted.

[0069] Figure 1 The figure is a schematic diagram of the underwater submersible of the present invention. The underwater submersible can be used to achieve precise motion control during submarine static / moving rescue. The sensors equipped on the underwater submersible include a Doppler speed meter, a gyrocompass, an inclination sensor, a depth gauge, a water leakage detector, etc. The actuators equipped on the underwater submersible include a thruster and a pump valve. The thrusters include a left main thruster 1 and a right main thruster 2, a bow thruster 3 and a stern thruster 4, a left vertical thruster 5 and a right vertical thruster 6. The underwater submersible can be equipped with different operating equipment according to different operating tasks. The underwater submersible is provided with 4 internal regulating water tanks, one on the port side and one on the starboard side for heel control, and one on the bow and one on the stern for longitudinal tilt control.

[0070] Figure 2The following is a hardware architecture diagram. The underwater submersible includes an intelligent planning system, a navigation system, a motion control system, etc. All systems transmit information through the PC / 104 bus. The intelligent planning system is used to solve and issue target instructions. The navigation system is connected to the inertial navigation device and GPS to provide high-precision navigation data. The motion control system is used to collect sensor data and decode it, solve the control algorithm and send thrust instructions to the thruster, and send the opening and closing instructions of the pump valve control switch to the pump valve. The Doppler velocimeter is used to measure the speed information of the relative water flow, the gyrocompass is used to measure the heading angle, the inclination sensor is used to measure the longitudinal and transverse inclination information, the depth gauge is used to measure the depth information, and the water leakage detector is used to measure whether the cabin is flooded.

[0071] Attached Figure 3 It is a software information flow diagram. The process from top to bottom is: the mother ship monitoring module is used to monitor the underwater submersible. The target solution and processing module solves the target instructions according to the task requirements and issues them. In the control instruction module, the control target is first initialized, including selecting the motion control mode, the type of motion control and the parameters. The above information is then sent to the motion control algorithm module, and the data collected, preprocessed, filtered and fused by different sensors are also sent to the motion control algorithm module. The motion control module is the core part. In the automatic control mode, the target is solved by the control algorithm and sent to the thruster and pump valve. The rotation of the thruster and the opening and closing of the pump valve control switch make the underwater submersible move according to the expected target position and attitude angle. At the same time, the information of the sensor and actuator is obtained and stored through the detection module.

[0072] Figure 4 is the automatic control principle diagram. It can realize the six-degree-of-freedom motion control of underwater submersibles. When performing longitudinal position control, the set value is the target longitudinal position, the measured value is the actual longitudinal position measured by the navigation system, the deviation is the difference between the target longitudinal position and the actual longitudinal position, the controller is the longitudinal position controller, the control signal is the thrust voltage signal that the corresponding thruster should send out obtained by solving the new S-surface controller of formula (1), the actuators are the left main thruster and the right main thruster, the controlled medium is the thrust of the thruster, the external disturbance refers to the increased current disturbance, the controlled parameter is the longitudinal position, and the sensor is the navigation system; the control of the lateral position, depth, and heading angle is similar.

[0073] When performing heel control, the set value is the target heel angle, the measured value is the actual heel angle measured by the inclination sensor, the deviation is the difference between the target heel angle and the actual heel angle, the controller is the heel angle controller, the control signal is the time when the pump valve control switch should be opened and closed obtained by solving equation (3), the actuator is the pump valve linkage system, the controlled medium is the opening and closing time of the pump valve control switch, the external disturbance refers to the increased current disturbance, the controlled parameter is the heel angle, and the sensor is the inclination sensor; the control of the pitch angle is similar.

[0074] Figure 5 This is an automatic control flow chart. When performing longitudinal, lateral, depth and heading angle automatic control, the target value is set first, then the control parameters of the controller are initialized, and then the controller is solved to obtain the thrust (torque) that each degree of freedom should provide. Then, the thrust distribution is carried out according to the "heading priority" strategy to obtain the thrust that each thruster should issue, and the thrust command is sent to the corresponding thruster in the form of analog voltage. The thruster rotates to drive the underwater submersible to move, and the sensor data is updated accordingly. The current value is compared with the target value to determine whether closed-loop control needs to be continued.

[0075] When performing automatic control of pitch and roll, first set the target value, then initialize the control parameters of the controller, and then solve the controller to obtain the time when the pitch and roll freedom pump and valve control switch should be opened and closed, and drive the pump and valve control switch to open or close in the form of digital switch quantity. The water inside the regulating water tank is moved by opening and closing the pump and valve, changing the position of the water inside the regulating water tank, thereby changing the pitch and roll angles of the submersible. The sensor data is updated accordingly, and the current value is compared with the target value to determine whether closed-loop control needs to continue.

[0076] The following simulation experiment is carried out using a submersible as an example. The submersible has a total length of about 5.2m, a maximum diameter of about 0.8m, and a weight of about 2.3t. The submersible is equipped with 6 thrusters, including left main thruster 1 and right main thruster 2, bow thruster 3 and tail thruster 4, left vertical thruster 5 and right vertical thruster 6. The control parameters are selected as =1.5, =2.5, =3.0, =3.5, =1.0, =1.5, =1.0, =1.3, = -1.6, =0.8, = -0.2, = -1.0, =0.6, =0.1. Considering that different tasks require different sensors and equipment, the control item The state change of a single task is customized and adaptive. In an embodiment, Make your selection according to formula (2).

[0077] Before thrust distribution, assuming that the thrusts of the left and right main thrusters are T1 and T2, the force arm to the center of the hull is and The thrust of the bow thruster and the stern thruster are T3 and T4, and the force arm to the center of the hull is and The thrusts of the left and right vertical thrusts are T5 and T6, and the force arm to the center of the hull is and .

[0078] Most of the operating tasks of the test platform are in a navigation state without an inclination. Only when conducting docking with a target or dynamic positioning operations with an inclination will obvious pitch and roll conditions appear. The thrust distribution strategy is explained based on the above two situations.

[0079] Formula (4) is the thrust distribution formula without inclination state:

[0080] (4)

[0081] in is the force (torque) in the fixed coordinate system, is the force (torque) in the moving coordinate system. The origin of the fixed coordinate system is usually chosen on the sea surface. The positive direction points to due north, The positive direction points to the east. The positive direction of the X-axis points to the center of the earth; the origin of the coordinate system of the motion coordinate system is generally taken at the center of gravity of the submersible, the positive direction of the X-axis points to the bow of the submersible, the positive direction of the Y-axis points to the starboard side of the submersible, and the positive direction of the Z-axis points to the bottom of the submersible.

[0082] Formula (5) and Formula (6) are the thrust distribution formulas with inclination state. is the heel angle at a certain moment, is the pitch angle at a certain moment.

[0083] (5)

[0084] (6)

[0085] Build a digital simulation platform, such as Figure 6 Assume that the underwater submersible is initially stationary on the water surface, there is an ocean current, the ocean current velocity is 0.5m / s, and the flow direction is 30°. The underwater submersible position and attitude angle control curve in this embodiment is as follows: Figure 7 As shown, Figure 7 (a) is the simulation curve diagram of longitudinal position control. Figure 7 (b) is the lateral position control simulation curve diagram. Figure 7 (c) is the depth control simulation curve. Figure 7 (d) in the figure is the simulation curve of heading control. Figure 7 (e) is the simulation curve of pitch control. Figure 7(f) in the figure is a curve diagram of the heel control simulation. In this embodiment, the underwater submersible motion control method based on the novel S-surface controller is applied. The results show that the position and attitude angle control is realized. The position and attitude angle control of the proposed control method has basically no overshoot and oscillation. The longitudinal position steady-state deviation is less than 0.05m, the lateral position steady-state deviation is less than 0.1m, the depth steady-state deviation is less than 0.05m, the heading angle steady-state deviation is less than 0.5°, the longitudinal tilt angle steady-state deviation is less than 0.2°, and the heel angle steady-state deviation is less than 0.3°.

[0086] The present invention also provides a controller, which adopts any one of the underwater submersible motion control methods based on the new S-plane controller described above.

[0087] In summary, the present invention proposes an underwater submersible motion control method and controller based on a novel S-surface controller, adopts an improved sigmoid function instead of the classical sigmoid function to improve the control response, takes into account the adaptive adjustment of the residual buoyancy due to the deformation of the hull, and takes into account the influence of the pitch and roll on the control when distributing the thrust, and realizes precise control of the pitch and roll by adjusting the position of the "ballast water" through the opening and closing of the pump-valve linkage system, thereby realizing six-degree-of-freedom motion control, which is suitable for engineering applications.

[0088] It should be noted that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0089] It should be pointed out that, according to the needs of implementation, the various steps / components described in this application can be split into more steps / components, and two or more steps / components or partial operations of steps / components can be combined into new steps / components to achieve the purpose of the present invention.

[0090] It will be easily understood by those skilled in the art that the above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for controlling underwater vehicle motion based on a novel S-surface controller, characterized in that: The method includes: The motion control method shown in equation (1) is constructed to realize the longitudinal, lateral, vertical and bow control of the underwater submersible: (1) In the formula, is the output information of the controller, indicating The thrust or torque required for the degree of freedom; =1,2,3,6, representing longitudinal, transverse, vertical and bow directions respectively; is an improved sigmoid function. is the symbolic function, and is the input information of the controller, representing the Deviation and rate of change of deviation in degrees of freedom; and They are and The control parameters, is an exponential function, is the fixed disturbance force for a certain period of time calculated by the adaptive method; For the The thrust or torque required for the degree of freedom; For the The maximum thrust or torque that a degree of freedom can provide, is the estimated static load including forces and moments; The motion control method shown in equation (3) is constructed to control the pitch and roll of the underwater submersible: (3) In the formula, =4, 5, representing heel and pitch respectively, Indicates the controller The time for the pump valve to open or close for each output. When it is greater than the first set threshold, the pump valve is opened or closed; and Indicates The deviation and rate of change of the input trim or heel; is the intelligent integral item, that is, when When it is less than the second set threshold, an integral term is introduced. When it is greater than the second set threshold, the integral item is cancelled; , and Respectively represent the proportional, differential and integral adjustment coefficients; After the control solution is completed, thrust distribution is required, and a thrust distribution strategy with heading priority is adopted.

2. The underwater vehicle motion control method based on the novel S-plane controller according to claim 1 is characterized in that: definition As shown in formula (2): (2) In the formula, is the static load of the vehicle’s force and moment, for depth; , and is the preset depth and satisfies ;in, is the maximum diving depth; this formula reflects the adaptability of single mission state changes.

3. The underwater vehicle motion control method based on the novel S-plane controller according to claim 1 is characterized in that: The underwater submersible includes a submersible body, sensors and actuators; Sensors include Doppler speedometer, gyrocompass, inclinometer, depth gauge and water leak detector; The actuator includes thrusters and pump valves; the thrusters include left main thruster and right main thruster, bow thruster and tail thruster, left vertical thruster and right vertical thruster; The underwater submersible is also provided with at least four internal regulating water tanks, at least one each on the port and starboard sides of the submersible body for heel control, and at least one each on the bow and stern of the submersible body for pitch control; The water inside the regulating water tank is moved by opening and closing the pump valve, thereby changing the position of the water inside the regulating water tank and changing the longitudinal and transverse inclination angles of the submersible.

4. The underwater submersible motion control method based on the novel S-plane controller according to claim 3 is characterized in that: The underwater submersible includes a CPU board, a serial port board, an AD board, a DA board and a DIO board, and each board transmits information through a bus; The CPU board contains an intelligent planning system, a navigation system and a motion control system; the intelligent planning system is used to solve and issue target instructions; the navigation system is connected to the inertial navigation device and GPS to provide high-precision navigation data; the motion control system is used to collect sensor data and decode it, solve the control algorithm and send thrust instructions to the thruster, and send the opening and closing instructions of the pump valve control switch to the pump valve; The serial port board is connected to the Doppler speed meter, gyrocompass and inclination sensor; the Doppler speed meter is used to measure the speed information of the relative water flow, the gyrocompass is used to measure the heading angle, and the inclination sensor is used to measure the longitudinal and transverse inclination information; The AD board is connected to a depth meter; the depth meter is used to measure depth information; The DA plate is connected to the thruster; The DIO board is connected to a water leakage detector and a pump valve control switch; the water leakage detector is used to measure whether water has entered the cabin, and the pump valve control switch is used to control the opening and closing of the pump valve.

5. The underwater submersible motion control method based on the novel S-plane controller according to claim 3 is characterized in that: The working process of the underwater submersible is as follows: The mother ship monitors the underwater submersible and issues tasks; Calculate target instructions and issue them according to mission requirements; Initialize the control target, including selecting the motion control mode, type and parameters of motion control; the above information is then sent to the motion control algorithm module, and the data collected, preprocessed, filtered and fused by different sensors are also sent to the motion control algorithm module; the motion control mode includes automatic control mode and remote control mode; in the automatic control mode, the target is solved by the control algorithm and sent to the thruster and pump valve, the rotation of the thruster and the opening and closing of the pump valve control switch make the underwater submersible move according to the expected target position and attitude angle; at the same time, the information of the sensor and actuator is obtained and stored.

6. The underwater submersible motion control method based on the novel S-plane controller according to claim 5 is characterized in that: The automatic control method is as follows: When performing longitudinal position control, the set value is the target longitudinal position, the measured value is the actual longitudinal position measured by the navigation system, the deviation is the difference between the target longitudinal position and the actual longitudinal position, the controller is the longitudinal position controller, the control signal is the thrust voltage signal that the corresponding thruster should send out obtained by solving the new S-plane controller of formula (1), the actuators are the left main thruster and the right main thruster, the controlled medium is the thrust of the thruster, the external disturbance refers to the increased current disturbance, the controlled parameter is the longitudinal position, and the sensor is the navigation system; the control of the lateral position, depth, and heading angle is similar; When performing heel control, the set value is the target heel angle, the measured value is the actual heel angle measured by the inclination sensor, the deviation is the difference between the target heel angle and the actual heel angle, the controller is the heel angle controller, the control signal is the time when the pump valve control switch should be opened and closed obtained by solving equation (3), the actuator is the pump valve, the controlled medium is the opening and closing time of the pump valve control switch, the external disturbance refers to the increased current disturbance, the controlled parameter is the heel angle, and the sensor is the inclination sensor; the control of the pitch angle is similar.

7. The underwater vehicle motion control method based on the novel S-plane controller according to claim 6 is characterized in that: The automatic control process is as follows: When performing automatic control of longitudinal, lateral, depth and heading angle, the target value is set first, and then the control parameters of the controller are initialized. Then the controller is solved to obtain the thrust or torque that should be provided by each degree of freedom. Then, the thrust distribution is carried out according to the heading priority strategy to obtain the thrust that each thruster should issue, and the thrust command is sent to the corresponding thruster in the form of analog voltage. The rotation of the thruster drives the underwater submersible to move, and the sensor data is updated accordingly. The current value is compared with the target value to determine whether the closed-loop control needs to be continued. When performing automatic control of pitch and roll, first set the target value, then initialize the control parameters of the controller, and then solve the controller to obtain the time when the pitch and roll freedom pump and valve control switch should be opened and closed, and drive the pump and valve control switch to open or close in the form of digital switch quantity. The water inside the regulating water tank is moved by opening and closing the pump and valve, changing the position of the water inside the regulating water tank, thereby changing the pitch and roll angles of the submersible. The sensor data is updated accordingly, and the current value is compared with the target value to determine whether closed-loop control needs to continue.

8. The underwater vehicle motion control method based on the novel S-plane controller according to claim 1 is characterized in that: The thrust distribution is as follows: Before thrust distribution, assuming that the thrusts of the left and right main thrusters are T1 and T2, the force arm to the center of the hull is and The thrust of the bow thruster and the stern thruster are T3 and T4, and the force arm to the center of the hull is and The thrusts of the left and right vertical thrusts are T5 and T6, and the force arm to the center of the hull is and ; Formula (4) is the thrust distribution formula without inclination state: (4) in is the force in the fixed coordinate system, is the force in the motion coordinate system; X is the longitudinal force in the motion coordinate system, Y is the lateral force in the motion coordinate system, Z is the vertical force in the motion coordinate system, and N is the bow moment in the motion coordinate system; Formula (5) and Formula (6) are the thrust distribution formulas with inclination state: (5) (6) in is the heel angle at a certain moment, is the pitch angle at a certain moment.

9. The underwater vehicle motion control method based on the novel S-plane controller according to claim 1 is characterized in that: The control parameters are selected as: =1.5, =2.5, =3.0, =3.5, =1.0, =1.5, =1.0, =1.3, = -1.6, =0.8, = -0.2, = -1.0, =0.6, =0.

1.

10. A controller, characterized in that: The controller adopts the underwater submersible motion control method based on the new S-plane controller as described in any one of claims 1 to 9.

Images