Picking mechanical arm coupling dynamics modeling method and device, terminal and medium

Abstract

The invention relates to the technical field of mechanical arm modeling, and particularly discloses a picking mechanical arm coupling dynamics modeling method and device, a terminal and a readable storage medium. The method comprises the steps that the structural composition of the picking mechanical arm is obtained, and a structural model is established; constructing a connecting rod coordinate system of adjacent joints based on an improved D-H parameter method; establishing forward and inverse kinematics models based on the connecting rod coordinate system; parameters of flexible elements in the mechanical arm are collected, and a joint flexible connection model and an elastic dynamic model are constructed; and further introducing a coupling constraint condition, and forming a coupling dynamics modeling framework by combining the elastic dynamics model and the overall dynamics model. According to the model, joint flexibility, structural vibration and dynamic response nonlinear characteristics can be considered, the modeling precision and control robustness of the picking mechanical arm under complex working conditions are improved, and the model is suitable for modeling and control algorithm design of the mechanical arm of a flexible structure.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm modeling, and in particular relates to a method, device, terminal and medium for modeling coupled dynamics of a picking robot arm. Background Art

[0002] With the rapid development of smart agriculture and automation technologies, harvesting robotic arms have garnered widespread attention in a variety of applications, including fruit and vegetable picking and greenhouse management. As a key component of automated agricultural machinery, the performance of these robotic arms is directly related to harvesting efficiency, fruit damage rates, and the economic and reliability of the system.

[0003] The dynamic modeling of traditional harvesting manipulators is mostly based on the rigid body assumption. By establishing a rigid body linkage model and applying the Newton-Euler method or Lagrangian method to construct the dynamic equations, trajectory planning and control strategy design are achieved. However, in actual agricultural operation environments, harvesting manipulators are usually made of multiple sections of lightweight materials, and there are often certain flexible connections between the joints, such as elastic joints or flexible drive units. Such flexible structures can cause structural vibration, end-deviation amplification, and response delays during rapid or high-precision movements. If these flexible factors are ignored, the control strategy will deviate from the actual response, and in severe cases, even cause system instability.

[0004] To overcome the limitations of rigid modeling methods in modeling flexible structures, some studies have begun to introduce flexible body dynamics modeling theory, and improve model accuracy through elastic body modeling, modal superposition, finite element method, etc. However, existing flexible modeling methods often face the following problems when applied to robotic arm systems: (1) There is nonlinear coupling between flexible deformation and joint drive, which is difficult to efficiently characterize with traditional methods; (2) The flexible model is complex to solve and cannot meet the real-time control requirements; (3) There is a lack of a unified coupling modeling framework, and the modeling process relies on a large number of prior assumptions, resulting in poor generalization ability. Summary of the Invention

[0005] In response to the problems in the prior art, the present invention provides a method, device, terminal and medium for modeling the coupled dynamics of a picking robot arm, which solves the problem that the robot arm modeling scheme in the prior art is often difficult to balance modeling accuracy and computational efficiency when dealing with multi-degree-of-freedom structures, complex working conditions and coupling of control algorithms, resulting in difficulty in controller design and poor stability in actual deployment.

[0006] The technical solution adopted in the present invention is as follows:

[0007] In a first aspect, the present application provides a method for modeling coupled dynamics of a picking robot arm, comprising the following steps:

[0008] Step S1, obtaining the actual structural composition of the picking robot arm, modeling the structure of the picking robot arm, and obtaining a picking robot arm structure model;

[0009] Step S2: establishing a link coordinate system of adjacent rotation joints based on the picking robot arm structure model in step S1 and the improved DH parameter method;

[0010] Step S3: establishing a forward kinematics model of the picking robot arm based on the picking robot arm structure model in step S1 and the picking robot arm connecting rod coordinate system in step S2;

[0011] According to the forward kinematics model of the picking robot arm and the algebraic method, the inverse kinematics model of the robot arm is derived and established;

[0012] Step S4: Acquire the elastic element data contained in the picking robot arm, establish a flexible deformation model for the flexible connection part, model each connecting rod as a joint flexible element, and construct an elastic dynamics model;

[0013] Step S5: construct a dynamic model of the picking robot arm, introduce coupling constraints based on the dynamic model and the elastic dynamic model, and construct a coupled dynamic model.

[0014] Preferably, in step S1, the picking robot arm structure model includes a combination of several rigid links and several rotational joints, wherein the links are connected by joints.

[0015] Preferably, a three-dimensional vector is used to represent the position in the reference coordinate system, and a third-order matrix is ​​used to represent the posture in the reference coordinate system. The link coordinate system {B} is represented relative to the reference coordinate system {A} as follows:

[0016]

[0017] in, represents the position vector of the origin of coordinate system {B} in {A}, p x 、p y 、p z Indicates that x in {A} A 、y A 、z A Components of the three axes; Represents the attitude matrix of the coordinate system {B} in {A}, called the rotation matrix relative to {A}; x B 、y B 、z B Represents the three unit axis vectors of the coordinate system {B}, using the unit coordinate axis x A 、y A 、z A Expressed as:

[0018]

[0019] therefore:

[0020] The homogeneous transformation matrix of coordinate system {B} relative to coordinate system {A} is defined as:

[0021] For the relative relationship between several coordinate systems, the operation rules followed are:

[0022] Where, Represents the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {A}, Represents the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {B};

[0023] Represents the velocity state of the connecting rod, including the connecting rod linear velocity and the connecting rod angular velocity; the three linear velocities and three angular velocities constitute the motion spinor; let the coordinate system {A} be the world coordinate system, and the coordinate system {B} be the connecting rod coordinate system attached to the robot arm connecting rod. The velocity of the coordinate system {B} in the coordinate system {A} is expressed as:

[0024] Among them, A Represents the linear velocity vector of the connecting rod, that is, the linear velocity vector of the origin of the connecting rod coordinate system {B}; ω A represents the angular velocity vector of the connecting rod, represents the unit rotation axis of the angular velocity, Indicates the magnitude of the angular velocity; the motion spin of the connecting rod in the world coordinate system is:

[0025]

[0026] in, Represents the velocity Jacobian matrix of the link coordinate system {B} in the world coordinate system.

[0027] Preferably, in step S2, joint i-1 and joint i are two adjacent joints, and coordinate system {i-1} and coordinate system {i} are corresponding link coordinate systems; the steps for establishing the link coordinate system are as follows:

[0028] Step S2-1, determine the z-axis direction, z i The axis coincides with the rotation axis of joint i, z i-1 The axis coincides with the rotation axis of joint i-1, and the positive direction of the z axis is the rotation direction of the corresponding joint;

[0029] Step S2-2, determine the origin of the coordinate system and obtain z i-1 Axis and z i Common normal to the axis;

[0030] If the common normal is unique, the origin of the coordinate system {i-1} is located at the intersection of the common normal and the axis of joint i-1;

[0031] If the common normal is not unique, manually select and set the origin of the coordinate system according to the actual situation;

[0032] If the common normal does not exist, the origin of the coordinate system {i-1} is located at the intersection of the two axes;

[0033] Step S2-3, determining the x-axis and y-axis directions;

[0034] The direction of the common normal is selected as the x-axis direction, and the y-axis direction is determined according to the right-hand rule.

[0035] Preferably, the transformation matrix of the two adjacent link coordinate systems is calculated based on the link coordinate system and DH parameters of the robot arm. The formula is as follows:

[0036]

[0037] The coordinate system {0} is the world coordinate system; the connecting rod length a i-1 The length of the connecting rod i-1 is z i-1 Axis and z i The length of the shaft's normal line; the connecting rod's torsion angle α i-1 is the connecting rod torsion angle α i-1 Indicates x i-1 The direction of z is the positive direction, i-1 Axis to z i Axis rotation angle; joint angle φ i For z i The direction of x is positive, i-1 Axis to x i Axis rotation angle; connecting rod offset d i From x i-1 With z i The intersection of the axes to the origin of the link coordinate system {i} along the positive direction of z i Directed distance in direction.

[0038] Preferably, in step S4, the joints of the robotic arm are equivalent to a spring-damper model, and the dynamic model of joint i is:

[0039]

[0040] Among them, τ i is the output torque of the reducer at joint i, τ mi is the motor end torque of joint i, is the rotation angle of the motor end of joint i, θ i is the rotation angle of connecting rod i, k i is the stiffness of joint i, b iis the damping of joint i, J mi is the moment of inertia of the motor rotor at joint i, J li is the moment of inertia of connecting rod i;

[0041] and τ mi The expression is:

[0042] Among them, τ motor,i Output torque of the motor rotor; N i is the transmission ratio of joint i; is the motor rotor angle of joint i.

[0043] Preferably, in step S5, it is assumed that the origin of the motor rotor reference coordinate system coincides with the origin of the connecting rod coordinate system and is located at the centroid of the motor end, and the rotation axis of the motor rotor and the connecting rod are the same;

[0044] Define the connecting rod angle as the connecting rod generalized coordinate q l The motor angle is the motor rotor and the motor generalized coordinates are q m , their generalized coordinates are expressed as:

[0045]

[0046] The kinetic energy of the connecting rod and motor rotor of the robotic arm system is obtained as:

[0047]

[0048] in, is the spatial inertia matrix of link i in the world coordinate system, is the spatial inertia matrix of the motor rotor i in the world coordinate system, is the motion rotation of link i in the world coordinate system, is the motion spin of the motor rotor i in the world coordinate system, and The expression is:

[0049]

[0050] in, is the Jacobian matrix of the link coordinate system {i} in the world coordinate system, To make The matrix z is a zero matrix with the fourth to sixth rows of the i-th column, i represents the unit vector of the axis of rotation of the motor rotor i in the world coordinate system, and

[0051] The total kinetic energy of the robotic arm system is:

[0052]

[0053] in, is the mass matrix of the connecting rod, is the mass matrix of the motor rotor; is the coupling matrix between the connecting rod and the motor rotor, is the motor moment of inertia matrix after deceleration;

[0054] The gravitational potential energy of the robotic arm system is expressed as:

[0055] Among them, m li 、m mi are the masses of the i-th connecting rod and the motor rotor, z li 、z mi are the mass centers of the i-th connecting rod and the motor rotor on the z-axis of the world coordinate system, respectively, and g is the acceleration due to gravity;

[0056] The dissipated energy generated by the elastic potential energy and friction damping of the joint flexible element is:

[0057]

[0058] Summing all potential energies and dissipated energies, the total potential energy V and total dissipated energy B are:

[0059]

[0060] The generalized coordinates of the robotic arm system are defined as The dynamic equation is obtained using the Lagrangian function L=TV:

[0061] Where Q = [Q l T Q m T ] T , Q l represents the generalized force on the connecting rod, Q m It represents the generalized force acting on the motor, and its expression is:

[0062]

[0063] Where τ ei is the external interference torque on the link i, and the dynamic equation of the flexible joint manipulator is obtained as:

[0064]

[0065] in, is the mass matrix of the complete dynamic equation, is the Coriolis matrix, is the gravity term vector, and are the stiffness matrix and damping matrix of the joint respectively;

[0066] in:

[0067]

[0068] in, is the connecting rod Coriolis matrix;

[0069] Let the coupling term S(q l )=0; Assume that the connecting rod end of the robot arm is not disturbed by external forces, that is, Q l =0; we get:

[0070]

[0071] The implicit Runge-Kutta method is used to solve the dynamic equations. Make the following variable substitutions:

[0072]

[0073] The dynamic equation of the manipulator expressed by the state equation is:

[0074]

[0075] In a second aspect, the present application provides a picking robot arm coupling dynamics modeling system, comprising:

[0076] The structural modeling module is used to obtain the actual structural composition of the picking robot arm and model the structure of the picking robot arm to obtain a structural model of the picking robot arm;

[0077] A coordinate system establishment module is used to establish the link coordinate system of adjacent rotary joints of the picking robot arm based on the improved DH parameter method and the structural model;

[0078] Kinematic modeling module, used to establish the forward kinematic model of the picking robot arm based on the structural model and the connecting rod coordinate system, and to establish the inverse kinematic model of the picking robot arm using algebraic methods based on the forward kinematic model;

[0079] The flexible modeling module is used to obtain the elastic element data contained in the picking robot arm, establish a flexible deformation model for the flexible connection parts, and model each link as a joint flexible element to construct an elastic dynamics model;

[0080] The coupled dynamics modeling module is used to introduce coupling constraints based on the dynamic model and elastic dynamics model to build a coupled dynamics model of the picking robot arm;

[0081] The calculation processing module is used to solve the generalized coordinates of the robotic arm system based on the coupled dynamics model, establish the Lagrangian dynamic equations of the system, and use the implicit Runge-Kutta method to solve the state.

[0082] In a third aspect, the present application provides a terminal, including:

[0083] A memory for storing a picking robot arm coupling dynamics modeling program;

[0084] The processor is used to implement the steps of the picking robot arm coupling dynamics modeling method as described in the first aspect when executing the picking robot arm coupling dynamics modeling system.

[0085] In a fourth aspect, the present application provides a computer-readable storage medium that stores computer instructions. When a computer reads the computer instructions in the storage medium, the computer executes a picking robot arm coupling dynamics modeling method as described in the first aspect.

[0086] It can be seen from the above technical solutions that this application has the following advantages:

[0087] 1. A unified method for modeling the coupled dynamics of a harvesting robot arm is provided. By constructing a multi-body system dynamics model including flexible arm segments and rigid joints, the integrity and theoretical consistency of the dynamics modeling are improved. The modeling capability of the influence of the deformation of the flexible arm segments on the end trajectory is also significantly enhanced, thereby effectively solving the problem of insufficient modeling accuracy and difficulty in describing the actual motion error of the end effector in the existing technology.

[0088] 2. By modeling the arm segment of the harvesting robot as a flexible beam structure and introducing the finite-order natural modal response using the modal superposition method, the elastic deformation of the arm segment can be effectively characterized while taking into account the computational efficiency and solution stability of the model without significantly increasing the amount of calculation.

[0089] 3. The Lagrangian method is used to derive the system dynamics equations, which facilitates the design of subsequent control algorithms, state observation, and parameter optimization while maintaining the closedness and structured characteristics of the system, thereby improving the engineering usability and versatility of the model. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] In order to more clearly illustrate the technical solution of the present application, the following is a brief introduction to the drawings required for the description. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0091] Figure 1 A schematic flow chart of a method for modeling coupled dynamics of a picking robot arm shown in some embodiments;

[0092] Figure 2 A schematic diagram of a coordinate system shown in some embodiments;

[0093] Figure 3 A schematic diagram of the angular velocity of the connecting rod shown in some embodiments;

[0094] Figure 4 Schematic diagram of DH parameters between connecting rods shown in some embodiments;

[0095] Figure 5 A coordinate system of a link of a picking robot arm as shown in some embodiments;

[0096] Figure 6 A Matlab Robotics Toolbox model of the picking robot arm shown in some embodiments;

[0097] Figure 7 A point cloud image of a robotic arm workspace shown in some embodiments;

[0098] Figure 8 is a cross-sectional view of a robotic arm workspace shown in some embodiments;

[0099] Figure 9 This is a joint equivalent model shown in some embodiments. DETAILED DESCRIPTION

[0100] In order to make the application objectives, features, and advantages of this application more obvious and easy to understand, the technical solutions protected by this application will be clearly and completely described below using specific embodiments and drawings. Obviously, the embodiments described below are only part of the embodiments of this application, not all of them. Based on the embodiments in this patent, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this patent.

[0101] This application provides a method for modeling coupled dynamics of a picking robot arm, including:

[0102] Step S1, obtaining the actual structural composition of the picking robot arm, modeling the structure of the picking robot arm, and obtaining a picking robot arm structure model;

[0103] Step S2: establishing a link coordinate system of adjacent rotation joints based on the picking robot arm structure model in step S1 and the improved DH parameter method;

[0104] Step S3: establishing a forward kinematics model of the picking robot arm according to the picking robot arm structure model in step S1 and the picking robot arm connecting rod coordinate system in step S2;

[0105] According to the forward kinematics model of the picking robot arm and the algebraic method, the inverse kinematics model of the robot arm is derived and established;

[0106] Step S4: Acquire the elastic element data contained in the picking robot arm, establish a flexible deformation model for the flexible connection part, model each connecting rod as a joint flexible element, and construct an elastic dynamics model;

[0107] Step S5: construct a dynamic model of the picking robot arm, introduce coupling constraints based on the dynamic model and the elastic dynamic model, and construct a coupled dynamic model.

[0108] In some embodiments, in the kinematic analysis of the manipulator, a systematic method is required to describe the position and posture (pose) of the manipulator link in the world coordinate system and the relative relationship between the links. Generally, the pose of the link is represented by attaching a reference coordinate system to the link, and the relative relationship between the coordinate systems is represented by the homogeneous transformation method. Among them, a three-dimensional vector can be used to represent the position in the reference coordinate system, and a third-order matrix can be used to represent the pose in the reference coordinate system. The link coordinate system {B} relative to the reference coordinate system {A} is represented as follows Figure 2 As shown;

[0109] The following formula is used for calculation:

[0110] Where, represents the position vector of the origin of coordinate system {B} in {A}, p x 、p y 、p z Indicates that x in {A} A 、y A 、z A Components of the three axes. Represents the attitude matrix of the coordinate system {B} in {A}, also known as the rotation matrix relative to {A}. B 、y B 、z B Represents the three unit axis vectors of the coordinate system {B}, using the unit coordinate axis x A 、y A 、z A Expressed as:

[0111]

[0112] therefore:

[0113] The homogeneous transformation matrix of coordinate system {B} relative to coordinate system {A} is defined as:

[0114] For the relative relationship between multiple coordinate systems, the operation rules are as follows:

[0115] Where, Represents the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {A}, Represents the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {B}.

[0116] When performing dynamic analysis on a robotic arm, in addition to the position and posture of the connecting rod, it is also necessary to know the velocity state of the connecting rod, including the linear velocity and angular velocity of the connecting rod. In robotics, the screw method is a commonly used method to describe the velocity state of the robotic arm, in which three linear velocities and three angular velocities constitute the motion screw. Let the coordinate system {A} be the world coordinate system, and the coordinate system {B} be the connecting rod coordinate system attached to the robotic arm connecting rod. Its velocity in the world coordinate system is expressed as follows: Figure 3 As shown;

[0117] The following formula is used for calculation:

[0118] Where, A Represents the linear velocity vector of the connecting rod, that is, the linear velocity vector of the origin of the connecting rod coordinate system {B}. A represents the angular velocity vector of the connecting rod, represents the unit rotation axis of the angular velocity, Indicates the magnitude of the angular velocity. The motion spin of the connecting rod in the world coordinate system is:

[0119] According to the above formula, we can get:

[0120]

[0121] In the formula Represents the velocity Jacobian matrix of the link coordinate system {B} in the world coordinate system.

[0122] Since all joints of the robot arm in this embodiment are rotary joints, the improved DH method is used to establish the link coordinate system of adjacent rotary joints. Figure 4 As shown;

[0123] Figure 4 In the example, joint i-1 and joint i are two adjacent joints, and the coordinate systems {i-1} and {i} are the corresponding link coordinate systems. To establish the link coordinate system: first determine the z-axis direction, z i The axis coincides with the rotation axis of joint i, z i-1 The axis coincides with the rotation axis of joint i-1, and its positive direction is the rotation direction of the corresponding joint; secondly, determine the origin of the coordinate system and find z i-1 Axis and z iThe common normal of the axis. If the common normal is unique, the origin of coordinate system {i-1} is located at the intersection of the common normal and the axis of joint i-1. If the common normal is not unique (for example, the two joint axes are parallel), a new normal is chosen based on the actual situation. If the common normal does not exist (for example, the two joint axes intersect), the origin of coordinate system {i-1} is located at the intersection of the two axes. Finally, determine the directions of the x-axis and y-axis. The direction of the common normal is usually chosen as the x-axis direction, and the y-axis direction is determined using the right-hand rule.

[0124] Figure 4 The meanings of the DH parameters are as follows:

[0125] Connecting rod length a i-1 : The length of connecting rod i-1 is z i-1 Axis and z i The length of the common normal to the axis;

[0126] Connecting rod torsion angle α i-1 : Connecting rod torsion angle α i-1 Indicates x i-1 The direction of z is the positive direction, i-1 Axis to z i The rotation angle of the axis.

[0127] Connecting rod offset d i :From x i-1 With z i The signed distance from the intersection of the axes to the origin of the link coordinate system {i} (positive direction along z i direction).

[0128] Joint angle φ i :z i The direction of x is positive, i-1 Axis to x i The rotation angle of the axis.

[0129] According to the link coordinate system and DH parameters of the robot arm, the transformation matrix of the two adjacent link coordinate systems can be calculated The formula is as follows:

[0130]

[0131] According to the improved DH method, the link coordinate system of the picking robot arm is established, as follows: Figure 5 As shown in Figure 1, the coordinate system {0} is the world coordinate system. Table 1 shows the corresponding DH parameter table.

[0132] Table 1

[0133] Connecting rod number i <![CDATA[α i-1 (°)]]> <![CDATA[a i-1 (mm)]]> <![CDATA[d i (mm)]]> <![CDATA[φ i (°)]]> 1 0 0 <![CDATA[d1]]> <![CDATA[θ1]]> 2 90 0 <![CDATA[d2]]> <![CDATA[θ2+90]]> 3 180 <![CDATA[a1]]> <![CDATA[d3]]> <![CDATA[θ3]]> 4 180 <![CDATA[a2]]> <![CDATA[d4]]> <![CDATA[θ4-90]]> 5 -90 0 <![CDATA[d5]]> <![CDATA[θ5]]> 6 90 0 <![CDATA[d6]]> <![CDATA[θ6]]>

[0134] In Table 1, θ iIn order to verify the accuracy of the kinematic model established by the DH parameter method, the Link function in the Matlab robot toolbox was used to establish a visual model of the picking robot arm, as shown in the figure. Figure 6 shown.

[0135] The forward kinematics of a robotic arm refers to the process of solving the posture of the end of the robotic arm when the angles of each joint of the robotic arm [θ1θ2θ3θ4θ5θ6] are known. The above formula can be used to calculate the homogeneous transformation matrix between the coordinate systems of adjacent links of the robotic arm

[0136]

[0137] Multiply the homogeneous transformation matrices between the adjacent link coordinate systems in the above formula in sequence to obtain the end pose matrix of the robotic arm

[0138]

[0139] The specific representation of each element in the pose matrix is:

[0140]

[0141] For the convenience of writing, the s i = sinθ i , c i =cosθ i , s 23 = sin(θ2-θ3),

[0142] c 23 =cos(θ2-θ3), s 234 =sin(θ2-θ3+θ4), c 234 =cos(θ2-θ3+θ4), subsequent

[0143] Similar abbreviations have the same meaning as here.

[0144] In the picking scenario, the target point position is usually determined first, then the expected position of the robot end effector is specified, and finally the picking operation is achieved by controlling the joint angle. This embodiment uses algebraic methods to derive the analytical solution of the robot arm's inverse kinematics:

[0145] The end pose matrix Multiply the formula on the left We can get:

[0146]

[0147] Performing matrix operations on the right side of the above formula yields:

[0148]

[0149] in:

[0150]

[0151] Combining the above formulas, and based on the equality of the matrix elements (2, 3) and (2, 4) at both ends, we can get the following equation:

[0152]

[0153] Simplifying the above formula, we get:

[0154] (p y -a y d6)c1-(p x -a x d6)s1=d3-d2-d4

[0155] According to the trigonometric function transformation, let p y -a y d6=m1,p x -a x d6=n1,d3-d2-d4=d,then we can get:

[0156]

[0157] It can be seen that: c5=a x s1-a y c1;

[0158] Therefore, we can get: θ5=±arccos(a x s1-a y c1);

[0159] According to the above formula, the matrix elements (2, 1) and (2, 2) at both ends are equal, and the following equations are obtained:

[0160]

[0161] From the above formula we can get:

[0162]

[0163] According to the above formula, the matrix elements (1, 4) and (3, 4) at both ends are equal, and the following equations are obtained:

[0164]

[0165] Eliminate θ from the above formula 23 , we get the equation for θ3: m2 2 +n2 2 -a12 -a2 2 =2a1a2c3;

[0166] in,

[0167] Solving for θ3 yields the following expression:

[0168] According to the above formula:

[0169]

[0170] in:

[0171] The expression of θ2 is solved as follows: θ2 = arctan2(s2,c2);

[0172] According to the above formula, the matrix elements (1, 3) and (3, 3) at both ends are equal, and the following equations are obtained:

[0173] Solve for θ 234 The expression is:

[0174]

[0175] Therefore, the expression of θ4 is: θ4=θ 234 -θ2+θ3;

[0176] From the above expression of joint angle, it can be seen that when m1 2 +n1 2 -d 2 When m2 is less than 0, the joint 1 cannot be solved, and the robot arm has a shoulder singularity. 2 +n2 2 -a1 2 -a2 2 When ≥2a1a2, joint three cannot be solved, and the robot arm has an elbow singularity; when s5=0, that is, θ5=0 or π, joint six cannot be solved, and the robot arm has a wrist singularity; and θ1, θ3, and θ5 may have two sets of solutions respectively, so the six-degree-of-freedom robot arm may have up to eight sets of inverse kinematics solutions, but due to the existence of singular positions, some solutions of the robot arm cannot be realized.

[0177] The workspace of a robotic arm refers to the set of positions within the range of joint angle variation that the origin of the coordinate system of the end effector of the robotic arm can reach due to all possible combinations of joint angles. The range of joint angles of the picking robotic arm is shown in Table 2. This embodiment uses numerical and graphical methods to solve the workspace of the robotic arm respectively. The numerical method uses the Monte Carlo method to solve the workspace of the robotic arm with the help of random sampling. Matlab randomly selects 100,000 sampling points to obtain a point cloud diagram of the workspace of the robotic arm. For easy observation, the three-dimensional point cloud diagram of the workspace of the robotic arm is shown in Figure 7 As shown, the XY, XZ and YZ plane projection diagrams.

[0178] Table 2

[0179] Joint Number Joint angle variation range (°) 1 -360～360 2 -135～135 3 -135～135 4 -175～175 5 -175～175 6 -360～360

[0180] Use the graphical method to solve the working space of the robot arm. The side section of the working space of the robot arm is as follows: Figure 8 As shown, the shaded part is the position that the end effector of the robot arm can reach. The acquisition process of each arc in the figure is as follows:

[0181] Arc 1: Place all joints in their initial positions. The robot arm is extended, and the end effector reaches its maximum reach. Keep the other joint angles constant. Rotate Joint 2 from its minimum angle to its maximum angle. The arc along which the origin of the end effector coordinate system passes is Arc 1, with center O1 and radius r1.

[0182] Arc 2 and Arc 3: Set Joint 2 to its maximum joint angle and all other joint angles to zero. Rotate Joint 3 from its minimum to its maximum joint angle. The arc traced by the origin of the end-effector coordinate system is Arc 2, with center O2 and radius r2. Similarly, set Joint 2 to its minimum joint angle and all other joint angles to zero, and rotate Joint 3 to create Arc 3.

[0183] Arc 4 and Arc 5: Set joint 3 to its maximum angle and all other joint angles to zero. Rotate joint 2 from its minimum angle to its maximum angle. The arc traced by the origin of the end-effector coordinate system is Arc 4, with center O4 and radius r4. Similarly, set joint 3 to its minimum angle and all other joint angles to zero, and rotate joint 3 to create Arc 5.

[0184] Because elastic components like harmonic reducers and torque sensors within the harvesting robot joints contribute to their flexibility, the desired rotation angle controlled by the motors often cannot be fed back to the connecting rods in a timely manner. Therefore, it is necessary to construct a dynamic model for the harvesting robot joints as flexible joints and solve the dynamic model to obtain the actual joint rotation angles.

[0185] The harmonic reducer is the main cause of joint flexibility. It consists of a wave generator, a flexible wheel, and a rigid wheel. The wave generator is connected to the motor part, and the rigid wheel is connected to the connecting rod part. To analyze the dynamics of the robot arm joint, this embodiment considers joint friction based on the linear torsion spring model and equates the robot arm joint to a spring damping model. The equivalent model of the robot arm joint is shown in the figure below. Figure 9 As shown, is the motor rotor angle of joint i, θ i is the rotation angle of connecting rod i, N i is the transmission ratio of joint i, k i is the stiffness of joint i, b i is the damping of joint i, J mi is the moment of inertia of the motor rotor at joint i, J li is the moment of inertia of link i. The dynamic model of joint i is:

[0186]

[0187] Where, τ i is the output torque of the reducer at joint i, τ mi is the motor end torque of joint i, is the motor end angle of joint i, and τ mi The expression is:

[0188]

[0189] Where, τ motor,i The output torque of the motor rotor.

[0190] Assume that the origin of the motor rotor reference coordinate system coincides with the origin of the connecting rod coordinate system and is located at the center of mass of the motor end, and the motor rotor and the connecting rod have the same rotation axis. This embodiment uses the Lagrange equation to derive the dynamic equations of the flexible joint manipulator system. First, define the connecting rod rotation angle as the connecting rod generalized coordinate q l The motor angle is the motor rotor and the motor generalized coordinates are q m , their generalized coordinates are expressed as:

[0191]

[0192] Then, the kinetic energy of the connecting rod and motor rotor of the robotic arm system is obtained as:

[0193]

[0194] Where, is the spatial inertia matrix of link i in the world coordinate system, is the spatial inertia matrix of the motor rotor i in the world coordinate system, is the motion rotation of link i in the world coordinate system, is the motion spin of the motor rotor i in the world coordinate system, and The expression is:

[0195]

[0196] Where, is the Jacobian matrix of the link coordinate system {i} in the world coordinate system, To make The matrix z is a zero matrix with the fourth to sixth rows of the i-th column, i represents the unit vector of the axis of rotation of the motor rotor i in the world coordinate system, and

[0197] The total kinetic energy of the robotic arm system is:

[0198]

[0199] Where, is the mass matrix of the connecting rod, is the mass matrix of the motor rotor. is the coupling matrix between the connecting rod and the motor rotor, is the motor moment of inertia matrix after deceleration.

[0200] The gravitational potential energy of the robotic arm system is expressed as:

[0201]

[0202] m li 、m mi are the masses of the i-th connecting rod and the motor rotor, z li 、z mi are the mass centers of the i-th connecting rod and the motor rotor on the z-axis of the world coordinate system, respectively, and g is the acceleration due to gravity.

[0203] The dissipated energy generated by the elastic potential energy and friction damping of the joint flexible element is expressed as:

[0204]

[0205] Summing all potential energies and dissipated energies, the total potential energy V and total dissipated energy B are:

[0206]

[0207] Finally, the generalized coordinates of the robotic arm system are defined as The dynamic equation is obtained using the Lagrangian function L=TV:

[0208]

[0209] Where, Q=[Q l T Q m T ] T , Q l represents the generalized force on the connecting rod, Q m It represents the generalized force acting on the motor, and its expression is:

[0210]

[0211] Where, τ ei is the external interference torque on the link i, and the dynamic equation of the flexible joint manipulator is obtained as:

[0212]

[0213] Where, is the mass matrix of the complete dynamic equation, is the Coriolis matrix, is the gravity term vector, and are the stiffness matrix and damping matrix of the joint respectively. Among them:

[0214]

[0215] Where, is the connecting rod Coriolis matrix.

[0216] The coupling term S(q l ) is only related to the reduction ratio and the moment of inertia of the motor end. Since the moment of inertia of the motor end of the harvesting robot studied in this embodiment is small, the coupling term S(q l )=0. Assume that the connecting rod end of the robot arm is not disturbed by external forces, that is, Q l =0. The above formula can be simplified to:

[0217]

[0218] This embodiment uses the implicit Runge-Kutta method to solve the dynamic equations, making the following variable substitutions:

[0219]

[0220] The dynamic equation of the manipulator expressed by the state equation is:

[0221]

[0222] The implicit Runge-Kutta method iteration formula is:

[0223]

[0224] Where subscript s is the discrete number; h is the discrete step length.

[0225] In some embodiments, the present application provides a picking robot arm coupling dynamics modeling system, including:

[0226] The structural modeling module is used to obtain the actual structural composition of the picking robot arm and model the structure of the picking robot arm to obtain a structural model of the picking robot arm;

[0227] A coordinate system establishment module is used to establish the link coordinate system of adjacent rotary joints of the picking robot arm based on the improved DH parameter method and the structural model;

[0228] Kinematic modeling module, used to establish the forward kinematic model of the picking robot arm based on the structural model and the connecting rod coordinate system, and to establish the inverse kinematic model of the picking robot arm using algebraic methods based on the forward kinematic model;

[0229] The flexible modeling module is used to obtain the elastic element data contained in the picking robot arm, establish a flexible deformation model for the flexible connection parts, and model each link as a joint flexible element to construct an elastic dynamics model;

[0230] The coupled dynamics modeling module is used to introduce coupling constraints based on the dynamic model and elastic dynamics model to build a coupled dynamics model of the picking robot arm;

[0231] The calculation processing module is used to solve the generalized coordinates of the robotic arm system based on the coupled dynamics model, establish the Lagrangian dynamic equations of the system, and use the implicit Runge-Kutta method to solve the state.

[0232] In some embodiments, the present application provides a terminal, including:

[0233] A memory for storing a picking robot arm coupling dynamics modeling program;

[0234] The processor is used to implement the steps of the picking robot arm coupling dynamics modeling method as described in the first aspect when executing the picking robot arm coupling dynamics modeling system.

[0235] In some embodiments, the present application provides a computer-readable storage medium that stores computer instructions. When a computer reads the computer instructions in the storage medium, the computer executes the described method for modeling the coupling dynamics of a picking robot arm.

[0236] It should be understood that the embodiments described in this specification are intended only to illustrate the principles of the embodiments of this specification. Other variations may also fall within the scope of this specification. By way of example and not limitation, alternative configurations of the embodiments of this specification may be considered consistent with the teachings of this specification. Accordingly, the embodiments of this specification are not limited to the embodiments explicitly described and illustrated in this specification.

Claims

1. A method for modeling coupled dynamics of a picking robot arm, characterized in that: The following steps are involved: Step S1, obtaining the actual structural composition of the picking robot arm, modeling the structure of the picking robot arm, and obtaining a picking robot arm structure model; Step S2: establishing a link coordinate system of adjacent rotation joints based on the picking robot arm structure model in step S1 and the improved DH parameter method; Step S3: establishing a forward kinematics model of the picking robot arm based on the picking robot arm structure model in step S1 and the picking robot arm connecting rod coordinate system in step S2; According to the forward kinematics model of the picking robot arm and the algebraic method, the inverse kinematics model of the robot arm is derived and established; Step S4: Acquire the elastic element data contained in the picking robot arm, establish a flexible deformation model for the flexible connection part, model each connecting rod as a joint flexible element, and construct an elastic dynamics model; Step S5: construct a dynamic model of the picking robot arm, introduce coupling constraints based on the dynamic model and the elastic dynamic model, and construct a coupled dynamic model.

2. The method for modeling the coupled dynamics of a picking robot arm according to claim 1, characterized in that: In step S1, the picking robot arm structure model includes a combination of several rigid links and several rotational joints, wherein the links are connected by joints.

3. The method for modeling the coupled dynamics of a picking robot arm according to claim 2, characterized in that: The position in the reference coordinate system is represented by a three-dimensional vector, and the posture in the reference coordinate system is represented by a third-order matrix. The link coordinate system {B} relative to the reference coordinate system {A} is represented as follows: in, represents the position vector of the origin of coordinate system {B} in {A}, p x 、p y 、p z Indicates that x in {A} A 、y A 、z A Components of the three axes; Represents the attitude matrix of the coordinate system {B} in {A}, called the rotation matrix relative to {A}; x B 、y B 、z B Represents the three unit axis vectors of the coordinate system {B}, using the unit coordinate axis x A 、y A 、z A Expressed as: therefore: The homogeneous transformation matrix of coordinate system {B} relative to coordinate system {A} is defined as: For the relative relationship between several coordinate systems, the operation rules followed are: Where, Represents the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {A}, Represents the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {B}; Represents the velocity state of the connecting rod, including the connecting rod linear velocity and the connecting rod angular velocity; the three linear velocities and three angular velocities constitute the motion spinor; let the coordinate system {A} be the world coordinate system, and the coordinate system {B} be the connecting rod coordinate system attached to the robot arm connecting rod. The velocity of the coordinate system {B} in the coordinate system {A} is expressed as: Among them, A Represents the linear velocity vector of the connecting rod, that is, the linear velocity vector of the origin of the connecting rod coordinate system {B}; ω A represents the angular velocity vector of the connecting rod, represents the unit rotation axis of the angular velocity, Indicates the magnitude of the angular velocity; the motion spin of the connecting rod in the world coordinate system is: in, Represents the velocity Jacobian matrix of the link coordinate system {B} in the world coordinate system.

4. The method for modeling the coupled dynamics of a picking robot arm according to claim 3, characterized in that: In step S2, joint i-1 and joint i are two adjacent joints, and coordinate system {i-1} and coordinate system {i} are corresponding link coordinate systems. The steps for establishing the link coordinate system are as follows: Step S2-1, determine the z-axis direction, z i The axis coincides with the rotation axis of joint i, z i-1 The axis coincides with the rotation axis of joint i-1, and the positive direction of the z axis is the rotation direction of the corresponding joint; Step S2-2, determine the origin of the coordinate system and obtain z i-1 Axis and z i Common normal to the axis; If the common normal is unique, the origin of the coordinate system {i-1} is located at the intersection of the common normal and the axis of joint i-1; If the common normal is not unique, manually select and set the origin of the coordinate system according to the actual situation; If the common normal does not exist, the origin of the coordinate system {i-1} is located at the intersection of the two axes; Step S2-3, determining the x-axis and y-axis directions; The direction of the common normal is selected as the x-axis direction, and the y-axis direction is determined according to the right-hand rule.

5. The method for modeling the coupled dynamics of a picking robot arm according to claim 4, characterized in that: According to the link coordinate system and DH parameters of the robot arm, the transformation matrix of the two adjacent link coordinate systems is calculated. The formula is as follows: The coordinate system {0} is the world coordinate system; the connecting rod length a i-1 The length of the connecting rod i-1 is z i-1 Axis and z i The length of the shaft's normal line; the connecting rod's torsion angle α i-1 is the connecting rod torsion angle α i-1 Indicates x i-1 The direction of z is the positive direction, i-1 Axis to z i Axis rotation angle; joint angle φ i For z i The direction of x is positive, i-1 Axis to x i Axis rotation angle; connecting rod offset d i From x i-1 With z i The intersection of the axes to the origin of the link coordinate system {i} along the positive direction of z i Directed distance in direction.

6. The method for modeling the coupled dynamics of a picking robot arm according to claim 1, characterized in that: In step S4, the joints of the robotic arm are equivalent to a spring-damper model, and the dynamic model of joint i is: Among them, τ i is the output torque of the reducer at joint i, τ mi is the torque at the motor end of joint i, is the rotation angle of the motor end of joint i, θ i is the rotation angle of connecting rod i, k i is the stiffness of joint i, b i is the damping of joint i, J mi is the moment of inertia of the motor rotor at joint i, J li is the moment of inertia of connecting rod i; and τ mi The expression is: Among them, τ motor,i Output torque of the motor rotor; N i is the transmission ratio of joint i; is the motor rotor angle of joint i.

7. The method for modeling the coupled dynamics of a picking robot arm according to claim 6, characterized in that: In step S5, it is assumed that the origin of the motor rotor reference coordinate system coincides with the origin of the connecting rod coordinate system and is located at the centroid of the motor end, and the rotation axis of the motor rotor and the connecting rod are the same; Define the connecting rod angle as the connecting rod generalized coordinate q l The motor angle is the motor rotor and the motor generalized coordinates are q m , their generalized coordinates are expressed as: The kinetic energy of the connecting rod and motor rotor of the robotic arm system is obtained as: in, is the spatial inertia matrix of link i in the world coordinate system, is the spatial inertia matrix of the motor rotor i in the world coordinate system, is the motion rotation of link i in the world coordinate system, is the motion spin of the motor rotor i in the world coordinate system, and The expression is: in, is the Jacobian matrix of the link coordinate system {i} in the world coordinate system, To make The matrix z is a zero matrix with the fourth to sixth rows of the i-th column, i represents the unit vector of the axis of rotation of the motor rotor i in the world coordinate system, and The total kinetic energy of the robotic arm system is: in, is the mass matrix of the connecting rod, is the mass matrix of the motor rotor; is the coupling matrix between the connecting rod and the motor rotor, is the motor moment of inertia matrix after deceleration; The gravitational potential energy of the robotic arm system is expressed as: Among them, m li 、m mi are the masses of the i-th connecting rod and the motor rotor, z li 、z mi are the mass centers of the i-th connecting rod and the motor rotor on the z-axis of the world coordinate system, respectively, and g is the acceleration due to gravity; The dissipated energy generated by the elastic potential energy and friction damping of the joint flexible element is: Summing all potential energies and dissipated energies, the total potential energy V and total dissipated energy B are: The generalized coordinates of the robotic arm system are defined as The dynamic equation is obtained using the Lagrangian function L=TV: in, Q l represents the generalized force on the connecting rod, Q m It represents the generalized force acting on the motor, and its expression is: Where τ ei is the external interference torque on the link i, and the dynamic equation of the flexible joint manipulator is obtained as: in, is the mass matrix of the complete dynamic equation, is the Coriolis matrix, is the gravity term vector, and are the stiffness matrix and damping matrix of the joint respectively; in: in, is the connecting rod Coriolis matrix; Let the coupling term S(q l )=0; Assume that the connecting rod end of the robot arm is not disturbed by external forces, that is, Q l =0; we get: The implicit Runge-Kutta method is used to solve the dynamic equations. Make the following variable substitutions: The dynamic equation of the manipulator expressed by the state equation is:

8. A picking robot arm coupling dynamics modeling system, characterized in that: include: The structural modeling module is used to obtain the actual structural composition of the picking robot arm and model the structure of the picking robot arm to obtain a structural model of the picking robot arm; A coordinate system establishment module is used to establish the link coordinate system of adjacent rotary joints of the picking robot arm based on the improved DH parameter method and the structural model; Kinematic modeling module, used to establish the forward kinematic model of the picking robot arm based on the structural model and the connecting rod coordinate system, and to establish the inverse kinematic model of the picking robot arm using algebraic methods based on the forward kinematic model; The flexible modeling module is used to obtain the elastic element data contained in the picking robot arm, establish a flexible deformation model for the flexible connection parts, and model each link as a joint flexible element to construct an elastic dynamics model; The coupled dynamics modeling module is used to introduce coupling constraints based on the dynamic model and elastic dynamics model to build a coupled dynamics model of the picking robot arm; The calculation processing module is used to solve the generalized coordinates of the robotic arm system based on the coupled dynamics model, establish the Lagrangian dynamic equations of the system, and use the implicit Runge-Kutta method to solve the state.

9. A terminal, characterized in that: include: A memory, used for storing a picking robot arm coupling dynamics modeling program; The processor is used to implement the steps of the picking robot arm coupling dynamics modeling method as described in any one of claims 1 to 7 when executing the picking robot arm coupling dynamics modeling system.

10. A computer-readable storage medium, characterized in that The storage medium stores computer instructions. When the computer reads the computer instructions in the storage medium, the computer executes the picking robot arm coupling dynamics modeling method according to any one of claims 1 to 7.

Images