Underwater flexible robot three-dimensional motion control method, device, equipment, medium and product

Abstract

The invention discloses an underwater flexible robot three-dimensional motion control method, device and equipment, a medium and a product, and relates to the field of flexible robot automation technology and nonlinear dynamics modeling analysis control. The method is suitable for a robot with periodic fluctuation and flexible motion characteristics. The method comprises the following steps: acquiring information data of a target underwater flexible robot; according to the information data, on the basis of a body wave equation of the target underwater flexible robot, a physical experiment and fluid mechanics simulation method is adopted, fitting of hydrodynamic parameters is conducted, and a hydrodynamic equation is obtained; based on the Lagrange theory, a kinetic model is determined according to the hydrodynamic equation; performing closed-loop motion control based on a three-dimensional space according to the kinetic model to realize three-dimensional trajectory tracking control of the target underwater flexible robot; the three-dimensional space is determined based on the speed, course and depth of the target underwater flexible robot. The invention aims to realize three-dimensional motion control of the underwater flexible robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of flexible robot automation technology and nonlinear dynamics modeling analysis control, in particular to a three-dimensional motion control method, device, equipment, medium and product of an underwater flexible robot. BACKGROUND

[0002] At present, the modeling and control researches on underwater robots are mostly concentrated on rigid underwater vehicles, and the dynamics modeling generally adopts a rigid six-degree-of-freedom model, which cannot accurately describe the influence of local deformation of flexible structures on overall motion, especially lacking the characteristic analysis of flexible motion in water environment interacting with water flow and its influence on motion control. Some existing researches attempt to regard the flexible body as a multi-segment rigid body or establish a linear model based on simplified assumptions, but there are still deficiencies, such as lack of a unified flexible robot modeling framework, strong coupling and poor adaptability of the control method, and lack of real-time available modeling-control integrated mechanism. SUMMARY

[0003] The purpose of the present application is to provide a three-dimensional motion control method, device, equipment, medium and product of an underwater flexible robot, which can realize three-dimensional motion control of the underwater flexible robot.

[0004] To achieve the above purpose, the present application provides the following solutions: In a first aspect, the present application provides a three-dimensional motion control method of an underwater flexible robot, which is applicable to a robot with periodic fluctuation and flexible motion characteristics; the method comprises: obtaining information data of a target underwater flexible robot; the information data comprises CAD shape information and fluid information; the fluid information comprises fluid density and flow rate; According to the information data, based on the body wave equation of the target underwater flexible robot, using physical experiments and fluid mechanics simulation methods, fitting of hydrodynamic parameters is performed to obtain a hydrodynamic equation; Based on Lagrange theory, a dynamics model is determined according to the hydrodynamic equation; According to the dynamics model, a closed-loop motion control based on three-dimensional space is performed to realize three-dimensional trajectory tracking control of the target underwater flexible robot; the three-dimensional space is determined based on the speed, heading and depth of the target underwater flexible robot.

[0005] In a second aspect, the present application provides a three-dimensional motion control device of an underwater flexible robot, comprising: An information data acquisition module is configured to acquire information data of a target underwater flexible robot; the information data comprises CAD shape information and fluid information; the fluid information comprises fluid density and flow rate; a fitting module configured to fit water dynamic parameters based on a body wave equation of the target underwater flexible robot, by using physical experiments and fluid dynamics simulation, according to the information data, to obtain a water dynamic equation; a dynamics model determination module configured to determine a dynamics model based on the water dynamic equation according to Lagrange theory; a control module configured to perform closed-loop motion control based on a three-dimensional space to realize three-dimensional trajectory tracking control of the target underwater flexible robot according to the dynamics model, wherein the three-dimensional space is determined based on velocity, heading and depth of the target underwater flexible robot.

[0006] In a third aspect, the present application provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the underwater flexible robot three-dimensional motion control method described above.

[0007] In a fourth aspect, the present application provides a computer readable storage medium having a computer program stored thereon, wherein the computer program is executed by a processor to implement the underwater flexible robot three-dimensional motion control method described above.

[0008] In a fifth aspect, the present application provides a computer program product comprising a computer program, wherein the computer program is executed by a processor to implement the underwater flexible robot three-dimensional motion control method described above.

[0009] According to the embodiments provided in the present application, the following technical effects are disclosed: The present application provides an underwater flexible robot three-dimensional motion control method, device, equipment, medium and product, which is suitable for robots with periodic fluctuation and flexible motion characteristics. Information data of a target underwater flexible robot is obtained. Water dynamic parameters are fitted based on a body wave equation of the target underwater flexible robot by using physical experiments and fluid dynamics simulation according to the information data, to obtain a water dynamic equation. A dynamics model is determined based on Lagrange theory according to the water dynamic equation. Closed-loop motion control based on a three-dimensional space is performed according to the dynamics model to realize three-dimensional trajectory tracking control of the target underwater flexible robot. The present application emphasizes the flexible motion characteristics of the robot, including periodicity and fluctuation, and considers the closed-loop control under the influence of random disturbance when interacting with the external environment. In addition, the present application determines a dynamics model based on Lagrange theory according to the water dynamic equation, and then realizes closed-loop motion control in the three-dimensional space of velocity, heading and depth. Therefore, the present application can realize three-dimensional motion control of the underwater flexible robot. BRIEF DESCRIPTION OF DRAWINGS

[0010] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed in the embodiments. Obviously, the drawings described below only constitute some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained without creative labor on the basis of these drawings.

[0011] Figure 1 A flow chart of a three-dimensional motion control method for an underwater flexible robot; Figure 2 A whole flow chart of a three-dimensional motion control method for an underwater flexible robot; Figure 3 A three-dimensional motion control chart; Figure 4 A hydrodynamic force data fitting result schematic diagram; Figure 5 A three-dimensional trajectory tracking control effect schematic diagram; Figure 6 A speed stabilization effect schematic diagram; Figure 7 A heading stabilization effect schematic diagram; Figure 8 A depth stabilization effect schematic diagram; Figure 9 A pitch angle stabilization effect schematic diagram; Figure 10 A structure diagram of a three-dimensional motion control device for an underwater flexible robot; Figure 11 A structure schematic diagram of a computer device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0012] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments only constitute some embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0013] The above purposes, features and advantages of the present application can be more obvious and easy to understand. The present application will be further described in detail below with reference to the drawings and specific embodiments.

[0014] In an exemplary embodiment, a three-dimensional motion control method for an underwater flexible robot is provided, which is suitable for a robot with periodic fluctuation and flexible motion characteristics. As shown in Figure 1 The method comprises the following steps. Step 100: obtaining information data of a target underwater flexible robot. The information data includes: CAD shape information and fluid information; the fluid information includes: fluid density and flow rate.

[0015] Step 200: according to the information data, based on the body wave equation of the target underwater flexible robot, using physical experiments and fluid mechanics simulation methods, fitting the hydrodynamic parameters to obtain the hydrodynamic equation.

[0016] Step 300: based on the Lagrange theory, determining the dynamic model according to the hydrodynamic equation.

[0017] Step 400: according to the dynamic model, performing closed-loop motion control based on three-dimensional space to realize three-dimensional trajectory tracking control of the target underwater flexible robot. The three-dimensional space is determined based on the speed, heading and depth of the target underwater flexible robot.

[0018] In an embodiment, according to the information data, based on the body wave equation of the target underwater flexible robot, using physical experiments and fluid mechanics simulation methods, fitting the hydrodynamic parameters to obtain the hydrodynamic equation, specifically including: Using the fluid mechanics simulation method, based on the body wave equation of the target underwater flexible robot, determining the fluid dynamic data according to the information data; the fluid dynamic data includes: forward thrust data and turning torque.

[0019] Using the physical experiment method, by changing the pitch rudder, the target underwater flexible robot performs uniform diving motion, and the pitch torque is determined based on the measured pitch angular velocity.

[0020] According to the fluid dynamic data and the pitch torque, fitting the hydrodynamic parameters of the preset hydrodynamic equation to obtain the hydrodynamic equation.

[0021] The expression corresponding to the body wave equation is: .

[0022] Wherein, is the lateral offset of the target underwater flexible robot relative to the body centerline at time t; t is the body wave state quantity along the axial direction from the head of the target underwater flexible robot to the tail tip; s are all constants; is the wave number corresponding to the body wave; is the wave frequency; is the turning offset.

[0023] The expression of the hydrodynamic equation is: .

[0024] Wherein,​ is the hydrodynamic force equation corresponding to the forward thrust at time t; t is the hydrodynamic force equation corresponding to the forward thrust at time t; is the linear hydrodynamic force coefficient coupled with inertia and friction; is the frequency nonlinear term of the mean thrust; is the nonlinear term of the wave amplitude; is the hydrodynamic force coefficient; is the frequency nonlinear term of the wave thrust; are all constant coefficients; is the wave frequency; is the phase delay of the thrust; is the hydrodynamic force equation corresponding to the turning moment at time t; t is the control variable of the turning motion; is the wave term hydrodynamic force coefficient of the turning moment; is the linear hydrodynamic force coefficient; is the second order nonlinear hydrodynamic force coefficient; is the hydrodynamic force equation corresponding to the heave moment at time t; is the fluid density; t is the resultant velocity of the three axis velocities; is the maximum cross-sectional area of the body of the target underwater flexible robot; is the body length of the target underwater flexible robot; S is the linear hydrodynamic force coefficient of the heave moment; L is the second order nonlinear hydrodynamic force coefficient; is the control variable of the heave rudder angle. In an embodiment, the method for determining the dynamic model specifically comprises: inputting the hydrodynamic force equation into the corresponding position of the preset robot control force vector of six degrees of freedom to determine the robot control force vector.

[0025] based on the Lagrange theory, determining the dynamic model according to the robot control force vector; the expression of the dynamic model is:

[0026] . .

[0027] wherein, is the added mass matrix; is the velocity-dependent damping term; is the damping matrix; is the velocity coupling term caused by rotation; is the Coriolis force matrix; is the six degrees of freedom generalized velocity vector of the system including linear velocity and angular velocity; is the time derivative of the velocity vector;​ is a coupling term of gravity and buoyancy; is a restoring force matrix; is a pose in three-dimensional space; is an external water flow disturbance; is a robot control force vector.

[0028] As an optional implementation, the three-dimensional space-based closed-loop motion control is performed according to the dynamic model to realize three-dimensional trajectory tracking control of the target underwater flexible robot, and specifically includes: According to the dynamic model, a discrete speed control law for the speed of the target underwater flexible robot is determined; the expression of the discrete speed control law is: .

[0029] According to the dynamic model, a discrete heading control law for the heading of the target underwater flexible robot is determined; the expression of the discrete heading control law is: .

[0030] According to the dynamic model, a pitch rudder angle control law for the depth of the target underwater flexible robot is determined; the expression of the pitch rudder angle control law is: .

[0031] wherein, is a discrete speed control law; is a discrete time variable; is a constant; and are both discrete time variables corresponding control parameters; is a discrete time variable corresponding to an error of the expected speed and the robot speed; is a cumulative error of the robot speed; is a robot motion period; is a time loop variable; is a discrete heading control law; is a heading control amount at the last time; and are both constants; is a heading angular velocity at the current time; is a heading error; are both controller parameters; is a pitch angle error; is a pitch angle error at the last time; is a cumulative pitch angle error; is a pitch rudder angle control period; is a pitch rudder angle control law.

[0032] The technical problem to be solved by the present application is to provide a three-dimensional dynamic modeling method and hydrodynamic equation for an underwater flexible robot, which is suitable for underwater bionic robotic fish, underwater bionic robotic snake and other robots with significant periodic fluctuation and flexible motion characteristics. The dynamics and accurate hydrodynamic force of the system are obtained through the Lagrange equation and the data-driven method respectively, so as to avoid directly modeling the complex coupling relationship of the internal mechanism of the robot. And based on the dynamic model, a three-dimensional motion controller for flexible robots is designed, which is real-time available.

[0033] The technical solution of the present application is: first, define the flexible motion of the robot; second, establish the hydrodynamic equation and fit the fluid dynamics data obtained based on experiments and computational fluid dynamics (CFD) to obtain the parameters; then input the hydrodynamic equation into the Lagrange dynamic model, and solve the robot state based on the internal and external force; finally, design a three-dimensional closed-loop motion controller for the robot based on the designed model, which can support the robot to realize specific trajectory tracking in underwater space. The flow chart of the present application is shown in Figure 2 , and the specific steps are as follows: Step 1: Define the body deformation of the flexible robot.

[0034] The motion characteristics of the underwater flexible robot are defined by the body wave equation: .

[0035] The amplitude envelope of transverse motion follows a quadratic function , where are constants, and the body wave is described by the wave number , where l is a multiple of the body length (BL), The value of can be set according to the actual fluctuation ability of the robot, generally within one dynamic range. Different combinations of the above parameters will represent different flexible motion modes. Based on this body wave equation, it can be seen that the motion of the target underwater flexible robot has fluctuation and periodicity, and the robot motion period is defined as . The motion of the flexible robot is defined by the body wave equation, which is input into step 2.

[0036] Step 2: Design the hydrodynamic equation of the flexible robot in the water environment, and give the method for determining the constant coefficients through data-driven.

[0037] Input the body wave equation of step 1 into the computational fluid dynamics (CFD) simulation software, in addition, import the CAD shape of the robot to be modeled, set the fluid density and flow rate, and change the fluctuation frequency fand turning bias , record the CFD output fluid force data of straight and turning motion under two-dimensional plane motion, including forward thrust data at different frequencies and turning moment .

[0038] Build experiments in the physical world, change the elevator to make the robot do uniform diving motion, record the pitch angular velocity measured by IMU under different elevator control r data, calculate the available pitch moment , wherein, is the rotational inertia of the pitch axis.

[0039] Put the two-dimensional plane data obtained by simulation and experiment and , respectively, into the designed hydrodynamic equation , fit to obtain the equation constant The hydrodynamic equation designed in the application is as follows: .

[0040] wherein, determined by the flexible robot wave state in step one (L is the length of the target underwater flexible robot), reflecting the nonlinear relationship between average thrust, frequency and amplitude.

[0041] is the result converted to angular units in step one , which is also the control quantity of turning motion.

[0042] Input the hydrodynamic equation after determining the coefficient into step three.

[0043] Step three: design a six-degree-of-freedom dynamics model of the robot.

[0044] define is the robot control force vector, input the hydrodynamic equation obtained in step two into the six-degree-of-freedom corresponding position of the control force vector to obtain . Define the linear velocity and angular velocity of the robot body coordinate system as , the displacement and attitude in the world coordinate system as , and the coordinate system conversion relationship as , wherein, respectively, are the forward thrust, the heading control moment, and the pitch control moment; respectively, are the forward velocity, the lateral velocity, the longitudinal velocity, and the roll angle velocity, the heading angle velocity, and the pitch angle velocity; respectively, are the forward displacement, the lateral displacement, the longitudinal displacement, and the roll angle, the heading angle, and the pitch angle; is the derivative of the six-freedom generalized position vector with respect to time; is the coordinate system rotation transformation of the position vector, are all Euler angle transformation matrices, is the Jacobian matrix, represents the direct correspondence between linear velocity and position rate of change, and the specific form is as follows: .

[0045] Definition Based on the Lagrange theory, the following dynamic model is established: .

[0046] The angle of attack is defined as , the sideslip angle is defined as . The difference between the gravity and the buoyancy is defined as , and the coordinate components of the center of gravity in the body coordinate system . is the external water flow disturbance, where are the forward disturbance, lateral disturbance, longitudinal disturbance, roll disturbance, heading disturbance, and pitch disturbance, respectively. In addition to the variables defined in steps one to three, the remaining parameters are constants, and the expanded form of the relevant matrix is as follows: .

[0047] The parameters in the added mass matrix are fixed constants, and their values are shown in Table 3. m is the mass of the robot, are the added mass coefficients, is the position coordinate of the center of mass in the body coordinate system, are the inherent moments of inertia of the robot.

[0048] .

[0049] In , the variables include the linear velocity and angular velocity variables such as , and the remaining parameters are fixed constants, and their values are shown in Table 3. are linear damping coefficients, are nonlinear damping coefficients.

[0050] .

[0051] In , the variables include the linear velocity and angular velocity variables such as , the angle of attack , the sideslip angle , and the resultant velocity​ ; other parameters are fixed constants, whose values are shown in Table 3. is the fluid density, S is the maximum cross-sectional area of the target underwater flexible robot body; L is the body length of the target underwater flexible robot; are the x-direction hydrodynamic coefficients, are the y-direction hydrodynamic coefficients, are the z-direction hydrodynamic coefficients, are the x-axis moment coefficients, are the z-axis moment coefficients, are the y-axis moment coefficients.

[0052] .

[0053] In the restoring matrix , the variables include attitude variables such as ; other parameters are fixed constants, whose values are shown in Table 3. is the gravity, is the difference between the gravity and the buoyancy.

[0054] Step four: design the robot's model-based closed-loop motion control system in three-dimensional space.

[0055] Based on the dynamic model of Step three, it has been determined that the controllable degrees of freedom are the thrust, the heading, the pitch, the position, and the depth control needs to be indirectly controlled through the heading and the pitch motion. Define the world coordinates of the desired trajectory in space as , and the desired velocity as .

[0056] Design the velocity, heading, and depth control systems, respectively.

[0057] According to the wave frequency , simplify the first row of the dynamic model of Step three to obtain the simplified model , where is the average velocity within a single period T, which more accurately reflects the actual motion state of the flexible robot when calculating the control amount error. The constant term is is an unknown but bounded disturbance. When the input is , the error between the control output of this simplified model and the actual control output of the robot is , and the output error between the model and the actual system is used to adaptively adjust the control parameters . The discrete velocity control law of the velocity controller is designed as follows: .

[0058] ​ .

[0059] are constants.

[0060] is a discrete time variable; is the robot motion cycle; is a time loop variable; and are discrete time variables corresponding control parameters, whose update law at discrete time variable k+1 is ; is the cumulative error of robot velocity; is a discrete time variable corresponding to the error of expected velocity and robot velocity; is the expected velocity, is the cycle average velocity; is a discrete time variable corresponding to the model error at time k; is a discrete time variable corresponding to the model error at time k.

[0061] The heading controller frequency is consistent with the velocity controller. In the design process, the heading angular velocity updated each time in the third step model fifth line q is input into the controller as the estimated value of the actual system heading angular velocity. The discrete heading control law is obtained as follows: .

[0062] wherein, are constants. is the heading control amount at the last time; is the heading angular velocity at the current time; is the heading error, wherein are reference positions on the expected trajectory, are discrete time variables k corresponding to the real position of the robot at time k; is the real heading.

[0063] The model obtained in step three is transformed into a pitch rudder angle tracking control problem, and an online pitch angle generation method based on error feedback and kinematics inverse solution is designed. The longitudinal kinematics model of the robot is obtained from the sixth line of the third step model . In the state where the current depth error is , an expected depth rate of change generator with proportional-integral-derivative structure is introduced . The is input into the cost function​ solving a single variable optimization problem obtaining a desired pitch angle control target . Thus, the pitch rudder angle control law is: .

[0064] where the depth control period is , is the depth control frequency, and the controller parameters are all constants. is the pitch angle error; is the pitch angle error at the previous time step; is the cumulative pitch angle error; is the discrete time variable k , the desired pitch angle at time , and the true pitch angle.

[0065] The integrated control flowchart is as shown in Figure 3 Under this control framework, the model-based three-dimensional trajectory tracking control of the flexible robot can be realized.

[0066] The modeling method proposed in the application breaks through the limitation of traditional rigid body modeling, and can be applied to underwater robots with non-rigid body, multiple joints, internal mechanism strong coupling, and high flexibility structure, has the advantages of structural universality and high modeling accuracy, and is particularly suitable for flexible underwater systems with complex topological structure and large deformation.

[0067] Based on the data-driven idea, the application directly fits the parameters of the hydrodynamic equation constructed based on the experimental and CFD simulation data without explicitly analyzing the expression of the complex fluid field, enhances the representation of the model for flexible deformation and periodic swing, and effectively reduces the complexity and error sensitivity of the traditional mechanism model derivation.

[0068] Based on the dynamic modeling framework, a real-time deployable closed-loop controller is further designed to support stable pose adjustment of the flexible robot in the underwater environment. The controller fully combines the dynamic model and the inverse kinematics, and has good real-time performance, stability and physical consistency.

[0069] The integrated modeling and control scheme constructed in the application has strong universality and scalability, and can effectively support the underwater flexible robot to realize stable trajectory tracking control in three-dimensional space in actual application.

[0070] In an embodiment, a flexible wave approximately in the shape of "S" is simulated, and the specific parameter settings are shown in Table 1. The body wave equation of step one is input into step two.

[0071] Step two: input the body wave equation defined in step one into the computational fluid dynamics (CFD) simulation software, import the CAD shape of the robot to be modeled (in this embodiment, an underwater robot similar to a tuna shape), set the fluid density , the flow velocity is 0, i.e. still water, change the wave frequency and the turning bias respectively, record the CFD output fluid force data results of straight line and turning motion under two-dimensional plane motion, including forward thrust data and turning moment at different frequencies. The CFD simulation is set to output 1000 data in 1s, so the thrust and turning moment are considered to be continuous.

[0072] Build an experiment in the physical world, change the pitch rudder to make the robot do uniform diving motion, record the pitch angular velocity r data measured by the IMU under different pitch rudder controls, calculate the pitch moment , where is the moment of inertia of the pitch axis, the numerical value is shown in Table 3.

[0073] Input the two-dimensional plane data and obtained by simulation and experiment into the hydrodynamic equation respectively, use the curve fitting tool in MATLAB to fit and obtain the equation constant . The hydrodynamic equation designed in this application is as follows: .

[0074] Determine the flexible robot wave state from step one (L=0.55m is the length of the robot).

[0075] In this embodiment, the single active joint of the robot is set at the position of L / 2, so the conversion relationship is . is the fluid density, the robot inherent parameters are shown in Table 3.

[0076] The hydrodynamic equation fitting results are shown in Table 2, Figure 4 is a matching diagram of part of the hydrodynamic force data and the hydrodynamic equation fitting results, and it can be seen that the fitting accuracy is adaptive in each frequency band. Input the hydrodynamic equation with the determined coefficient into step three.

[0077] Step three: define . The initial state of the robot is , . The specific settings are shown in Table 3. is the external water flow disturbance, which is set to Uniform distribution over the compliance zone , in addition . The relevant matrix expansion is as follows, and the intrinsic parameters in the dynamic model are shown in Table 3. Note that, in addition to the variables defined in steps 1-3, the remaining parameters are constants.

[0078] Step 4: Import the model from step 3 into MATLAB for simulation, set the simulation time to 100s, and define the world coordinates of the desired trajectory in space as:

[0079] Next, calculate the speed, heading, and depth control laws respectively.

[0080] Set the robot's motion frequency f = 2.5Hz, simplify the first row of the dynamic model from step 3 to obtain the simplified model , where is the average speed over a single period T, which more accurately reflects the actual motion state of the flexible robot when calculating control error. The constant term is , which is an unknown but bounded disturbance. When the input is , the error between the control output of this simplified model and the actual control output of the robot is , and the control parameter is adjusted adaptively using the output error between the model and the actual system. The discrete speed control law of the speed controller is designed as .

[0081] is a constant.

[0082] The frequency of the heading controller is consistent with that of the speed controller. During the design process, the updated heading angular velocity q input from the fifth row of the model in step 3 is used as the estimated value of the actual system's heading angular velocity. The discrete heading control law is obtained as . is a constant.

[0083] The depth control problem of the robot is transformed into a pitch angle tracking control problem by the model obtained in step 3, and an online pitch angle generation method based on error feedback and inverse kinematics is designed. The longitudinal kinematics model of the robot is obtained from the sixth row of the model in step 3 . Under the condition that the current depth error of the robot is , a desired depth rate generator with proportional-integral-derivative structure is introduced , where , which can be regarded as a "feedforward + feedback" estimator for the depth tracker, suppressing steady-state deviation and enhancing response capability. Input into the cost function , solve the single-variable optimization problem Obtaining a desired pitch angle control target . Thus, the pitch rudder angle control law is: .

[0084] wherein, , the depth control frequency , the controller parameters .

[0085] The overall control block diagram is as shown in Figure 3 Under this control framework, the model-based three-dimensional trajectory tracking control of the flexible robot can be realized, and the three-dimensional trajectory tracking motion control effect of the robot in space is as shown in Figure 5 The effect illustrates the effectiveness of the three-dimensional motion control framework; the speed, heading, depth, and pitch angle stability effects of the robot are as shown in Figures 6 to 9 The results illustrate the stability and convergence effects of each control law.

[0086] Table 1: Flexible motion parameter setting table of the robot

[0087] Table 2: Parameter fitting result table of the hydrodynamic equation

[0088] Table 3: Intrinsic parameter setting table of the robot

[0089] The significant difference between the present application and the prior art lies in emphasizing the flexible motion characteristics of the robot, including periodicity and fluctuation, considering the closed-loop control system under the influence of random disturbance when the flexible deformation occurs when interacting with the external environment.

[0090] The modeling method proposed by the prior art is for a multi-link flexible robot, and the pose information of the end of the flexible robot is obtained from the joint kinematics recursive relationship, while the present application does not consider the joint motion of the flexible robot, specifically, it does not use the approximate processing of the joint link for flexible motion, but directly obtains the hydrodynamic force through a data-driven manner, and brings it into the dynamics model to solve the pose information, and the method has a significant difference.

[0091] In addition, the present application is a modeling method of an underwater flexible robot and a closed-loop controller design method including speed, heading, and depth.

[0092] In an exemplary embodiment, as shown in Figure 10 a three-dimensional motion control device of an underwater flexible robot is provided, comprising: An information data acquisition module is configured to acquire information data of the target underwater flexible robot, wherein the information data comprises CAD shape information and fluid information, and the fluid information comprises fluid density and fluid velocity.

[0093] A fitting module is configured to fit the hydrodynamic parameters based on the body wave equation of the target underwater flexible robot, by using physical experiments and fluid mechanics simulation methods, to obtain a hydrodynamic equation according to the information data.

[0094] A dynamics model determination module is configured to determine a dynamics model based on the hydrodynamic equation according to Lagrange theory.

[0095] A control module is configured to perform closed-loop motion control based on a three-dimensional space according to the dynamics model, to realize three-dimensional trajectory tracking control of the target underwater flexible robot, wherein the three-dimensional space is determined based on the velocity, heading and depth of the target underwater flexible robot.

[0096] In an exemplary embodiment, a computer device can be provided, which can be a server or a terminal, and an internal structure diagram thereof can be as shown in Figure 11 The computer device includes a processor, a memory, an input / output interface (I / O) and a communication interface. The processor, the memory and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. The processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operating system and the computer program in the non-volatile storage medium to run. The database of the computer device is configured to store underwater flexible robot three-dimensional motion control data. The input / output interface of the computer device is configured to exchange information between the processor and external devices. The communication interface of the computer device is configured to communicate with external terminals through network connection. The computer program is executed by the processor to implement the underwater flexible robot three-dimensional motion control method.

[0097] Those skilled in the art can understand that Figure 11 The structure shown in the above embodiment is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied. The specific computer device can include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0098] In an exemplary embodiment, a computer device is also provided, which includes a memory and a processor, the memory stores a computer program, and the processor implements the steps in the above method embodiments when executing the computer program.

[0099] In an example embodiment, a computer readable storage medium storing a computer program is provided, the computer program, when executed by a processor, implements the steps of any of the above method embodiments.

[0100] In an example embodiment, a computer program product is provided, comprising a computer program, the computer program, when executed by a processor, implements the steps of any of the above method embodiments.

[0101] It should be noted that the user information (including but not limited to user equipment information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in the present application are all information and data authorized by the user or authorized by all parties, and the collection, use and processing of related data need to comply with relevant regulations.

[0102] It can be understood by those skilled in the art that all or part of the processes in the above-mentioned embodiments can be completed by a computer program instructing related hardware, and the computer program can be stored in a non-volatile computer readable storage medium. When the computer program is executed, it can include the processes of the above-mentioned embodiments. Any reference to memory, database or other medium used in the embodiments provided by the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (Read-Only Memory, ROM), magnetic tape, floppy disk, flash memory, optical storage, high-density embedded non-volatile memory, resistive random access memory (Resistive Random Access Memory, ReRAM), magnetoresistive random access memory (Magnetoresistive Random Access Memory, MRAM), ferroelectric memory (Ferroelectric Random Access Memory, FRAM), phase change memory (Phase Change Memory, PCM), graphene memory, etc. Volatile memory can include random access memory (Random Access Memory, RAM) or external cache memory, etc. As an illustration but not limitation, RAM can be in various forms, such as static random access memory (Static Random Access Memory, SRAM) or dynamic random access memory (Dynamic Random Access Memory, DRAM), etc.

[0103] The database involved in each of the embodiments provided in the present application can include at least one of a relational database and a non-relational database. The non-relational database can include a distributed database based on a blockchain, and the like, without being limited thereto. The processor involved in each of the embodiments provided in the present application can be a general-purpose processor, a central processing unit, a graphics processing unit, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, and the like, without being limited thereto.

[0104] The technical features of the above embodiments can be combined in any manner. To make the description concise, all possible combinations of the technical features in the above embodiments are not described, but it should be considered that any combination of the technical features is within the scope of the present disclosure, as long as there is no contradiction.

[0105] The principles and implementation manners of the present application are described by using specific examples herein, and the above embodiments are only used to help understand the method of the present application and its core idea. Meanwhile, for those skilled in the art, the specific implementation manners and application ranges can be changed according to the idea of the present application. In summary, the content of the present description should not be understood as a limitation of the present application.

Claims

1. A method for controlling three-dimensional motion of an underwater flexible robot, the method comprising: The underwater flexible robot three-dimensional motion control method is suitable for robots with periodic fluctuation and flexible motion characteristics; The method comprises: obtaining information data of a target underwater flexible robot; the information data comprises CAD shape information and fluid information; the fluid information comprises fluid density and flow rate; According to the information data, based on the body wave equation of the target underwater flexible robot, the water dynamic parameters are fitted by using physical experiments and fluid mechanics simulation methods to obtain a water dynamic equation; Based on the Lagrange theory, the dynamic model is determined according to the water dynamic equation; According to the dynamic model, three-dimensional space-based closed-loop motion control is performed to realize three-dimensional trajectory tracking control of the target underwater flexible robot; the three-dimensional space is determined based on the speed, heading and depth of the target underwater flexible robot.

2. The method of claim 1, wherein, According to the information data, based on the body wave equation of the target underwater flexible robot, the water dynamic parameters are fitted by using physical experiments and fluid mechanics simulation methods to obtain a water dynamic equation, which specifically comprises: The fluid dynamic data comprises forward thrust data and turning torque data; The physical experiment method is used to change the pitch rudder to make the target underwater flexible robot perform uniform diving motion, and the pitch torque is determined based on the measured pitch angular velocity; According to the fluid dynamic data and the pitch torque, the water dynamic parameters of the preset water dynamic equation are fitted to obtain a water dynamic equation.

3. The method of claim 2, wherein, The expression of the body wave equation is: ； wherein, is t the lateral offset of the target underwater flexible robot relative to the centerline of the body at time t; s is the body wave state quantity along the axial direction from the head to the tail tip of the target underwater flexible robot; are all constants; is the wave number corresponding to the body wave; is the wave frequency; is the turning bias.

4. The method of claim 2, wherein, The expression of the water dynamic equation is: ； wherein, is t is the hydrodynamic equation corresponding to the forward thrust data at time t; is the linear fluid dynamic coefficient coupled with inertia and friction; is the frequency nonlinear term of the average thrust; is the nonlinear term of the wave amplitude; is the fluid dynamic coefficient; is the frequency nonlinear term of the wave thrust; are constant coefficients; is the wave frequency; is the thrust phase delay; is t is the hydrodynamic equation corresponding to the turning moment at time t; is the control variable of the turning motion; is the wave term fluid dynamic coefficient of the turning moment; is the linear fluid dynamic coefficient; is the second order nonlinear fluid dynamic coefficient; is t is the hydrodynamic equation corresponding to the pitch moment at time t; is the fluid density; is the resultant velocity of the three axis velocities; S is the maximum cross-sectional area of the body of the target underwater flexible robot; L is the body length of the target underwater flexible robot; is the linear fluid dynamic coefficient of the pitch moment; is the second order nonlinear fluid dynamic coefficient; is the control variable of the pitch rudder angle.

5. The method of claim 1, wherein, The determination method of the dynamic model specifically comprises: The water dynamic equation is input into a preset robot control force vector six-degree-of-freedom corresponding position to determine a robot control force vector; Based on the Lagrange theory, the dynamic model is determined according to the robot control force vector; the expression of the dynamic model is: ； wherein, is an added mass matrix; is a velocity dependent damping term; is a damping matrix; is a rotation induced velocity coupling term; is a Coriolis force matrix; is a six degree of freedom generalized velocity vector of the system including linear and angular velocities; is a time derivative of the velocity vector; is a gravity and buoyancy coupling term; is a restoring force matrix; is a pose in three dimensional space; is an external water flow disturbance; is a robot control force vector.

6. The method of claim 1, wherein, According to the dynamic model, three-dimensional space-based closed-loop motion control is performed to realize three-dimensional trajectory tracking control of the target underwater flexible robot, which specifically comprises: According to the dynamic model, a discrete speed control law for the speed of the target underwater flexible robot is determined; the expression of the discrete speed control law is: ； According to the dynamic model, a discrete heading control law for the heading of the target underwater flexible robot is determined; the expression of the discrete heading control law is: ； According to the dynamic model, a pitch rudder angle control law for the depth of the target underwater flexible robot is determined; the expression of the pitch rudder angle control law is: ； wherein, is a discrete velocity control law; is a discrete time variable; is a constant; and are both discrete time variables corresponding control parameters; is a discrete time variable corresponding error of desired velocity and robot velocity; is a cumulative error of robot velocity; is a robot motion cycle; is a time loop variable; is a discrete heading control law; is a heading control quantity of last time; and are both constants; is a heading angular velocity of current time; is a heading error; are both controller parameters; is a pitch angle error; is a pitch angle error of last time; is a cumulative pitch angle error; is a pitch rudder angle control cycle; is a pitch rudder angle control law.

7. An underwater flexible robot three-dimensional motion control device, characterized by, It comprises: An information data acquisition module is configured to acquire information data of a target underwater flexible robot; The information data comprises CAD shape information and fluid information; the fluid information comprises fluid density and flow rate; A fitting module is configured to fit water dynamic parameters according to the information data, based on the body wave equation of the target underwater flexible robot, by using physical experiments and fluid mechanics simulation methods to obtain a water dynamic equation; A dynamics model determining module is configured to determine a dynamics model based on the water dynamics equation according to Lagrange theory; A control module is configured to perform three-dimensional space-based closed-loop motion control according to the dynamics model to achieve three-dimensional trajectory tracking control of the target underwater flexible robot; the three-dimensional space is determined based on the velocity, heading and depth of the target underwater flexible robot.

8. A computer device comprising: A memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the underwater flexible robot three-dimensional motion control method of any one of claims 1-6.

9. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the underwater flexible robot three-dimensional motion control method of any one of claims 1-6.

10. A computer program product comprising a computer program, characterized in that, The computer program is executed by the processor to implement the underwater flexible robot three-dimensional motion control method of any one of claims 1-6.

Images