A Modeling and Motion Control Simulation Method for a Drill Rig Cylindrical Coordinate Manipulator

Abstract

This invention relates to a modeling and motion control simulation method for a cylindrical coordinate manipulator used in drilling rigs. The method includes: constructing a link coordinate system for the cylindrical coordinate manipulator using the D-H coordinate system modeling method, obtaining link parameters, and constructing a manipulator model; constructing the forward kinematics model and inverse kinematics model of the cylindrical coordinate manipulator based on the link parameters; obtaining the manipulator's pose based on the forward and inverse kinematics models, and constructing a dynamic model of the cylindrical coordinate manipulator using the Lagrange method; and performing motion control simulation of the manipulator based on the manipulator model, kinematic model, and dynamic model. This invention simulates and analyzes the kinematic characteristics of the manipulator based on the solution of its kinematic and dynamic models, establishing its simulation model. This provides a theoretical basis for subsequent high-precision control of the manipulator, as well as for the design verification, control algorithm development, and performance optimization of the cylindrical coordinate manipulator used in drilling rigs.

Description

Technical Field

[0001] This invention relates to the field of deep-sea operation robot modeling and simulation technology, and in particular to a method for modeling and motion control simulation of a drilling rig cylindrical coordinate robot. Background Technology

[0002] In recent years, deep-sea underwater equipment technology has developed rapidly, accelerating the exploration and development of deep-sea resources. Through 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 holding irreplaceable significance for the discovery and utilization of seabed resources. Among these, deep-sea subsea drilling rigs are a crucial component of deep-sea underwater exploration technology and equipment. Integrating power, communication, control, and drilling functions, subsea drilling rigs are highly integrated and complex. With their unique design and technological and economic advantages, they have broad application prospects in future modern marine engineering.

[0003] As a key component of the drilling rig system, the robotic arm not only enhances the operational flexibility of the equipment but also strengthens the system's adaptability and reliability. The complex and ever-changing deep-sea environment makes it difficult for traditional fixed tools to meet the demands of precise operations. With its flexible joint structure and precise control capabilities, the robotic arm can perform complex operations within confined spaces, thereby ensuring the accuracy of drilling work. Facing the high pressure and complex environment of the deep sea, the robotic arm can simulate the movements of a human hand to complete tasks such as grasping, moving, and installing drill pipes, enabling the subsea drilling rig to maintain efficient operation.

[0004] Cylindrical coordinate manipulators are a crucial component in ensuring the efficient execution of exploration and sampling tasks by subsea drilling rigs. These manipulators typically consist of rotary and translational joints and operate in complex environments, posing challenges to their control. Summary of the Invention

[0005] The purpose of this invention is to provide a modeling and motion control simulation method for a cylindrical coordinate manipulator for a drilling rig. This method aims 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 for a subsea drilling rig, ensuring its adaptability and flexibility under different operating conditions.

[0006] To achieve the above objectives, the present invention provides the following solution:

[0007] A method for modeling and motion control simulation of a drilling rig cylindrical coordinate robot includes:

[0008] Based on the DH coordinate system modeling method, the link coordinate system of the drilling rig cylindrical coordinate manipulator is constructed, the link parameters are obtained, and the manipulator model is built.

[0009] Based on the connecting rod parameters, construct the forward kinematics model and the inverse kinematics model of the drilling rig cylindrical coordinate manipulator;

[0010] Based on the forward kinematics model and the inverse kinematics model, the pose of the manipulator is obtained, and a dynamic model of the drilling rig cylindrical coordinate manipulator is constructed by combining the Lagrange method.

[0011] The motion control simulation of the robot arm is performed based on the robot arm model, the forward kinematics model, the inverse kinematics model, and the dynamics model.

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

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

[0014] Based on the link parameters, obtain the link transformation matrix;

[0015] Based on the link transformation matrix, the forward kinematics model is obtained;

[0016] Based on the inverse transformation and the link transformation matrix, the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator is obtained;

[0017] The inverse kinematics model is obtained based on the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator.

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

[0019] Based on the degrees of freedom of the drilling rig's cylindrical coordinate manipulator, obtain the dimensions of the Jacobian matrix;

[0020] Based on the dimensions of the Jacobian matrix, and combined with the geometric parameters and pose of the robotic arm, the Jacobian matrix is ​​obtained.

[0021] Based on the Jacobian matrix, obtain the Jacobian matrix of the linear velocity of the end effector;

[0022] Based on 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 based on the velocity and position coordinates.

[0023] Based on the kinetic and potential energies, the Lagrange dynamic equations are obtained;

[0024] Partial derivatives are calculated based on the Lagrange dynamics equations and motion equations to obtain the torque equations for each component of the manipulator; based on the torque equations, the dynamic model of the drilling rig's cylindrical coordinate manipulator is obtained.

[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 of the hydraulic motor and the angle directly generated, d3 is the horizontal displacement of the manipulator, and o0, z1, and z2 are matrix symbols.

[0028] Optionally, the Lagrange dynamic equation is:

[0029]

[0030] Where T represents kinetic energy, U represents potential energy, and q i For generalized coordinates, τ i For the force / torque acting on the i-th coordinate system, This represents the joint angular velocity.

[0031] Optionally, the simulation of robot motion control based on the robot model, the forward kinematics model, the inverse kinematics model, and the dynamics model includes:

[0032] Based on the forward kinematics model and the inverse kinematics model, combined 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 are solved.

[0033] The beneficial effects of this invention are as follows: A model of the manipulator was obtained using the DH coordinate system modeling method, and its forward and inverse kinematic models were derived. The pose of the end effector frame was given based on the manipulator's joint parameters. Finally, the pose and motion trajectory were demonstrated and analyzed using the MATLAB Robotics Toolbox, allowing for intuitive observation of the manipulator's motion and thus optimizing its motion control. Real-time tracking and analysis of the manipulator's trajectory provide a comprehensive method for the dynamic control of drilling rig cylindrical coordinate manipulators. Attached Figure Description

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

[0035] Figure 1 This is a schematic diagram of a three-dimensional model of a cylindrical coordinate manipulator for a subsea drilling rig, according to an embodiment of the present invention.

[0036] Figure 2 This is a simplified schematic diagram of the robotic arm according to an embodiment of the present invention;

[0037] Figure 3 This is a schematic diagram of the cylindrical coordinate system according to an embodiment of the present invention;

[0038] Figure 4 This is a three-dimensional schematic diagram and top view of the workspace of the robotic arm according to an embodiment of the present invention;

[0039] Figure 5 This is a schematic diagram of the movement of the robotic arm according to an embodiment of the present invention;

[0040] Figure 6 This is a schematic diagram of the reference coordinate system of the cylindrical coordinate robot in an embodiment of the present invention;

[0041] Figure 7 The following are flowcharts illustrating the forward and inverse kinematics of an embodiment of the present invention;

[0042] Figure 8 This is a schematic diagram of a robotic arm model according to an embodiment of the present invention;

[0043] Figure 9 This is a schematic diagram of the state of the robotic arm after joint movement according to an embodiment of the present invention;

[0044] Figure 10 This is a schematic diagram of the three-dimensional motion trajectory of the robotic arm according to an embodiment of the present invention;

[0045] Figure 11 The figures above are displacement, velocity, and acceleration curves of each joint of the robotic arm in an embodiment of the present invention. Among them, (a) is the displacement curve of the robotic arm joint 1 during its movement, (b) is the displacement curve of the robotic arm joint 2 during its movement, (c) is the displacement curve of the robotic arm joint 3 during its movement, (d) is the velocity curve of the robotic arm joint 1 during its movement, (e) is the velocity curve of the robotic arm joint 2 during its movement, (f) is the velocity curve of the robotic arm joint 3 during its movement, (g) is the acceleration curve of the robotic arm joint 1 during its movement, (h) is the acceleration curve of the robotic arm joint 2 during its movement, and (i) is the acceleration curve of the robotic arm joint 3 during its movement.

[0046] Figure 12 This is a flowchart of a drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to an embodiment of the present invention. Detailed Implementation

[0047] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

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

[0049] like Figure 12 As shown in the figure, this embodiment provides a method for modeling and motion control simulation of a drilling rig cylindrical coordinate robot, including:

[0050] Based on the DH coordinate system modeling method, the link coordinate system of the drilling rig cylindrical coordinate manipulator is constructed, the link parameters are obtained, and the manipulator model is built.

[0051] Based on the linkage parameters, construct the forward kinematics model and inverse kinematics model of the drilling rig cylindrical coordinate manipulator;

[0052] Based on the forward and inverse kinematics models, the pose of the manipulator is obtained, and a dynamic model of the drilling rig cylindrical coordinate manipulator is constructed using the Lagrange method.

[0053] The motion control simulation of the robot arm is carried out based on the robot arm model, forward kinematics model, inverse kinematics model and dynamics model.

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

[0055] Furthermore, the forward and inverse kinematic models of the drilling rig's cylindrical coordinate manipulator are constructed as follows:

[0056] Based on the link parameters, obtain the link transformation matrix;

[0057] Obtain the forward kinematics model based on the link transformation matrix;

[0058] Based on the inverse transformation and link transformation matrix, the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator is obtained;

[0059] Based on the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator, the inverse kinematics model is obtained.

[0060] Furthermore, the construction of the drilling rig cylindrical coordinate manipulator dynamic model includes:

[0061] Based on the degrees of freedom of the drilling rig's cylindrical coordinate manipulator, obtain the dimensions of the Jacobian matrix;

[0062] Based on the dimensions of the Jacobian matrix, combined with the geometric parameters and pose of the robotic arm, the Jacobian matrix is ​​obtained.

[0063] Obtain the Jacobian matrix of the linear velocity of the end effector based on the Jacobian matrix;

[0064] Based on 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 based on the velocity and position coordinates;

[0065] Based on kinetic and potential energy, the Lagrange equations of motion are derived;

[0066] The torque equations for each component of the robot are obtained by calculating the partial derivatives of the Lagrange dynamics equations.

[0067] Based on the torque equation, the dynamic model of the drilling rig's cylindrical coordinate robot is obtained.

[0068] Furthermore, the Jacobian matrix for 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 Lagrange equation for the robotic arm is:

[0072]

[0073] Where T represents kinetic energy, U represents potential energy, and q i For generalized coordinates.

[0074] Furthermore, the simulation of robot motion control based on the robot model, forward kinematics model, inverse kinematics model, and dynamics model includes:

[0075] Based on the forward and inverse kinematics models, combined with the dynamics model, the motion trajectory of the robot is generated, and the displacement, velocity, and acceleration of each joint of the robot during the motion are solved.

[0076] The following description, in conjunction with the accompanying drawings, further illustrates this embodiment:

[0077] 1. Cylindrical coordinate system and DH modeling of the robotic arm:

[0078] The cylindrical coordinate robot of the subsea drilling rig is a crucial component for completing the actions of gripping drill rods and drill strings, and drilling. The 3D model is as follows: Figure 1 As shown, the abstract simplified structure diagram is as follows: Figure 2As shown. A spatial rectangular coordinate system and a cylindrical coordinate system are established with the vertical spindle of the robot arm as the Z-axis, as follows: Figure 3 As shown. Suppose that in the cylindrical space, the end effector of the robot moves at point P. Let (r, θ) be the polar coordinates of the projection point of point P onto 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). The position in the spatial rectangular coordinate system is transformed into the cylindrical coordinate system as shown in equation (1).

[0079]

[0080] Where r is the radial distance, which is the distance between the origin and the projection of point P onto the XOY plane, representing the distance of point P from the main axis of the robot arm. max The maximum radial distance; the azimuth angle θ is exactly the angle between the projection and the X-axis, θ∈[0,2π]. x The azimuth angle is represented by 'z' in Cartesian coordinates; 'z' represents the height. The cylindrical coordinate manipulator structurally employs one rotary joint and two translational joints. Its motion coordinate system is cylindrical, which is based on a Cartesian design where the first translational joint is transformed into a rotary joint about the Z-axis. Its workspace is a cylinder. The cylindrical coordinate system establishes the cylindrical coordinate system of the drilling rig's cylindrical coordinate manipulator, thus describing the entire space in which the manipulator moves.

[0081] Assuming a three-degree-of-freedom cylindrical coordinate system manipulator has a rotation angle working range of [0, 360°], and due to the influence of its own column, its radius r is adjustable within the range of [r2, r1], and its vertical adjustable range is [0, d]. Then, its three-dimensional working space schematic diagram and top view are as follows: Figure 4 As shown.

[0082] The DH coordinate system modeling method is the most widely used method in kinematic modeling of robot arms. To mathematically describe the relative motion and pose relationships between the links, a spatial coordinate system is fixed to each link. The geometric dimensions of each link can be described by four parameters, the meanings of which are as follows:

[0083] 'a' represents the length of the link, which, in the link coordinate system, is along the x-axis. i axis, from z i axis to z i+1 The distance of the axis translation.

[0084] α represents the torsion angle of the link, which, in the link coordinate system, is about x... i axis, from z i axis to z i+1 The angle through which the shaft rotates. The parameters 'a' and 'α' describe the characteristics of the connecting rod itself.

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

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

[0087] To measure the position and motion relationships between each link, a coordinate system must be established for each link. By considering the relationships between these coordinate systems, the position and motion of the actuator within the coordinate system are established. The steps for establishing the link coordinate system according to the DH standard method are as follows:

[0088] (1) Establish the first coordinate system (X0, Y0, Z0); this origin serves as the initial position of the manipulator's base center of mass. This step allows us to obtain other positional parameters of the reference point in space;

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

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

[0091] (4) Along z i-1 and z i The common normal between them and through o i Establish x i ;

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

[0093] (6) Establish the framework for end-effectors. n x n y n z n .

[0094] (7) Create link parameter a i d i α i θ i .

[0095] The motion diagram of the robotic arm is as follows Figure 5 As shown, θ1 is the rotation angle of the robot arm, d2 is the longitudinal movement distance of the robot arm, and d3 is the lateral movement distance of the robot arm. The coordinate system of each link is as follows: Figure 6As shown, the linkage 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-axis between z1 and z2 intersects, and o2 is at the intersection. The direction of x2 is chosen to be parallel to x1, so θ2 is zero. Finally, a third coordinate system is chosen at the end of linkage 3.

[0096] 2. Forward and inverse kinematics modeling of the robotic arm:

[0097] (1) Forward kinematics modeling:

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

[0099]

[0100] The values ​​of the first four parameters can be summarized based on the established DH 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 robot's joints are driven by hydraulic motors and hydraulic cylinders. The hydraulic motors rotate, directly generating a rotation angle θ1, while the hydraulic cylinders move axially, generating (d2, d3). Table 1 shows that θ1, d2, and d3 are variables used when the robot establishes its reference frame. The homogeneous transformation between each pair of coordinate systems is analyzed. Assigning values ​​to each parameter will cause the robot to move and reach a certain position. The link parameters are shown in Table 1, and the corresponding T matrix is ​​shown below:

[0104]

[0105] (a)

[0106]

[0107] (b)

[0108]

[0109] (c)

[0110]

[0111] (d)

[0112] (3) In the formula, (d) is the position matrix of the cylindrical coordinate robot.

[0113] In the above link transformation matrices, s isinθ i c i Represents cosθ i .

[0114] According to robotics engineering theory, (d) in equation (3) can be described 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 direction vector, the Y-axis of the end effector coordinate system; and n is the normal vector, the X-axis of the end effector coordinate system. x p y p z Here are the coordinates of the origin of the end effector coordinate system relative to the base coordinate system. Forward kinematics modeling refers to the process of calculating the pose of the end effector given the joint variables of the manipulator.

[0117] The pose of the robotic arm's end effector can be represented as:

[0118]

[0119] (2) Inverse kinematics modeling:

[0120] The study of forward kinematics can be used to determine whether a robot's motion meets certain conditions, and it can also be used to construct inverse kinematics. Flowcharts of the forward and inverse kinematics of a robot are shown below. Figure 7 As shown.

[0121] The study of inverse kinematics reveals whether a robot can achieve desired motion and position. When a robot performs position control and trajectory planning, or when the position and orientation of the hand have been declared, this information is used to determine the rotation angle θ of the joints to drive the motors of each joint to adapt to the hand's position and orientation requirements. Therefore, the study of inverse kinematics involves measuring the parameter θ, which is crucial for robot motion control systems. The measurement of θ is a backtesting operation of the aforementioned equations.

[0122] Inverse Transform Multiplying both sides of equation (4) on the left yields:

[0123]

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

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

[0126]

[0127] The above equations are the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate robot. Based on the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate robot, the inverse motion model is obtained, which is used to control the position of the robot and form the motion trajectory of the robot.

[0128] 3. Robotic Arm Dynamics Modeling Methods:

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

[0130]

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

[0132]

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

[0134]

[0135] The linear velocity of the end effector can be obtained by considering only 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 of the hydraulic motor and the angle directly generated, d3 is the horizontal displacement of the manipulator, and o0, z1, and z2 are matrix symbols.

[0138] Robotic arm dynamics can reveal the relationships between position, velocity, acceleration, and joint torque. Therefore, dynamic modeling of a robotic arm aims to understand the relationship between its motion and the forces acting on it. The dynamic model of a robotic arm can be obtained using the Lagrange equations.

[0139] Solving the Lagrange equations of motion requires calculating kinetic and potential energy. For a manipulator with multiple joints, its kinetic energy is the sum of the kinetic energies of each joint, and the velocities of the joints and the end effector are related through the Jacobian matrix. Therefore, the end effector velocity can be expressed as a function of the joint velocities using the Jacobian matrix, making it easier to calculate the system's kinetic energy. Furthermore, the Jacobian matrix can convert joint variables into the position coordinates of the end effector, allowing for a more accurate calculation of the manipulator system's potential energy.

[0140] Based on the state-space equation form of the robot's dynamics equations, the standard state-space dynamics equations of the manipulator can be expressed as:

[0141]

[0142] M(q) is the mass inertia matrix of the robot. For a 3-joint robot, this matrix is ​​a 3*3 angular symmetric matrix. The elements of the mass inertia matrix depend on the robot's joint angle q. This is the vector of centrifugal force and Coriolis force. This term is related to the manipulator's joint angle q and joint angular velocity. Related, Let G(q) be the angular acceleration, and let G(q) be the gravitational 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 a robotic arm from an energy perspective, deriving the Lagrange equations of dynamics by calculating the kinetic and potential energy of the robotic arm.

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

[0145]

[0146] T represents kinetic energy, and U represents potential energy. q i For generalized coordinates, τ i For the force / torque acting on the i-th coordinate system, Using the joint angular velocity as an example, partial derivatives of the Lagrange dynamics equations are calculated to obtain the torque equations for each link of the manipulator. The final dynamic model is represented by the dynamic equations for the torques of the three links.

[0147] The first dynamic equation for the connecting rod torque is as follows:

[0148]

[0149] m i Let θ be the mass of the i-th link, and θ1 be the position of the rotary joint. For speed, Let d be the acceleration, and d3 be the axial distance traveled. The axial velocity, For axial motion acceleration, This is centripetal acceleration.

[0150] Similarly, by taking the partial derivative with respect to the second joint of the robot, we obtain the torque of joint 2, and the dynamic equation of joint 2 can be derived as follows:

[0151]

[0152] d2 is the axial movement distance. The axial velocity, This is the axial acceleration.

[0153] The dynamic equations involve the description of the joint position, velocity, and acceleration of the robot arm. The dynamic equation of the third joint is:

[0154]

[0155] in, For Coriolis acceleration.

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

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

[0158] 1. Construct a complete robot model in the toolbox based on the parameters of each link of the robot and the DH parameter method mentioned above.

[0159] 2. Based on the forward and inverse kinematics models, calculate the pose of the robot's end effector and the corresponding joint angles using the functions in the toolbox.

[0160] Based on the established dynamic model of the manipulator, after determining the initial and target positions, the corresponding functions in the toolbox are used to generate the motion trajectory of the manipulator, including solving for joint positions, velocities, and accelerations.

[0161] This embodiment uses the Robotics Toolbox in MATLAB to model and control a robotic arm. Based on the DH parameters of the three-DOF robotic arm given above (the first joint is a rotational joint, and the other two are translational joints), a robotic arm model is created. The initial state of the robotic arm model is as follows: Figure 8 As shown, Figure 8The left side of the screen displays the teaching demonstration status bar, showing the real-time position information of the robot's end effector. In the model, the rotation angle range of the rotary joint is set to 0-360°, and the movement range of the locating joint is set to 0-1000mm. The robot's state after undergoing rotation and locating joint movement is shown below. Figure 9 As shown. The initial position of the robotic arm's working body is (0,0,0), and the final position is (pi / 2,500,300), meaning the rotary joint rotates by 90°, and the displacements of the two translating joints are 500mm and 300mm respectively. Figure 10 It displays the 3D path between the start and end positions of the input, and shows the points corresponding to the initial and final positions.

[0162] Based on the kinematic model of the manipulator established above, the displacement, velocity, and acceleration of each joint of the manipulator during its movement are solved using the MATLAB Robotics Toolbox. For example... Figure 11 Figures (a)-(i) show the displacement, velocity, and acceleration curves corresponding to joints 1, 2, and 3 of the robotic arm. The set time range is 0-10s. The displacement of the rotary joint gradually increases from 0 to 1.6 rad, the displacement of the horizontal traverse joint gradually increases from 0 to 500 mm, and the displacement of the vertical traverse joint gradually increases from 0 to 300 mm. The overall trend of the velocity of each joint is that it first accelerates and then decelerates, which is consistent with the control law of the robotic arm, that is, it accelerates when it is far from the target position and decelerates when it approaches the target position.

[0163] In summary, the dynamic model can describe the relationship between the robot's motion characteristics such as velocity and acceleration and joint torques or forces, as well as the coupling relationships between joints, thus accurately describing the robot's mechanical behavior during motion. Based on the established robot dynamic model, the robot's motion trajectory, including joint position, velocity, and acceleration trajectories, is generated using corresponding functions in the toolbox. That is, the dynamic model is obtained by modeling and controlling the robot's motion in the MATLAB Robotics Toolbox, resulting not only in the motion model of the cylindrical coordinate robot but also in its three-dimensional motion trajectory and the displacement, velocity, and acceleration curves of each joint's motion. By observing the dynamic behavior of the cylindrical coordinate robot system in an integrated and visualized manner, the parameter design and control strategy of the cylindrical coordinate robot for the subsea drilling rig can be verified and optimized.

[0164] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made to the technical solutions of the present invention by those skilled in the art without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A method for modeling and motion control simulation of a drilling rig cylindrical coordinate robot, characterized in that, include: Based on the DH coordinate system modeling method, the link coordinate system of the drilling rig cylindrical coordinate manipulator is constructed, the link parameters are obtained, and the manipulator model is built. Based on the connecting rod parameters, construct the forward kinematics model and the inverse kinematics model of the drilling rig cylindrical coordinate manipulator; Based on the forward kinematics model and the inverse kinematics model, the pose of the manipulator is obtained, and a dynamic model of the drilling rig cylindrical coordinate manipulator is constructed by combining the Lagrange method. The construction of the drilling rig cylindrical coordinate manipulator dynamic model includes: Based on the degrees of freedom of the drilling rig's cylindrical coordinate manipulator, obtain the dimensions of the Jacobian matrix; Based on the dimensions of the Jacobian matrix, and combined with the geometric parameters and pose of the robotic arm, the Jacobian matrix is ​​obtained. Based on the Jacobian matrix, obtain the Jacobian matrix of the linear velocity of the end effector; Based on 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 based on the velocity and position coordinates. Based on the kinetic and potential energies, the Lagrange dynamic equations are obtained; The torque equations for each component of the robot are obtained by calculating the partial derivatives based on the Lagrange dynamics equations. Based on the torque equation, obtain the dynamic model of the drilling rig's cylindrical coordinate manipulator; The Jacobian matrix of the linear velocity of the end effector is: ； Where J is the Jacobian matrix of the linear velocity of the end effector, θ1 is the rotation of the hydraulic motor and the angle directly generated, d3 is the horizontal displacement of the manipulator, and o0, z1, and z2 are matrix symbols. The motion control simulation of the robot arm is performed based on the robot arm 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, characterized in that, The link parameters include: link length, link torsion angle, distance between adjacent links, and included angle between adjacent links.

3. The drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to claim 1, characterized in that, The construction of the forward and inverse kinematic models of the drilling rig cylindrical coordinate robot includes: Based on the link parameters, obtain the link transformation matrix; Based on the link transformation matrix, the forward kinematics model is obtained; Based on the inverse transformation and the link transformation matrix, the solution to the inverse kinematics problem of the drilling rig cylindrical coordinate manipulator is obtained; The inverse kinematics model is obtained based on the solution to 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, characterized in that, The Lagrange dynamic equation is as follows: ； Where T represents kinetic energy, U represents potential energy, and q i For generalized coordinates, τ i For the force / torque acting on the i-th coordinate system, This represents the joint angular velocity.

5. The drilling rig cylindrical coordinate manipulator modeling and motion control simulation method according to claim 1, characterized in that, The simulation of robot motion control based on the robot model, the forward kinematics model, the inverse kinematics model, and the dynamics model includes: Based on the forward kinematics model and the inverse kinematics model, combined 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 are solved.

Images