Mechanical arm control method and system based on kinematics and dynamics general modeling

Abstract

The invention belongs to the field of industrial six-degree-of-freedom mechanical arm automatic simulation control, and provides a mechanical arm control method and system based on kinematics and dynamics general modeling. According to the technical scheme, a kinematic model of the tail end of the mechanical arm is constructed, a six-degree-of-freedom industrial mechanical arm dynamics mathematical model is constructed, an inverse kinematic method is designed based on the constructed kinematic model, and the constructed dynamics model is combined with a control method to conduct end effector trajectory tracking control on the industrial six-degree-of-freedom mechanical arm to verify the control effect. And it is ensured that the predetermined track of the end effector of the mechanical arm is converted into the expected rotation angle of each joint shaft, and effective control over the industrial six-degree-of-freedom mechanical arms of different models and different sizes is achieved.

Description

Technical Field

[0001] The present invention belongs to the field of automated simulation control of industrial six-degree-of-freedom robotic arms, and in particular relates to a robotic arm control method and system based on general kinematics and dynamics modeling. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] Industrial six-degree-of-freedom robotic arms have been widely used in many fields such as automobile manufacturing, electronic product assembly, and logistics sorting due to their high flexibility, high precision, and wide operating range. Such robotic arms usually have six independent degrees of freedom, including translation along the x-axis, y-axis, and z-axis, and rotation around these three axes, enabling them to perform complex grasping, handling, and assembly operations in three-dimensional space. However, precisely because of its multi-degree-of-freedom characteristics, the dynamic characteristics of the robotic arm become particularly complex, involving a high degree of coupling and nonlinear problems, which places extremely high demands on the design and implementation of the control algorithm.

[0004] Although the Denavit-Hartenberg modeling (DH modeling) method and its improved methods have achieved certain results in the field of robot modeling, a complete and effective universal dynamic modeling method has not yet been formed for industrial six-degree-of-freedom robot arms of different models and sizes (length and mass of each link of the robot arm). This results in researchers and engineers needing to perform a large amount of customized modeling work for specific robots when facing diverse robot application requirements, which is not only time-consuming and labor-intensive, but also difficult to ensure the universality and portability of the model. In addition, since there is no accurate kinematic and dynamic model for industrial six-degree-of-freedom robot arms of different models that can fully reflect the dynamic characteristics of the robot arm, the control accuracy is low when combined with advanced control algorithms. Summary of the invention

[0005] In order to solve at least one technical problem existing in the above-mentioned background technology, the present invention provides a robot arm control method and system based on general kinematic and dynamic modeling, which proposes a general kinematic and dynamic modeling method for a six-degree-of-freedom robot arm according to the actual robot arm modeling adjustment scheme of different models and sizes, and combines the BackStepping control method to perform end effector trajectory tracking control on the industrial six-degree-of-freedom robot arm.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] A first aspect of the present invention provides a robot arm control method based on general kinematics and dynamics modeling, comprising:

[0008] Determine the parameters of each link and joint of the industrial six-degree-of-freedom robot arm;

[0009] Combining the parameters of the links and joints of the industrial six-degree-of-freedom robot and the established kinematic model of the end of the robot, the homogeneous transformation matrix of the end effector of the robot relative to the coordinate system of the robot base is obtained;

[0010] Combined with the kinematic model of the end of the robot arm, the motion analysis of the driven robot arm is carried out according to the torque applied by the driver and the external force acting on the robot arm, and a mathematical model of the dynamics of the six-degree-of-freedom industrial robot arm is constructed;

[0011] According to the expected position and posture of the end effector of the robot arm in space, combined with the homogeneous transformation matrix of the end effector of the robot arm relative to the coordinate system of the robot arm base, the rotation angle of each joint movement of the robot arm is inversely solved to control the movement of the robot arm.

[0012] Furthermore, the process of constructing the kinematic model of the end of the robotic arm includes:

[0013] Establish the connecting rod fixed connection coordinate system based on the preset coordinate system establishment principle;

[0014] Establish the rotation matrix between adjacent coordinate systems based on the connecting rod fixed coordinate system;

[0015] Construct a transformation matrix between adjacent coordinate systems based on the rotation matrix between adjacent coordinate systems;

[0016] By converting the transformation matrix between adjacent coordinate systems, the representation of the last coordinate system of the six-degree-of-freedom robot arm in the base coordinates is obtained, that is, the functional relationship between the position and posture of the end effector and the base of the robot arm. At this point, the kinematic model of the end of the robot arm is completed.

[0017] Furthermore, the preset coordinate system establishment principles include:

[0018] First, Z i Axis along joint axis J i+1 Axial direction;

[0019] Second, the origin O i Z i-1 With Z i The intersection of the axis or its common perpendicular line and the joint axis J i The intersection of

[0020] Third, X i Axis along Z i-1 With Z i The common perpendicular direction of the axis is from the joint axis J i Pointing to joint axis J i+1 ;

[0021] Fourth, Y iThe axis is determined according to the right-hand rule.

[0022] Furthermore, the motion analysis of the driven robotic arm based on the torque applied by the driver and the external force acting on the robotic arm constructs a six-degree-of-freedom industrial robotic arm dynamics mathematical model, including:

[0023] Extrapolate the angular velocity, angular acceleration, linear velocity and linear acceleration of the coordinate origin, the acceleration of the center of mass of the connecting rod, the gravity acceleration, inertial force and inertial moment of the connecting rod from the first connecting rod and the first coordinate origin of the robot arm to the sixth connecting rod and the sixth coordinate origin;

[0024] Infer the force and torque on the link and the torque of the joint from the sixth link and the sixth coordinate origin of the robot arm to the first link and the first coordinate origin;

[0025] According to the torque calculation of the joints, the inertia matrix, Coriolis force and gravity in the dynamic model of the six-degree-of-freedom industrial robot arm are obtained, and the construction of the mathematical dynamic model of the six-degree-of-freedom industrial robot arm is completed.

[0026] Furthermore, the angular velocity, angular acceleration, linear velocity and linear acceleration of the coordinate origin, the acceleration of the connecting rod center of mass, the gravitational acceleration, inertia force and inertia moment of the connecting rod are extrapolated from the first connecting rod and the first coordinate origin of the robot arm to the sixth connecting rod and the sixth coordinate origin:

[0027] Extrapolate the coordinate systems of each link from the first link coordinate system B i The angular velocity of the origin is:

[0028] in, i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of is the joint axis J i Angular velocity, i-1 Z i-1 =[0,0,1] T , 0 ω0=[0,0,0] T ;

[0029] Extrapolate the coordinate systems of each link from the first link coordinate system B i Angular acceleration at the origin:

[0030] in is the connecting rod coordinate system B i The angular acceleration at the origin, i R i-1is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of is the joint axis J i Angular velocity, is the joint axis J i Angular acceleration, i-1 Z i-1 =[0,0,1] T , i-1 ω i-1 is the connecting rod coordinate system B i-1 Angular velocity of the origin;

[0031] Extrapolate the coordinate systems of each link from the first link coordinate system B i Linear velocity at the origin:

[0032] i v i = i R i-1 i-1 v i-1 + i ω i × i P Oi-1,Oi ,in i v i is the connecting rod coordinate system B i The linear velocity at the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of

[0033] Extrapolate the coordinate systems of each link from the first link coordinate system B i Linear acceleration at the origin:

[0034] in is the connecting rod coordinate system B i The linear acceleration at the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i POi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of

[0035] Extrapolate the center of mass C of each link from the center of mass of the first link i Acceleration:

[0036] in is the center of mass C i The acceleration of is the connecting rod coordinate system B i The linear acceleration at the origin, i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of

[0037] Push each connecting rod L outward from the first connecting rod i The gravitational acceleration is:

[0038] i g= i R i-1 i-1 g, where i g is the connecting rod L i The gravitational acceleration, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of 0 g = 9.81;

[0039] Push each connecting rod L outward from the first connecting rod i Inertial force on:

[0040] in L is the connecting rod i Inertial force, m i L is the connecting rod i quality, is the center of mass C i Acceleration;

[0041] Push each connecting rod L outward from the first connecting rod i Moment of inertia on:

[0042] in For connecting rod L i The moment of inertia,i ( Ci I i ) is the connecting rod L i The inertia tensor of is the connecting rod coordinate system B i The angular acceleration at the origin, i ω i is the connecting rod coordinate system B i Angular velocity at the origin.

[0043] Furthermore, the force and torque on the connecting rod are deduced from the sixth connecting rod and the sixth coordinate origin of the robot arm to the first connecting rod and the first coordinate origin. Among the torques of the joints:

[0044] The sixth connecting rod pushes each connecting rod L inwardly i Force on:

[0045] in i F i,i-1 For connecting rod L i The force on i R i+1 is the connecting rod coordinate system B i+1 To connecting rod coordinate system B i The rotation matrix, m i For connecting rod L i quality, i g is the connecting rod L i The gravitational acceleration, For connecting rod L i The inertial force, 7 F 7,6 =[0,0,0] T ;

[0046] The sixth connecting rod pushes each connecting rod L inwardly i Torque on:

[0047] in i M i,i-1 For connecting rod L i The torque on i R i+1 is the connecting rod coordinate system B i+1 To connecting rod coordinate system B i The rotation matrix of i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of i P Oi,Ci is the connecting rod coordinate system B i To the centroid C i The position vector of i F i,i-1For connecting rod L i The force on is the inertia moment of the connecting rod Li, 7 M 7,6 =[0,0,0] T ;

[0048] Calculate the axis and moment of each joint:

[0049] τ i =( i-1 Z i-1 ) T i-1 M i,i-1 , where τ i is the joint axis J i Joint torque, i-1 Z i-1 =[0,0,1] T , i-1 M i,i-1 For connecting rod L i The torque on .

[0050] Furthermore, the six-degree-of-freedom robot dynamics model And the gravity vector G(θ) contains the gravitational acceleration g, let The inertia matrix M(θ) can be obtained, let The Coriolis force vector make The gravity vector G(θ) can be obtained.

[0051] Furthermore, the parameters of each link and joint of the industrial six-degree-of-freedom robot arm include link length, link angle, link offset and joint angle.

[0052] A second aspect of the present invention provides a robotic arm control system based on general kinematics and dynamics modeling, comprising:

[0053] A parameter determination module, which is used to determine the parameters of each link and joint of an industrial six-degree-of-freedom robotic arm;

[0054] A kinematic model building module is used to combine the parameters of the links and joints of the industrial six-degree-of-freedom robot and the established kinematic model of the end of the robot to obtain the homogeneous transformation matrix of the end effector of the robot relative to the coordinate system of the robot base;

[0055] The dynamic model building module is used to build a six-degree-of-freedom industrial robot dynamic mathematical model by combining the kinematic model of the end of the robot arm and analyzing the motion of the driven robot arm according to the torque applied by the driver and the external force acting on the robot arm;

[0056] The control module is used to control the movement of the robot arm by inversely solving the rotation angle of each joint movement of the robot arm according to the expected position and posture of the end effector of the robot arm in space, combined with the homogeneous transformation matrix of the end effector of the robot arm relative to the coordinate system of the robot arm base.

[0057] Compared with the prior art, the present invention has the following beneficial effects:

[0058] The present invention forms a complete and effective universal dynamic modeling method for industrial six-degree-of-freedom robotic arms of different models and sizes, and designs an inverse kinematics method for the six-degree-of-freedom robotic arm based on the constructed kinematic model to ensure that the predetermined trajectory of the robotic arm end effector is converted into the expected rotation angle of each joint axis. This ensures the versatility and portability of the model when faced with diverse robotic arm application requirements.

[0059] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0061] Figure 1 is a flow chart of a robot arm control method based on general kinematics and dynamics modeling provided by an embodiment of the present invention;

[0062] Figure 2 is a general structural diagram of a six-degree-of-freedom industrial robot arm provided by an embodiment of the present invention;

[0063] Figure 3 A six-degree-of-freedom industrial robot arm provided by an embodiment of the present invention is established based on the DH kinematic coordinate system;

[0064] Figure 4 1 is a comparison diagram of the tracking effect of axis 1 to axis 3 provided by an embodiment of the present invention; wherein (a) is a comparison diagram of the tracking effect of the first axis, (b) is a comparison diagram of the tracking effect of the second axis, and (c) is a comparison diagram of the tracking effect of the third axis;

[0065] Figure 5 4-axis-6 tracking effect comparison diagram provided by an embodiment of the present invention; wherein (a) is a 4-axis tracking effect comparison diagram, (b) is a 5-axis tracking effect comparison diagram, and (c) is a 6-axis tracking effect comparison diagram;

[0066] Figure 61 is a comparison diagram of axis 1-axis 3 tracking errors provided by an embodiment of the present invention; wherein (a) is a comparison diagram of the tracking error of the first axis, (b) is a comparison diagram of the tracking error of the second axis, and (c) is a comparison diagram of the tracking error of the third axis;

[0067] Figure 7 It is a comparison diagram of axis 4-axis 6 tracking errors provided by an embodiment of the present invention; wherein, (a) is a comparison diagram of the tracking error of the 4th axis, (b) is a comparison diagram of the tracking error of the 5th axis, and (c) is a comparison diagram of the tracking error of the 6th axis. DETAILED DESCRIPTION

[0068] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0069] It should be noted that the following detailed descriptions are all illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0070] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0071] The industrial six-degree-of-freedom universal manipulator is widely used in the fields of automobile manufacturing, electronic product assembly, logistics sorting, etc. However, due to its multi-degree-of-freedom, coupling and nonlinear characteristics, the dynamic characteristics of the manipulator become complicated, which has become a major challenge to improve the control performance. Therefore, it is urgent to study the general method of kinematic and dynamic modeling of the industrial six-degree-of-freedom manipulator. However, the complete and effective method of theoretical modeling of the manipulator based on traditional DH modeling and improved DH modeling has not been fully proposed, and it is impossible to adjust the modeling of actual manipulators of different models and sizes. In view of the above problems, the present invention proposes a general modeling method for the kinematics and dynamics of a six-degree-of-freedom manipulator, and combines the BackStepping control method to perform end effector trajectory tracking control on the industrial six-degree-of-freedom manipulator, and test the application effect of the model.

[0072] Embodiment 1

[0073] like Figure 2 Shown is a general structural diagram of a six-degree-of-freedom industrial robot arm, which includes 6 joint axes, 6 connecting rods, and 1 base.

[0074] Based on this robotic arm structure, a robotic arm control method based on general kinematic and dynamic modeling is designed. The main steps include: constructing a kinematic model based on traditional DH, constructing a dynamic model based on DH, designing an inverse kinematics method based on the constructed kinematic model, and combining the constructed dynamic model with the traditional BackStepping control method to perform end effector trajectory tracking control on an industrial six-degree-of-freedom robotic arm to verify the control effect.

[0075] like Figure 1 As shown, the specific steps include:

[0076] Step 1: Construct a DH-based kinematic model

[0077] The kinematic model of an industrial six-degree-of-freedom robot arm is an accurate description of the motion relationship between the coordinate systems of the robot arm's links. It can obtain the kinematic equation of the end of the robot arm, that is, the functional relationship between the position and posture of the end effector and the base of the robot arm. The modeling process is as follows: definition of necessary variables, establishment of coordinate system, determination of standard DH parameters, construction of standard DH conversion matrix, and representation of the end effector in base coordinates.

[0078] Step 101: Required variable definitions

[0079] a i : Link length, joint axis J i and joint axis J i+1 The length of the common perpendicular line between the two axes;

[0080] α i : Link angle, joint axis J i and joint axis J i+1 The angle between

[0081] d i : Connecting rod offset, common vertical line a i With joint axis J i The intersection of i+1 With joint axis J i The directed distance of the intersection point;

[0082] θ i : Joint angle, common vertical line a i The extension line and the common perpendicular line a i+1 Around the joint axis J i The angle formed by the rotation.

[0083] Wherein, i is the joint axis number from the robot arm base to the end effector. In this embodiment, i=1, 2, ..., 6.

[0084] Step 102: Connecting rod fixed coordinate system B i Establish:

[0085] In order to describe the relative position relationship of each connecting rod, a fixed coordinate system is set for each connecting rod. The principles for establishing the coordinate system are as follows:

[0086] 1. Z i Axis along joint axis J i+1 Axial direction;

[0087] 2.Origin O i Z i-1 With Z i The intersection of the axis or its common perpendicular line and the joint axis J i The intersection of

[0088] 3.X i Axis along Z i-1 With Z i The common perpendicular direction of the axis is from the joint axis J i Pointing to joint axis J i+1 ;

[0089] 4.Y i The axis is determined according to the right-hand rule.

[0090] According to the coordinate system establishment principle, a six-degree-of-freedom industrial robot arm coordinate system based on the DH kinematic coordinate system is established, such as Figure 3 shown.

[0091] Step 103: Determine the standard DH parameters:

[0092] First, the connecting rod length a i : Along X i Axis, Z i-1 With Z i distance;

[0093] Second, the connecting rod angle α i : Around X i Axis, Z i-1 With Z i Angle

[0094] Third, the joint angle θ i : Around Z i Axis, X i-1 With X i Angle

[0095] Fourth, joint offset d i : Along Z i-1 Axis, X i-1 With X i distance.

[0096] The general DH parameters of the six-DOF industrial robot arm are shown in Table 1.

[0097] Table 1 General DH parameters of six-DOF industrial robot arms

[0098]

[0099] Step 104: Standard DH conversion matrix construction:

[0100] First, establish the rotation matrix between adjacent coordinate systems (connecting rod fixed coordinate system B i The coordinates on B i-1 Indicated in ):

[0101]

[0102] Based on the rotation matrix, the transformation matrix between adjacent coordinate systems (connecting rod fixed coordinate system B i The coordinates on B i-1 Indicated in ):

[0103]

[0104] Step 105: The end of the actuator is expressed in base coordinates:

[0105] The last coordinate system of the six-DOF manipulator is represented in the base coordinates by the transformation matrix between adjacent coordinate systems, that is, the representation of the actuator end in the base coordinates:

[0106] 0 T6= 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6,

[0107] The established kinematic model is the representation of the actuator end in base coordinates. 0 T6.

[0108] At this point, the kinematic model of the end effector is established, that is, the functional relationship between the position and posture of the end effector and the base of the robot.

[0109] In this embodiment, the standard DH kinematic modeling is implemented by Simulink, and the kinematic modeling function (forward_kinematic_pack) receives the input theta (corresponding to the θ in the DH parameter) i change value), theta_offset (corresponding to the θ in the DH parameter i Initial value), d (corresponding to d in DH parameter i ), a (corresponding to a in DH parameter i ), alpha (corresponding to α in DH parameter i) does not need to be changed, and outputs the homogeneous transformation matrix of the actuator end relative to the robot base coordinate system.

[0110] Step 2: Combined with the kinematic model of the end of the robot arm, the motion analysis of the driven robot arm is carried out according to the torque applied by the driver and the external force acting on the robot arm, and a mathematical model of the dynamics of the six-degree-of-freedom industrial robot arm is constructed;

[0111] The industrial six-degree-of-freedom robot dynamics model describes in detail how the torque applied by the driver and the external force acting on the robot jointly affect and drive the movement of the robot. The establishment of this model is the basis for understanding the dynamic behavior of the robot, improving the movement accuracy of the robot, and optimizing the design and control strategy of the robot.

[0112] The modeling process is as follows:

[0113] Step 201: First, coordinate system B is derived by extrapolating from the first link and the first coordinate origin of the robot arm to the sixth link and the sixth coordinate origin. i Angular velocity, angular acceleration, linear velocity, linear acceleration of the origin, center of mass C of the connecting rod i The acceleration of the connecting rod L i Gravitational acceleration, inertial force, and inertial moment;

[0114] Among them, the connecting rod coordinate system B is extrapolated from the connecting rod coordinate system 1. i (i=1,2,...,6) Angular velocity of the origin:

[0115] Coordinate system B i Angular velocity at the origin: in i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix (obtained from the kinematic model), is the joint axis J i Angular velocity (measured by the sensor), i-1 Z i-1 =[0,0,1] T , 0 ω0=[0,0,0] T .

[0116] Among them, the connecting rod coordinate system B is extrapolated from the connecting rod coordinate system 1. i Angular acceleration of the origin (i=1,2,...,6):

[0117] Coordinate system B i Angular acceleration at the origin: in is the connecting rod coordinate system B i The angular acceleration at the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of is the joint axis J i Angular velocity, is the joint axis J i Angular acceleration (measured by sensor or by The joint angular velocity is derived), i-1 Z i-1 =[0,0,1] T , i-1 ω i-1 is the connecting rod coordinate system B i-1 Angular velocity of the origin (obtained in step 1),

[0118] Among them, the connecting rod coordinate system B is extrapolated from the connecting rod coordinate system 1. i (i=1,2,...,6) Linear velocity of the origin:

[0119] Coordinate system B i Linear velocity at the origin: i v i = i R i-1 i-1 v i-1 + i ω i × i P Oi-1,Oi ,in i v i is the connecting rod coordinate system B i The linear velocity at the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector (obtained by measuring the size of the robot arm), 0 v0=[0,0,0] T .

[0120] Among them, the connecting rod coordinate system B is extrapolated from the connecting rod coordinate system 1. i (i=1,2,...,6) Linear acceleration of the origin:

[0121] Coordinate system B i Linear acceleration at the origin: in is the connecting rod coordinate system B i The linear acceleration at the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of

[0122] Among them, the center of mass C is extrapolated from the connecting rod center of mass 1 i Acceleration:

[0123] Centroid C i Acceleration: in is the center of mass C i The acceleration of is the connecting rod coordinate system B i The linear acceleration at the origin, i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of .

[0124] Among them, connecting rod 1 pushes each connecting rod L i Gravitational acceleration of (i=1,2,...,6):

[0125] Gravity acceleration: i g= i R i-1 i-1 g, where i g is the connecting rod L i The gravitational acceleration, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of 0 g=9.81.

[0126] Among them, connecting rod 1 pushes each connecting rod L iInertial force on (i=1,2,...,6):

[0127] Connecting rod L i Inertial force on: in L is the connecting rod i Inertial force, m i L is the connecting rod i Quality (measured), is the center of mass C i Acceleration.

[0128] Among them, connecting rod 1 pushes each connecting rod L i Moment of inertia on (i=1,2,...,6):

[0129] Connecting rod L i Moment of inertia on: in L is the connecting rod i The moment of inertia, i ( Ci I i ) is the connecting rod L i The inertia tensor of (fixed value of the robot arm size), is the connecting rod coordinate system B i The angular acceleration at the origin, i ω i is the connecting rod coordinate system B i Angular velocity at the origin.

[0130] Step 202: Next, the force and torque on the link i are deduced from the sixth link and the sixth coordinate origin of the robot arm to the first link and the first coordinate origin, and the joint axis J i The torque;

[0131] Among them, the connecting rod L is pushed inward by the connecting rod 6. i Force on (i=1,2,...,6):

[0132] Connecting rod L i Force on: in i F i,i-1 L is the connecting rod i The force on i R i+1 is the connecting rod coordinate system B i+1 To connecting rod coordinate system B i The rotation matrix, m i L is the connecting rod i quality, i g is the connecting rod L i The gravitational acceleration, L is the connecting rod i The inertial force,7 F 7,6 =[0,0,0] T .

[0133] Among them, the connecting rod L is pushed inward by the connecting rod 6. i Torque on (i=1,2,...,6):

[0134] Connecting rod L i Torque on: in i M i,i-1 L is the connecting rod i The torque on i R i+1 is the connecting rod coordinate system B i+1 To connecting rod coordinate system B i The rotation matrix of i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of i P Oi,Ci is the connecting rod coordinate system B i To the centroid C i The position vector (obtained by measurement), i F i,i-1 L is the connecting rod i The force on L is the connecting rod i The moment of inertia, 7 M 7,6 =[0,0,0] T .

[0135] Among them, calculate each joint axis J i Joint torque:

[0136] Joint torque: τ i =( i-1 Z i-1 ) T i-1 M i,i-1 , where τ i is the joint axis J i Joint torque, i-1 Z i-1 =[0,0,1] T , i-1 M i,i-1 L is the connecting rod i The torque on .

[0137] Step 203: Finally, according to the joint axis J i The inertia matrix M, Coriolis force C, and gravity G in the dynamic model of the six-degree-of-freedom industrial robot arm are calculated by torque calculation, and the mathematical dynamic model of the six-degree-of-freedom industrial robot arm is constructed.

[0138] Among them, the torque expression of the industrial six-degree-of-freedom robot arm is τ is the joint torque, M(θ) is the inertia matrix of the six-degree-of-freedom manipulator, is the Coriolis force matrix of the six-degree-of-freedom robot, G(θ) is the gravity vector, θ is the joint rotation angle, is the joint angular velocity, is the joint angular acceleration, and the detailed establishment method is as follows:

[0139] Among them, the inertia matrix M(θ) is calculated according to the joint torque:

[0140] Dynamic model of six-degree-of-freedom robot And the gravity vector G(θ) contains the gravity acceleration g, so The inertia matrix M(θ) can be obtained.

[0141] Among them, the Coriolis force vector is calculated based on the joint torque

[0142] Dynamic model of six-degree-of-freedom robotic arm And the gravity vector G(θ) contains the gravity acceleration g, so The Coriolis force vector can be obtained

[0143] Among them, the gravity vector G(θ) is calculated according to the joint torque:

[0144] Dynamic model of six-degree-of-freedom robotic arm make The gravity vector G(θ) can be obtained.

[0145] Step 3: Construct the inverse kinematic solution of the robot end effector;

[0146] That is, it is a process of reversely solving the angles that each joint of the robot arm should reach based on the expected position and posture of the robot arm's end effector (operating tool, gripper, welding gun, etc.) in space.

[0147] The detailed steps are as follows:

[0148] Step 301: Obtain the representation of the actuator end in base coordinates through the kinematic model 0 T6;

[0149] Step 302: Make

[0150] because 0 T6(θ6)= 0 T1(θ1) 1 T2(θ2) 2 T3(θ3)3 T4(θ4) 4 T5(θ5) 5 T6(θ6), that is

[0151]

[0152] Among them, the 3*3 matrix composed of a11-a33 is the expected posture of the end effector of the robot arm in space; the 3*1 vector composed of p, q, and r is the expected position of the end effector of the robot arm in space, p is the coordinate on the x-axis of the spatial coordinate, q is the coordinate on the y-axis of the spatial coordinate, and r is the coordinate on the z-axis of the spatial coordinate;

[0153] Step 303: Substitute 0 T1 is moved right to the right side of the equal sign, that is:

[0154]

[0155] Step 304: The above equations correspond to each other to obtain the rotation angle θ1 of the joint 1 corresponding to the end trajectory of the robot arm actuator;

[0156] Step 305: Repeat the above steps to obtain the rotation angles θ2-θ6 of the joints 2-6 corresponding to the end trajectory of the robot arm actuator, that is, the inverse kinematic solution of the robot arm end effector is completed.

[0157] Step 4: Verify the end effector trajectory tracking control effect of the industrial six-degree-of-freedom robot arm;

[0158] In order to prove the improvement effect of the industrial six-degree-of-freedom robot dynamics model on the control effect, the present invention adopts the obtained dynamics model combined with the traditional BackStepping control method, and compares it with the PD control method without the dynamics model. It can be seen that the traditional BackStepping control method using this dynamics model has a faster tracking speed and smaller transient error and steady-state error. The comparison is as follows:

[0159] The tracking effect of each joint axis of the six-degree-of-freedom robot arm is achieved by combining the traditional BackStepping control method with the PD control method without a dynamic model based on the obtained dynamic model. Figure 4 and Figure 5As shown, the red solid line is based on the obtained dynamic model combined with the traditional BackStepping control method, the green dotted line is the PD control method without the dynamic model, and the blue dotted line is the set expected trajectory. It can be clearly seen that the dynamic model obtained in combination with the traditional BackStepping control method has a faster tracking speed and a smaller steady-state error, and has a better control effect. This also proves that the six-degree-of-freedom robot arm dynamics model proposed in the present invention not only enables the control algorithm to obtain an application basis, but also enables the control algorithm to obtain better control performance.

[0160] The tracking error of each joint axis of the six-degree-of-freedom robot arm is obtained by combining the traditional BackStepping control method with the PD control method without a dynamic model. Figure 6 and 7 As shown, the red solid line is based on the obtained dynamic model combined with the traditional BackStepping control method, and the blue dotted line is the PD control method without the dynamic model. It can be clearly seen that the blue dotted line is larger than the red solid line in the front section of tracking, that is, the dynamic model combined with the traditional BackStepping control method has a smaller transient error. In addition, after the system enters the steady-state stage, the blue dotted line is still larger than the red solid line, that is, the dynamic model combined with the traditional BackStepping control method has a smaller steady-state error and better control effect. This also proves that the six-degree-of-freedom robot arm dynamics model proposed in the present invention not only enables the control algorithm to obtain an application basis, but also enables the control algorithm to obtain better control accuracy.

[0161] Embodiment 2

[0162] This embodiment provides a robotic arm control system based on general kinematics and dynamics modeling, including:

[0163] A parameter determination module, which is used to determine the parameters of each link and joint of an industrial six-degree-of-freedom robotic arm;

[0164] A kinematic model building module is used to combine the parameters of the links and joints of the industrial six-degree-of-freedom robot and the established kinematic model of the end of the robot to obtain the homogeneous transformation matrix of the end effector of the robot relative to the coordinate system of the robot base;

[0165] The dynamic model building module is used to build a six-degree-of-freedom industrial robot dynamic mathematical model by combining the kinematic model of the end of the robot arm and analyzing the motion of the driven robot arm according to the torque applied by the driver and the external force acting on the robot arm;

[0166] A control module is used to reversely solve the rotation angle of each joint movement of the robot arm according to the desired position and posture of the robot arm end effector in space and the homogeneous transformation matrix of the robot arm end effector relative to the robot arm base coordinate system;

[0167] The control verification module is used to verify the trajectory tracking control effect of the end effector of the industrial six-degree-of-freedom robot arm based on the rotation angle of each joint movement of the robot arm and the mathematical model of the dynamics of the six-degree-of-freedom industrial robot arm.

[0168] Embodiment 3

[0169] This embodiment provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by a processor, the steps in the robot arm control method based on general kinematics and dynamics modeling as described above are implemented.

[0170] Embodiment 4

[0171] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps in the robot arm control method based on general kinematic and dynamic modeling as described above are implemented.

[0172] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A robot arm control method based on general kinematics and dynamics modeling, characterized in that: include: Determine the parameters of each link and joint of the industrial six-degree-of-freedom robot arm; Combining the parameters of the links and joints of the industrial six-degree-of-freedom robot and the established kinematic model of the end of the robot, the homogeneous transformation matrix of the end effector of the robot relative to the coordinate system of the robot base is obtained; Combined with the kinematic model of the end of the robot arm, the motion analysis of the driven robot arm is carried out according to the torque applied by the driver and the external force acting on the robot arm, and a mathematical model of the dynamics of the six-degree-of-freedom industrial robot arm is constructed; According to the expected position and posture of the end effector of the robot arm in space, combined with the homogeneous transformation matrix of the end effector of the robot arm relative to the coordinate system of the robot arm base, the rotation angle of each joint movement of the robot arm is inversely solved to control the movement of the robot arm.

2. The robot arm control method based on general kinematics and dynamics modeling as claimed in claim 1, characterized in that: The process of constructing the kinematic model of the end of the robotic arm includes: Establish the connecting rod fixed connection coordinate system based on the preset coordinate system establishment principle; Establish the rotation matrix between adjacent coordinate systems based on the connecting rod fixed coordinate system; Construct a transformation matrix between adjacent coordinate systems based on the rotation matrix between adjacent coordinate systems; By converting the transformation matrix between adjacent coordinate systems, the representation of the last coordinate system of the six-degree-of-freedom robot arm in the base coordinates is obtained, that is, the functional relationship between the position and posture of the end effector and the base of the robot arm. At this point, the kinematic model of the end of the robot arm is completed.

3. The robot arm control method based on general kinematics and dynamics modeling as claimed in claim 2, characterized in that: The preset coordinate system establishment principles include: First, Z i Axis along joint axis J i+1 Axial direction; Second, the origin O i Z i-1 With Z i The intersection of the axis or its common perpendicular line and the joint axis J i The intersection of Third, X i Axis along Z i-1 With Z i The common perpendicular direction of the axis is from the joint axis J i Pointing to joint axis J i+1 ; Fourth, Y i The axis is determined according to the right-hand rule.

4. The robot arm control method based on general kinematics and dynamics modeling as claimed in claim 1, characterized in that: The motion analysis of the driven robotic arm based on the torque applied by the driver and the external force acting on the robotic arm is used to construct a six-degree-of-freedom industrial robotic arm dynamic mathematical model, including: Extrapolate the angular velocity, angular acceleration, linear velocity and linear acceleration of the coordinate origin, the acceleration of the center of mass of the connecting rod, the gravity acceleration, inertial force and inertial moment of the connecting rod from the first connecting rod and the first coordinate origin of the robot arm to the sixth connecting rod and the sixth coordinate origin; Infer the force and torque on the link and the torque of the joint from the sixth link and the sixth coordinate origin of the robot arm to the first link and the first coordinate origin; According to the torque calculation of the joints, the inertia matrix, Coriolis force and gravity in the dynamic model of the six-degree-of-freedom industrial robot arm are obtained, and the construction of the mathematical dynamic model of the six-degree-of-freedom industrial robot arm is completed.

5. The robot arm control method based on general kinematics and dynamics modeling as claimed in claim 1, characterized in that: From the first link and the first coordinate origin of the robot arm to the sixth link and the sixth coordinate origin, the angular velocity, angular acceleration, linear velocity and linear acceleration of the coordinate origin, the acceleration of the center of mass of the link, the gravity acceleration, inertia force and inertia moment of the link are extrapolated: Extrapolate the coordinate systems of each link from the first link coordinate system B i The angular velocity of the origin is: in, i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of is the joint axis J i Angular velocity, i-1 Z i-1 =[0,0,1] T , 0 ω0=[0,0,0] T ; Extrapolate the coordinate systems of each link from the first link coordinate system B i Angular acceleration at the origin: in is the connecting rod coordinate system B i The angular acceleration at the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of is the joint axis J i Angular velocity, is the joint axis J i Angular acceleration, i-1 Z i-1 =[0,0,1] T , i-1 ω i-1 is the connecting rod coordinate system B i-1 Angular velocity of the origin; Extrapolate the coordinate systems of each link from the first link coordinate system B i Linear velocity at the origin: i v i = i R i-1 i-1 v i-1 + i ω i × i P Oi-1,Oi ,in i v i is the connecting rod coordinate system B i The linear velocity at the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of Extrapolate the coordinate systems of each link from the first link coordinate system B i Linear acceleration at the origin: in is the connecting rod coordinate system B i The linear acceleration at the origin, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of Extrapolate the center of mass C of each link from the center of mass of the first link i Acceleration: in is the center of mass C i The acceleration of is the connecting rod coordinate system B i The linear acceleration at the origin, i ω i is the connecting rod coordinate system B i The angular velocity of the origin, i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of Push each connecting rod L outward from the first connecting rod i The gravitational acceleration is: i g= i R i-1 i-1 g, where i g is the connecting rod L i The gravitational acceleration, i R i-1 is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The rotation matrix of 0 g = 9.81; Push each connecting rod L outward from the first connecting rod i Inertial force on: in L is the connecting rod i Inertial force, m i L is the connecting rod i quality, is the center of mass C i Acceleration; Push each connecting rod L outward from the first connecting rod i Moment of inertia on: in L is the connecting rod i The moment of inertia, i ( Ci I i ) is the connecting rod L i The inertia tensor of is the connecting rod coordinate system B i The angular acceleration at the origin, i ω i is the connecting rod coordinate system B i Angular velocity at the origin.

6. The robot arm control method based on general kinematics and dynamics modeling as claimed in claim 1, characterized in that: From the sixth link and the sixth coordinate origin of the robot arm to the first link and the first coordinate origin, the forces and moments on the links are deduced. Among the moments of the joints: The sixth connecting rod pushes each connecting rod L inwardly i Force on: in i F i,i-1 L is the connecting rod i The force on i R i+1 is the connecting rod coordinate system B i+1 To connecting rod coordinate system B i The rotation matrix, m i L is the connecting rod i quality, i g is the connecting rod L i The gravitational acceleration, L is the connecting rod i The inertial force, 7 F 7,6 =[0,0,0] T ; The sixth connecting rod pushes each connecting rod L inwardly i Torque on: in i M i,i-1 L is the connecting rod i The torque on i R i+1 is the connecting rod coordinate system B i+1 To connecting rod coordinate system B i The rotation matrix of i P Oi-1,Oi is the connecting rod coordinate system B i-1 To connecting rod coordinate system B i The position vector of i P Oi,Ci is the connecting rod coordinate system B i To the centroid C i The position vector of i F i,i-1 L is the connecting rod i The force on L is the connecting rod i The moment of inertia, 7 M 7,6 =[0,0,0] T ; Calculate each joint axis J i Joint torque: τ i =( i-1 Z i-1 ) Ti-1 M i,i-1 , where τ i is the joint axis J i Joint torque, i-1 Z i-1 =[0,0,1] T , i-1 M i,i-1 L is the connecting rod i The torque on .

7. The robot arm control method based on general kinematics and dynamics modeling as claimed in claim 1, characterized in that: Dynamic model of six-degree-of-freedom robotic arm And the gravity vector G(θ) contains the gravitational acceleration g, let The inertia matrix M(θ) can be obtained, let The Coriolis force vector make The gravity vector G(θ) can be obtained.

8. The robot arm control method based on general kinematics and dynamics modeling as claimed in claim 1, characterized in that: The parameters of each link and joint of the industrial six-degree-of-freedom robot arm include link length, link angle, link offset and joint angle.

9. The robot arm control method based on general kinematics and dynamics modeling as claimed in claim 1, characterized in that: When verifying the trajectory tracking control effect of the end effector of an industrial six-degree-of-freedom robot arm, the BackStepping control method is used.

10. A robotic arm control system based on general kinematics and dynamics modeling, characterized in that: include: A parameter determination module, which is used to determine the parameters of each link and joint of an industrial six-degree-of-freedom robotic arm; A kinematic model building module is used to combine the parameters of the links and joints of the industrial six-degree-of-freedom robot and the established kinematic model of the end of the robot to obtain the homogeneous transformation matrix of the end effector of the robot relative to the coordinate system of the robot base; The dynamic model building module is used to build a six-degree-of-freedom industrial robot dynamic mathematical model by combining the kinematic model of the end of the robot arm and analyzing the motion of the driven robot arm according to the torque applied by the driver and the external force acting on the robot arm; The control module is used to control the movement of the robot arm by inversely solving the rotation angle of each joint movement of the robot arm according to the expected position and posture of the end effector of the robot arm in space, combined with the homogeneous transformation matrix of the end effector of the robot arm relative to the coordinate system of the robot arm base.

Images