Underwater snakelike robot and three-dimensional dynamic modeling method and control method thereof

Abstract

The invention provides an underwater snakelike robot and a three-dimensional dynamic modeling method and a control method thereof. The underwater snakelike robot comprises a head joint, a plurality of orthogonal joints and a tail joint and is suitable for the kinetic model. The three-dimensional dynamic modeling method comprises the following steps: acquiring initial state information and model parameters; constructing a kinematic equation according to the initial state of the snakelike robot; constructing a water resistance equation according to the model parameters and a kinematic equation; and constructing a kinetic equation according to the kinematic equation and the water resistance equation. The control method comprises a control method constructed based on the dynamical model and a control method of the exponential spiral gait, is suitable for the underwater snakelike robot and the dynamical model, and has a better movement effect. The invention belongs to the field of underwater robots, mainly solves the problems of simulation and control of three-dimensional motion of an underwater snake-shaped robot, improves the precision of motion control, and widens the three-dimensional motion form of the underwater snake-shaped robot.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of underwater snake-like robots, in particular to an underwater snake-like robot and a three-dimensional dynamic modeling method and control method thereof. BACKGROUND

[0002] Underwater snake-like robots can achieve various types of motion due to their slender body and high flexibility. Therefore, they can move in various narrow environments, which also makes them have high value in the inspection of offshore oil and the exploration of sunken ships. However, in the current research, underwater snake-like robots often focus on two-dimensional plane models due to their more flexible motion space and more complex force conditions. In order to expand the research of underwater snake-like robots to three dimensions, a three-dimensional dynamic modeling method of underwater snake-like robots is needed to make the three-dimensional motion simulation and control function of underwater snake-like robots more accurate and perfect, and a new three-dimensional gait is also needed to expand the three-dimensional motion form of snake-like robots. SUMMARY

[0003] In order to solve the above-mentioned simulation and control problem of three-dimensional motion of underwater snake-like robots, the present application provides an underwater snake-like robot and a three-dimensional dynamic modeling method and control method thereof.

[0004] In the first aspect, the present application provides a three-dimensional dynamic modeling method of an underwater snake-like robot, wherein the underwater snake-like robot is a snake-like robot that can be simplified as a multi-link mechanism, including a spherical hinge linked snake-like robot and an orthogonal connected snake-like robot, and the snake-like robot in the second aspect of the present application is an example satisfying the model. The three-dimensional dynamic modeling method of the underwater snake-like robot includes the following steps:

[0005] According to the length, mass, number of links, initial attitude and other parameters of the underwater snake-like robot, the kinematic equation of the underwater snake-like robot is determined;

[0006] According to the kinematic equation and water resistance parameter of the underwater snake-like robot, the water resistance equation of the underwater snake-like robot is determined;

[0007] The dynamic equation is obtained according to the kinematic equation and the water resistance equation.

[0008] Optionally, the determination of the kinematic equation of the underwater snake-like robot according to the length, mass, number of links, initial attitude and other parameters of the underwater snake-like robot includes:

[0009] The position relationship between the center of mass of each link and the overall center of mass is obtained based on the model connection relationship;

[0010] According to the position relationship between the mass centers of the rods and the overall mass center, the velocity relationship and the acceleration relationship between the mass centers of the rods and the overall mass center are obtained based on differential kinematics;

[0011] According to the position relationship between the mass centers of the rods and the joint angles, the position relationship between the mass centers of the rods and the joint angles is obtained based on the model connection relationship;

[0012] According to the position relationship between the mass centers of the rods and the joint angles, the velocity and acceleration relationship between the mass centers of the rods and the joint angles is obtained based on differential dynamics.

[0013] For the kinematic equation, optionally, the position relationship between the mass centers of the rods and the overall mass center is:

[0014]

[0015] wherein, is a vector composed of the positions of the mass centers of the rods of the snake robot in the global coordinate system, , , and are summation and difference matrices commonly used in the field of snake robot models, is a diagonal matrix composed of , is a vector composed of the yaw link angles, is a vector composed of the pitch link angles, is an N-dimensional column vector composed of 1, and N is the number of links, is the coordinate value of the overall mass center of the snake robot in the global coordinate system.

[0016] For the kinematic equation, optionally, the velocity relationship and the acceleration relationship between the mass centers of the rods and the overall mass center can be obtained by taking the first derivative and the second derivative of the position relationship between the mass centers of the rods and the overall mass center with respect to time t;

[0017] For the kinematic equation, optionally, the velocity and acceleration relationship between the mass centers of the rods and the joint angles is:

[0018]

[0019] For the kinematic equation, optionally, the velocity and acceleration relationship between the mass centers of the rods and the joint angles can be obtained by taking the first derivative and the second derivative of the position relationship between the mass centers of the rods and the joint angles with respect to time t.

[0020] Optionally, the water resistance equation obtained according to the kinematic parameters and the water resistance parameters comprises:

[0021] A water resistance expression of each rod obtained based on single rod force analysis;

[0022] The water resistance moment expression of each rod is derived based on single rod force analysis.

[0023] For the water resistance equation, the water resistance expression of each rod is:

[0024]

[0025] For the water resistance expression, wherein, The term is the added mass force, and its expression is:

[0026]

[0027] wherein, , and the derivatives of other positions are the corresponding velocities and angular velocities, which will not be described later. , , is the ocean current velocity matrix in the X, Y, and Z directions. is the acceleration resistance coefficient, is the yaw angular velocity resistance coefficient, is the pitch angular velocity resistance coefficient, and the remaining symbols are symbols that have appeared before and will not be described again, and the same applies later.

[0028] For the water resistance expression, wherein, The term is the linear water resistance, and its expression is:

[0029]

[0030] wherein, , , are the X, Y, and Z direction water resistance coefficients, , , is the column vector of the X, Y, and Z direction ocean current velocities, , , are the direction coefficient matrices of the X, Y, and Z direction water resistance coefficients, and specifically:

[0031]

[0032]

[0033]

[0034] For the water resistance expression, wherein, The term is the nonlinear water resistance, and its expression is:

[0035]

[0036] wherein, , , is a relative velocity in a relative coordinate system, and specifically is:

[0037]

[0038] For the water resistance equation, optionally, the water resistance moment expression of each rod is:

[0039]

[0040] wherein, is a yaw angle plane moment, is a pitch angle plane moment, , , , , , is a resistance parameter due to pressure difference.

[0041] Optionally, the obtaining the dynamic equation according to the kinematic equation and the water resistance equation comprises:

[0042] a force balance equation based on the kinematic equation and the water resistance equation;

[0043] a moment balance equation based on the kinematic equation and the water resistance equation.

[0044] For the dynamic equation, optionally, the force balance equation based on the kinematic equation and the water resistance equation is:

[0045]

[0046] wherein, is a dynamic acceleration coefficient matrix, is a block diagonal matrix with as a diagonal block, , , is a water resistance term composed of a linear resistance term and a nonlinear resistance term. The right half of the equation can be replaced by the symbol . The specific expression of is:

[0047]

[0048] wherein, is a mass matrix, is a diagonal matrix with elements .

[0049] Optionally, for the dynamic equations, the torque balance equations obtained based on the kinematic equations and the water resistance equations are:

[0050]

[0051] in, and The input torque represents the yaw and pitch angles. , , The interaction forces between the two joints in the X, Y, and Z directions are expressed as follows:

[0052]

[0053] in, , , Representing the aforementioned matrices respectively , , The first row forms the row vector, and the remaining terms are similarly derived; thus, the complete dynamic equations are obtained.

[0054] Secondly, the present invention provides a control method for a snake-shaped underwater robot, comprising:

[0055] Based on the underwater snake robot model construction method in the first aspect, the kinematic model, water resistance model, and dynamic model of the underwater snake robot are obtained;

[0056] Calculate the required parameters based on any model expression in the kinematic model, water resistance model, and dynamic model of the underwater snake robot.

[0057] Based on the required parameters, relevant control equations are constructed to control the underwater snake robot.

[0058] Thirdly, the present invention provides an orthogonal joint underwater snake robot model that satisfies the above-mentioned dynamic model, comprising: a snake robot head joint (1), a snake robot orthogonal joint (2), and a snake robot tail joint (3); wherein:

[0059] The snake-like robot head joint (1) mainly includes a head joint end cap (1) and a head joint base (2).

[0060] The orthogonal joint (2) of the snake robot mainly includes an orthogonal joint end cap (201), an orthogonal joint coupling one (202), an orthogonal joint base (203), an orthogonal joint coupling two (204), an orthogonal joint heat sink (205), and an orthogonal joint motor (206).

[0061] The snake-like robot's tail joint (3) mainly includes a tail joint end cap (301), a tail joint coupling one (302), a tail joint base (303), a tail joint coupling two (304), a tail joint heat sink (305), and a tail joint motor (306).

[0062] Fourthly, this invention provides a control method for the exponential spiral motion gait of an underwater snake-like robot applicable to the dynamic model of the first aspect and the second aspect. By adjusting the rotation angle of the servo motor to the following functional equation, the robot can achieve exponential spiral motion:

[0063]

[0064] in For the first One joint angle, For the first Yaw reference joint angle For the first One pitch reference joint angle, The amplitude of the swing. Let be the angular frequency of the oscillation. The phase difference of each reference joint angle, The turning angle coefficient is the yaw direction. This is the turning angle coefficient in the pitch direction.

[0065] The beneficial effects of this invention are:

[0066] 1. It solves the problem of establishing a three-dimensional motion model for underwater snake robots, enabling three-dimensional dynamic simulation of underwater snake robots, and thus more clearly predicting and analyzing the motion of actual underwater snake robots;

[0067] 2. A three-dimensional dynamic model is provided for the motion control of underwater snake robots, which improves the control accuracy based on the dynamic model and broadens the control methods that can be used for underwater snake robots;

[0068] 3. The underwater snake robot model proposed in this invention is suitable for this model, which improves the practical value of the dynamic modeling method;

[0069] 4. The present invention proposes a robot gait suitable for the dynamic model and the underwater snake robot model to realize the three-dimensional movement of the underwater snake robot, which has better movement efficiency than the conventional two-dimensional gait. Attached Figure Description

[0070] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0071] Figure 1 This is a flowchart of a dynamic modeling process according to an embodiment of the present invention;

[0072] Figure 2 This is a simulation flowchart of a snake robot according to an embodiment of the present invention;

[0073] Figure 3 A schematic diagram of the overall assembly of the underwater snake-like robot;

[0074] Figure 4 This is a schematic diagram of the head joint assembly;

[0075] Figure 5 This is a schematic diagram of an orthogonal joint assembly.

[0076] Figure 6 This is a cross-sectional view of the orthogonal joint assembly;

[0077] Figure 7 Schematic diagram of the tail joint assembly;

[0078] Figure 8 This is a cross-sectional view of the tail joint assembly.

[0079] The attached diagram lists the components represented by each number as follows:

[0080] 1-Head joint, 2-Orthogonal joint, 3-Tail joint, 101-Head joint end cap, 102-Head joint base, 201-Orthogonal joint end cap, 202-Orthogonal joint coupling one, 203-Orthogonal joint base, 203a-Cable outlet hole on orthogonal joint base, 204-Orthogonal joint coupling two, 205-Orthogonal joint heat sink, 206-Orthogonal joint motor, 301-Tail joint end cap, 302-Tail joint coupling one, 303-Tail joint base, 303a, Cable outlet hole on tail joint base, 304-Tail joint coupling two, 305-Tail joint heat sink, 306-Tail joint motor. Detailed Implementation

[0081] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below 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 set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present invention. It should be understood that the accompanying drawings and embodiments of the present invention are for illustrative purposes only and are not intended to limit the scope of protection of the present invention.

[0082] like Figure 2 As shown in the figure, an underwater snake robot dynamics simulation method provided by an embodiment of the present invention includes:

[0083] Step S100: Given the initial pose information, various robot parameters, water resistance parameters, and simulation parameters;

[0084] Specifically, the following parameters are required for the snake robot: link angles, joint angles, single joint length, number of joints, single joint mass, and major and minor axis lengths of the cross-sectional ellipse. Regarding water resistance parameters, the following parameters are required: liquid density, water resistance coefficients in the X, Y, and Z directions, water flow velocity, and added mass coefficients in the Y and Z directions. Regarding simulation parameters, the following parameters are required: total simulation duration, simulation interval, and reference joint angles.

[0085] Step S200: Based on the simulation time and simulation interval, the acceleration at the current moment is discretized and calculated using the dynamic model;

[0086] Specifically, calculations are performed starting from the initial pose, based on simulation intervals. First, the input joint torques are calculated using reference joint angles and a preset control scheme. Optionally, the control equations are:

[0087]

[0088] After obtaining the input torque, substituting it into the dynamic equations allows us to solve for the position acceleration and angular acceleration of each link at that moment. Optionally, the final solution expression for the dynamic equations is:

[0089]

[0090] Regarding positional acceleration, its correlation coefficient will be explained in the invention description. Regarding yaw angle acceleration... The solution coefficients are specifically:

[0091]

[0092] It should be noted that, and This is a summation and difference matrix commonly used in the field of snake robot modeling. , for The diagonal matrix formed by these elements is written similarly for other types. This is a vector composed of the yaw link angles. The vector consisting of the pitch linkage angles. This is an N-dimensional column vector composed of 1s, where N is the number of links. The above symbols are basic symbols in the field of snake robot dynamics and will not be explained further.

[0093] because and The main factor introduced is the pitch angle correlation coefficient, which will not be discussed here. However, the pitch angle acceleration... The solution coefficients are explained in the calculation. It should be noted that subscripts marked with a single number without separate declaration represent the row vector of the corresponding number in the matrix, such as... Reference The first row forms the row vector; the elements marked with two numbers in the table below are the elements at the corresponding positions in the matrix, and so on.

[0094] The additional coefficients introduced in the above coefficients are specifically as follows:

[0095]

[0096]

[0097] Regarding pitch acceleration The solution coefficients are:

[0098]

[0099] This is for and The median coefficient is as follows:

[0100]

[0101] This is for The median coefficient is as follows:

[0102]

[0103]

[0104] The coefficients for solving position acceleration are:

[0105]

[0106]

[0107]

[0108]

[0109]

[0110]

[0111]

[0112]

[0113]

[0114]

[0115]

[0116] For acceleration drag coefficient Yaw angular velocity drag coefficient Pitch angular velocity drag coefficient ,in Add a mass coefficient in the Y direction. Add a mass coefficient in the Z direction. For It is a block diagonal matrix of diagonal blocks.

[0117] Based on all the above coefficients, the position acceleration and angular acceleration can be calculated, so step S200 is completed.

[0118] Step S300: Update the robot's pose for the next moment based on the acceleration results and kinematic equations;

[0119] Specifically, its kinematic update logic is as follows:

[0120]

[0121] in, , and Represents the various types of acceleration, velocity, and displacement at the current moment. and Represents the various types of velocity and position at the next moment. This represents a time interval. Using the updated position and velocity information, the pose at the next moment can be obtained.

[0122] In step S400, determine whether the next moment is the simulation end time. If so, end the simulation and obtain the poses corresponding to the discrete points of all simulation times, which can then be used to assemble a motion animation. Otherwise, return to step S200 and continue iteratively calculating the acceleration at the next moment.

[0123] In some more specific embodiments, the control of the snake robot can be achieved based on the above-described dynamic model;

[0124] Optionally, through the above snake robot simulation process, the input torque corresponding to discrete points of all simulation times can be obtained, and then fabricated using 3D printing. Figure 3 By inputting stress torques onto each joint of the snake-like robot shown, control of the snake-like robot can be achieved.

[0125] In some more specific embodiments, based on the above dynamic model, it is possible to design and manufacture, such as Figure 3 The underwater snake-like robot shown includes: a head joint (1), an orthogonal joint (2), and a tail joint (3); wherein:

[0126] The snake-like robot head joint (1) mainly includes a head joint end cap (1) and a head joint base (2).

[0127] The snake-shaped robot head joint base (2) has a large enough space to store robot control-related equipment; a sealing ring is sandwiched between the head joint base and the head joint end cap and is fastened with screws to prevent water damage.

[0128] The orthogonal joint (2) of the snake robot mainly includes an orthogonal joint end cap (201), an orthogonal joint coupling one (202), an orthogonal joint base (203), an orthogonal joint coupling two (204), an orthogonal joint heat sink (205), and an orthogonal joint motor (206).

[0129] A sealing ring is sandwiched between the orthogonal joint end cap (201) and the orthogonal joint base (203) and secured with screws to prevent water leakage. The orthogonal joint heat sink is in close contact with the motor of the orthogonal joint, with the two sides of the heat sink contacting the motor and water respectively to help dissipate heat from the motor.

[0130] The snake-like robot's tail joint (3) mainly includes a tail joint end cap (301), a tail joint coupling one (302), a tail joint base (303), a tail joint coupling two (304), a tail joint heat sink (305), and a tail joint motor (306).

[0131] A sealing ring is sandwiched between the tail joint end cap (201) and the tail joint base (203) and secured with screws to prevent water leakage. The tail joint heat sink is in close contact with the motor of the orthogonal joint, with the two sides of the heat sink contacting the motor and water respectively to help dissipate heat from the motor.

[0132] In some more specific embodiments, a novel helical gait for a snake robot can be achieved based on the above-described dynamics model;

[0133] Optionally, in the above simulation control, by setting the reference joint angle to the reference joint angle described in the invention, the input torque to achieve the gait can be obtained, thereby controlling the snake robot to move in a new exponential spiral gait.

[0134] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the technical solutions of the embodiments of the present invention.

Claims

1. A three-dimensional dynamic modeling method for an underwater snake-like robot, characterized in that: Based on parameters such as the underwater snake robot's length, mass, number of links, and initial posture, its kinematic equations are determined. Based on the kinematic equations and water resistance parameters of the underwater snake robot, its water resistance equation is determined; The dynamic equations are derived from the kinematic equations and the hydrodynamic equations. The process of determining the kinematic equations of the underwater snake robot based on its length, mass, number of links, initial posture, and other parameters includes: The positional relationship between the centroids of each member and the overall centroid is obtained based on the model connection relationship; Based on differential kinematics, the velocity and acceleration relationships between the centers of mass of each link and the overall center of mass are obtained according to the positional relationship between the centers of mass of each link and the overall center of mass. The positional relationships of the centroids and joint angles of each member are obtained based on the model connection relationships; Based on differential dynamics, the velocity and acceleration relationship between the center of mass and joint angle of each member is obtained according to the positional relationship between the center of mass and joint angle of each member. For the kinematic equations, the positional relationship between the centers of mass of each member and the overall center of mass is as follows: ； in, Let be the vector composed of the positions of the centroids of each link of the snake robot in the global coordinate system. , , and This is a summation and difference matrix commonly used in the field of snake robot modeling. for The diagonal matrix formed This is a vector composed of the yaw link angles. The vector consisting of the pitch linkage angles. Let N be an N-dimensional column vector consisting of 1s, where N is the number of links. Let be the coordinates of the center of mass of the snake robot in the global coordinate system; For the kinematic equations, the velocity and acceleration relationships between the center of mass of each member and the center of mass of the whole can be obtained by taking the first and second derivatives of the positional relationship between the center of mass of each member and the center of mass of the whole with respect to time t. For the kinematic equations, the relationship between the velocity and acceleration of the center of mass and joint angle of each member is as follows: ； For the kinematic equations, the relationship between the velocity and acceleration of the center of mass and joint angle of each member can be obtained by taking the first and second derivatives of the positional relationship of the center of mass and joint angle of each member with respect to time t.

2. The three-dimensional dynamic modeling method for underwater snake-like robots according to claim 1, characterized in that, The process of deriving the water resistance equation based on the kinematic parameters and water resistance parameters includes: Expressions for the water resistance of each member derived from the force analysis of a single member; Expressions for the water resistance torque of each member derived from the force analysis of a single member; For the water resistance equation, the expressions for the water resistance of each member are as follows: ； For the expression of water resistance, where, The term represents the additional mass force, and its expression is: ； in, , The derivatives at other positions correspond to the velocities and angular velocities, which will not be explained further. , , This is the diagonal matrix of ocean current velocities in the X, Y, and Z directions. The acceleration drag coefficient, The drag coefficient is the yaw rate. This represents the pitch angular velocity drag coefficient. The other symbols have appeared previously and will not be explained again. The same applies to subsequent symbols. For the expression of water resistance, where, The term represents linear water resistance, and its expression is: ； in, , , These are the water resistance coefficients in the X, Y, and Z directions, respectively. , , Let X be the column vector of ocean current velocities in the X, Y, and Z directions. , , These are the direction coefficient matrices for the water resistance coefficients in the X, Y, and Z directions, respectively. For the expression of water resistance, where, The term represents nonlinear water resistance, and its expression is: ； in, , , The relative velocity in the relative coordinate system is as follows: ； For the water resistance equation, the expressions for the water resistance moments of each member are as follows: ； in, The yaw angle plane moment, The pitch angle plane moment, , , , , , This is the resistance parameter caused by the pressure difference.

3. The three-dimensional dynamic modeling method for underwater snake-like robots according to claim 1, characterized in that, The dynamic equations derived from the kinematic equations and the water resistance equations include: Force balance equations derived from kinematic equations and water resistance equations; The torque balance equation is derived from the kinematic equation and the water resistance equation. For the dynamic equations, the force balance equations derived from the kinematic equations and the water resistance equations are as follows: ； in, This is the dynamic acceleration coefficient matrix. For This is a block diagonal matrix of diagonal blocks. , , This is the water resistance term, which is the sum of linear and nonlinear resistance terms. The latter half of the right-hand side of the equation can be represented by the symbol... To replace it. The specific expression is: ； in, For the quality matrix, For elements a diagonal matrix; For the dynamic equations, the torque balance equations derived from the kinematic equations and the water resistance equations are as follows: ； in, and The input torque represents the yaw and pitch angles. , , The interaction forces between the two joints in the X, Y, and Z directions are expressed as follows: ； ； in, , , Representing the aforementioned matrices respectively , , The first row forms the row vector, and the remaining terms are similarly derived; thus, the complete dynamic equations are obtained.

4. A control method for an underwater snake-like robot, characterized in that, include: According to any one of the underwater snake robot model construction methods described in claims 1 to 3, the kinematic model, water resistance model, and dynamic model of the underwater snake robot are obtained; Calculate the required parameters based on any model expression in the kinematic model, water resistance model, and dynamic model of the underwater snake robot. Based on the required parameters, relevant control equations are constructed to control the underwater snake robot.

5. An underwater snake-like robot, characterized in that, include: The snake robot has a head joint (1), an orthogonal joint (2), and a tail joint (3); among which: The snake-like robot head joint (1) mainly includes a head joint end cap (1) and a head joint base (2). The orthogonal joint (2) of the snake robot mainly includes an orthogonal joint end cap (201), an orthogonal joint coupling one (202), an orthogonal joint base (203), an orthogonal joint coupling two (204), an orthogonal joint heat sink (205), and an orthogonal joint motor (206). The snake-like robot's tail joint (3) mainly includes a tail joint end cap (301), a tail joint coupling one (302), a tail joint base (303), a tail joint coupling two (304), a tail joint heat sink (305), and a tail joint motor (306).

6. The control method for an underwater snake-like robot according to claim 4, characterized in that: The underwater snake-like robot exhibits an exponential spiral gait, with the servo motor rotation angle function being: 。

Images