A dynamics simulation platform and method of underwater robot based on sliding mode control

Abstract

The present invention discloses an underwater robot dynamics simulation platform and method based on sliding mode control, belonging to the field of underwater robots, the method comprises: a human-machine interaction module, a motion control module, a dynamics model module and an ocean statics and ocean dynamics module; wherein the human-machine interaction module is used to receive upper machine position instructions and feedback data analysis results; the motion control module is used to output propulsion force based on upper machine position instructions; the ocean statics and ocean dynamics module is used to calculate dynamic and static hydraulics; the dynamics model module is used to obtain simulation calculation results based on propulsion force and dynamic and static hydraulics. The underwater robot dynamics simulation platform of the present invention adopts modular design, the input and output of the module are integrated by submodules, and when there is a need for modification, only the submodule needs to be adjusted, the control algorithm, trajectory planning and other modules in the simulation can be simply and quickly switched, and the open source algorithm and planning algorithm can be expanded to the module to achieve rapid testing.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater robots, and in particular relates to an underwater robot dynamics simulation platform and method based on sliding mode control. Background Art

[0002] With the development of science and technology, underwater robots are increasingly used in the fields of ocean exploration, seabed exploration, military reconnaissance, etc. However, due to the complexity and unknown nature of the underwater environment, the development and testing of underwater robots face huge challenges. In order to solve this problem, researchers usually use simulation technology to build a virtual model of the underwater robot on the computer, simulate and test it under various working conditions to evaluate its performance and reliability. At the same time, control system technology also plays a key role in the development of underwater robots, which determines the movement mode and operation accuracy of underwater robots.

[0003] The existing underwater robot simulation technology is mainly based on specific simulation platforms, such as MATLAB / Simulink, etc., and the underwater robot simulation is realized by writing a lot of code. In addition, there are also some commercial simulation software, such as Aquasim, Virtual Robot Experimentation Platform (V-REP), etc., which can provide a more intuitive and convenient simulation environment. In terms of control systems, most underwater robots use rule-based control methods, that is, the robot's motion control is realized through preset control algorithms.

[0004] Although the existing underwater robot simulation and control technology has solved the research and development problems of underwater robots to a certain extent, there are still some shortcomings. First, the existing simulation platform has a low degree of modularity and is difficult to adapt to various types of underwater robots and complex simulation scenarios. Secondly, the existing simulation platform has poor scalability. Once new functions need to be added or existing functions need to be modified, a large amount of code often needs to be rewritten, which not only increases the development workload but also prolongs the research and development cycle. Finally, most of the existing underwater robot control systems rely on specific hardware devices, which not only increases the research and development costs, but also limits its application in different environments. Summary of the invention

[0005] In order to solve the above technical problems, the present invention proposes an underwater robot dynamics simulation platform and method based on sliding mode control to solve the problems existing in the above prior art.

[0006] To achieve the above object, the present invention provides an underwater robot dynamics simulation platform and method based on sliding mode control, comprising:

[0007] Human-computer interaction module, motion control module, dynamics model module and ocean statics and ocean dynamics module;

[0008] Wherein, the human-computer interaction module is used to receive the on-board instructions and feedback the data analysis results;

[0009] The motion control module is used to output propulsion force based on the upper position instruction;

[0010] The ocean statics and ocean dynamics modules are used to calculate dynamic and static water forces;

[0011] The dynamic model module is used to obtain simulation calculation results based on the propulsion force and the dynamic and static water forces.

[0012] Optionally, the dynamic model module and the ocean statics and ocean dynamics module are connected in a closed loop;

[0013] Wherein, the dynamic model module calculates the thrust and posture data of the underwater robot based on the propulsion force, and transmits the data to the ocean statics and ocean dynamics module;

[0014] The ocean statics and ocean dynamics module is used to calculate the static and dynamic hydraulic forces based on the gravity, buoyancy and thrust of the underwater robot, and transmit the static and dynamic hydraulic forces to the dynamic model module.

[0015] Optionally, the human-computer interaction module includes a visualization submodule and a button component submodule;

[0016] The visualization submodule is used to view the motion interface, motion parameter curve, parameter configuration and underwater robot historical data in real time;

[0017] The button assembly submodule is used to control the underwater robot.

[0018] Optionally, the motion control module includes a control algorithm submodule and a propulsion force control submodule;

[0019] Wherein, the control algorithm submodule is used to convert the on-board position instruction into a control signal;

[0020] The propulsion force control submodule is used to output propulsion force based on the control signal.

[0021] Optionally, the dynamic model module includes a physical submodule, a torsion torque submodule and a rotation pair submodule;

[0022] Wherein, the entity submodule is used to provide entity models of various components of the underwater robot;

[0023] The torque submodule is used to construct the external force and output torque of each entity component;

[0024] The rotation pair submodule is used to establish the motion relationship between the propeller and the entity.

[0025] A simulation method for underwater robot dynamics based on sliding mode control, the simulation method is applied to an underwater robot dynamics simulation platform based on sliding mode control, and the simulation method comprises the following steps:

[0026] The human-computer interaction module obtains a host computer instruction from the host computer and transmits the host computer instruction to the motion control module;

[0027] The motion control module processes the received command signal and outputs a propulsion force signal;

[0028] The dynamics module simultaneously receives the propulsion force signal of the motion control module and the output parameters of the ocean statics and ocean dynamics modules to construct a dynamics model of the underwater robot; and performs simulation calculation based on the dynamics model of the underwater robot to obtain a simulation result.

[0029] Optionally, the constructed underwater robot dynamic model is:

[0030]

[0031] Where M RB and C RB is the mass matrix and Coriolis effect matrix associated with the rigid body, M A is the additional mass matrix, C A is the Coriolis effect matrix caused by the additional mass, D(v) includes all the resistance F drag and lift F lift The damping matrix is ​​g(η), g(η) is the combined effect of gravity and buoyancy, v is the velocity vector of the system, is the acceleration of the system, and τ is the external disturbance.

[0032] Optionally, the motion control module controls the underwater robot based on a sliding mode, wherein the designed sliding mode surface is:

[0033]

[0034] In the formula, e=qq d , q d is the ideal position, B=diag(b1,…,b n ),b i >0.

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

[0036] The underwater robot dynamics simulation platform of the present invention adopts a modular design, dividing the entire platform into four independent modules. The input and output of the module are integrated by sub-modules. When there is a need for modification, only the sub-module needs to be adjusted. The control algorithm, trajectory planning and other modules in the simulation can be switched simply and quickly, and the open source algorithm and planning algorithm can be expanded to the module to achieve rapid testing.

[0037] The underwater machine dynamics simulation platform of the present invention is constructed based on Simulink of MATLAB, and contains a variety of basic simulation modules, which can meet the simulation needs of various underwater robots. Among them, many modules in Simscape can quickly establish various robot models; once it is necessary to add new functions or modify existing functions, only simple configuration and adjustment are required based on the original modules, without rewriting a large amount of code, which greatly reduces the development workload and shortens the R&D cycle.

[0038] The human-computer interaction module is equipped with a variety of host computer interfaces, which can switch between a variety of human-computer interaction devices and evaluate the interactive effects of interactive devices on underwater robots in different usage scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The drawings constituting a part of the present application are used to provide a further understanding of the present application. The illustrative embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0040] Figure 1 This is a diagram showing the relationship between various modules of the underwater robot dynamics simulation platform according to an embodiment of the present invention. DETAILED DESCRIPTION

[0041] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0042] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0043] Embodiment 1

[0044] like Figure 1As shown, in this embodiment, a dynamic simulation platform for underwater robots based on sliding mode control is provided, including a human-computer interaction module, a motion control module, a dynamic model module, and an ocean statics and ocean dynamics module. Among them, the human-computer interaction module is used to receive the upper position instruction; the motion control module is used to output the propulsion force based on the upper position instruction; the ocean statics and ocean dynamics module is used to calculate the dynamic and static water force; the dynamic model module is used to obtain the simulation calculation results based on the propulsion force and the dynamic and static water force.

[0045] As a preferred solution of this embodiment, the dynamic model module and the ocean statics and ocean dynamics module are closed-loop connected; wherein the dynamic model module calculates the thrust and posture data of the underwater robot based on the propulsion force; the ocean statics and ocean dynamics module is used to calculate the dynamic and static hydraulic forces based on the thrust and posture data of the underwater robot, and transmit the dynamic and static hydraulic forces to the dynamic model module.

[0046] As a preferred solution of this embodiment, the human-computer interaction module includes a visualization submodule and a button component submodule; wherein the visualization submodule is used to view the motion interface, motion parameter curve, parameter configuration and underwater robot historical data in real time; the button component submodule is used to control the underwater robot. The specific implementation process includes:

[0047] The human-computer interaction module is mainly used for human-computer interaction. In this module, you can view real-time data, modify parameters, switch controllers, etc., and provide an interactive interface between the computer and the simulation platform; including real-time motion interface, motion parameter curve, parameter configuration, historical data, dashboard and other visualization sub-modules, as well as button component sub-modules such as switch controller, route switch button, and chart button.

[0048] As a preferred solution of this embodiment, the motion control module includes a control algorithm submodule and a propulsion control submodule; wherein the control algorithm submodule is used to convert the upper position instruction into a control signal; the propulsion control submodule is used to output the propulsion based on the control signal. The propulsion control submodule is further divided into a main propulsion control submodule, a vertical propulsion submodule, and a horizontal propulsion submodule. The specific implementation process is:

[0049] The motion control module is used to control the thrust of the propeller of the virtual model. The main function of the underwater robot motion control module is to control the propulsion submodule to output the propulsion to the underwater robot dynamics module according to the signal output by the control algorithm. It includes the control algorithm submodule, the main propulsion control submodule, the vertical propulsion submodule, and the horizontal propulsion submodule; the control algorithm submodule is used to convert the received input signal into a control signal and output it to the three propulsion submodules; in this way, the virtual entity moves according to the desired trajectory.

[0050] As a preferred solution of this embodiment, the dynamic model module includes a physical submodule, a torque submodule and a rotation pair submodule; wherein the physical submodule is used to provide a physical model of each component of the underwater robot; the torque submodule is used to construct the external force and output torque of each physical component; the rotation pair submodule is used to construct the motion relationship between the propeller and the entity. The specific implementation process is:

[0051] The dynamic model module is used to construct a virtual underwater robot model, which includes a physical sub-module, a torque sub-module, and a rotation sub-module. The physical sub-module includes physical models of various components of the underwater robot, which are connected to each other by connection ports; the torque sub-module is used to construct the torque and torque exerted on and output by each physical component; the rotation sub-module is used to construct the motion relationship between the propeller and the entity; in this way, the construction of the virtual entity and its related dynamic model is completed.

[0052] The underwater robot ocean statics and ocean dynamics module is connected to the underwater robot dynamics model module. The ocean statics and ocean dynamics module is used to receive the underwater robot thrust and posture data from the dynamics module. Based on this, the robot's fluid statics, lift and drag, additional torque, fluid dynamics and other data are calculated and then transmitted back to the underwater robot dynamics module.

[0053] A simulation method for underwater robot dynamics based on sliding mode control, the simulation platform is built based on Simulink in MATLAB, and includes the following steps:

[0054] After receiving relevant instructions from the host computer or computer, the human-computer interaction module starts sending data to the motion control module of the simulation platform to execute the specified algorithm.

[0055] The algorithm submodule in the motion control module processes the received signal and sends it to the propulsion submodule, and outputs the propulsion signal to the dynamics module.

[0056] The dynamics module receives parameters sent by the motion control module and the ocean statics and ocean dynamics module at the same time, and performs simulation and settlement within the module. The calculation results are obtained as the output of the dynamics module and sent to the visualization submodule of the human-computer interaction module and the ocean statics and ocean dynamics module.

[0057] The human-computer interaction module displays various data in the visualization submodule and can adjust the simulation conditions through various component submodules.

[0058] The specific model building process is as follows:

[0059] Construct the body coordinate system of the underwater robot and the position coordinate system of the earth:

[0060] is the fixed position and direction vector of the underwater robot in the earth coordinate system, where [xyz] T For fixed position, is the direction vector.

[0061] v=[V B ω B ] T =[uvwpqr] T is the linear velocity and angular velocity of the underwater robot in a fixed coordinate system, where V B =[uvw] T is the linear velocity, ω B =[pqr] T is the angular velocity. propulsion =[XYZKMN] T It is the total force and torque acting on the underwater robot in the body coordinate system.

[0062] Establish the underwater robot dynamics model:

[0063]

[0064] Among them, M RB and C RB is the mass matrix and Coriolis effect matrix associated with the rigid body, M A is the additional mass matrix, C A is the Coriolis effect matrix caused by the additional mass, D(v) includes all the resistance F drag and lift F lift The damping matrix is ​​g(η), g(η) is the combined effect of gravity and buoyancy. v is the velocity vector of the system, is the acceleration of the system, and τ is the external disturbance.

[0065] Let the coordinates of the center of gravity of the underwater robot be x G =y G =z G =0, then the inertia matrix M is:

[0066]

[0067] g(η) is the combined effect of gravity and buoyancy, expressed as:

[0068]

[0069] Establish the ocean statics and ocean dynamics model of underwater robot:

[0070] To ensure the static balance of the underwater robot, the center of gravity of the underwater robot in the submerged state is set directly below the center of buoyancy. Gravity and buoyancy will generate a restoring torque to counteract external interference. Under the influence of the torque, external interference will cause a roll or pitch rotation around the robot axis and help the underwater robot return to the horizontal direction.

[0071] The lift F on the underwater robot in the damping matrix D(v) is lift and resistance F drag It can be expressed as

[0072]

[0073] where ρ is the fluid density, V is the robot velocity, A is the robot cross-sectional reference area, C l and C d are the lift and drag coefficients.

[0074] The additional mass matrix M of the underwater robot A It can be expressed as:

[0075]

[0076] in:

[0077]

[0078] where k 11 is the k-factor coefficient of the prolate ellipsoid, L is the axial length of the underwater robot, dx is the microelement of the cross section of the underwater robot on the axial length, x is the distance from the head end of the underwater robot to the microelement, and ρ is the liquid density. r(x) is the approximate radius of the ellipse on the microelement. r1r2 are the major axis and minor axis of the ellipse on the cross section, respectively.

[0079] The motion control algorithm takes sliding mode control as an example:

[0080] The dynamic model of the underwater robot is:

[0081]

[0082] The sliding surface is designed as

[0083]

[0084] where e = qq d , q d is the ideal position, B=diag(b1,…,b n ),b i >0 defines the Lyapunov function as:

[0085]

[0086] but

[0087]

[0088] The control law is taken as:

[0089]

[0090] Where M0, C0, K0, τ0 are the nominal values ​​of M, C, K, τ respectively, ΔM=M-M0, ΔC=C-C0, ΔK=K-K0, Δτ=τ-τ0.

[0091]

[0092] where Γ=diag(γ1,γ1,…,γ n ),(γ i >0).

[0093] Pick

[0094]

[0095] but

[0096]

[0097] The above are only preferred specific implementations of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. An underwater robot dynamics simulation platform based on sliding mode control, characterized in that: include: Human-computer interaction module, motion control module, dynamic model module and ocean statics and ocean dynamics module; Wherein, the human-computer interaction module is used to receive the on-board instructions and feedback the data analysis results; The motion control module is used to output propulsion force based on the upper position instruction; The ocean statics and ocean dynamics modules are used to calculate dynamic and static water forces; The dynamic model module is used to construct a dynamic model of the underwater robot based on the propulsion force and the dynamic and static hydraulic forces; and to perform simulation calculation based on the dynamic model of the underwater robot to obtain a simulation calculation result; The constructed underwater robot dynamic model is: Where M RB and C RB is the mass matrix and Coriolis effect matrix associated with the rigid body, M A is the additional mass matrix, C A is the Coriolis effect matrix caused by the additional mass, D(v) includes all the resistance F drag and lift F lift The damping matrix is ​​g(η), g(η) is the combined effect of gravity and buoyancy, v is the velocity vector of the system, is the acceleration of the system, τ is the external disturbance, F propulsion Represents all the forces and moments acting on the underwater robot in the body coordinate system; The additional mass matrix M of the underwater robot A It is expressed as: in: In the formula, k 11 is the k-factor coefficient of the prolate ellipsoid, L is the axial length of the underwater robot, dx is the microelement of the cross section of the underwater robot on the axial length, x is the distance from the head end of the underwater robot to the microelement, ρ is the liquid density, r(x) is the approximate radius of the ellipse on the microelement, r1 and r2 are the major axis and minor axis of the ellipse on the cross section respectively; The motion control module controls the underwater robot based on sliding mode, wherein the designed sliding mode surface is: In the formula, e=qq d , q d is the ideal position, B = diag (b1, ..., b n ), b i >0.

2. The underwater robot dynamics simulation platform based on sliding mode control according to claim 1 is characterized in that: The dynamic model module and the ocean statics and ocean dynamics module are connected in a closed loop; Wherein, the dynamic model module calculates the thrust and posture data of the underwater robot based on the propulsion force, and transmits the data to the ocean statics and ocean dynamics module; The ocean statics and ocean dynamics module is used to calculate the static and dynamic hydraulic forces based on the gravity, buoyancy and thrust of the underwater robot, and transmit the static and dynamic hydraulic forces to the dynamic model module.

3. The underwater robot dynamics simulation platform based on sliding mode control according to claim 1 is characterized in that: The human-computer interaction module includes a visualization submodule and a button component submodule; The visualization submodule is used to view the motion interface, motion parameter curve, parameter configuration and underwater robot historical data in real time; The button assembly submodule is used to control the underwater robot.

4. The underwater robot dynamics simulation platform based on sliding mode control according to claim 1 is characterized in that: The motion control module includes a control algorithm submodule and a propulsion control submodule; Wherein, the control algorithm submodule is used to convert the on-board position instruction into a control signal; The propulsion force control submodule is used to output propulsion force based on the control signal.

5. The underwater robot dynamics simulation platform based on sliding mode control according to claim 1 is characterized in that: The dynamic model module includes a solid submodule, a torsion torque submodule and a rotation pair submodule; Wherein, the entity submodule is used to provide entity models of various components of the underwater robot; The torque submodule is used to construct the external force and output torque of each entity component; The rotation pair submodule is used to establish the motion relationship between the propeller and the entity.

6. A method for simulating underwater robot dynamics based on sliding mode control, the method being applied to the underwater robot dynamics simulation platform based on sliding mode control according to claim 1, characterized in that: The simulation method comprises the following steps: The human-computer interaction module obtains a host computer instruction from the host computer and transmits the host computer instruction to the motion control module; The motion control module processes the received command signal and outputs a propulsion force signal; The dynamics module simultaneously receives the propulsion force signal of the motion control module and the output parameters of the ocean statics and ocean dynamics modules to construct a dynamics model of the underwater robot; and performs simulation calculation based on the dynamics model of the underwater robot to obtain a simulation result.

Images