Model construction method, control method and equipment of snakelike underwater robot

Abstract

The invention provides a snakelike underwater robot model construction method, a snakelike underwater robot control method and equipment, and relates to the technical field of underwater robots. Based on a snakelike underwater robot, the snakelike underwater robot comprises a base, connecting rods and joints which are connected with one another; the model construction method of the snakelike underwater robot comprises the steps that an underwater robot coordinate system of the snakelike underwater robot is acquired, and the underwater robot coordinate system comprises a connecting rod coordinate system, a propeller coordinate system and a world coordinate system; based on a Jacobian matrix, obtaining a kinematics equation according to the coordinate system of the underwater robot; obtaining a kinetic equation according to the kinematic equation; and obtaining a snakelike underwater robot model through the kinetic equation. According to the invention, the control precision of the snakelike underwater robot is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robots, and in particular to a model building method, a control method and a device for a snake-like underwater robot. Background Art

[0002] Due to its unique structural design, snake-like underwater robots can imitate the movement of real snakes and have extremely high flexibility and maneuverability. This allows them to move freely in a small space and complete tasks that are difficult for traditional underwater equipment to reach. While getting rid of communication cables, snake-like underwater robots also have a more slender body, can enter the inside of pipes for exploration, can adapt to more complex environments by changing their body structure, and can achieve different functional requirements by carrying different functional modules. However, in actual use, due to its complex structure, it often becomes overdriven in the task of controlling its position and direction, making it impossible to complete high-precision tasks in complex environments. Summary of the invention

[0003] The problem solved by the invention is how to improve the control accuracy of the snake-like underwater robot.

[0004] In order to solve the above problems, the present invention provides a model building method, a control method and a device for a snake-like underwater robot.

[0005] In a first aspect, the present invention provides a model construction method of a snake-like underwater robot, based on the snake-like underwater robot, the snake-like underwater robot includes a base, a connecting rod and a joint connected to each other, and the model construction method of the snake-like underwater robot includes: Acquire an underwater robot coordinate system of the snake-like underwater robot, wherein the underwater robot coordinate system includes a connecting rod coordinate system, a propeller coordinate system and a world coordinate system; Based on the Jacobian matrix, a kinematic equation is obtained according to the underwater robot coordinate system; Obtaining a dynamic equation according to the kinematic equation; The snake-like underwater robot model is obtained through the dynamic equation.

[0006] Optionally, obtaining the kinematic equation according to the underwater robot coordinate system includes: Based on the homogeneous transformation matrix, a forward kinematics equation is obtained according to the underwater robot coordinate system; Based on the Jacobian matrix, the inverse kinematics equation is obtained according to the forward kinematics equation.

[0007] Optionally, the connecting rod coordinate system includes a base coordinate system, and the forward kinematics equation obtained according to the underwater robot coordinate system includes: Obtaining a joint coordinate transformation matrix according to the underwater robot coordinate system; Among them, the joint coordinate transformation matrix is: , in, is the transformation relationship of the homogeneous transformation matrix between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system, is the rotation transformation relationship between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system, The position transformation relationship between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system; Obtaining an underwater robot coordinate transformation matrix according to the underwater robot coordinate system and the joint coordinate transformation matrix; Among them, the underwater robot coordinate transformation matrix is: , in, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the connecting rod coordinate system, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the base coordinate system; The forward kinematics equation is obtained according to the underwater robot coordinate transformation matrix.

[0008] Optionally, obtaining the inverse kinematics equation according to the forward kinematics equation includes: Based on differential kinematics, the connecting rod velocity in each coordinate system is obtained according to the adjoint matrix; Wherein, the connecting rod speed is: , in, is the connecting rod speed of the i+1th connecting rod, is the adjoint matrix, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the connecting rod coordinate system, is the connecting rod speed of the ith connecting rod, is the rotation coordinate of the i-th joint, is the i-th joint angle; The inverse kinematics equation is obtained through the connecting rod velocity.

[0009] Optionally, obtaining a dynamic equation according to the kinematic equation comprises: According to the kinematic equation, a kinetic energy equation of the connecting rod is obtained; Wherein, the connecting rod kinetic energy equation is: , in, is the inertia matrix of link i in the link coordinate system, is the connecting rod kinetic energy, is the inertia matrix of link i relative to the base, for The transposed matrix of is the Jacobian matrix, is the generalized velocity vector of the base, is the joint angle; Obtaining a mass matrix according to the connecting rod kinetic energy equation; The dynamic equation is obtained through the mass matrix.

[0010] Optionally, obtaining the dynamic equation through the mass matrix includes: Obtaining the dynamic equation through the mass matrix; Wherein, the kinetic equation is: , in, is the mass matrix, is the Coriolis force matrix, is the effect of fluid dynamic damping, is the generalized hydrostatic force, for The derivative of is the generalized velocity vector of the base, are the command force and torque.

[0011] Optionally, the command forces and torques include joint torques and thruster torques; Wherein, the command force and torque are: , in, are the command force and torque, is the thruster torque, is the joint torque, is the control input of the snake-like underwater robot, is the joint control input, Configure the matrix for thrust.

[0012] In a second aspect, the present invention provides a control method for a snake-like underwater robot, comprising: obtaining a snake-like underwater robot model according to the model construction method of the snake-like underwater robot as described in the first aspect; The snake-like underwater robot is controlled according to the snake-like underwater robot model.

[0013] In a third aspect, the present invention provides an electronic device, including a memory and a processor; The memory is used to store computer programs; The processor is used to implement the model building method of the snake-like underwater robot as described in the first aspect or the control method of the snake-like underwater robot as described in the second aspect when executing the computer program.

[0014] In a fourth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the model building method of the serpentine underwater robot as described in the first aspect or the control method of the serpentine underwater robot as described in the second aspect is implemented.

[0015] The beneficial effects of the model construction of the snake-like underwater robot of the present invention are: By defining the underwater robot coordinate system, the underwater robot coordinate system includes the connecting rod coordinate system, the propeller coordinate system and the world coordinate system. By establishing three coordinate systems, the coordinate relationship of the underwater robot is clarified to avoid errors. The kinematic equation is obtained according to the underwater robot coordinate system and the Jacobian matrix is ​​used to improve the accuracy of subsequent calculations. The dynamic equation is obtained according to the kinematic equation, and the serpentine underwater robot model is obtained through the dynamic equation, which can achieve precise control of each part of the serpentine underwater robot and improve the control accuracy of the serpentine underwater robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 This is a schematic structural diagram of a snake-shaped underwater robot according to an embodiment of the present invention; Figure 2 A schematic diagram of a flow chart of a method for constructing a model of a snake-like underwater robot according to an embodiment of the present invention; Figure 3 A schematic diagram of a flow chart of another method for constructing a model of a snake-like underwater robot according to an embodiment of the present invention; Figure 4 The figure is a schematic diagram of the structure of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0017] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. Although certain embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention can be implemented in various forms and should not be interpreted as being limited to the embodiments described herein. On the contrary, these embodiments are provided to provide a more thorough and complete understanding of the present invention. It should be understood that the drawings and embodiments of the present invention are only for exemplary purposes and are not intended to limit the scope of protection of the present invention.

[0018] It should be understood that the various steps described in the method embodiments of the present invention may be performed in different orders and / or in parallel. In addition, the method embodiments may include additional steps and / or omit the steps shown. The scope of the present invention is not limited in this respect.

[0019] The term "including" and its variations used in this document are open inclusions, that is, "including but not limited to"; the term "based on" means "based at least in part on"; the term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one other embodiment"; the term "some embodiments" means "at least some embodiments"; the term "optionally" means "optional embodiments". The relevant definitions of other terms will be given in the following description. It should be noted that the concepts of "first", "second", etc. mentioned in the present invention are only used to distinguish different devices, modules or units, and are not used to limit the order or interdependence of the functions performed by these devices, modules or units.

[0020] It should be noted that the modifications of "one" and "plurality" mentioned in the present invention are illustrative rather than restrictive, and those skilled in the art should understand that, unless otherwise clearly indicated in the context, it should be understood as "one or more".

[0021] The names of the messages or information exchanged between multiple devices in the embodiments of the present invention are only used for illustrative purposes, and are not used to limit the scope of these messages or information.

[0022] like Figure 1 The figure shows the connecting rod and joint part of the serpentine underwater robot, and the connecting rod includes a tail connecting rod, a central control section connecting rod, a head connecting rod, and a power section connecting rod. Among them, 3-1 is the tail connecting rod, 3-2 is the central control section connecting rod, 3-3 is the joint, 3-4 is the head connecting rod, and 3-5 is the power section connecting rod. The serpentine underwater robot is essentially an underwater manipulator with a movable base, and the joints and connecting rods are connected to the base. The joint is used to connect the connecting rods on both sides, and each connecting rod is connected to the joint in turn. The power section connecting rod is connected to a propeller, and the propeller is used to provide power for the serpentine underwater robot.

[0023] like Figure 2 As shown, a model construction method of a snake-like underwater robot provided by an embodiment of the present invention includes: Step 210, obtaining the underwater robot coordinate system of the snake-like underwater robot, wherein the underwater robot coordinate system includes a connecting rod coordinate system, a propeller coordinate system and a world coordinate system.

[0024] Specifically, three coordinate systems are selected to describe the above relationship, namely the link coordinate system, the thruster coordinate system and the world coordinate system. The underwater robot consists of n+1 links connected by n revolute joints, each of which has two degrees of freedom and has three-dimensional motion capabilities. A coordinate system is attached to the rear end of each link, and the numbering rule is numbered from the tail to the front, using the symbol F and the subscript The base is chosen to coincide with the rear end of link 0, the term end effector refers to the front end of link n, and the reference system corresponding to the base and the end effector is the base coordinate system and the end effector coordinate system The robot is also equipped with m thrusters, numbered . Coordinate system is the assumed world coordinate system.

[0025] Step 220, based on the Jacobian matrix, obtain the kinematic equation according to the underwater robot coordinate system; Specifically, the Jacobian matrix provides a means to linearize complex nonlinear relationships, which makes solving inverse kinematics problems more direct and simple. Even in high-degree-of-freedom robotic systems, inverse kinematics problems can be solved efficiently through iterative methods. Using the Jacobian matrix, inverse kinematics problems can be flexibly solved by adjusting different optimization objectives (such as minimizing joint motion, avoiding singularities, keeping joint angles within a safe range, etc.).

[0026] Step 230, obtaining a dynamic equation according to the kinematic equation; Specifically, the kinematic model includes a forward kinematic model and an inverse kinematic model, which transforms the geometric relationship describing the robot's motion into a dynamic model describing the relationship between force and acceleration.

[0027] Step 240, obtaining a snake-like underwater robot model through the dynamic equation.

[0028] Specifically, the serpentine underwater robot model is obtained through the dynamic equation, which can solve the modeling problem of the serpentine underwater robot and provide a reference solution for the subsequent control method design of the serpentine underwater robot.

[0029] In this embodiment, by defining an underwater robot coordinate system, the underwater robot coordinate system includes a connecting rod coordinate system, a propeller coordinate system and a world coordinate system. By establishing three coordinate systems, the coordinate relationship of the underwater robot is clarified to avoid errors. The kinematic equation is obtained according to the underwater robot coordinate system and the Jacobian matrix is ​​used to improve the accuracy of subsequent calculations. The dynamic equation is obtained according to the kinematic equation, and the serpentine underwater robot model is obtained through the dynamic equation, which can achieve precise control of various parts of the serpentine underwater robot and improve the control accuracy of the serpentine underwater robot.

[0030] Optionally, obtaining the kinematic equation according to the underwater robot coordinate system includes: Based on the homogeneous transformation matrix, a forward kinematics equation is obtained according to the underwater robot coordinate system; Based on the Jacobian matrix, the inverse kinematics equation is obtained according to the forward kinematics equation.

[0031] Optionally, the connecting rod coordinate system includes a base coordinate system, and the forward kinematics equation obtained according to the underwater robot coordinate system includes: Obtaining a joint coordinate transformation matrix according to the underwater robot coordinate system; Among them, the joint coordinate transformation matrix is: , in, is the transformation relationship of the homogeneous transformation matrix between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system, is the rotation transformation relationship between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system, The position transformation relationship between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system; Obtaining an underwater robot coordinate transformation matrix according to the underwater robot coordinate system and the joint coordinate transformation matrix; Among them, the underwater robot coordinate transformation matrix is: , in, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the connecting rod coordinate system, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the base coordinate system, is the transformation relationship of the homogeneous transformation matrix between the bth connecting rod coordinate system and the first connecting rod coordinate system; The forward kinematics equation is obtained according to the underwater robot coordinate transformation matrix.

[0032] Specifically, the robot is an articulated structure composed of multiple interconnected rigid bodies, that is, a multi-body system, and the homogeneous transformation matrix is ​​used to describe this multi-body system. The full name of the DH method is the Denavit-Hartenberg parameter method, which is a widely used representation method in robotics to define the link coordinate system and describe the relative position and posture between the joints of the robot. The improved DH method is used to derive the transformation between two consecutive joints of the robot starting from the base: , Where c* and s* are cos(*) and sin(*), respectively. For example and , Number the joints. is the i-th joint angle, is the i-1th connecting rod torsion angle, is the i-th offset.

[0033] The above formula can be written as a matrix: , in, is the transformation relationship between the i-1th coordinate system and the i-th coordinate system, and They are The upper left 3*3 submatrix and the upper right 3*1 column vector.

[0034] For any link coordinate system of the robot, it can be expressed by the transformation matrix: , in, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the connecting rod coordinate system, The world coordinate system With the base coordinate system The transformation relationship between the homogeneous transformation matrices is: To describe the rotation matrix of the connecting rod, For The position transformation relationship between the origin and the i-th connecting rod coordinate system.

[0035] In order to describe the position and posture of the robot, define the world coordinate system With base coordinate system The transformation relationship of the homogeneous transformation matrix is ​​recorded as: , in, is the rotation matrix describing the orientation of the base, For Origin to The position vector of the origin; Since multiple linear transformations can be equivalent to one transformation in the form of product, the transformation matrix from the base to the end effector is: , in, The world coordinate system With the end effector coordinate system The transformation relationship between the homogeneous transformation matrices is: is the base coordinate system With the end effector coordinate system The transformation relationship between the homogeneous transformation matrices is: is the rotation matrix describing the orientation of the base and the end effector, For Origin to The position vector of the origin, is the rotation matrix describing the orientation of the end effector, For Origin to The position vector of the origin.

[0036] In this optional embodiment, the forward kinematics equation is obtained according to the underwater robot coordinate system using a homogeneous transformation matrix, and the DH algorithm is used to simplify the complex robot structure into a series of standard coordinate transformations, thereby simplifying the robot's forward kinematics modeling process.

[0037] Optionally, obtaining the inverse kinematics equation according to the forward kinematics equation includes: Based on differential kinematics, the connecting rod velocity in each coordinate system is obtained according to the adjoint matrix; Wherein, the connecting rod speed is: , in, is the connecting rod speed of the i+1th connecting rod, is the adjoint matrix, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the connecting rod coordinate system, is the connecting rod speed of the ith connecting rod, is the rotation coordinate of the i-th joint, is the i-th joint angle; The inverse kinematics equation is obtained through the connecting rod velocity.

[0038] Specifically, differential kinematics describes the relationship between the velocities of different parts of the robot. The velocity relationship from the base to each joint can be described by the Jacobian matrix. According to the definition of differential kinematics, the adjoint matrix is: , in, is the adjoint matrix, is the antisymmetric form of the position vector, and R is the rotation matrix representing the end effector of the robot, which describes the direction and posture of the end effector relative to the base.

[0039] The speed is transformed into: , in, is the linear velocity of the base, is the angular velocity of the base, is the joint velocity vector, and T is the transformation matrix.

[0040] in, .

[0041] Generalized velocity of a given base , the speed of the connecting rod in each coordinate system can be obtained by recursion: , Is Given the joint rotation coordinate vector, is the derivative of the i-th joint angle vector, and the last three elements represent x, y, and z, respectively, depending on the joint around the coordinate system Rotation along the z-axis or y-axis; definition is the generalized velocity vector of the base, including the velocity of the base and the angles of each joint, and redefines the velocity vector for: , in, is the generalized velocity vector obtained by combining the base velocity and the joint angular velocity, is the joint angular velocity vector, is the Jacobian matrix, and i is the connecting rod number.

[0042] is the Jacobian matrix, defined as: , , It is a 6×6 unit matrix.

[0043] In this optional embodiment, the Jacobian matrix is ​​used to derive the inverse kinematics formula. The Jacobian matrix provides a means to linearize complex nonlinear relationships, which makes solving the inverse kinematics problem more direct and simple.

[0044] Optionally, obtaining a dynamic equation according to the kinematic equation comprises: According to the kinematic equation, a kinetic energy equation of the connecting rod is obtained; Wherein, the connecting rod kinetic energy equation is: , in, is the inertia matrix of link i in the link coordinate system, is the connecting rod kinetic energy, is the velocity vector, is the generalized velocity vector, is the inertia matrix of link i relative to the base, for The transposed matrix of is the Jacobian matrix, is the generalized velocity vector of the base, is the joint angle; Obtaining a mass matrix according to the connecting rod kinetic energy equation; The dynamic equation is obtained through the mass matrix.

[0045] Specifically, all link inertia matrices are expressed in the same coordinate system through the Jacobian matrix, and then the total rigid body inertia matrix is ​​obtained by adding the contribution of each link, which is: , in, is the rigid body mass matrix, the upper left part is the inertia of the robot as a single rigid body, and the upper right part and lower left corner are the inertia matrices of the coupling between the joint motion and the robot base, which reflect the influence of the joint acceleration on the base motion and the dual influence of the base acceleration on the joint motion. is the inertia matrix of the robot when the base is fixed.

[0046] For the same radius r and different lengths The additional mass matrix of each connecting rod is for: , in, is the density of water, is the additional mass coefficient, is the parameter including the added mass effect, r is the connecting rod radius, l i is the connecting rod length.

[0047] The expression of the added mass matrix is: , in, is the rigid body additional matrix, The Coriolis force matrix is: , in, , in, Respectively The first three and last three items.

[0048] matrix is the influence of fluid dynamic damping, n is the correlation coefficient, and its expression is: , Among them, the matrix is the hydrodynamic damping matrix for each corresponding link, using to represent the fluid dynamic force and torque on connecting rod i.

[0049] The hydrodynamic force generated by the robot's movement in water is complex and highly nonlinear. When modeling the hydrodynamic force, the water velocity is not considered. First, the resistance of the cylindrical link i is given. The linear resistance on the link i is called , the linear resistance on connecting rod i is: , in, , in, is the linear drag coefficient, ρ is the density of water, r is the radius of the connecting rod, is the length of connecting rod i, , and is a constant, reference speed, rotation related damping coefficient mixed moment of inertia damping coefficient; In addition to the linear resistance, the model also includes nonlinear resistance, the longitudinal oscillation on the connecting rod i and roll The nonlinear resistance in is: , , in, is the longitudinal nonlinear resistance coefficient, is the nonlinear resistance coefficient of rolling, assuming that the ocean current velocity is zero, is the velocity of the ith object in the turbulent state, is the velocity of the i-th object in the rolling state, and the total resistance on the connecting rod i is the sum of the linear and nonlinear resistances: , in, is nonlinear resistance.

[0050] Generalized hydrostatics Composed of forces and moments on the base and moments on the joints due to the hydrostatic forces on the links of the robot, the generalized hydrostatic force is: , in, is the i-th Jacobian matrix, are the hydrostatic force and moment on connecting rod i, respectively: , in, is the direction of gravity in the inertial system, taking a constant, is the rotation matrix of the ith part, the matrix is also a constant, and its expression is: , in, is the density of water, is the gravitational constant, is the identity matrix in three-dimensional space, , are the mass and volume of connecting rod i respectively; vector , are the locations of the center of gravity or mass and the center of buoyancy of link i in their respective coordinate systems.

[0051] Optionally, obtaining the dynamic equation through the mass matrix includes: Obtaining the dynamic equation through the mass matrix; Wherein, the kinetic equation is: , in, is the mass matrix, is the Coriolis force matrix, is the effect of fluid dynamic damping, is the generalized hydrostatic force, for The derivative of is the generalized velocity vector of the base, are the command force and torque.

[0052] Specifically, the kinetic equation is: , in, Add a mass matrix to the entire robot and the rigid body mass matrix The sum of , the formula is: , in, is the rigid body mass matrix, Attach the matrix to the rigid body.

[0053] Optionally, the command forces and torques include joint torques and thruster torques; Wherein, the command force and torque are: , in, are the command force and torque, is the thruster torque, is the joint torque, is the control input of the snake-like underwater robot, is the joint control input, Configure the matrix for thrust.

[0054] In some more specific embodiments, the model construction method of the snake-like underwater robot further includes a propeller configuration matrix, wherein the propeller configuration matrix includes generalized forces and moments applied to the robot. Provided by joint motors and thrusters, generalized forces and moments The vector can be divided into joint torque and the generalized forces and moments of the thruster : , The robot is equipped with several thrusters. Unlike traditional underwater robots, the position of the thrusters relative to the base depends on the joint angles. Affects the position and orientation of the base and the joint angles, which are controlled by the joint motors, thruster forces and control input The relationship between is highly nonlinear and can be simplified to a linear relationship: , in, (containing m thrusters and n joints) is the thrust configuration matrix.

[0055] Given below Each link has an associated matrix that describes how the thruster applies forces and torques on that link. This matrix is ​​called ,in, is the number of thrusters for connecting rod i, Can be: , in, is the thrust direction of the j-th thruster mounted on connecting rod i, is the link i coordinate system The position vector represented by ; The forces and moments on the connecting rod i are expressed by the body Jacobian Converted to forces and moments on the base and moments on the individual joints: .

[0056] In some more specific embodiments, the model building method of the snake-like underwater robot further includes position configuration, and the position configuration is as follows: Rigid body relative to the world coordinate system , The position is defined as: , Among them, x, y, z (longitudinal swing, horizontal swing, vertical swing) are body coordinate systems respectively. The origin of the inertial coordinate system is the coordinate of the x, y, and z directions; the linear velocity of the rigid body in the inertial coordinate system is the time derivative , the integral is .

[0057] Define the body-fixed coordinate system of a rigid body relative to the body-fixed coordinate system Inertial coordinate system The linear velocity of the body is called the fixed linear velocity of the body: , The relationship between the rigid body linear velocity expressed in the inertial system and the fixed linear velocity of the body is: , in, is the rotation matrix from the inertial frame to the body fixed frame: , in, is the rotation matrix of the angle ϕ around the x-axis, is the rotation matrix of the angle θ around the y-axis, is the rotation matrix of the angle ψ around the z-axis, , and is the basic rotation matrix around three independent axes of the continuous coordinate system. The rotation matrix is ​​defined as: , in, and They are and Abbreviation of .

[0058] The model construction method of the snake-like underwater robot further includes performing direction configuration based on the Euler angle representation method, and the direction configuration is as follows: The Euler angle representation uses 3 parameters to represent the orientation, while the unit quaternion representation consists of 4 parameters and 1 norm constraint, so it is non-minimal. Using the unit quaternion to represent the direction avoids the problem of representing singularities. The unit quaternion vector can be defined as: , in, , , in, yes The angle of rotation described by a rotation about an axis, so each rotation is described by an angle and an axis.

[0059] The unit quaternion vector p has unit length. Numerically integrating the time derivative of the quaternion may cause the resulting quaternion norm to deviate from the unit. Therefore, the following formula is used to calculate the updated quaternion: , Among them, γ≥0, for First-order derivative, is the Jacobian matrix associated with parameter p, where k and oq are specific reference frames, is the correlation vector, is the correlation coefficient.

[0060] In some more specific embodiments, in combination Figure 3 As shown, the model construction method of the snake-like underwater robot also includes a motion control framework, wherein the motion control framework includes a navigation module, a motion controller, a dynamic controller and thrust distribution.

[0061] The navigation module uses a second-order damper to generate a smooth reference trajectory. The second-order damper expression is as follows: , in , and is a positive definite matrix. The output of the reference trajectory generation block is the acceleration , integrate it to get the reference velocity required by the inverse kinematics module .

[0062] The goal of the motion controller is to determine the desired time derivatives of the end effector position and orientation Calculate the required base and joint velocities One solution to the differential inverse kinematics problem is to use the pseudo-inverse of the Jacobian: , in, is the pseudo-inverse of the Jacobian matrix, is the posture matrix of the end effector in the inertial coordinate system I, q is the joint variable vector, is the desired velocity vector of the end effector, if the Jacobian If the row is full rank, the pseudo-inverse can be calculated as follows: , This method solves the problem of the irreversibility of the Jacobian matrix, so that the code can be written. However, when the Jacobian matrix is ​​close to a singular configuration, it will be "ill-conditioned" and a large joint velocity will be calculated. When it is in a singular configuration, the joint velocity will be infinite. In order to solve this problem, the damped least squares method is proposed: , in, is a small positive number.

[0063] Output of Differential Inverse Kinematics is the input of the dynamic controller. The dynamic control module is responsible for calculating the required force and torque based on the input of the motion control module. The control law is given by: , in, is the upper left submatrix of the mass matrix, is the relative speed representation, ,in are the linear acceleration and angular acceleration required by the base respectively.

[0064] By calculating the linear acceleration required by the base, a PI controller is used to control the linear velocity of the end effector. The control law is as follows: , in, is a diagonal matrix, representing the gain coefficients of the three degrees of freedom, are the actual speed and the expected speed respectively.

[0065] Similarly, a PD controller is used to calculate the angular acceleration, and the control law is as follows: , in, is a diagonal matrix, representing the gain coefficients of the three degrees of freedom, are the actual and desired angular velocity and angular acceleration, respectively.

[0066] For joint control, a simple proportional controller is used: , The thruster configuration of the USM will change depending on its current shape. In certain joint configurations, it may become a singular configuration, meaning that the thrusters are arranged in such a way that some degrees of freedom of the USM cannot be controlled. When approaching such a configuration, the thruster output will be very high to compensate for the fact that the thrusters have little effect on one or more degrees of freedom. To address this, a thrust distribution algorithm is used to solve the optimization problem: , in, is the related vector variable, is the desired force and torque, and λ is the damping factor used to adjust the weights so that it is preferred to keep the thruster forces small compared to trying to produce the desired force and torque. The complete thruster configuration matrix is ​​given by The first six lines of , which describe the forces and moments from the thruster on the base link.

[0067] By choosing a smaller damping coefficient , which can reduce the priority of minimizing the thrust force. It can be solved by damped pseudo-inverse: , in, , in, for The pseudo-inverse matrix of is a scalar parameter, is a 6×6 identity matrix.

[0068] In some more specific embodiments, a robot model is established in MATLAB or Simulink. The robot has five long connecting rods. For the joints with two degrees of freedom, the joints are modeled as two continuous single-degree-of-freedom joints connected by a short connecting rod in the middle. Thus, the entire robot model has 9 connecting rods and 8 joints. For two continuous joints, it is uniformly stipulated that the first one rotates around the z-axis and the second one rotates around the y-axis. In addition, there are 4 thrusters on the second long connecting rod (connecting rod numbered 2) and the fourth long connecting rod (connecting rod numbered 6) of the robot, wherein the thrusters on the second long connecting rod are arranged as two main thrusters up and down and two vertical thrusters on the left and right, and the thrusters on the fourth long connecting rod are arranged as two side thrusters up and down and two vertical thrusters on the left and right.

[0069] A control method for a snake-like underwater robot provided by an embodiment of the present invention includes: Obtain a snake-like underwater robot model according to the model construction method of the snake-like underwater robot as described above; The snake-like underwater robot is controlled according to the snake-like underwater robot model.

[0070] like Figure 4 As shown, an electronic device 400 provided by an embodiment of the present invention includes a memory 410 and a processor 420; the memory 410 is used to store a computer program; the processor 420 is used to implement the model building method of the serpentine underwater robot as described above or the control method of the serpentine underwater robot as described above when executing the computer program.

[0071] An embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the model building method of the serpentine underwater robot as described above or the control method of the serpentine underwater robot as described above is implemented.

[0072] An electronic device 400 that can be used as a server or client of the present invention will now be described, which is an example of a hardware device that can be applied to various aspects of the present invention. The electronic device 400 is intended to represent various forms of digital electronic computer equipment, such as laptop computers, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device 400 can also represent various forms of mobile devices, such as personal digital processing, cellular phones, smart phones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples, and are not intended to limit the implementation of the present invention described and / or required herein.

[0073] The electronic device 400 includes a computing unit, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory (ROM) or a computer program loaded from a storage unit into a random access memory (RAM). In the RAM, various programs and data required for the operation of the device can also be stored. The computing unit, ROM, and RAM are connected to each other via a bus. An input / output (I / O) interface is also connected to the bus.

[0074] A person of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment method can be implemented by instructing the relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, the storage medium can be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc. In the present application, the unit described as a separate component may or may not be physically separated, and the component displayed as a unit may or may not be a physical unit, that is, it may be located in one place, or it may be distributed on multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the embodiment of the present invention. In addition, each functional unit in each embodiment of the present invention can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of a software functional unit.

[0075] Although the present invention is disclosed as above, the protection scope of the present invention is not limited thereto. Those skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the protection scope of the present invention.

Claims

1. A method for constructing a model of a snake-like underwater robot, characterized in that: Based on the snake-like underwater robot, the snake-like underwater robot includes a base, a connecting rod and a joint connected to each other, and the model construction method of the snake-like underwater robot includes: Acquire an underwater robot coordinate system of the snake-like underwater robot, wherein the underwater robot coordinate system includes a connecting rod coordinate system, a propeller coordinate system and a world coordinate system; Based on the Jacobian matrix, a kinematic equation is obtained according to the underwater robot coordinate system; Obtaining a dynamic equation according to the kinematic equation; The snake-like underwater robot model is obtained through the dynamic equation; The kinematic equation obtained according to the underwater robot coordinate system includes: Based on the homogeneous transformation matrix, a forward kinematics equation is obtained according to the underwater robot coordinate system; Based on the Jacobian matrix, an inverse kinematics equation is obtained according to the forward kinematics equation; The connecting rod coordinate system includes a base coordinate system, and the forward kinematics equation obtained according to the underwater robot coordinate system includes: Obtaining a joint coordinate transformation matrix according to the underwater robot coordinate system; Among them, the joint coordinate transformation matrix is: ， in, is the transformation relationship of the homogeneous transformation matrix between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system, is the rotation transformation relationship between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system, The position transformation relationship between the i-1th connecting rod coordinate system and the i-th connecting rod coordinate system; Obtaining an underwater robot coordinate transformation matrix according to the underwater robot coordinate system and the joint coordinate transformation matrix; Among them, the underwater robot coordinate transformation matrix is: ， in, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the connecting rod coordinate system, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the base coordinate system; The forward kinematics equation is obtained according to the underwater robot coordinate transformation matrix.

2. The model construction method of the snake-like underwater robot according to claim 1, characterized in that: The inverse kinematics equation is obtained according to the forward kinematics equation, comprising: Based on differential kinematics, the connecting rod velocity in each coordinate system is obtained according to the adjoint matrix; Wherein, the connecting rod speed is: ， in, is the connecting rod speed of the i+1th connecting rod, is the adjoint matrix, is the underwater robot coordinate transformation matrix, is the connecting rod speed of the ith connecting rod, is the rotation coordinate of the i-th joint, is the i-th joint angle; The inverse kinematics equation is obtained through the connecting rod velocity.

3. The model construction method of the snake-like underwater robot according to claim 2, characterized in that: The step of obtaining a dynamic equation according to the kinematic equation comprises: According to the kinematic equation, a kinetic energy equation of the connecting rod is obtained; Wherein, the connecting rod kinetic energy equation is: ， in, is the inertia matrix of link i in the link coordinate system, is the connecting rod kinetic energy, is the inertia matrix of link i relative to the base, for The transposed matrix of is the Jacobian matrix, is the generalized velocity vector of the base, is the joint angle; Obtaining a mass matrix according to the connecting rod kinetic energy equation; The dynamic equation is obtained through the mass matrix.

4. The model construction method of the snake-like underwater robot according to claim 3, characterized in that: The step of obtaining the dynamic equation through the mass matrix includes: Obtaining the dynamic equation through the mass matrix; Wherein, the kinetic equation is: ， in, is the mass matrix, is the Coriolis force matrix, is the effect of fluid dynamic damping, is the generalized hydrostatic force, for The derivative of is the generalized velocity vector of the base, are the command force and torque.

5. The model construction method of the snake-like underwater robot according to claim 4, characterized in that: The command forces and torques include joint torques and thruster torques; Wherein, the command force and torque are: ， in, are the command force and torque, is the thruster torque, is the joint torque, is the control input of the snake-like underwater robot, is the joint control input, Configure the matrix for thrust.

6. The method for constructing a model of a snake-like underwater robot according to claim 2, characterized in that: Based on differential kinematics, the connecting rod velocity in each coordinate system is obtained according to the adjoint matrix, including: Based on differential kinematics, the connecting rod velocity in each coordinate system is obtained according to the adjoint matrix; The adjoint matrix is: ， in, is the adjoint matrix, is the antisymmetric form of the position vector, and R is the rotation matrix representing the end effector of the robot, which describes the direction and posture of the end effector relative to the base.

7. The model construction method of a snake-like underwater robot according to claim 3, characterized in that: The mass matrix is ​​obtained according to the connecting rod kinetic energy equation, including: Obtaining the mass matrix according to the connecting rod kinetic energy equation; The mass matrix is: ， in, is the rigid body mass matrix of the mass matrix, is the inertia of the robot as a single rigid body, and is the inertia matrix of the coupling between the joint motion and the robot base, is the inertia matrix of the robot when the base is fixed.

8. A control method for a snake-like underwater robot, characterized in that: include: Obtaining a snake-like underwater robot model according to the snake-like underwater robot model construction method according to any one of claims 1 to 7; The snake-like underwater robot is controlled according to the snake-like underwater robot model.

9. An electronic device, characterized in that: including memory and processor; The memory is used to store computer programs; The processor is used to implement the model building method of the snake-like underwater robot as described in any one of claims 1 to 7 or the control method of the snake-like underwater robot as described in claim 8 when executing the computer program.

10. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, which, when executed by a processor, implements the model building method of the serpentine underwater robot as described in any one of claims 1 to 7 or the control method of the serpentine underwater robot as described in claim 8.

Images