Underwater robot motion control method based on fuzzy active disturbance rejection control

Abstract

The invention discloses an underwater robot motion control method based on fuzzy active disturbance rejection control. As the basis of underwater robot motion control research, firstly, a kinetic model is established for an underwater robot; and then an active disturbance rejection controller is used as a basic controller. In order to avoid the problems of phase lag and long adjustment time caused by a tracking differentiator, the linear active disturbance rejection controller designed by the invention does not use the tracking differentiator (TD), and only keeps a linear extended state observer (LESO) and a linear state error feedback rate (LSEF). Finally, in order to adapt to the problem that the underwater robot faces control model change when executing different operation tasks, a fuzzy controller is added on the basis of the improved linear active-disturbance-rejection controller, parameters in the linear active-disturbance-rejection controller are dynamically adjusted through a fuzzy inference rule, and therefore the control model change is avoided. Therefore, optimal control parameters can be adjusted for robot motion control under different conditions.

Description

Technical Field

[0001] The invention belongs to the technical field of autonomous underwater robot motion control, and in particular relates to an underwater robot motion control method based on fuzzy self-anti-disturbance control. Background Art

[0002] The ocean world is rich in natural resources, mineral resources and biological resources. Therefore, the exploration of the marine environment, the protection of organisms and the understanding of the ocean world have become hot issues in the research of countries around the world. At present, due to the increasing demand for marine engineering in various countries, more and more researchers are committed to developing underwater robots for exploring the ocean world and completing various underwater tasks. Underwater robots are flexible and can work in water for a long time. They have gradually become an important tool for human beings to develop and utilize marine resources. Among them, underwater unmanned vehicles (UUVs) are one of the typical representatives, with the advantages of low cost, high maneuverability and strong autonomy. Unlike land robots, UUVs are subject to special disturbances in the underwater environment such as buoyancy and ocean currents. How to ensure that UUVs can accurately and efficiently perform tasks along the preset trajectory underwater is a difficulty faced by control technology. Therefore, it is particularly important to improve the robustness and adaptability of underwater unmanned vehicles in complex environments.

[0003] Common methods for underwater robot motion control include PID control, sliding mode control, and auto-disturbance rejection control. The PID controller is also called a proportional-integral-differential controller. It has a simple algorithm and does not require a model of the control object. It controls by eliminating errors through errors. However, the traditional PID control parameters are fixed and cannot adapt to high-precision control in complex underwater environments. The sliding mode controller has the advantages of fast response speed, applicability to nonlinear systems and parameter changes and insensitivity to disturbances. However, the traditional sliding mode control has the problems of jitter caused by discontinuous switching items, which will lead to high-frequency changes in the propeller speed, resulting in high energy consumption, increased friction, and propeller damage. The traditional auto-disturbance rejection controller has strong anti-interference ability, faster response speed, and small overshoot, but there are still problems such as too many adjustment parameters, phase lag, and longer adjustment time.

[0004] The present invention proposes an underwater robot motion control method based on fuzzy anti-disturbance control after investigating and analyzing the demand for precise motion control of underwater robots at home and abroad and the shortcomings of existing motion control methods. Summary of the invention

[0005] The present invention provides a motion control method for an underwater robot based on fuzzy anti-disturbance control. The underwater robot is a multivariable strongly coupled system, which will be disturbed by various external environments during the movement. In order to make the underwater robot move along the desired trajectory and accurately adjust the posture to ensure the reliability and stability of the task, it is necessary to select a suitable control method to accurately control the posture of the underwater robot. When the underwater robot performs the task, the control model will change, and the controller with fixed parameters cannot adapt to the control requirements in different environments.

[0006] In order to solve the above technical problems, the present invention takes into account the advantages of strong nonlinearity, strong coupling, good robustness, etc., takes an active disturbance rejection controller as the basic controller, and in order to avoid the problem of too many tuning parameters in the traditional active disturbance rejection controller, the controller is improved into a linear active disturbance rejection controller, which retains the advantages of the active disturbance rejection controller, while reducing the parameters to be adjusted, and is easier to implement in engineering.

[0007] The linear active disturbance rejection controller also has three modules: tracking differentiator (TD), linear extended state observer (LESO), and linear state error feedback rate (LSEF). The core of the linear active disturbance rejection control is the linear extended state observer with extremely strong disturbance observation and estimation capabilities. In order to avoid the phase lag and long adjustment time caused by the tracking differentiator, the linear active disturbance rejection controller designed by the present invention does not use the tracking differentiator (TD), but only retains the linear extended state observer (LESO) and the linear state error feedback rate (LSEF).

[0008] In order to adapt to the problem of changes in control models faced by underwater robots when performing different work tasks, the present invention adds a fuzzy controller on the basis of the improved linear anti-disturbance controller, and dynamically adjusts the parameters in the linear anti-disturbance controller through fuzzy inference rules, so as to adjust the optimal control parameters for robot motion control under different conditions.

[0009] The technical solution adopted by the present invention to achieve the above-mentioned purpose is: a motion control method of an underwater robot based on fuzzy anti-disturbance control, comprising the following steps:

[0010] Dynamic modeling is the basis for conducting research on underwater robot motion control. First, an inertial coordinate system and a body coordinate system are established based on its motion characteristics. Usually, the inertial coordinate system is established with a fixed position on the ground as the origin, and the origin of the body coordinate system is set at the center of gravity of the underwater adhesion robot.

[0011] The position vector and attitude vector of the underwater robot in the inertial coordinate system are:

[0012]

[0013] The linear velocity and angular velocity vectors in the body coordinate system are:

[0014]

[0015] The dynamic equation of the underwater robot in vector form in body coordinates is:

[0016]

[0017] in, represents the added mass matrix, represents the rigid body mass and inertia matrix, represents the centripetal force and rigid body Coriolis force matrices, represents the fluid viscous damping force, is the combined force of gravity and buoyancy, It represents two parts: its own driving force and the interference force from the external environment.

[0018] According to the established dynamic model, a fuzzy anti-disturbance controller is designed to realize the decoupling control of the underwater robot system. Then a three-dimensional space trajectory tracking control method is designed to realize the motion control of the underwater robot.

[0019] In general, for a second-order nonlinear time-varying system:

[0020]

[0021] In the formula is an unknown function, is the unknown disturbance, is the control input, To measure the output, the corresponding LESO expression of this second-order system design is as follows:

[0022]

[0023] Where: is the parameter that needs to be adjusted in LESO. In the second-order system, according to the characteristic equation of the observer, the observer gain can be obtained by using the pole configuration method. The expression is , , .

[0024] The second-order LSEF is expressed as follows:

[0025]

[0026] In the formula, To set the signal value to be tracked, is the controller gain. In a second-order system, according to the characteristic equation of the controller, the pole configuration method can be used to obtain The expression is , .

[0027] for enter The output coupled system, considering its system state space expression is as follows:

[0028]

[0029] Distractors It can be regarded as external interference and internal interference acting on the The total disturbance on the channel. The influence of other channels on the current control channel is regarded as part of the external disturbance, and the total disturbance is observed through LESO. The control process of each channel becomes a single-input single-output process, thus achieving the first Channel decoupling.

[0030] The improved linear active disturbance rejection controller can well solve the decoupling control problem of multivariable coupled systems. and the output vector Embed in parallel A fuzzy active disturbance rejection controller can realize the decoupling control of the multi-input multi-output system.

[0031] When underwater robots perform tasks, the control model will change. If the controller parameters remain unchanged, the error between the actual trajectory and the expected trajectory of the underwater robot during movement will become larger, which requires real-time adjustment of the controller parameters to adapt to this change. A fuzzy anti-disturbance control method is designed, and fuzzy control is introduced into the state error feedback law and the extended state observer for parameter self-tuning, which improves the system's adaptability and robustness.

[0032] Fuzzy control is a control technology based on fuzzy set theory, fuzzy language variables, and fuzzy logic reasoning. Its essence is based on the ideas and basic theories of fuzzy mathematics. It improves control accuracy through empirical methods and has good anti-interference ability. It plays a huge role in some complex nonlinear motion control models.

[0033] The core of fuzzy control is to integrate and transform the expert knowledge and the experience of skilled operators to form fuzzy language rules that can be recognized by computers. Fuzzy controllers are composed of four basic parts: fuzzification interface, knowledge base, inference engine, and defuzzification interface.

[0034] The present invention adds a fuzzy controller on the basis of improving the linear ADRC so as to adjust the controller parameters in real time to adapt to such changes. The parameters in the ADRC are dynamically adjusted through fuzzy inference rules. The parameters of the fuzzy ADRC are and .

[0035] The input of the fuzzy controller is the error e between the desired posture and the current posture and the error change rate Since the triangular membership function has the characteristics of high sensitivity, the triangular membership function is selected to describe the fuzzy set in the design process of the fuzzy controller. According to the fuzzy rules, the weighted average method is used to obtain the correction value controller parameters and .

[0036] Finally, the underwater robot is precisely controlled according to the designed fuzzy anti-disturbance controller, and a motion control method in underwater three-dimensional space is designed for the robot. Combined with the depth sensor and gyroscope module carried by the robot, the difference between the actual and expected trajectories is calculated to assign a control module to each propeller. According to the required force of the robot, the corresponding steering and speed are assigned to the propeller, thereby completing the motion control of the autonomous underwater robot.

[0037] Compared with the prior art, the present invention has the following advantages and technical effects:

[0038] 1. The present invention adopts an improved fuzzy ADRC method. Compared with the traditional ADRC, the improved linear ADRC retains the advantages of strong nonlinearity, strong coupling, good robustness, etc., while reducing the problem of too many parameters to be adjusted, and is easier to implement in engineering. In order to avoid the problems of phase lag and long adjustment time caused by the tracking differentiator, the linear ADRC designed by the present invention does not use a tracking differentiator (TD).

[0039] 2. The present invention adds a fuzzy controller on the basis of improving the linear ADRC, dynamically adjusts the parameters in the ADRC according to the fuzzy inference rules, and searches for the optimal control parameters for the robot online. It can enable underwater robots to achieve precise motion control in different operation processes. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 The control principle diagram of the fuzzy ADRC controller according to the embodiment of the present invention is as follows:

[0041] Figure 2 The motion control principle diagram of the autonomous underwater robot according to the embodiment of the present invention

[0042] Figure 3 The fuzzy control block diagram of the embodiment of the present invention is as follows:

[0043] Figure 4 The fuzzy controller input and output relationship mapping diagram of the embodiment of the present invention is

[0044] Figure 5 Schematic diagram of the coordinate system constructed for the underwater robot according to an embodiment of the present invention

[0045] Figure 6 Schematic diagram of an underwater robot control system according to an embodiment of the present invention

[0046] Figure 7 The fuzzy anti-disturbance control method is used in the embodiment of the present invention to adjust the yaw angle Tracking performance comparison chart

[0047] Figure 8 This is a comparison chart of the X-axis tracking performance using the fuzzy anti-disturbance control method of the embodiment of the present invention.

[0048] Fig. 9 This is a comparison diagram of the Z-axis tracking performance using the fuzzy anti-disturbance control method of the embodiment of the present invention. DETAILED DESCRIPTION

[0049] In order to make the purpose of the embodiments of the present invention and the specific control process more obvious and easy to understand, the specific implementation methods of the present invention are further described in detail below in conjunction with the accompanying drawings. It should be pointed out that the specific implementation methods described here are only used to explain and illustrate the present invention, and are not used to limit the present invention.

[0050] The present invention relates to an underwater robot motion control method based on a fuzzy anti-disturbance controller. In this embodiment, a five-degree-of-freedom micro-sized underwater robot is used as the control object. The main control system composition diagram is shown in FIG. Figure 6 As shown, the whole system works according to the following process:

[0051] The underwater robot control system is closely connected with the host computer system, propulsion system, lighting observation system and sensor system. The host computer communicates with the Raspberry Pi through Ethernet, and the Raspberry Pi sends control instructions to the ESP8266 control board through serial communication. According to the received control instructions, the ESP8266 sends control signals to the ESC, LED lights and other devices to achieve precise control of the thrusters and lighting equipment. The underwater camera transmits the observed video images to the Raspberry Pi through the video stream, and then displays them on the host computer system. The robot's depth information and posture information are obtained by the gyroscope and depth sensor. The data is first transmitted to the ESP8266 control board through the serial port, and then transmitted to the host computer platform by the Raspberry Pi. The energy supply of the entire control system comes from the power module.

[0052] The present invention can realize the posture, depth and precise trajectory tracking control of the underwater robot at different speeds based on the fuzzy anti-disturbance control method. The specific steps are as follows:

[0053] The first step is to construct the inertial coordinate system and body coordinate system of the underwater robot. The coordinate system construction is referred to Figure 5 , establish the transformation equation between the two coordinate systems:

[0054]

[0055] The coordinate transformation matrix establishes a connection between the velocity of the body coordinate system and the posture and velocity in the inertial coordinate system, realizing the unification of underwater operation and surface control data, and laying the foundation for the establishment of the subsequent dynamic model of the underwater adhesion robot and the design of the control system.

[0056] The second step is to design an improved linear ADRC. The extended state observer and state error feedback rate in the traditional ADRC are linearized and the tracking differentiator is discarded. This can ensure high anti-interference and good robustness while avoiding the phase lag and long adjustment time caused by the tracking differentiator. At the same time, the parameters that need to be adjusted are also reduced.

[0057] for enter The output coupled system, considering its system state space expression is as follows:

[0058]

[0059] The amplification factor of the system It is a function related to state variables and time, and the corresponding magnification coefficient matrix is ​​as follows:

[0060]

[0061] The third step is to design and add a fuzzy controller based on the improved linear ADRC so as to adjust the controller parameters in real time to adapt to this change. The fuzzy control block diagram is as follows: Figure 3 As shown. The parameters of the fuzzy ADRC are dynamically adjusted through fuzzy inference rules. The parameters of the fuzzy ADRC are and .

[0062] The input of the fuzzy controller is the error e between the desired posture and the current posture and the error change rate Since the triangular membership function has the characteristics of high sensitivity, the triangular membership function is selected to describe the fuzzy set in the design process of the fuzzy controller. According to the fuzzy rules, the weighted average method is used to obtain the correction value controller parameters and .

[0063] The input and output of fuzzy control are expressed in five language values: negative big (NB), negative small (NS), zero (ZO), positive small (PS), and positive big (PB). When it increases, the control ability of the system will be enhanced and the response speed will be faster, but overshoot and oscillation will occur easily. When the value is higher, the accuracy of the observer can be improved and the anti-interference ability of the system can be enhanced, but the noise signal will be amplified, causing system oscillation. Design according to the characteristics of these parameters , The fuzzy rules are shown in Table 1 and Table 2.

[0064] Table 1 Fuzzy rule table

[0065]

[0066] Table 2 Fuzzy rule table

[0067]

[0068] The fuzzy controller is designed using the fuzzy control toolbox. The fuzzy domain of the error e is set to [-0.1, 0.1], and the error change rate is The fuzzy domain is set to [-1, 1], and the controller bandwidth and observer bandwidth The fuzzy domain is set to [-40, 40], and the fuzzy control input e, With output , The mapping relationship between Figure 4 shown.

[0069] The control principle diagram of the fuzzy ADRC designed in the end is as follows: Figure 1 Finally, the underwater robot is precisely controlled based on the designed fuzzy anti-disturbance controller, and a motion control method in underwater three-dimensional space is designed for the robot. The motion control principle diagram of the autonomous underwater robot is shown in Figure 2 shown.

[0070] The robot uses a depth sensor to obtain depth information in the Z-axis direction, uses a global camera to obtain information in the X-axis and Y-axis directions in the horizontal plane, and uses a gyroscope module to obtain the robot's own posture information. The robot's expected motion trajectory is designed based on the underwater operation task, and the expected trajectory between each direction is decomposed. The difference between the actual and expected trajectory is calculated to assign a control module to each propeller, and the corresponding steering and speed are assigned to the propeller according to the force required by the robot, thereby completing the motion control of the autonomous underwater robot.

[0071] Figure 7~Figure 9 These are comparison diagrams of the results of motion control experiments on underwater robots using a fuzzy anti-disturbance controller according to an embodiment of the present invention. It can be seen from the three diagrams that the tracking error of the method proposed in the present invention achieves a relatively high tracking accuracy, thereby verifying the effectiveness of the method.

[0072] The above content is only an explanation and interpretation of the robot motion control method based on the fuzzy anti-disturbance controller. In addition, the above implementation is only an example, and any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. The underwater robot motion control method based on fuzzy anti-disturbance control is characterized by: The following steps are involved: According to its motion characteristics, an inertial coordinate system and a body coordinate system are established, and a dynamic model of the underwater robot is established in the coordinate system; According to the established dynamic model, an improved linear active disturbance rejection controller is designed. In order to avoid the problems of phase lag and prolonged adjustment time caused by the tracking differentiator, the linear active disturbance rejection controller designed by the present invention does not use a tracking differentiator (TD); Based on the improved linear active disturbance rejection controller, fuzzy control is introduced into the state error feedback law (LSEF) and the extended state observer (LESO) for parameter self-tuning to improve the adaptability and robustness of the system. The fuzzy anti-disturbance controller realizes the decoupling control of the underwater robot system, and then a three-dimensional space trajectory tracking control method is designed to realize the motion control of the underwater robot.

2. The underwater robot motion control method based on fuzzy anti-disturbance control according to claim 1 is characterized in that: The dynamic model of the underwater robot is established in the inertial coordinate system and the body coordinate system. The following steps are involved: The position vector and attitude vector of the underwater robot in the inertial coordinate system are: The linear velocity and angular velocity vectors in the body coordinate system are: The dynamic equation of the underwater robot in vector form in body coordinates is: in, represents the added mass matrix, represents the rigid body mass and inertia matrix, represents the centripetal force and rigid body Coriolis force matrices, represents the fluid viscous damping force, is the combined force of gravity and buoyancy, It represents two parts: its own driving force and the interference force from the external environment.

3. The underwater robot motion control method based on fuzzy anti-disturbance control according to claim 1 is characterized in that: The improved linear active disturbance rejection controller is designed according to the established dynamic model, including the following steps: For the second-order nonlinear time-varying system, the corresponding linear extended state observer (LESO) and linear state error feedback rate (LSEF) are designed, which are expressed as follows: LESO: LSEF: for enter The coupled system of the output regards the influence of other channels on the current control channel as part of the external disturbance, and observes the total disturbance through LESO. The control process of each channel becomes a single-input single-output process, thus realizing the first Channel decoupling; In order to avoid the phase lag and longer adjustment time caused by the tracking differentiator, the improved linear active disturbance rejection controller does not use the tracking differentiator (TD), and only retains the linear extended state observer (LESO) and the linear state error feedback rate (LSEF); The improved linear active disturbance rejection controller can well solve the decoupling control problem of multivariable coupled systems. and the output vector Embed in parallel A fuzzy active disturbance rejection controller can realize the decoupling control of the multi-input multi-output system.

4. The underwater robot motion control method based on fuzzy anti-disturbance control according to claim 1 is characterized in that: Based on the designed improved linear ADRC, a fuzzy controller is added to dynamically adjust the parameters of the ADRC through fuzzy inference rules. The parameters of the fuzzy ADRC are: and ; The core of fuzzy control is to integrate and transform the expert knowledge and the experience of skilled operators to form fuzzy language rules that can be recognized by computers. The fuzzy controller consists of four basic parts: fuzzification interface, knowledge base, inference engine, and defuzzification interface; The input of the fuzzy controller is the error e between the desired posture and the current posture and the error change rate ,Since the triangular membership function has the characteristics of high sensitivity, the triangular membership function is selected to describe the fuzzy set in the design process of the fuzzy controller. According to the fuzzy rules, the weighted average method is used to obtain the correction value controller parameters and .

5. The underwater robot motion control method based on fuzzy anti-disturbance control according to claim 1 is characterized in that: The underwater robot is precisely controlled according to the designed fuzzy anti-disturbance controller, and a motion control method is designed for the robot in underwater three-dimensional space. Combined with the depth sensor and gyroscope module carried by the robot, the difference between the actual and expected trajectories is calculated to assign a control module to each propeller. According to the required force of the robot, the corresponding steering and speed are assigned to the propeller, thereby completing the motion control of the autonomous underwater robot.

Images