Drilling machine cylindrical coordinate manipulator modeling and motion control simulation method

Abstract

The invention relates to a drilling machine cylindrical coordinate manipulator modeling and motion control simulation method, which comprises the following steps: constructing a connecting rod coordinate system of a drilling machine cylindrical coordinate manipulator according to a D-H coordinate system modeling method, acquiring connecting rod parameters and constructing a manipulator model; according to the connecting rod parameters, constructing a forward kinematics model and an inverse kinematics model of the cylindrical coordinate manipulator of the drilling machine; according to the forward kinematics model and the inverse kinematics model, the pose of the manipulator is obtained, and a dynamical model of the cylindrical coordinate manipulator of the drilling machine is constructed in combination with a Lagrange method; and performing manipulator motion control simulation according to the manipulator model, the kinematic model and the dynamical model. According to the method, on the basis of solving the kinematics and dynamics models of the cylindrical coordinate manipulator, the kinematics characteristics of the manipulator are subjected to simulation analysis, and the simulation model of the manipulator is established. And then, a theoretical basis is provided for subsequent high-precision control of the manipulator and design verification, control algorithm development and performance optimization of the cylindrical coordinate manipulator of the drilling machine.

Description

Technical Field

[0001] The present invention relates to the technical field of modeling and simulation of deep - sea operation manipulators, and particularly to a method for modeling and motion control simulation of a cylindrical - coordinate manipulator for a drilling rig. Background Technique

[0002] In recent years, the technology of deep - sea underwater equipment has developed rapidly, and the exploration and development of deep - sea resources have also been accelerated. Through means such as deep - sea exploration and sampling, accurate and comprehensive seabed data and samples can be obtained, providing a scientific basis for resource exploration and development, and having irreplaceable significance for the exploration and utilization of seabed resources. Among them, the deep - sea seabed drilling rig is an important part of deep - sea underwater exploration technology equipment. The seabed drilling rig integrates functions such as power, communication, control, and drilling, with high integration and complexity. With its unique design and technical and economic advantages, it has broad application prospects in future modern ocean engineering.

[0003] As a key component in the drilling rig system, the manipulator not only improves the operation flexibility of the equipment but also enhances the adaptability and reliability of the system. The deep - sea environment is complex and changeable, and traditional fixed tools are difficult to meet the requirements of precise operations. With its flexible joint structure and precise control ability, the manipulator can perform complex operations in a narrow space, thus ensuring the accuracy of drilling work. Facing the high - pressure and complex deep - sea environment, the manipulator can simulate the actions of a human hand to complete tasks such as grasping, transporting, and installing drill pipes, enabling the seabed drilling rig to maintain an efficient operation state.

[0004] The cylindrical - coordinate manipulator is an important link to ensure the efficient execution of exploration and sampling tasks by the seabed drilling rig. Such manipulators are usually composed of rotary and translational joints, and the operating environment is complex, which poses challenges to the control of the manipulator. Summary of the Invention

[0005] The purpose of the present invention is to provide a method for modeling and motion control simulation of a cylindrical - coordinate manipulator for a drilling rig, aiming to perform simulation based on the modeling of the manipulator, thereby providing support for control, and providing a theoretical basis for the design verification, control algorithm development, and performance optimization of the cylindrical - coordinate manipulator of the seabed drilling rig, ensuring its adaptability and flexibility under different operating conditions.

[0006] To achieve the above - mentioned purpose, the present invention provides the following solutions:

[0007] A method for modeling and motion control simulation of a cylindrical - coordinate manipulator for a drilling rig, including:

[0008] According to the D - H coordinate system modeling method, construct the link coordinate system of the cylindrical - coordinate manipulator of the drilling rig, obtain the link parameters, and construct the manipulator model;

[0009] Construct the forward kinematic model and inverse kinematic model of the drilling rig cylindrical coordinate manipulator according to the link parameters;

[0010] According to the forward kinematic model and the inverse kinematic model, obtain the pose of the manipulator, and construct the dynamic model of the drilling rig cylindrical coordinate manipulator by combining the Lagrangian method;

[0011] Conduct manipulator motion control simulation according to the manipulator model, the forward kinematic model, the inverse kinematic model and the dynamic model.

[0012] Optionally, the link parameters include: the length of the link, the twist angle of the link, the distance between adjacent links, and the angle between adjacent links.

[0013] Optionally, constructing the forward kinematic model and inverse kinematic model of the drilling rig cylindrical coordinate manipulator includes:

[0014] Obtain the link transformation matrix according to the link parameters;

[0015] Obtain the forward kinematic model according to the link transformation matrix;

[0016] Obtain the solution of the inverse kinematic problem of the drilling rig cylindrical coordinate manipulator according to the inverse transformation and the link transformation matrix;

[0017] Obtain the inverse kinematic model according to the solution of the inverse kinematic problem of the drilling rig cylindrical coordinate manipulator.

[0018] Optionally, constructing the dynamic model of the drilling rig cylindrical coordinate manipulator includes:

[0019] Obtain the dimension of the Jacobian matrix according to the degrees of freedom of the drilling rig cylindrical coordinate manipulator;

[0020] Obtain the Jacobian matrix according to the dimension of the Jacobian matrix, combining the geometric parameters of the manipulator and the pose of the manipulator;

[0021] Obtain the Jacobian matrix of the linear velocity of the end effector according to the Jacobian matrix;

[0022] According to the Jacobian matrix of the linear velocity of the end effector, obtain the velocity of the joint and the velocity of the end effector, convert the joint variables into the position coordinates of the end effector, and obtain the kinetic energy and potential energy according to the velocity and position coordinates;

[0023] Obtain the Lagrangian dynamic equation according to the kinetic energy and potential energy;

[0024] Perform partial derivative calculation according to the Lagrangian dynamic equation and the motion equation to obtain the torque equation of each link of the manipulator; according to the torque equation, obtain the dynamic model of the drilling rig cylindrical coordinate manipulator.

[0025] Optionally, the Jacobian matrix of the linear velocity of the end effector is:

[0026]

[0027] where J is the Jacobian matrix of the linear velocity of the end effector, θ1 is the rotation angle directly generated by the hydraulic motor, d3 is the horizontal displacement of the manipulator, and o0, z1, and z2 are all matrix codes.

[0028] Optionally, the Lagrangian dynamics equation is:

[0029]

[0030] where T represents kinetic energy, U represents potential energy, q i is the generalized coordinate, τ i is the force / moment acting on the i-th coordinate system, is the joint angular velocity.

[0031] Optionally, the manipulator motion control simulation according to the manipulator model, the forward kinematic model, the inverse kinematic model, and the dynamic model includes:

[0032] According to the forward kinematic model and the inverse kinematic model, combined with the dynamic model, generate the motion trajectory of the manipulator, and solve the displacement, velocity, and acceleration of each joint of the manipulator during the motion process.

[0033] The beneficial effects of the present invention are as follows: The model of the manipulator is obtained by the D-H coordinate system modeling method, and its forward and inverse kinematic models are deduced. According to the joint parameters of the manipulator, the pose of the end effector frame is given. Finally, using the MATLAB Robotics Toolbox tool to demonstrate and analyze its pose and motion trajectory, the motion of the manipulator can be intuitively observed, so as to optimize the motion control of the manipulator. It can track and analyze the manipulator trajectory in real time, providing a comprehensive method for the dynamic control of the cylindrical coordinate manipulator of the drilling rig. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0035] Figure 1 It is a schematic three-dimensional model diagram of the subsea drilling rig cylindrical coordinate manipulator according to the embodiment of the present invention;

[0036] Figure 2 Schematic diagram of the manipulator in an embodiment of the present invention;

[0037] Figure 3 Schematic diagram of the cylindrical coordinate system in an embodiment of the present invention;

[0038] Figure 4 Three-dimensional schematic diagram and top view of the working space of the manipulator in an embodiment of the present invention;

[0039] Figure 5 Schematic diagram of the movement of the manipulator in an embodiment of the present invention;

[0040] Figure 6 Schematic diagram of the reference coordinate system of the cylindrical coordinate manipulator in an embodiment of the present invention;

[0041] Figure 7 Forward and inverse kinematics flowcharts in an embodiment of the present invention;

[0042] Figure 8 Schematic diagram of the manipulator model in an embodiment of the present invention;

[0043] Figure 9 Schematic diagram of the state of the manipulator after joint movement in an embodiment of the present invention;

[0044] Figure 10 Schematic diagram of the three-dimensional motion trajectory of the manipulator in an embodiment of the present invention;

[0045] Figure 11 Displacement, velocity, and acceleration curves of each joint movement process of the manipulator in an embodiment of the present invention, where (a) is the displacement curve of the manipulator joint 1 movement process, (b) is the displacement curve of the manipulator joint 2 movement process, (c) is the displacement curve of the manipulator joint 3 movement process, (d) is the velocity curve of the manipulator joint 1 movement process, (e) is the velocity curve of the manipulator joint 2 movement process, (f) is the velocity curve of the manipulator joint 3 movement process, (g) is the acceleration curve of the manipulator joint 1 movement process, (h) is the acceleration curve of the manipulator joint 2 movement process, and (i) is the acceleration curve of the manipulator joint 3 movement process;

[0046] Figure 12 Flowchart of a method for modeling and motion control simulation of a cylindrical coordinate manipulator of a drilling rig in an embodiment of the present invention. Detailed implementation manners

[0047] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0048] To make the above objects, features, and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0049] As Figure 12 shown, this embodiment provides a modeling and motion control simulation method for a drilling rig cylindrical coordinate manipulator, including:

[0050] According to the D-H coordinate system modeling method, construct the link coordinate system of the drilling rig cylindrical coordinate manipulator, obtain the link parameters, and construct the manipulator model;

[0051] According to the link parameters, construct the forward kinematics model and inverse kinematics model of the drilling rig cylindrical coordinate manipulator;

[0052] According to the forward kinematics model and inverse kinematics model, obtain the pose of the manipulator, and combine the Lagrangian method to construct the dynamics model of the drilling rig cylindrical coordinate manipulator;

[0053] Conduct manipulator motion control simulation according to the manipulator model, forward kinematics model, inverse kinematics model, and dynamics model.

[0054] Furthermore, the link parameters include: the length of the link, the twist angle of the link, the distance between adjacent links, and the angle between adjacent links.

[0055] Furthermore, constructing the forward kinematics model and inverse kinematics model of the drilling rig cylindrical coordinate manipulator includes:

[0056] According to the link parameters, obtain the link transformation matrix;

[0057] According to the link transformation matrix, obtain the forward kinematics model;

[0058] According to the inverse transformation and the link transformation matrix, obtain the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator;

[0059] According to the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator, obtain the inverse kinematics model.

[0060] Furthermore, constructing the dynamics model of the drilling rig cylindrical coordinate manipulator includes:

[0061] According to the degrees of freedom of the drilling rig cylindrical coordinate manipulator, obtain the dimension of the Jacobian matrix;

[0062] According to the dimension of the Jacobian matrix, combined with the geometric parameters of the manipulator and the pose of the manipulator, the Jacobian matrix is obtained;

[0063] According to the Jacobian matrix, the Jacobian matrix of the linear velocity of the end effector is obtained;

[0064] According to the Jacobian matrix of the linear velocity of the end effector, the velocity of the joint and the velocity of the end effector are obtained, the joint variables are converted into the position coordinates of the end effector, and the kinetic energy and potential energy are obtained according to the velocity and position coordinates;

[0065] According to the kinetic energy and potential energy, the Lagrangian dynamics equation is obtained;

[0066] According to the Lagrangian dynamics equation, partial derivative calculation is carried out to obtain the torque equation of each link of the manipulator;

[0067] According to the torque equation, the dynamic model of the manipulator of the drill string cylindrical coordinate is obtained.

[0068] Furthermore, the Jacobian matrix of the linear velocity of the end effector is:

[0069]

[0070] where J is the Jacobian matrix of the linear velocity of the end effector.

[0071] Furthermore, the Lagrangian equation of the manipulator is:

[0072]

[0073] where T represents kinetic energy, U represents potential energy, and q i is the generalized coordinate.

[0074] Furthermore, the manipulator motion control simulation based on the manipulator model, forward kinematic model, inverse kinematic model and dynamic model includes:

[0075] According to the forward kinematic model and the inverse kinematic model, combined with the dynamic model, the motion trajectory of the manipulator is generated, and the displacement, velocity and acceleration of each joint of the manipulator during the motion are solved.

[0076] The following further explains this embodiment with reference to the accompanying drawings:

[0077] 1. Manipulator cylindrical coordinate system and D-H modeling:

[0078] The manipulator of the subsea drill string cylindrical coordinate is an important component for the subsea drill to complete the grasping and drilling actions of drill pipes and drilling tools. The three-dimensional model is as Figure 1 shown, and the abstract simplified structure diagram is as Figure 2As shown in the figure. A spatial rectangular coordinate system and a cylindrical coordinate system are established with the vertical main axis of the manipulator as the Z-axis as follows Figure 3 As shown in the figure. Assume that within the cylindrical space, the end effector of the manipulator moves at point P. Let (r, θ) be the polar coordinates of the projection of point P on the XOY plane, and z be the vertical coordinate of point P. Then (r, θ, z) is called the cylindrical coordinate of point P, denoted as P(r, θ, z). Then the position in the spatial rectangular coordinate system is transformed into the cylindrical coordinate system as shown in Equation (1).

[0079]

[0080] Among them, r is the radius vector, which is the distance between the projection of point P on the XOY plane and the origin, representing the distance of point P from the main axis of the manipulator. r max is the maximum radius vector; the azimuth angle θ is exactly the angle between the projection and the X-axis, θ ∈ [0, 2π], and θ x is the azimuth angle of the spatial rectangular coordinate; z represents the height. The cylindrical coordinate manipulator adopts a rotary joint and two translational joints in its structure. Its motion coordinate system is the cylindrical coordinate, which changes the first translational joint on the basis of the right-angle type into a rotary joint around the axis of the Z-axis, and its working space is a cylinder. The role of the cylindrical coordinate system is to establish the cylindrical coordinate system of the drilling rig cylindrical coordinate manipulator, that is, to describe the space of the entire manipulator's motion.

[0081] Assume that the working range of the rotation angle of a certain three-degree-of-freedom cylindrical coordinate system manipulator is [0, 360°]. Due to the influence of its own column, the adjustable range of the radius r length is [r2, r1], and the adjustable range in the vertical direction is [0, d]. Then the three-dimensional schematic diagram and top view of its working space are as follows Figure 4 As shown in the figure.

[0082] The D-H coordinate system modeling method is the most widely used method in the kinematic modeling of manipulators. In order to describe the relative motion and pose relationship between each link using mathematical methods, a spatial coordinate system is fixedly connected to each link. The geometric dimensions of each link can be described by four parameters, and the meanings of these four parameters are as follows:

[0083] a represents the length of the link. In the link coordinate system, along the x i axis, the distance translated from the z i axis to the z i+1 axis.

[0084] α represents the twist angle of the link. In the link coordinate system, around the x i axis, the angle rotated from the z i axis to the z i+1 axis. The a and α parameters describe the characteristics of the link itself.

[0085] d represents the distance between two adjacent links, in the link coordinate system, along the z i axis, the translation distance from the x i axis to the x i+1 axis.

[0086] θ represents the angle between two adjacent links. In the link coordinate system, around the z i axis, the rotation angle from the x i axis to the x i+1 axis. These two parameters, d and θ, describe the relationship between adjacent links.

[0087] To measure the position and motion relationship between each link, a coordinate system must be determined on each link. By considering the relationship between each coordinate system, the position and motion of the actuator in the coordinate system are established. The steps to establish the link coordinate system according to the D-H standard method are as follows:

[0088] (1) Establish the first coordinate system (X0, Y0, Z0); this origin serves as the initial position of the centroid of the manipulator base. This step can obtain other position parameters of the reference point in space;

[0089] (2) Set the origin on the z0 axis and then select the x0 and y0 axes according to the right-hand rule;

[0090] (3) Locate the origin o i ;

[0091] (4) Along the common normal between z i-1 and z i and passing through o i establish x i ;

[0092] (5) Determine y i according to the right-hand coordinate system rule;

[0093] (6) Establish the end effector frame o n x n y n z n .

[0094] (7) Create the link parameters a i , d i , α i , θ i .

[0095] The motion schematic diagram of the manipulator is as shown in Figure 5 . θ1 is the rotation angle of the manipulator, d2 is the longitudinal movement distance of the manipulator, and d3 is the lateral movement distance of the manipulator. The coordinate system of each link is as shown in Figure 6As shown, the link coordinate system is as follows: The position of O0 along z0 and the direction of the x0 axis are arbitrary. When θ1 = 0, the x0 and x1 axes are perpendicular to the page. The z axes between z1 and z2 intersect, and o2 is at the intersection. The direction of x2 is chosen to be parallel to x1, so θ2 is zero. Finally, the third coordinate system is selected at the end of link 3.

[0096] 2. Forward and inverse kinematic modeling of the manipulator:

[0097] (1) Forward kinematic modeling:

[0098] All link mechanisms can be represented by the four parameters introduced earlier. As coordinate transformations of the coordinate system, a systematic analysis of the manipulator structure is required. The D-H standard modeling method represents the transformation of each link as shown in Equation (2):

[0099]

[0100] The values of the above four parameters can be summarized according to the established D-H coordinate system. Table 1 shows the values of all parameters.

[0101] Table 1

[0102] Joint <![CDATA[a i (mm)]]> <![CDATA[α i (°)]]> <![CDATA[d i (mm)]]> <![CDATA[θ i (°)]]> 1 0 0 <![CDATA[d1]]> <![CDATA[θ1*]]> 2 0 -90 <![CDATA[d2*]]> 0 3 0 0 <![CDATA[d3*]]> 0

[0103] The joints of the manipulator are driven by hydraulic motors and hydraulic cylinders. The hydraulic motor rotates and directly generates the rotation angle θ1, and the axial movement of the hydraulic cylinder generates (d2, d3). As can be seen from Table 1, θ1, d2, and d3 are variables when establishing the reference system of the robot. The homogeneous transformation between each two coordinate systems is analyzed. If values are assigned to each parameter, the robot will move and reach a certain position. The link parameters are shown in Table 1, and the corresponding T matrix is as follows:

[0104]

[0105] (a)

[0106]

[0107] (b)

[0108]

[0109] (c)

[0110]

[0111] (d)

[0112] The (d) in Equation (3) is the position sub-matrix of the cylindrical coordinate manipulator.

[0113] In the above link transformation matrices, s iDenote sinθ i , c i Denote cosθ i .

[0114] According to the theory of robotics engineering, describe (d) in Equation (3) as Equation (4):

[0115]

[0116] In the above formula, p is the position vector, the origin of the end effector coordinate system; a is the approach vector, the Z-axis of the end effector coordinate system; o is the orientation vector, the Y-axis of the end effector coordinate system; n is the normal vector, the X-axis of the end effector coordinate system. p x , p y , p z are the coordinates of the origin of the end effector coordinate system relative to the base coordinate system. Forward kinematic modeling refers to the process of calculating the pose of the end of the manipulator given the joint variables of the manipulator.

[0117] The pose of the end of the manipulator can be expressed as:

[0118]

[0119] (2) Inverse kinematic modeling:

[0120] The study of the forward kinematic problem can be used to solve whether the movement of the robot meets the conditions, and this study can also be used to construct the study of the inverse kinematics. The flowcharts of the forward kinematics and inverse kinematics of the manipulator are as Figure 7 shown.

[0121] The study of the inverse kinematic problem can understand whether the robot can achieve the desired movement and position. When the robot performs position control and trajectory planning or the position and pose of the hand have been declared, use this information to determine the rotation angle θ of the joint to drive the motors of each joint to adapt to the position and pose requirements of the hand. Therefore, the study of inverse kinematics is the measurement of the parameter θ, which is of great significance to the robot motion control system. The measurement of θ is the back-calculation operation of the above equations.

[0122] Multiply both sides of Equation (4) by the inverse transformation on the left to get:

[0123]

[0124] θ1 = Atan2(p x , p y ) (7)

[0125] d2 = p z - d1 (8)

[0126]

[0127] The above equation is the solution to the inverse kinematic problem of the drilling rig cylindrical coordinate manipulator. Based on the solution to the inverse kinematic problem of the drilling rig cylindrical coordinate manipulator, an inverse motion model is obtained to control the position of the manipulator and form the motion trajectory of the manipulator.

[0128] 3. Manipulator dynamics modeling method:

[0129] The relationship between the end effector of the manipulator and the joint velocities can be given by the Jacobian matrix, as Figure 5 shown. The cylindrical coordinate manipulator has the following coupling variables: q = (θ1, d2, d3). Since the manipulator has one rotational joint and two translational joints, i.e., three degrees of freedom, the dimension of the Jacobian matrix in this case is 6*3, and its form is:

[0130]

[0131] z0 = z1 = [0 0 1] T , o0 = [0 0 0] T , and the expressions of z2 and o3 are as follows:

[0132]

[0133] After derivation and calculation, the Jacobian matrix can be obtained as:

[0134]

[0135] The linear velocity of the end effector can be obtained by only considering the first three rows of the matrix. Therefore, the Jacobian matrix of the linear velocity of the end effector can be expressed as:

[0136]

[0137] where J is the Jacobian matrix of the linear velocity of the end effector, θ1 is the rotation angle directly generated by the hydraulic motor, d3 is the horizontal displacement of the manipulator, and o0, z1, and z2 are all matrix codes.

[0138] The dynamics of the manipulator can show the relationship between position, velocity, acceleration, and joint torque. Therefore, the dynamics modeling of the manipulator aims to understand the relationship between the motion of the manipulator and the forces it receives. The dynamics model of the manipulator can be obtained using the Lagrangian dynamics equation.

[0139] Solving the dynamic equation by Lagrange method requires calculating the kinetic energy and potential energy. For a manipulator with multiple joints, its kinetic energy is the sum of the kinetic energies of each joint, and the velocity of the joint is related to the velocity of the end effector through the Jacobian matrix. Therefore, the velocity of the end effector can be expressed as a function of the joint velocities with the help of the Jacobian matrix, and then the kinetic energy of the system can be calculated more conveniently. Through the Jacobian matrix, the joint variables can be converted into the position coordinates of the end effector, so as to calculate the potential energy of the manipulator system more accurately.

[0140] According to the state - space equation form of the robot dynamic equation, the standard dynamic equation of the manipulator in state - space can be expressed as:

[0141]

[0142] M(q) is the mass - inertia matrix of the manipulator. For a 3 - joint manipulator, this matrix is a 3×3 symmetric matrix, and the elements of the mass - inertia matrix depend on the robot joint angle q; is the centrifugal and Coriolis force vector. This term is related to the manipulator joint angle q and the joint angular velocity and is is the angular acceleration, G(q) is the gravity term of the manipulator, which is related to the joint angle q of each joint of the manipulator.

[0143] The Lagrange method analyzes the dynamics of the manipulator from the perspective of energy, and obtains the Lagrange dynamic equation by calculating the kinetic energy and potential energy of the manipulator.

[0144] The Lagrange equation (dynamic equation) of an n - degree - of - freedom manipulator can be expressed as:

[0145]

[0146] T represents the kinetic energy, U represents the potential energy. q i is the generalized coordinate, τ i is the force / moment acting on the i - th coordinate system, is the joint angular velocity. By taking the partial derivative of the Lagrange dynamic equation, the torque equations of each link of the manipulator are obtained. The final expression of the dynamic model is the dynamic equations of the torques of the three links.

[0147] The first dynamic equation of the link torque is as follows:

[0148]

[0149] m i is the mass of the i - th link, θ1 is the position of the rotating joint, is the velocity, is the acceleration, d3 is the axial moving distance, is the axial moving velocity, is the axial motion acceleration, is the centripetal acceleration.

[0150] Similarly, taking the partial derivative of the second joint of the manipulator, the torque of joint 2 is obtained, and the dynamic equation of joint 2 can be obtained as:

[0151]

[0152] d2 is the axial moving distance, is the axial motion velocity, is the axial motion acceleration.

[0153] The dynamic equation involves the description of the manipulator joint position, velocity, and acceleration. The dynamic equation of the third joint is:

[0154]

[0155] Among them, is the Coriolis acceleration.

[0156] The above dynamic equations (17)-(19) can describe the relationship between the motion characteristics such as the velocity and acceleration of the manipulator and the joint torque or force. Establishing an accurate system dynamic model provides a basis for subsequent analysis, design, and control of the manipulator using the Robotics Toolbox in MATLAB.

[0157] The simulation control based on the constructed model specifically includes:

[0158] 1. Construct a complete manipulator model in the toolbox according to the parameters of each link of the manipulator and the above D-H parameter method.

[0159] 2. According to the forward and inverse kinematic models, calculate the pose of the end effector of the manipulator and the corresponding joint angles of the end effector pose through the functions in the toolbox.

[0160] According to the established manipulator dynamic model, after determining the initial and target positions, use the corresponding functions in the toolbox to generate the motion trajectory of the manipulator, including solving the joint position, velocity, and acceleration.

[0161] In this embodiment, the Robotics Toolbox is used to model and control the manipulator in MATLAB. According to the D-H parameters of the three-degree-of-freedom manipulator given above: the first joint is a rotating joint, and the other two are moving joints, a manipulator model is created, and the initial state of the manipulator model is as Figure 8 shown, Figure 8On the left side is the teaching demonstration status bar, which can display the position information of the end of the manipulator in real time. In the model, the rotation angle range of the rotary joint is set to 0 - 360°, and the movement range of the linear joint is 0 - 1000 mm. The state of the manipulator after angle rotation and linear joint movement is as Figure 9 shown. The initial position of the working body of the manipulator is (0, 0, 0), and the final movement position is (pi / 2, 500, 300), that is, the rotation angle of the rotary joint is 90°, and the displacements of the two linear joints are 500 mm and 300 mm respectively. Figure 10 shows the three-dimensional path between the input start and end positions and shows the points corresponding to the initial and final positions.

[0162] According to the kinematic model of the manipulator established above, the MATLAB Robotics Toolbox is used to solve the displacements, velocities, and accelerations of each joint during the movement of the manipulator. As Figure 11 (a)-(i) shows the displacement, velocity, and acceleration curves corresponding to joints 1, 2, and 3 of the manipulator. The set time range is 0 - 10 s. The displacement of the rotary joint gradually moves from 0 to 1.6 rad, the displacement of the horizontal linear joint gradually moves from 0 to 500 mm, and the displacement of the vertical linear joint gradually moves from 0 to 300 mm. The overall trend of the velocity of each joint is to increase first and then decrease, which conforms to the control law of the manipulator, that is, when the distance from the target position is far, it accelerates, and when approaching the target position, it decelerates to the target position.

[0163] In summary, the dynamic model can describe the relationship between the motion characteristics such as the velocity and acceleration of the manipulator and the joint torque or force, as well as the coupling relationship between the joints, so as to accurately describe the mechanical behavior of the robot during the movement. According to the established dynamic model of the manipulator, the corresponding functions in the toolbox are used to generate the motion trajectory of the manipulator, including the joint position, velocity, and acceleration trajectories. That is, the dynamic model is obtained by modeling and motion control of the manipulator in the MATLAB Robotics Toolbox. Not only the motion model of the cylindrical coordinate manipulator is obtained, but also its three-dimensional motion trajectory and the displacement, velocity, and acceleration curves of each joint during the motion process are obtained. By observing the dynamic behavior of the cylindrical coordinate manipulator system in an integrated and visual way, the parameter design and control strategy of the cylindrical coordinate manipulator of the subsea drill can be verified and optimized.

[0164] The embodiments described above are only descriptions of the preferred embodiments of the present invention, and do not limit the scope of the present invention. Without departing from the design spirit of the present invention, various deformations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention should fall within the protection scope determined by the claims of the present invention.

Claims

1. A drilling rig cylindrical coordinate manipulator modeling and motion control simulation method, characterized in that: include: According to the DH coordinate system modeling method, the connecting rod coordinate system of the drilling rig cylindrical coordinate manipulator is constructed, the connecting rod parameters are obtained and the manipulator model is constructed; According to the connecting rod parameters, a forward kinematics model and an inverse kinematics model of the drilling rig cylindrical coordinate manipulator are constructed; According to the forward kinematics model and the inverse kinematics model, the manipulator position and posture are obtained, and a dynamic model of the drilling rig cylindrical coordinate manipulator is constructed in combination with the Lagrangian method; A manipulator motion control simulation is performed according to the manipulator model, the forward kinematics model, the inverse kinematics model and the dynamics model.

2. The drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to claim 1 is characterized in that: The connecting rod parameters include: the length of the connecting rod, the torsion angle of the connecting rod, the distance between adjacent connecting rods, and the angle between adjacent connecting rods.

3. The drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to claim 1 is characterized in that: The forward kinematics model and inverse kinematics model of the cylindrical coordinate manipulator of the drilling rig include: According to the connecting rod parameters, obtaining a connecting rod transformation matrix; According to the connecting rod transformation matrix, obtaining the forward kinematics model; Obtaining a solution to the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator according to the inverse transformation and the connecting rod transformation matrix; The inverse kinematics model is obtained according to the solution of the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator.

4. The drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to claim 1 is characterized in that: The construction of the drilling rig cylindrical coordinate manipulator dynamics model includes: According to the degrees of freedom of the cylindrical coordinate manipulator of the drilling rig, the dimensions of the Jacobian matrix are obtained; According to the dimension of the Jacobian matrix, the Jacobian matrix is ​​obtained in combination with the geometric parameters of the manipulator and the manipulator posture; According to the Jacobian matrix, obtaining the Jacobian matrix of the linear velocity of the end effector; According to the Jacobian matrix of the linear velocity of the end effector, the velocity of the joint and the velocity of the end effector are obtained, the joint variables are converted into the position coordinates of the end effector, and the kinetic energy and potential energy are obtained according to the velocity and the position coordinates; According to the kinetic energy and potential energy, obtaining the Lagrangian dynamic equation; Perform partial derivative calculations based on the Lagrangian dynamics equation to obtain the torque equations of each link of the manipulator; According to the torque equation, the dynamics model of the drilling rig cylindrical coordinate manipulator is obtained.

5. The drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to claim 4 is characterized in that: The Jacobian matrix of the end effector linear velocity is: Among them, J is the Jacobian matrix of the linear velocity of the end effector, θ1 is the rotation of the hydraulic motor and directly generates the angle of rotation, d3 is the horizontal displacement of the manipulator, and o0, z1, and z2 are all matrix codes.

6. The drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to claim 4 is characterized in that: The Lagrangian dynamics equation is: Where T represents kinetic energy, U represents potential energy, and q i is the generalized coordinate, τ i is the force / torque acting on the ith coordinate system, is the joint angular velocity.

7. The drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to claim 1 is characterized in that: Performing manipulator motion control simulation according to the manipulator model, the forward kinematics model, the inverse kinematics model and the dynamics model includes: According to the forward kinematics model and the inverse kinematics model, in combination with the dynamics model, the motion trajectory of the manipulator is generated, and the displacement, velocity and acceleration of each joint of the manipulator during the motion process are solved.

Images