Model construction method, control method and device of serpentine underwater robot

Abstract

The present invention provides a method and device for constructing a model and a control method of a snake-shaped underwater robot, relating to the technical field of underwater robots. Based on the snake-shaped underwater robot, the snake-shaped underwater robot includes a base, connecting rods and joints connected to each other. The method for constructing the model of the snake-shaped underwater robot includes: obtaining an underwater robot coordinate system of the snake-shaped underwater robot, wherein the underwater robot coordinate system includes a connecting rod coordinate system, a thruster coordinate system and a world coordinate system; based on the Jacobian matrix, obtaining a kinematic equation according to the underwater robot coordinate system; obtaining a dynamic equation according to the kinematic equation; and obtaining a snake-shaped underwater robot model through the dynamic equation. The present invention realizes the improvement of the control accuracy of the snake-shaped underwater robot.

Description

Technical Field

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

[0002] Due to its unique structural design, the snake-shaped underwater robot can imitate the movement mode of real snakes and has extremely high flexibility and mobility. This enables them to move freely in narrow spaces and complete tasks that are difficult for traditional underwater equipment to reach. The snake-shaped underwater robot can get rid of the communication cable while having a more slender body. It can enter the pipeline for exploration, can adapt to a more complex environment by changing its body structure, and can achieve different functional requirements by carrying different functional modules. However, in actual use, due to its complex structure, it will become over-driven in the task of controlling its position and direction, and thus cannot complete highly accurate work in a complex environment. Summary of the Invention

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

[0004] To solve the above problems, the present invention provides a method and device for constructing a model of a snake-shaped underwater robot, and a control method thereof.

[0005] In a first aspect, the present invention provides a method for constructing a model of a snake-shaped underwater robot. Based on the snake-shaped underwater robot, the snake-shaped underwater robot includes a base, a connecting rod and a joint that are connected to each other. The method for constructing a model of the snake-shaped underwater robot includes:

[0006] Obtain the underwater robot coordinate system of the snake-shaped underwater robot, where the underwater robot coordinate system includes a connecting rod coordinate system, a thruster coordinate system and a world coordinate system;

[0007] Based on the Jacobian matrix, obtain the kinematic equation according to the underwater robot coordinate system;

[0008] Obtain the dynamic equation according to the kinematic equation;

[0009] Obtain the snake-shaped underwater robot model through the dynamic equation.

[0010] Optionally, the obtaining the kinematic equation according to the underwater robot coordinate system includes:

[0011] Based on the homogeneous transformation matrix, obtain the forward kinematic equation according to the underwater robot coordinate system;

[0012] Based on the Jacobian matrix, obtain the inverse kinematic equation according to the forward kinematic equation.

[0013] Optionally, the link coordinate system includes a base coordinate system, and obtaining the forward kinematic equation according to the underwater robot coordinate system includes:

[0014] Obtaining a joint coordinate transformation matrix according to the underwater robot coordinate system;

[0015] Wherein, the joint coordinate transformation matrix is:

[0016] ,

[0017] Wherein, is the transformation relationship of the homogeneous transformation matrix between the (i - 1)-th link coordinate system and the i-th link coordinate system, is the rotation transformation relationship between the (i - 1)-th link coordinate system and the i-th link coordinate system, the position transformation relationship between the (i - 1)-th link coordinate system and the i-th link coordinate system;

[0018] Obtaining an underwater robot coordinate transformation matrix according to the underwater robot coordinate system and the joint coordinate transformation matrix;

[0019] Wherein, the underwater robot coordinate transformation matrix is:

[0020] ,

[0021] Wherein, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the link coordinate system, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the base coordinate system;

[0022] Obtaining the forward kinematic equation according to the underwater robot coordinate transformation matrix.

[0023] Optionally, obtaining the inverse kinematic equation according to the forward kinematic equation includes:

[0024] Based on differential kinematics, obtaining the link velocities in each coordinate system according to the adjoint matrix;

[0025] Wherein, the link velocities are:

[0026] ,

[0027] Wherein, is the link velocity of the (i + 1)-th link, is the adjoint matrix, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the link coordinate system, is the link velocity of the i-th link, is the rotational coordinate of the i-th joint, is the i-th joint angle;

[0028] The inverse kinematic equation is obtained through the link velocity.

[0029] Optionally, obtaining the dynamic equation according to the kinematic equation includes:

[0030] Obtaining the link kinetic energy equation according to the kinematic equation;

[0031] Among them, the link kinetic energy equation is:

[0032] ,

[0033] Among them, is the inertia matrix of link i in the link coordinate system, is the link kinetic energy, is the inertia matrix of link i expressed relative to the base, is the transpose matrix of, is the Jacobian matrix, is the generalized velocity vector of the base, is the joint angle;

[0034] Obtaining the mass matrix according to the link kinetic energy equation;

[0035] The dynamic equation is obtained through the mass matrix.

[0036] Optionally, obtaining the dynamic equation through the mass matrix includes:

[0037] Obtaining the dynamic equation through the mass matrix;

[0038] Among them, the dynamic equation is:

[0039] ,

[0040] Among them, is the mass matrix, is the Coriolis force matrix, is the influence of hydrodynamic damping, is the generalized hydrostatic force, is the derivative of, is the generalized velocity vector of the base, is the commanded force and torque.

[0041] Optionally, the commanded force and torque include joint torque and thruster torque;

[0042] Among them, the command force and torque are as follows:

[0043] ,

[0044] Among them, is the command force and torque, is the thruster torque, is the joint torque, is the control input of the snake-shaped underwater robot, is the joint control input, is the thrust configuration matrix.

[0045] In a second aspect, the present invention provides a control method for a snake-shaped underwater robot, including: obtaining a snake-shaped underwater robot model according to the model construction method of the snake-shaped underwater robot as described in the first aspect;

[0046] Controlling the snake-shaped underwater robot according to the snake-shaped underwater robot model.

[0047] In a third aspect, the present invention provides an electronic device, including a memory and a processor;

[0048] The memory is used to store a computer program;

[0049] The processor is used to implement the model construction method of the snake-shaped underwater robot as described in the first aspect or the control method of the snake-shaped underwater robot as described in the second aspect when executing the computer program.

[0050] In a fourth aspect, the present invention provides a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the model construction method of the snake-shaped underwater robot as described in the first aspect or the control method of the snake-shaped underwater robot as described in the second aspect is implemented.

[0051] The beneficial effects of the model construction of the snake-shaped underwater robot of the present invention are as follows:

[0052] By defining an underwater robot coordinate system, the underwater robot coordinate system includes a link coordinate system, a thruster coordinate system, and a world coordinate system. By establishing the three coordinate systems, the coordinate relationship of the underwater robot is clarified, avoiding omissions and errors. According to the underwater robot coordinate system and using the Jacobian matrix, the kinematic equation is obtained, improving the accuracy of subsequent calculations. According to the kinematic equation, the dynamic equation is obtained, and through the dynamic equation, the snake-shaped underwater robot model is obtained, which can achieve precise control of each part of the snake-shaped underwater robot and improve the control accuracy of the snake-shaped underwater robot. Description of the Drawings

[0053] Figure 1 Schematic structural diagram of a snake-shaped underwater robot according to an embodiment of the present invention;

[0054] Figure 2 Schematic flowchart of a method for constructing a model of a snake-shaped underwater robot according to an embodiment of the present invention;

[0055] Figure 3 Schematic flowchart of another method for constructing a model of a snake-shaped underwater robot according to an embodiment of the present invention;

[0056] Figure 4 Schematic structural diagram of an electronic device according to an embodiment of the present invention. Detailed implementation manners

[0057] To make the above objects, features, and advantages of the present invention more apparent and understandable, the following describes the specific embodiments of the present invention in detail with reference to the accompanying drawings. Although some embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be construed as limited to the embodiments described herein. On the contrary, these embodiments are provided to more thoroughly and completely understand the present invention. It should be understood that the drawings and embodiments of the present invention are only for exemplary purposes and are not used to limit the protection scope of the present invention.

[0058] It should be understood that the various steps recorded in the method embodiments of the present invention can be executed in different orders and / or executed 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 regard.

[0059] The term "including" and its variants used herein are open-ended, that is, "including but not limited to"; the term "based on" is "at least partially based on"; the term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional 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 such as "first" and "second" mentioned in the present invention are only used to distinguish different devices, modules, or units, and are not used to limit the order of functions performed by these devices, modules, or units or their interdependent relationships.

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

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

[0062] As Figure 1 shown, it is a connecting rod and joint part of the snake-shaped underwater robot. The connecting rods include 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 snake-shaped underwater robot is essentially an underwater manipulator with a movable base, and the joints and connecting rods are connected to the base. The joints are used to connect the connecting rods on both sides, and the connecting rods are sequentially connected to the joints. A thruster is connected to the power section connecting rod, and the thruster is used to provide power for the snake-shaped underwater robot.

[0063] As Figure 2 shown, a method for constructing a model of a snake-shaped underwater robot provided by an embodiment of the present invention includes:

[0064] Step 210, obtain the underwater robot coordinate system of the snake-shaped underwater robot, where the underwater robot coordinate system includes a connecting rod coordinate system, a thruster coordinate system, and a world coordinate system.

[0065] Specifically, three coordinate systems are selected to describe the above relationships, namely the connecting rod coordinate system, the thruster coordinate system, and the world coordinate system. The underwater robot is composed of n + 1 connecting rods and is connected by n rotating joints, where each joint has two degrees of freedom and has three-dimensional motion ability. A coordinate system is attached to the rear end of each connecting rod, and the numbering rule is to number from the tail to the front, represented by the symbol F and a subscript indicating that the base is selected to coincide with the rear end of connecting rod 0, and the term end effector refers to the front end of connecting rod n. The reference systems corresponding to the base and the end effector are the base coordinate system and the end effector coordinate system . The robot is also equipped with m thrusters, numbered . The coordinate system is the assumed world coordinate system.

[0066] Step 220, based on the Jacobian matrix, obtain the kinematic equation according to the underwater robot coordinate system;

[0067] Specifically, the Jacobian matrix provides a means to linearize complex non-linear relationships, making the solution of the inverse kinematics problem more straightforward and simple. Even in robotic systems with a high degree of freedom, the inverse kinematics problem can be effectively solved through iterative methods. Using the Jacobian matrix, the inverse kinematics problem can be flexibly solved by adjusting different optimization objectives (such as minimizing joint movement, avoiding singularities, keeping joint angles within a safe range, etc.).

[0068] Step 230, obtaining a dynamic equation according to the kinematic equation;

[0069] Specifically, the kinematic model includes a forward kinematic model and an inverse kinematic model, which transforms the geometric relationships describing the movement of the robot into a dynamic model describing the relationships between forces and accelerations.

[0070] Step 240, obtaining a serpentine underwater robot model through the dynamic equation.

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

[0072] In this embodiment, by defining an underwater robot coordinate system, which includes a link coordinate system, a thruster coordinate system, and a world coordinate system, the coordinate relationships of the underwater robot are clarified by establishing the three coordinate systems, avoiding omissions. According to the underwater robot coordinate system and using the Jacobian matrix, a kinematic equation is obtained, improving the accuracy of subsequent calculations. According to the kinematic equation, a dynamic equation is obtained, and through the dynamic equation, a serpentine underwater robot model is obtained, which can achieve precise control of each part of the serpentine underwater robot and improve the control accuracy of the serpentine underwater robot.

[0073] Optionally, the obtaining of the kinematic equation according to the underwater robot coordinate system includes:

[0074] Based on the homogeneous transformation matrix, a forward kinematic equation is obtained according to the underwater robot coordinate system;

[0075] Based on the Jacobian matrix, an inverse kinematic equation is obtained according to the forward kinematic equation.

[0076] Optionally, the link coordinate system includes a base coordinate system, and the obtaining of the forward kinematic equation according to the underwater robot coordinate system includes:

[0077] Obtaining a joint coordinate transformation matrix according to the underwater robot coordinate system;

[0078] Wherein, the joint coordinate transformation matrix is:

[0079] ,

[0080] Among them, is the transformation relationship of the homogeneous transformation matrix between the (i - 1)-th link coordinate system and the i-th link coordinate system, is the rotation transformation relationship between the (i - 1)-th link coordinate system and the i-th link coordinate system, is the position transformation relationship between the (i - 1)-th link coordinate system and the i-th link coordinate system;

[0081] Obtain the underwater robot coordinate transformation matrix according to the underwater robot coordinate system and the joint coordinate transformation matrix;

[0082] Among them, the underwater robot coordinate transformation matrix is:

[0083] ,

[0084] Among them, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the link 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 b-th link coordinate system and the 1st link coordinate system;

[0085] Obtain the forward kinematic equation according to the underwater robot coordinate transformation matrix.

[0086] Specifically, the robot is an articulated structure composed of multiple interconnected rigid bodies, that is, a multi-body system. The homogeneous transformation matrix is used to describe such a 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 for defining link coordinate systems and describing the relative positions and postures between the joints of the robot. Using the improved DH method, the transformation between two consecutive joints of the robot starting from the base is derived as:

[0087] ,

[0088] Among them, c* and s* are cos(*) and sin(*) respectively. For example and , is the joint number, is the i-th joint angle, is the (i - 1)-th link twist angle, is the i-th offset.

[0089] Writing the above formula in matrix form is:

[0090] ,

[0091] wherein, is the transformation relationship between the (i - 1)-th coordinate system and the i-th coordinate system, and are respectively the upper left 3×3 sub-matrix and the upper right 3×1 column vector of

[0092] For any link coordinate system of the robot, it can be represented by a transformation matrix as:

[0093] ,

[0094] wherein, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the link coordinate system, is the world coordinate system and the base coordinate system the transformation relationship of the homogeneous transformation matrix between them, is the rotation matrix describing the link, is from the origin to the position transformation relationship between the origin and the i-th link coordinate system.

[0095] To describe the position and orientation of the robot, the transformation relationship of the homogeneous transformation matrix between the defined world coordinate system and the base coordinate system is denoted as:

[0096] ,

[0097] wherein, is the rotation matrix describing the base direction, is from the origin to the position vector of the origin;

[0098] Since multiple linear transformations can be equivalently represented as a single transformation in the form of a product, the transformation matrix from the base to the end effector is:

[0099] ,

[0100] wherein, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the end effector coordinate system between them, is the base coordinate system and the end effector coordinate system the transformation relationship of the homogeneous transformation matrix between them, The rotation matrix for describing the base direction and the end - effector direction From The origin to The position vector of the origin The rotation matrix for describing the end - effector direction From The origin to The position vector of the origin

[0101] In this optional embodiment, the forward kinematic equation is obtained according to the underwater robot coordinate system by using the homogeneous transformation matrix, and the complex robot structure is simplified into a series of standard coordinate transformations by using the DH algorithm, which simplifies the forward kinematic modeling process of the robot.

[0102] Optionally, obtaining the inverse kinematic equation according to the forward kinematic equation includes:[[]]

[0103] Based on differential kinematics, the link velocities in each coordinate system are obtained according to the adjoint matrix;

[0104] Wherein, the link velocity is:[[]]

[0105] ,

[0106] Wherein, Is the link velocity of the (i + 1)-th link, Is the adjoint matrix, Is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the link coordinate system, Is the link velocity of the i - th link, Is the rotation coordinate of the i - th joint, Is the i - th joint angle;

[0107] The inverse kinematic equation is obtained through the link velocities.

[0108] 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:[[]]

[0109] ,

[0110] Wherein, Is the adjoint matrix, Is the skew - symmetric form of the position vector, and R is the rotation matrix representing the end - effector of the robot, which describes the direction and attitude of the end - effector relative to the base.

[0111] The velocity transformation is:[[]]

[0112] ,

[0113] wherein, 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.

[0114] wherein, .

[0115] Given the generalized velocity of the base, the velocities of the links in their respective coordinate systems can be obtained by recursion:

[0116] ,

[0117] is the joint rotation coordinate vector given by , is the derivative of the i-th joint angle vector, and the last three elements respectively represent x, y, and z, specifically depending on whether the joint rotates about the z-axis or y-axis of the coordinate system ;

[0118] Define as the generalized velocity vector of the base, which includes the velocity of the base and the respective joint angles, and redefine the velocity vector as:

[0119] ,

[0120] wherein, 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 link number.

[0121] The Jacobian matrix is defined as:

[0122] ,

[0123] ,

[0124] is the 6×6 identity matrix.

[0125] In this optional embodiment, the inverse kinematics formula is derived using the Jacobian matrix. The Jacobian matrix provides a means to linearize complex non-linear relationships, which makes the solution of the inverse kinematics problem more direct and simple.

[0126] Optionally, obtaining the dynamic equation according to the kinematic equation includes:

[0127] Obtain the kinetic energy equation of the connecting rod according to the kinematic equation;

[0128] Among them, the kinetic energy equation of the connecting rod is:

[0129] ,

[0130] Among them, is the inertia matrix of link i in the link coordinate system, is the kinetic energy of the connecting rod, is the velocity vector, is the generalized velocity vector, is the inertia matrix of link i expressed relative to the base, is the transpose matrix of, is the Jacobian matrix, is the generalized velocity vector of the base, is the joint angle;

[0131] Obtain the mass matrix according to the kinetic energy equation of the connecting rod;

[0132] Obtain the dynamic equation through the mass matrix.

[0133] Specifically, express all link inertia matrices in the same coordinate system through the Jacobian matrix, and then obtain the total rigid body inertia matrix by adding the contributions of each link, which is:

[0134] ,

[0135] Among them, is the rigid body mass matrix, and the upper left part is the inertia of the robot as a single rigid body, while the upper right part and the lower left corner are the inertia matrices of the coupling between joint motion and the robot base, which reflect the influence of joint acceleration on the base motion and the dual influence of base acceleration on joint motion, is the inertia matrix of the robot when the base is fixed.

[0136] For cylindrical links with the same radius r and different lengths , the additional mass matrix of each link is:

[0137] ,

[0138] Among them, is the density of water, is the additional mass coefficient, is the parameter including the additional mass effect, r is the link radius, li is the connecting rod length.

[0139] The expression of the added mass matrix is:

[0140] ,

[0141] where, is the rigid body added matrix, and the Coriolis force matrix is:

[0142] ,

[0143] where, ,

[0144] where, respectively represent the first three terms and the last three terms.

[0145] The matrix is the influence of hydrodynamic damping, n is the correlation coefficient, and its expression is:

[0146] ,

[0147] where, the matrix is the hydrodynamic damping matrix of each corresponding connecting rod, and is used to represent the hydrodynamic force and moment on the connecting rod i.

[0148] The hydrodynamic force generated by the movement of the robot in water is complex and highly nonlinear. When modeling the hydrodynamic force, the water flow velocity is not considered. First, the drag force on the cylindrical connecting rod i is given. The linear drag force on the connecting rod i is called , and the linear drag force on the connecting rod i is:

[0149] ,

[0150] where, ,

[0151] where, is the linear drag coefficient, ρ is the density of water, r is the radius of the connecting rod, is the length of the connecting rod i, , and are constants, the reference velocity, the rotational related damping coefficient, and the mixed rotational inertia damping coefficient;

[0152] In addition to the linear drag force, the model also includes the nonlinear drag force. The surge and roll nonlinear drag forces on the connecting rod i are:

[0153] ,

[0154] ,

[0155] Among them, is the surge nonlinear resistance coefficient, is the roll nonlinear resistance coefficient. Assuming the ocean current speed is zero, is the speed of the i-th object in the surge condition, is the speed of the i-th object in the roll condition. The total resistance on link i is the sum of the linear and nonlinear resistances, which is:

[0156] ,

[0157] Among them, is the nonlinear resistance.

[0158] Generalized hydrostatic force is composed of the forces and moments on the base and the moments acting on the joints due to the hydrostatic forces on each link of the robot. The generalized hydrostatic force is:

[0159] ,

[0160] Among them, is the i-th Jacobian matrix, is the hydrostatic force and moment on link i, which are respectively:

[0161] ,

[0162] Among them, is the direction of gravity in the inertial frame, which is taken as a constant, is the rotation matrix of the i-th part. The matrix is also a constant, and its expression is:

[0163] ,

[0164] Among them, is the density of water, is the gravitational constant, is the identity matrix in three-dimensional space, , are the mass and volume of link i respectively; the vectors , are the positions of the center of gravity or mass center and the center of buoyancy of link i in their respective coordinate systems.

[0165] Optionally, obtaining the dynamic equation through the mass matrix includes:

[0166] Obtaining the dynamic equation through the mass matrix;

[0167] Among them, the kinetic equation is as follows:

[0168] ,

[0169] Among them, is the mass matrix, is the Coriolis force matrix, is the influence of hydrodynamic damping, is the generalized hydrostatic force, is the derivative of, is the generalized velocity vector of the base, is the commanded force and torque.

[0170] Specifically, the kinetic equation is as follows:

[0171] ,

[0172] Among them, is the additional mass matrix of the entire robot and the rigid body mass matrix sum, and its formula is:

[0173] ,

[0174] Among them, is the rigid body mass matrix, is the rigid body additional matrix.

[0175] Optionally, the commanded force and torque include joint torque and thruster torque;

[0176] Among them, the commanded force and torque are:

[0177] ,

[0178] Among them, is the commanded 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, is the thrust configuration matrix.

[0179] In some more specific embodiments, the method for constructing the model of the snake-like underwater robot further includes a thruster configuration matrix, and the thruster configuration matrix includes the generalized force and torque applied to the robot provided by the joint motor and the thruster, and the vector of the generalized force and torque can be divided into joint torques Generalized forces and moments of the thrusters :

[0180] ,

[0181] The robot is equipped with several thrusters. Different from traditional underwater robots, the position of the thrusters relative to the base depends on the joint angles. The thruster forces affect the position and orientation of the base as well as the joint angles, and the joint angles are controlled by joint motors. The relationship between the thruster forces and the control inputs is highly nonlinear and can be simplified to a linear relationship:

[0182] ,

[0183] where (including m thrusters and n joints) is the thrust configuration matrix.

[0184] The following gives the derivation. Each link has an associated matrix for describing how the thrusters apply forces and moments on that link. This matrix is called , where is the number of thrusters on link i, can be:

[0185] ,

[0186] where is the thrust direction of the j-th thruster installed on link i, is the position vector expressed in the coordinate system of link i;

[0187] The forces and moments on link i are converted to the forces and moments on the base and the moments on each joint through the body Jacobian :

[0188] .

[0189] In some more specific embodiments, the method for constructing the model of the snake-shaped underwater robot further includes position configuration, and the position configuration is as follows:

[0190] The position of the rigid body relative to the world coordinate system , is defined as:

[0191] ,

[0192] where x, y, z (surge, sway, heave) are the body coordinate system The coordinates of the origin of the rigid body in the x, y, and z directions with respect to the inertial coordinate system; the linear velocity of the rigid body in the inertial coordinate system is the time derivative , and integrating gives .

[0193] Define the linear velocity of the body-fixed coordinate system of the rigid body with respect to the inertial coordinate system represented by the body-fixed coordinate system and call it the body-fixed linear velocity:

[0194] ,

[0195] The relationship between the linear velocity of the rigid body represented in the inertial system and the body-fixed linear velocity is:

[0196] ,

[0197] where is the rotation matrix from the inertial system to the body-fixed system:

[0198] ,

[0199] where is the rotation matrix for rotating by an angle ϕ about the x-axis, is the rotation matrix for rotating by an angle θ about the y-axis, is the rotation matrix for rotating by an angle ψ about the z-axis, , and are the basic rotation matrices about three independent axes of the continuous coordinate system, and the rotation matrix is defined as:

[0200] ,

[0201] where and are abbreviations of and respectively.

[0202] The model construction method of the snake-shaped underwater robot further includes direction configuration based on the Euler angle representation method, and the direction configuration is as follows:

[0203] The Euler angle representation method uses 3 parameters to represent the orientation, while the unit quaternion representation method 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:

[0204] ,

[0205] where ,​ ,

[0206] Among them, is the angle of rotation about the axis described, so each rotation is described by an angle and an axis.

[0207] The unit quaternion vector p has unit length. Numerically integrating the time derivative of the quaternion may cause the norm of the resulting quaternion to deviate from unity. Therefore, the following formula is used to calculate the updated quaternion:

[0208] ,

[0209] where γ ≥ 0, is the first derivative, is the Jacobian matrix related to the parameter p, where k and oq are specific reference frames, is the related vector, is the related coefficient.

[0210] In some more specific embodiments, as shown in combination with Figure 3 , the method for constructing the model of the snake-shaped underwater robot further includes a motion control framework, where the motion control framework includes a navigation module, a motion controller, a dynamic controller, and thrust allocation.

[0211] The navigation module uses a second-order damper to generate a smooth reference trajectory. The expression of the second-order damper is as follows:

[0212] ,

[0213] where , and are positive definite matrices. The output of the reference trajectory generation block is the acceleration , and integrating it gives the reference velocity required by the inverse kinematics module.

[0214] The goal of the motion controller is to calculate the required base velocity and joint velocity according to the required time derivatives of the end effector position and orientation . One solution to the differential inverse kinematics problem is to use the pseudo-inverse of the Jacobian determinant:

[0215] ,

[0216] where is the pseudo-inverse of the Jacobian matrix, is the attitude matrix of the end effector in the inertial coordinate system I, q is the vector of joint variables, is the desired velocity vector of the end effector. If the Jacobian is full row rank, the pseudo-inverse can be calculated by the following formula:

[0217] ,

[0218] This method solves the problem of non-invertibility of the Jacobian matrix, thus enabling the writing of code. However, when approaching the singular configuration, the Jacobian matrix becomes "ill-conditioned", resulting in a very large joint velocity being calculated. When in the singular configuration, the joint velocity becomes infinite. To solve this problem, the damped least squares method is proposed:

[0219] ,

[0220] where is a very small positive number.

[0221] The output of differential inverse kinematics is the input of the dynamic controller. The dynamic control module is responsible for calculating the required forces and torques based on the input of the motion control module. The control law is given by the following formula:

[0222] ,

[0223] where is the upper left submatrix of the mass matrix, is the representation of the relevant velocity, , where are the linear acceleration and angular acceleration required by the base respectively.

[0224] To control the linear velocity of the end effector by calculating the linear acceleration required by the base, a PI controller is used in this paper, and its control law is as follows:

[0225] ,

[0226] where is a diagonal matrix, representing the gain coefficients of the three degrees of freedom respectively, are the actual velocity and the desired velocity respectively.

[0227] Similarly, a PD controller is used to calculate the angular acceleration, and its control law is as follows:

[0228] ,

[0229] where is a diagonal matrix, representing the gain coefficients of the three degrees of freedom respectively, are the actual and desired angular velocities and angular accelerations respectively.

[0230] For joint control, a simple proportional controller is used to achieve:

[0231] ,

[0232] The thruster configuration of the USM will change according to its current shape. In some joint configurations, it may become a singular configuration, which means that the arrangement of the thrusters makes it impossible to control some degrees of freedom of the USM. When approaching such a configuration, the thruster output will be very high to compensate for the problem that the thrusters have little effect on one or more degrees of freedom. To solve this problem, a thrust allocation algorithm is used to solve the optimization problem:

[0233] ,

[0234] where, is the relevant vector variable, is the desired force and moment, λ is the damping factor used to adjust the weight, so that priority is given to keeping the thruster force small compared to trying to generate the required force and moment. The matrix consists of the first six rows of the complete thruster configuration matrix given, that is, the part describing the forces and moments from the thrusters on the base link.

[0235] By choosing a smaller damping coefficient , the priority of minimizing the thruster force can be reduced. It can be solved by the damped pseudoinverse:

[0236] ,

[0237] where, ,

[0238] where, is the pseudoinverse matrix of, is a scalar parameter, is the 6×6 identity matrix.

[0239] In some more specific embodiments, a robot model is established in MATLAB or Simulink. The robot has a total of five long linkages. For the joints with two degrees of freedom of motion, the joints are modeled as two consecutive single-degree-of-freedom joints, connected by a short linkage in the middle. Thus, the entire robot model has 9 linkages and 8 joints. For two consecutive joints, it is uniformly specified that the first rotates around the z-axis and the second joint rotates around the y-axis. In addition, there are 4 thrusters on the second long linkage (linkage number 2) and the fourth long linkage (linkage number 6) of the robot. Among them, the thrusters on the second long linkage are arranged with two main thrusters up and down and two vertical thrusters left and right, and the thrusters on the fourth long linkage are arranged with two side thrusters up and down and two vertical thrusters left and right.

[0240] A control method for a snake-shaped underwater robot provided by an embodiment of the present invention includes:

[0241] Obtaining a snake-shaped underwater robot model according to the model construction method of the snake-shaped underwater robot as described above;

[0242] Controlling the snake-shaped underwater robot according to the snake-shaped underwater robot model.

[0243] As Figure 4 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 construction method of the snake-shaped underwater robot as described above or the control method of the snake-shaped underwater robot as described above when executing the computer program.

[0244] A computer-readable storage medium provided by an embodiment of the present invention has a computer program stored thereon, and when the computer program is executed by a processor, it implements the model construction method of the snake-shaped underwater robot as described above or the control method of the snake-shaped underwater robot as described above.

[0245] Now, an electronic device 400 that can be used as a server or a client of the present invention will be described. It 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 devices, such as, laptop computers, desktop computers, workstations, 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 herein and / or claimed.

[0246] The electronic device 400 includes a computing unit that 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 device operation 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.

[0247] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. 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 methods. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM), etc. In this application, the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they can be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiments of the present invention. In addition, the functional units in each embodiment of the present invention can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above integrated units can be implemented in the form of hardware or in the form of software functional units.

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

Claims

1. A method for constructing a model of a snake-shaped underwater robot, characterized in that, Based on the snake-shaped underwater robot, the snake-shaped underwater robot includes a base, connecting rods and joints that are interconnected, and the method for constructing the model of the snake-shaped underwater robot includes: Obtain the underwater robot coordinate system of the snake-shaped underwater robot, where the underwater robot coordinate system includes a connecting rod coordinate system, a thruster coordinate system, and a world coordinate system; Based on the Jacobian matrix, obtain the kinematic equation according to the underwater robot coordinate system; Obtain the dynamic equation according to the kinematic equation; Obtain the snake-shaped underwater robot model through the dynamic equation; The obtaining of the kinematic equation according to the underwater robot coordinate system includes: Based on the homogeneous transformation matrix, obtain the forward kinematic equation according to the underwater robot coordinate system; Based on the Jacobian matrix, obtain the inverse kinematic equation according to the forward kinematic equation; The connecting rod coordinate system includes a base coordinate system, and the obtaining of the forward kinematic equation according to the underwater robot coordinate system includes: Obtain the joint coordinate transformation matrix according to the underwater robot coordinate system; Wherein, the joint coordinate transformation matrix is: ， wherein, is the transformation relationship of the homogeneous transformation matrix between the (i - 1)-th and the i-th link coordinate systems, is the rotation transformation relationship between the (i - 1)-th and the i-th link coordinate systems, is the position transformation relationship between the (i - 1)-th and the i-th link coordinate systems; Obtain the underwater robot coordinate transformation matrix according to the underwater robot coordinate system and the joint coordinate transformation matrix; Wherein, the underwater robot coordinate transformation matrix is: ， wherein, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the link coordinate system, is the transformation relationship of the homogeneous transformation matrix between the world coordinate system and the base coordinate system; Obtain the forward kinematic equation according to the underwater robot coordinate transformation matrix; The obtaining of the inverse kinematic equation according to the forward kinematic equation includes: Based on differential kinematics, obtain the connecting rod velocities in each coordinate system according to the adjoint matrix; Wherein, the connecting rod velocities are: ， wherein, is the link velocity of the (i + 1)-th link, is the said adjoint matrix, is the coordinate transformation matrix of the underwater robot, is the link velocity of the i-th link, is the rotation coordinate of the i-th joint, is the i-th joint angle; Obtain the inverse kinematic equation through the connecting rod velocities; The obtaining of the dynamic equation according to the kinematic equation includes: Obtain the connecting rod kinetic energy equation according to the kinematic equation; Wherein, the connecting rod kinetic energy equation is: ， wherein, is the inertia matrix of link i in the link coordinate system, is the kinetic energy of the link, is the inertia matrix of link i expressed relative to the base, is the transpose matrix of, is the Jacobian matrix, is the generalized velocity vector of the base, is the joint angle, is the generalized velocity vector; Obtain the mass matrix according to the connecting rod kinetic energy equation; Obtain the dynamic equation through the mass matrix.

2. The method for constructing a model of a snake-shaped underwater robot according to claim 1, wherein The obtaining of the dynamic equation through the mass matrix includes: Obtain the dynamic equation through the mass matrix; Wherein, the dynamic equation is: ， wherein, is the mass matrix, is the Coriolis force matrix, is the influence of hydrodynamic damping, is the generalized hydrostatic force, is the derivative of, is the generalized velocity vector of the base, is the commanded force and torque.

3. The method for constructing a model of the snake-shaped underwater robot according to claim 2, characterized in that, The commanded forces and torques include joint torques and thruster torques; Wherein, the commanded forces and torques are: ， Among them, is 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, is the thrust configuration matrix, with m thrusters and n joints.

4. The method for constructing a model of a snake-shaped underwater robot according to claim 1, wherein, The obtaining of the connecting rod velocities in each coordinate system based on differential kinematics according to the adjoint matrix includes: Based on differential kinematics, obtain the connecting rod velocities in each coordinate system according to the adjoint matrix; The adjoint matrix is: ， Among them, is the adjoint matrix, is the skew-symmetric 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.

5. The method for constructing a model of the snake-shaped underwater robot according to claim 1, wherein The obtaining of the mass matrix according to the connecting rod kinetic energy equation includes: Obtain the mass matrix according to the connecting rod kinetic energy equation; The mass matrix is: ， wherein, 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.

6. A control method for a snake-shaped underwater robot, characterized in that, Includes: Obtain the snake-shaped underwater robot model according to the method for constructing the model of the snake-shaped underwater robot according to any one of claims 1 to 5; Control the snake-shaped underwater robot according to the snake-shaped underwater robot model.

7. An electronic device, characterized in that, Includes a memory and a processor; The memory is used for storing a computer program; The processor is used for, when executing the computer program, implementing the method for constructing the model of the snake-shaped underwater robot according to any one of claims 1 to 5 or the control method of the snake-shaped underwater robot according to claim 6.

8. A computer-readable storage medium, characterized in that, A computer program is stored on the storage medium, and when the computer program is executed by a processor, it implements the model construction method of the snake-shaped underwater robot according to any one of claims 1 to 5 or the control method of the snake-shaped underwater robot according to claim 6.

Images