Unmanned autonomous submersible vehicle track tracking control method

Abstract

The invention relates to a flight path tracking control method for an unmanned autonomous submersible vehicle, and belongs to the technical field of unmanned submersible vehicle control. The method comprises the following steps: firstly, establishing a nonlinear dynamic model of the submersible vehicle, analyzing kinematics and dynamic characteristics of the nonlinear dynamic model, considering various underwater forces and moments, and defining an inertial coordinate system and a submersible vehicle body coordinate system to obtain a kinematics equation and a dynamic equation so as to provide a basis for control design; secondly, a non-singular integral terminal sliding mode control method is adopted, a sliding mode surface is designed, the singular problem of traditional sliding mode control is avoided, a sliding mode control law is deduced, system finite time convergence to a balance point is achieved, and an expected state is rapidly tracked. And finally, a brand new and simple adaptive control strategy is provided, an adaptive law is designed according to system errors and sliding mode surface information, a gain matrix is adjusted in real time, and system uncertainty and external interference are compensated. The method has the advantages of being high in robustness, rapid in convergence, simple and easy to implement.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned submersible control, and particularly relates to a method for controlling the trajectory tracking of an autonomous unmanned submersible. Background Art

[0002] As an important underwater operation equipment, unmanned submersibles play a key role in many fields such as ocean scientific research, ocean resource development, and military reconnaissance. During underwater navigation, achieving precise trajectory tracking control is the basis for unmanned submersibles to complete various tasks. However, due to the complexity and uncertainty of the underwater environment, many challenges are brought to the trajectory tracking control of unmanned submersibles.

[0003] There are various interference factors in the underwater environment, such as irregular changes in water flow, the influence of marine organisms, and complex seabed topography. These interferences will cause deviations between the actual motion state and the desired state of the unmanned submersible, thus affecting the accuracy of trajectory tracking. At the same time, the dynamic characteristics of the unmanned submersible itself are also highly nonlinear, and its parameters such as mass and moment of inertia will change with the change of load, which further increases the difficulty of control.

[0004] Currently, the existing methods for controlling the trajectory tracking of unmanned submersibles mainly include traditional PID control, sliding mode control, etc. The traditional PID control method has a simple structure and is easy to implement, but it is sensitive to changes in system parameters and external interferences, and has poor robustness, making it difficult to ensure good control effects in complex underwater environments. Sliding mode control has strong robustness and can effectively suppress external interferences and uncertainties in system parameters, but traditional sliding mode control has chattering problems, which will affect the control accuracy and system stability. In addition, some existing control methods also have deficiencies in terms of convergence speed and cannot quickly enable the unmanned submersible to track the desired trajectory, thus affecting the execution efficiency of tasks.

[0005] With the continuous deepening of ocean development and utilization, higher requirements are put forward for the trajectory tracking control performance of unmanned submersibles. Not only does the controller need to have strong robustness and be able to work stably in complex underwater environments, but it also needs to have fast convergence and be able to quickly and accurately track the desired trajectory. Therefore, it is of great practical significance to study a method for controlling the trajectory tracking of an autonomous unmanned submersible that can simultaneously meet the requirements of strong robustness and fast convergence. Summary of the Invention

[0006] The purpose of the present invention is to solve the deficiencies of the existing methods for controlling the trajectory tracking of unmanned submersibles in terms of strong robustness and fast convergence, and to provide a method for controlling the trajectory tracking of an autonomous unmanned submersible to ensure that the unmanned submersible can quickly and accurately track the desired trajectory in a complex underwater environment.

[0007] To achieve the above object, the technical solution of the present invention is: an unmanned autonomous underwater vehicle trajectory tracking control method, including:

[0008] Analyze the kinematic and dynamic characteristics of the unmanned underwater vehicle, and establish a non-linear dynamic model of the unmanned underwater vehicle;

[0009] Design a non-singular integral terminal sliding mode surface, and design a sliding mode control law according to the non-singular integral terminal sliding mode surface to perform trajectory tracking control on the unmanned underwater vehicle;

[0010] According to the error of the system and the sliding mode surface information, design an adaptive law to adjust the gain matrix in the sliding mode control law in real time.

[0011] The present invention also provides a computer-readable storage medium, on which computer program instructions that can be run by a processor are stored. When the processor runs the computer program instructions, the method steps as described above can be implemented.

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

[0013] 1. Strong robustness: The present invention adopts a non-singular integral terminal sliding mode control method and an adaptive control strategy, which can effectively suppress external interference and system parameter uncertainty. Specifically, the adaptive control strategy adjusts the gain matrix in the sliding mode control law in real time according to the error of the system and the sliding mode surface information , compensating for system uncertainty and external interference; the non-singular integral terminal sliding mode control method passes through the sliding mode surface (where , , ) and the design of the sliding mode control law , enabling the system to still stably track the desired trajectory when facing underwater environment such as irregular changes in water flow, the influence of marine organisms, complex seabed topography and other interferences, as well as changes in the dynamic parameters of the underwater vehicle itself.

[0014] 2. Fast convergence: The design of the non-singular integral terminal sliding mode surface enables the system to converge to the equilibrium point within a finite time. By considering the Lyapunov function , taking its derivative and combining with the control law derivation, it can be obtained , satisfying the conditions of the finite-time stability theory ( , ), thus realizing fast trajectory tracking and improving the execution efficiency of the task.

[0015] 3. Simple and easy to implement: The proposed adaptive control strategy is simple and effective, and only needs to be based on the error of the system and the sliding mode surface information, according to the adaptive law ( ) Real-time adjustment of the gain matrix That's all. It is easy to implement in the actual control system of an underwater vehicle, reducing the control cost and complexity. Description of the Drawings

[0016] Figure 1 It is a schematic diagram of the method flow of the present invention. Detailed Embodiment

[0017] The technical solution of the present invention will be specifically described below with reference to the drawings.

[0018] The present invention provides a method for trajectory tracking control of an autonomous underwater vehicle, including:

[0019] Analyze the kinematic and dynamic characteristics of the underwater vehicle, and establish a non-linear dynamic model of the underwater vehicle;

[0020] Design a non-singular integral terminal sliding mode surface, and design a sliding mode control law according to the non-singular integral terminal sliding mode surface to perform trajectory tracking control on the underwater vehicle;

[0021] Design an adaptive law to adjust the gain matrix in the sliding mode control law in real time according to the system error and sliding mode surface information.

[0022] The following is the specific implementation process of the present invention.

[0023] 1. Establish the non-linear dynamic model of the underwater vehicle

[0024] 1.1 Coordinate system definition

[0025] When establishing the non-linear dynamic model of the underwater vehicle, it is first necessary to define a suitable coordinate system. Usually, two coordinate systems are adopted: the inertial coordinate system and the underwater vehicle body coordinate system . The inertial coordinate system is a reference coordinate system fixed on the earth, used to describe the position and attitude of the underwater vehicle in the global space. The underwater vehicle body coordinate system is fixed on the underwater vehicle, its origin is located at the center of mass of the underwater vehicle, and the coordinate axes coincide with the geometric symmetry axes of the underwater vehicle.

[0026] Let be the position and attitude vector of the underwater vehicle in the inertial coordinate system, where represents the position of the center of mass of the underwater vehicle in the inertial coordinate system, respectively represent the roll angle, pitch angle and yaw angle of the underwater vehicle around , , axes; the velocity vector of the underwater vehicle in the underwater vehicle body coordinate system, represents the linear velocity of the centroid of the unmanned submersible in the body coordinate system of the unmanned submersible. represents the angular velocity of the unmanned submersible about the body coordinate axes of the unmanned submersible.

[0027] 1.2 Kinematic Equation

[0028] The kinematic equation of the submersible describes the relationship between its position and attitude in the inertial coordinate system and the velocity in the body coordinate system. According to the coordinate transformation principle, the kinematic equation can be obtained as follows:

[0029] where, is the transformation matrix from the body coordinate system to the inertial coordinate system, and its specific form is:

[0030] is the rotation matrix, which is used to describe the attitude transformation of the submersible, and its expression is:

[0031] is the matrix used to describe the angular velocity transformation, and its expression is:

[0032] Here, represents , represents , represents .

[0033] 1.3 Dynamic Equation

[0034] According to the Newton - Euler equation, considering various forces and torques acting on the submersible underwater, the dynamic equation of the submersible can be established. The forces and torques acting on the submersible mainly include inertial force, Coriolis force, damping force, gravity, buoyancy, and control input force and torque.

[0035] The inertial force and Coriolis force can be expressed as , where is the inertia matrix, including the mass matrix and added mass matrix of the submersible, is the Coriolis force and centripetal force matrix. The damping force can be expressed as , where is the damping matrix. The gravity and buoyancy can be expressed as . The control input force and torque are expressed as .

[0036] Therefore, the dynamic model equation of the submersible is:

[0037] Inertia matrix can be expressed as:

[0038] where, is the rigid body mass matrix of the submersible, is the added mass matrix. The Coriolis force and centripetal force matrix satisfies , and is related to the inertia matrix . The damping matrix usually contains linear damping and nonlinear damping terms, and can be expressed as , where and are the linear and nonlinear damping coefficient matrices respectively. The gravity and buoyancy vectors are related to the mass distribution, buoyancy distribution and attitude of the submersible, and their specific expressions need to be calculated according to the specific structure and parameters of the submersible.

[0039] Through the above steps, a nonlinear dynamic model of the unmanned submersible is established. This model can accurately reflect the motion characteristics of the submersible underwater and provides a basis for the subsequent design of trajectory tracking control.

[0040] 2. Adopt the nonsingular integral terminal sliding mode control method

[0041] 2.1 Design of the nonsingular integral terminal sliding mode surface

[0042] In the trajectory tracking control of the unmanned autonomous submersible, in order to make the system converge to the equilibrium point in finite time and avoid the singularity problem of traditional sliding mode control, a nonsingular integral terminal sliding mode surface is designed. Let the tracking error of the unmanned submersible be , which represents the difference between the actual state and the desired state of the unmanned submersible. The nonsingular integral terminal sliding mode surface is defined as:

[0043] where, and are positive design parameters, and their values will affect the convergence speed and stability of the system. is a constant that satisfies , and the selection of this constant plays a key role in the performance of the sliding mode surface. When approaches zero, the term can accelerate the convergence speed of the system, making the system state converge to the equilibrium point in finite time.

[0044] 2.2 Derivation of the sliding mode control law

[0045] Based on the designed nonsingular integral terminal sliding surface , the sliding mode control law is derived . First, differentiating the sliding surface gives:

[0046] According to the nonlinear dynamic model of the underwater vehicle , the velocity error is analyzed. Let the desired velocity be , then the tracking error , and differentiating it gives .

[0047] To make the sliding surface satisfy the sliding mode reaching condition , the sliding mode control law is designed as:

[0048] where is the desired velocity, is a positive gain matrix, is the sign function, and its definition is:

[0049] Through such a control law design, it can be ensured that the system state quickly tracks the desired state. When the sliding surface is non-zero, the sign function will generate a control action opposite to the sign of , making the system state approach the sliding surface, finally reaching the sliding surface and sliding on it, thus realizing the tracking of the desired state.

[0050] 2.3 Finite-time convergence analysis

[0051] Next, it is proved that the nonsingular integral terminal sliding mode control method can make the system converge in finite time. Considering the Lyapunov function , differentiating it gives:

[0052] Substituting the sliding mode control law into the expression of and simplifying it in combination with the dynamic model of the underwater vehicle. Since is a positive gain matrix, according to the sliding mode reaching condition, we have:

[0053] Because , so , which indicates that the Lyapunov function is monotonically decreasing. According to the theory of finite-time stability, when (where , ), the system state can converge to the equilibrium point within a finite time. In non-singular integral terminal sliding mode control, due to the design of the sliding surface and the action of the control law, the conditions for finite-time convergence are satisfied, thus ensuring that the underwater vehicle can quickly track the desired trajectory.

[0054] 2.4 Combination with the application background

[0055] In a complex underwater environment, the underwater vehicle is subject to external disturbances such as irregular changes in water flow, the influence of marine organisms, and complex seabed topography. At the same time, its own dynamic characteristics are also highly non-linear. The non-singular integral terminal sliding mode control method can effectively suppress these disturbances and uncertainties through the design of the sliding surface and the adjustment of the control law. For example, when encountering a sudden change in water flow, the tracking error will change, the sliding surface will also change accordingly, and the control law will adjust the control input in real time according to the change of , enabling the underwater vehicle to quickly adapt to changes in the external environment and ensuring the accuracy and stability of trajectory tracking.

[0056] 3. Propose a new and simple adaptive control strategy

[0057] 3.1 Necessity of adaptive control

[0058] In the trajectory tracking control of an autonomous underwater vehicle, due to the complexity and uncertainty of the underwater environment, such as irregular changes in water flow, the influence of marine organisms, and changes in dynamic parameters caused by changes in the vehicle's own load, the system has uncertainties and external disturbances. Traditional fixed-parameter control methods are difficult to cope with these changes. Therefore, an adaptive control strategy is needed to adjust the control parameters in real time to ensure the robustness and control performance of the system.

[0059] 3.2 Design idea of the adaptive law

[0060] The core of the adaptive control strategy of the present invention is to adjust the gain matrix in the sliding mode control law in real time according to the error of the system and the information of the sliding surface. By adjusting the gain matrix , the uncertainties and external disturbances of the system can be compensated, enabling the system to better track the desired trajectory.

[0061] 3.3 Specific design of the adaptive law

[0062] Define the tracking error is the difference between the desired state and the actual state, and the sliding surface is , where and are positive design parameters, is a constant that satisfies .

[0063] Consider the nonlinear dynamic model of the underwater vehicle , where is the inertia matrix, is the velocity vector, is the Coriolis and centripetal force matrix, is the damping matrix, is the gravity and buoyancy vector, is the control input vector.

[0064] The sliding mode control law is , where is the desired velocity, is a positive gain matrix, is the sign function.

[0065] In order to enable the system to adaptively compensate for uncertainties and external disturbances, the adaptive law is designed as , where is the positive adaptive gain.

[0066] 3.4 Stability analysis of the adaptive law

[0067] Consider the Lyapunov function , where is 's estimated value.

[0068] Take the time derivative of :

[0069] Substitute the derivative of the sliding surface into the above formula, and simplify it by combining the nonlinear dynamic model and the sliding mode control law.

[0070] After a series of derivations (the detailed derivation process is omitted here), we can obtain , where is a positive constant.

[0071] According to the Lyapunov stability theory, when , the system is stable. Therefore, through the designed adaptive law , the stability of the system can be guaranteed, and the uncertainties and external disturbances of the system can be adaptively compensated.

[0072] 3.5 Application of Adaptive Control Strategy

[0073] In practical applications, first, according to the actual parameters and mission requirements of the underwater vehicle, determine the parameters in the nonlinear dynamics model. Then, design the non-singular integral terminal sliding mode surface and the sliding mode control law according to the above design method, and adjust the control parameters in real time according to the adaptive law.

[0074] Obtain the motion state information of the underwater vehicle in real time through sensors, and calculate the tracking error and the sliding mode surface . According to the adaptive law adjust the gain matrix in real time . Substitute the adjusted into the sliding mode control law to calculate the control input

[0075] to drive the propulsion system of the underwater vehicle and achieve trajectory tracking control.

[0076] The overall innovation points of the method of the present invention are as follows:

[0077] 1. Establish an accurate nonlinear dynamics model: By analyzing in detail the kinematic and dynamic characteristics of the underwater vehicle, considering various forces and torques, define the inertial coordinate system and the body coordinate system of the vehicle to obtain the kinematic equation and the dynamic equation to accurately describe the underwater motion state of the vehicle and lay a foundation for control design.

[0078] 2. Adopt the non-singular integral terminal sliding mode control method: Design the non-singular integral terminal sliding mode surface ( ) to avoid the singular problem of traditional sliding mode control. Combine with the sliding mode control law to achieve the finite-time convergence of the system to the equilibrium point and quickly track the desired state.

[0079] 3. Propose a new and simple adaptive control strategy: According to the system error and the sliding mode surface information, design the adaptive law to adjust the gain matrix in the sliding mode control law in real time, compensate for system uncertainties and external disturbances, ensure system stability, and this strategy is simple and easy to implement, reducing the control cost and complexity.

[0080] The above are the preferred embodiments of the present invention. Any changes made to the technical solution of the present invention that do not exceed the scope of the technical solution of the present invention in terms of the functions and effects produced shall fall within the protection scope of the present invention.

Claims

1. An autonomous underwater vehicle trajectory tracking control method, characterized in that Including: Analyze the kinematic and dynamic characteristics of the unmanned submersible, and establish a nonlinear dynamic model of the unmanned submersible; Design a non-singular integral terminal sliding mode surface, and design a sliding mode control law according to the non-singular integral terminal sliding mode surface to conduct trajectory tracking control of the unmanned submersible; According to the error and sliding mode surface information of the system, design an adaptation law to adjust the gain matrix in the sliding mode control law in real time.

2. The method for controlling the track tracking of an unmanned autonomous submersible according to claim 1, characterized in that, The nonlinear dynamic model of the unmanned submersible is expressed as follows: represents the inertial force and the Coriolis force, M is the inertia matrix, including the mass matrix and the added mass matrix of the unmanned submersible, is the velocity vector of the unmanned submersible in the body coordinate system of the unmanned submersible, is the acceleration vector of the unmanned submersible in the body coordinate system of the unmanned submersible, is the Coriolis force and centripetal force matrix, represents the damping force, is the damping matrix, represents the gravity and buoyancy, is the position and attitude vector of the unmanned submersible in the inertial coordinate system, represents the control input vector.

3. A method for controlling the trajectory tracking of an autonomous underwater vehicle according to claim 2, characterized in that, In the process of establishing the non-linear dynamic model of the unmanned submersible, an inertial coordinate system is defined and the body coordinate system of the unmanned submersible . The position and attitude vectors of the unmanned submersible in the inertial coordinate system , represents the position of the center of mass of the unmanned submersible in the inertial coordinate system. respectively represent the roll angle, pitch angle and yaw angle of the unmanned submersible around , , axes. The velocity vector of the unmanned submersible in the body coordinate system of the unmanned submersible , represents the linear velocity of the center of mass of the unmanned submersible in the body coordinate system of the unmanned submersible. represents the angular velocity of the unmanned submersible around the body coordinate axes of the unmanned submersible.

4. A method for autonomous underwater vehicle trajectory tracking control according to claim 2, characterized in that, In the process of establishing the nonlinear dynamic model of the unmanned submersible, the inertia matrix , is the rigid body mass matrix of the unmanned submersible, is the added mass matrix.

5. The method for controlling the path tracking of an unmanned autonomous submersible according to claim 2, wherein, In the process of establishing the non - linear dynamic model of the unmanned submersible, the damping matrix , and are the linear damping coefficient matrix and the non - linear damping coefficient matrix respectively.

6. The method for controlling the track tracking of an unmanned autonomous submersible according to claim 2, wherein In the process of establishing the non-linear dynamic model of an unmanned submersible, the Coriolis force and centripetal force matrices satisfy , and are related to the inertia matrix .

7. A method for autonomous underwater vehicle trajectory tracking control according to claim 2, characterized in that The non-singular integral terminal sliding mode surface is expressed as follows: The sliding mode control law is expressed as follows: is the tracking error of the unmanned submersible, is the speed error of the unmanned submersible, and t is the time of the entire tracking process, , are positive design parameters, is a constant and satisfies , is the desired acceleration of the unmanned submersible, is a positive gain matrix, is the sign function, and its definition is: 。 8. A method for track tracking control of an unmanned autonomous underwater vehicle according to claim 7, characterized in that, The adaptation law is expressed as follows: is a positive adaptive gain.

9. The method for controlling the track tracking of an unmanned autonomous underwater vehicle according to claim 8, wherein The method is applied to the trajectory tracking control of an unmanned underwater vehicle. Specifically, the motion state information of the unmanned underwater vehicle is obtained in real time through sensors, and the tracking error is calculated. and the nonsingular integral terminal sliding mode surface ; According to the adaptive law , the gain matrix is adjusted in real time, and the adjusted gain matrix is substituted into the sliding mode control law to calculate the control input vector , and the propulsion system of the unmanned underwater vehicle is driven to achieve the trajectory tracking control of the unmanned underwater vehicle.

10. A computer-readable storage medium, on which computer program instructions capable of being run by a processor are stored. When the processor runs the computer program instructions, the method steps described in any one of claims 1-9 can be implemented.

Images