A picking mechanical arm coupling dynamics modeling method, device, terminal and medium

Abstract

The application relates to the technical field of mechanical arm modeling, and particularly discloses a picking mechanical arm coupled dynamics modeling method, device, terminal and readable storage medium. The method comprises the following steps: obtaining the structural composition of the picking mechanical arm and establishing a structural model; 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; collecting flexible element parameters in the mechanical arm, constructing a joint flexible connection model and an elastic dynamics model; further introducing a coupling constraint condition, combining the elastic dynamics model and the overall dynamics model, and forming a coupled dynamics modeling framework. The model can consider the nonlinear characteristics of joint flexibility, structural vibration and dynamic response, improve the modeling accuracy and control robustness of the picking mechanical arm under complex working conditions, and is suitable for modeling and control algorithm design of the flexible structure mechanical arm.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm modeling technology, specifically relating to a method, device, terminal, and medium for modeling coupled dynamics of a harvesting robotic arm. Background Technology

[0002] With the rapid development of smart agriculture and automation technology, robotic harvesting arms have received widespread attention in various application scenarios such as fruit and vegetable harvesting and greenhouse management. As an important component of automated agricultural machinery, the performance of robotic harvesting arms directly affects harvesting efficiency, fruit damage rate, and the economy and reliability of the system.

[0003] Traditional harvesting robotic arms are mostly modeled based on the assumption of rigid bodies. This involves establishing rigid link models and applying Newton-Euler or Lagrange methods to construct dynamic equations for trajectory planning and control strategy design. However, in actual agricultural operations, harvesting robotic arms are typically made of multiple lightweight materials, and the joints often have flexible connections, such as elastic joints or flexible drive units. These flexible structures can cause structural vibrations, amplified end-effector deviations, and response delays during rapid or high-precision movements. Ignoring these flexible factors will lead to control strategies deviating from the true response, and in severe cases, even system instability.

[0004] To overcome the limitations of rigid modeling methods in flexible structure modeling, some studies have begun to introduce flexible body dynamics modeling theory, using methods such as elastic body modeling, modal superposition, and finite element method to improve model accuracy. 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 characterize efficiently using traditional methods; (2) The solution of flexible models is complex and difficult to meet the requirements of real-time control; (3) There is a lack of a unified coupling modeling framework, the modeling process relies on a large number of prior assumptions, and the generalization ability is poor. Summary of the Invention

[0005] This invention addresses the problems in the prior art by providing a method, device, terminal, and medium for coupled dynamics modeling of a picking robot arm. It solves the problem that existing robot arm modeling schemes often struggle to balance modeling accuracy and computational efficiency when dealing with multi-degree-of-freedom structures, complex working conditions, and the coupling of control algorithms, resulting in high controller design difficulty and poor stability in actual deployment.

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

[0007] Firstly, this application provides a method for modeling the coupled dynamics of a harvesting robot, comprising the following steps:

[0008] Step S1: Obtain the actual structural composition of the harvesting robot arm, model the structure of the harvesting robot arm, and obtain the structural model of the harvesting robot arm;

[0009] Step S2: Establish the link coordinate system of adjacent rotary joints based on the harvesting robot arm structure model in Step S1 and the improved DH parameter method;

[0010] Step S3: Based on the structural model of the harvesting robot arm in Step S1 and the linkage coordinate system of the harvesting robot arm in Step S2, establish the forward kinematic model of the harvesting robot arm.

[0011] Based on the forward kinematics model of the harvesting robot and the algebraic method, the inverse kinematics model of the robot is derived and established.

[0012] Step S4: Obtain the data of the elastic elements contained in the harvesting robot arm, establish a flexible deformation model for the flexible connection parts, model each link as a joint flexible element, and construct an elastic dynamic model.

[0013] Step S5: Construct a dynamic model of the harvesting robotic arm. Based on the dynamic model and the elastic dynamic model, introduce coupling constraints to construct a coupled dynamic model.

[0014] Preferably, in step S1, the picking robotic arm structural model includes a combination of several rigid links and several rotary 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 attitude in the reference coordinate system. The form of the link coordinate system {B} relative to the reference coordinate system {A} is as follows:

[0016]

[0017] in, Let {B} represent the position vector of the origin of coordinate system {B} within {A}. , , Indicates in {A} , , Components of the three axes; Let {B} be the attitude matrix of coordinate system {A}, and let {B} be the rotation matrix relative to {A}. , , Represent the three unit axis vectors of coordinate system {B} using unit coordinate axes. , , Represented as:

[0018]

[0019] therefore:

[0020] Define the homogeneous transformation matrix of coordinate system {B} relative to coordinate system {A} as: ;

[0021] The operational rules governing the relative relationships between several coordinate systems are as follows: ;

[0022] In the formula, Let represent the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {A}. Let {C} be the homogeneous transformation matrix relative to coordinate system {B}.

[0023] The velocity state of the link is represented by its linear velocity and angular velocity; the three linear velocities and three angular velocities constitute the kinematic screw; let coordinate system {A} be the world coordinate system, and coordinate system {B} be the link coordinate system attached to the robotic arm link. The velocity of coordinate system {B} in coordinate system {A} is expressed as: ;

[0024] in, This represents the linear velocity vector of the link, that is, the linear velocity vector of the origin of the link coordinate system {B}; This represents the angular velocity vector of the connecting rod. The unit axis of rotation representing angular velocity. The magnitude of the angular velocity is represented by: The spinor of the link in the world coordinate system is:

[0025]

[0026] in, Let {B} be the velocity Jacobian matrix of the link coordinate system 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 the corresponding link coordinate systems; the steps for establishing the link coordinate system are as follows:

[0028] Step S2-1: Confirm Axial direction, The axis of rotation of the shaft coincides with that of joint i. The axis of rotation of the shaft coincides with that of joint i-1. The positive direction of the axis is the direction of rotation of the corresponding joint;

[0029] Step S2-2: Determine the origin of the coordinate system and obtain... shaft and The 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, the origin of the coordinate system should be manually selected and set 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, Confirm shaft and Axial direction;

[0034] Choose the direction of the common normal as... The axis direction is determined according to the right-hand rule. Axial direction.

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

[0036]

[0037] Where coordinate system {0} is the world coordinate system; link length The length of link i-1 is shaft and Common normal length of the shaft; connecting rod torsion angle For the link twist angle Indicated by The direction is the positive direction. Axis to Shaft rotation angle; joint rotation angle For The direction is the positive direction. Axis to Shaft rotation angle; connecting rod offset From and The positive direction from the intersection of the axes to the origin of the link coordinate system {i} Directed distance in a given direction.

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

[0039]

[0040] in, The output torque of the joint i reducer, For the torque at the motor end of joint i, For the joint i motor end rotation angle, Let be the rotation angle of link i. Let be the stiffness of joint i. For the damping of joint i, Let i be the moment of inertia of the motor rotor at joint i. Let be the moment of inertia of link i;

[0041] and The expression is: ;

[0042] in, The output torque of the motor rotor; Let i be the transmission ratio of joint i; Let be the rotor angle of the motor at joint i.

[0043] Preferably, in step S5, 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 at the motor end, and the rotation axes of the motor rotor and the connecting rod are the same.

[0044] Define the link rotation angle as the link's generalized coordinate system. The motor rotation angle is the motor rotor, and the motor's generalized coordinate system is the motor's rotation angle. The generalized coordinates of the two are represented as follows:

[0045]

[0046] The kinetic energies of the robotic arm system's linkages and motor rotor are obtained as follows:

[0047]

[0048] in, Let be the spatial inertia matrix of link i in the world coordinate system. Let be the spatial inertia matrix of the motor rotor i in the world coordinate system. Let be the spinor of link i in the world coordinate system. Let be the spinor of the motor rotor i in the world coordinate system. and The expression is:

[0049]

[0050] in, Let {i} be the Jacobian matrix of the link coordinate system in the world coordinate system. In order to make The matrix whose fourth to sixth rows in the i-th column are zeros. Let represent the unit vector of the axis of rotation of motor rotor i in the world coordinate system, and ;

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

[0052]

[0053] in, Here is the mass matrix of the link. The mass matrix of the motor rotor; Let be the coupling term matrix between the connecting rod and the motor rotor. This is the moment of inertia matrix of the motor after deceleration;

[0054] The gravitational potential energy of the robotic arm system can be expressed as: ;

[0055] in, , Let be the masses of the i-th connecting rod and the motor rotor, respectively. , The centroids of the i-th link and the motor rotor in the world coordinate system are respectively... Representation on the axis, It is the acceleration due to gravity;

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

[0057]

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

[0059]

[0060] Define the generalized coordinates of the robotic arm system as follows: Using the Lagrange function The dynamic equations are obtained as follows: ;

[0061] in, , This represents the generalized force acting on the link. The generalized force acting on the motor is expressed as:

[0062]

[0063] In the formula Given the external disturbance torque on link i, the dynamic equation of the flexible joint manipulator is obtained as follows:

[0064]

[0065] in, The mass matrix of the complete kinetic equations. For Coriolis matrix, The vector of the gravity term. and These are the stiffness matrix and damping matrix of the joint, respectively;

[0066] in:

[0067] ;

[0068] in, For the link Coriolis matrix;

[0069] Let coupling terms Assume that the linkage end of the robotic arm is not affected by external forces, i.e. ;get:

[0070]

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

[0072]

[0073] The dynamic equations of the robotic arm, expressed in terms of state equations, can be obtained as follows:

[0074] .

[0075] Secondly, this application provides a coupled dynamics modeling system for a harvesting robotic arm, comprising:

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

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

[0078] The kinematic modeling module is used to establish the forward kinematic model of the harvesting robot based on the structural model and the link coordinate system, and to establish the inverse kinematic model of the harvesting robot based on the forward kinematic model using an algebraic method.

[0079] The flexible modeling module is used to acquire data on the elastic elements contained in the harvesting 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 construct a coupled dynamics model of the harvesting robot arm by introducing coupled constraints based on the dynamics model and the elastic dynamics model.

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

[0082] Thirdly, this application provides a terminal, including:

[0083] The memory is used to store the coupled dynamics modeling program of the harvesting robot arm;

[0084] A processor is used to implement the steps of the harvesting manipulator coupling dynamics modeling method as described in the first aspect when executing the harvesting manipulator coupling dynamics modeling program.

[0085] Fourthly, this 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 method for modeling coupled dynamics of a picking robot arm as described in the first aspect.

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

[0087] 1. A unified method for coupled dynamic modeling of harvesting robotic arms is provided. By constructing a multibody system dynamic model including flexible arm segments and rigid joints, the completeness and theoretical consistency of dynamic modeling are improved. It also significantly enhances the ability to model the influence of flexible arm segment deformation on the end effector trajectory, thereby effectively solving the problems 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 a finite-order intrinsic 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 computational load.

[0089] 3. The system dynamic equations are derived using the Lagrange method. While maintaining the system's closed and structured characteristics, this facilitates the design of subsequent control algorithms, state observation, and parameter optimization, thereby improving the model's engineering usability and versatility. Attached Figure Description

[0090] To more clearly illustrate the technical solution of this application, the accompanying drawings used in the description will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0091] Figure 1 This is a schematic diagram of the coupled dynamics modeling method for a harvesting robot arm, as shown in some embodiments.

[0092] Figure 2 These are schematic diagrams of coordinate systems shown in some embodiments;

[0093] Figure 3 This is a schematic diagram of the link angular velocity shown in some embodiments;

[0094] Figure 4 This is a schematic diagram of the DH parameters between links as shown in some embodiments;

[0095] Figure 5 The coordinate system of the picking robot arm shown in some embodiments;

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

[0097] Figure 7 Here are point cloud diagrams of the workspace of the robotic arm as shown in some embodiments;

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

[0099] Figure 9 The joint equivalent model is shown in some embodiments. Detailed Implementation

[0100] To make the purpose, features, and advantages of this application more apparent and understandable, specific embodiments and accompanying drawings will be used to clearly and completely describe the technical solution protected by this application. Obviously, the embodiments described below are only some embodiments of this application, and not all embodiments. Based on the embodiments in this patent, all other embodiments obtained by those skilled in the art without inventive effort are within the scope of protection of this patent.

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

[0102] Step S1: Obtain the actual structural composition of the harvesting robot arm, model the structure of the harvesting robot arm, and obtain the structural model of the harvesting robot arm;

[0103] Step S2: Establish the link coordinate system of adjacent rotary joints based on the harvesting robot arm structure model in Step S1 and the improved DH parameter method;

[0104] Step S3: Based on the structural model of the harvesting robot arm in Step S1 and the linkage coordinate system of the harvesting robot arm in Step S2, establish the forward kinematic model of the harvesting robot arm.

[0105] Based on the forward kinematics model of the harvesting robot and the algebraic method, the inverse kinematics model of the robot is derived and established.

[0106] Step S4: Obtain the data of the elastic elements contained in the harvesting robot arm, establish a flexible deformation model for the flexible connection parts, model each link as a joint flexible element, and construct an elastic dynamic model.

[0107] Step S5: Construct a dynamic model of the harvesting robotic arm. Based on the dynamic model and the elastic dynamic model, introduce coupling constraints to construct a coupled dynamic model.

[0108] In some embodiments, kinematic analysis of a robotic arm requires a systematic method to describe the position and orientation (pose) of the robotic arm links in the world coordinate system, as well as the relative relationships between the links. Generally, the pose of the links is represented by attaching a reference coordinate system to them, and the relative relationships between the coordinate systems are represented using a homogeneous transformation method. Specifically, 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 orientation. The representation of the link coordinate system {B} relative to the reference coordinate system {A} is as follows: Figure 2 As shown;

[0109] The calculation is performed using the following formula: ;

[0110] In the formula, Let {B} represent the position vector of the origin of coordinate system {B} within {A}. , , Indicates in {A} , , The components of the three axes. This represents the attitude matrix of coordinate system {B} in {A}, also known as the rotation matrix relative to {A}. , , Represent the three unit axis vectors of coordinate system {B} using unit coordinate axes. , , Represented as:

[0111]

[0112] therefore: ;

[0113] Define the homogeneous transformation matrix of coordinate system {B} relative to coordinate system {A} as: ;

[0114] The operational rules governing the relative relationships between multiple coordinate systems are as follows: ;

[0115] In the formula, Let represent the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {A}. Let {C} be the homogeneous transformation matrix relative to coordinate system {B}.

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

[0117] The calculation is performed using the following formula: ;

[0118] In the formula, This represents the linear velocity vector of the link, which is the linear velocity vector of the origin of the link coordinate system {B}. This represents the angular velocity vector of the connecting rod. The unit axis of rotation representing angular velocity. This represents the magnitude of the angular velocity. The spinor of the link in the world coordinate system is: ;

[0119] The above formula can be used to calculate:

[0120]

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

[0122] Since all joints of the robotic arm in this embodiment are rotary joints, a modified DH method is used to establish the link coordinate system of adjacent rotary joints. A schematic diagram of the DH parameters between links is shown below. Figure 4 As shown;

[0123] Figure 4 In the diagram, joint i-1 and joint i are two adjacent joints, and coordinate systems {i-1} and {i} are the corresponding link coordinate systems. To establish the link coordinate system: first determine... Axial direction, The axis of rotation of the shaft coincides with that of joint i. The axis coincides with the rotation axis of joint i-1, and their positive directions are the rotation directions of the corresponding joints; next, determine the origin of the coordinate system and find... shaft and The common normal to the axes. 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 (e.g., the axes of the two joints are parallel), it is selected according to the actual situation; if the common normal does not exist (e.g., the axes of the two joints intersect), the origin of coordinate system {i-1} is located at the intersection of the two axes; finally, determine... shaft and Axial direction. The direction of the common normal is usually chosen as the axis direction. The axis direction is then determined using the right-hand rule. Axial direction.

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

[0125] Linkage length The length of link i-1 is shaft and The length of the common normal to the axis;

[0126] Linkage torsion angle Linkage torsion angle Indicated by The direction is the positive direction. Axis to The rotation angle of the axis.

[0127] Linkage offset :from and The directed distance (positive direction) from the intersection of the axes to the origin of the link coordinate system {i} direction).

[0128] Joint angle :by The direction is the positive direction. Axis to The rotation angle of the axis.

[0129] Based on the link coordinate system and DH parameters of the robotic arm, the transformation matrix between two adjacent link coordinate systems can be calculated by relating it to equation (2-4) in Section 2.2. The formula is as follows:

[0130]

[0131] Based on the structure of the harvesting robot arm and the improved DH method rules, a link coordinate system for the harvesting robot arm is established, such as... Figure 5 As shown, coordinate system {0} is the world coordinate system. Table 1 shows the corresponding DH parameter table.

[0132] Table 1

[0133] 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, This represents the joint angle of joint i. To verify the accuracy of the kinematic model established using the DH parameter method, a visual model of the picking robot arm is created using the Link function in the Matlab Robotics Toolbox, such as... Figure 6 As shown.

[0135] Forward kinematics of a robotic arm refers to the kinematics of a robotic arm with known angles of each joint. The process of solving for the end effector posture of the robotic arm. The homogeneous transformation matrix between the coordinate systems of adjacent links of the robotic arm can be calculated using the above formula. , , , , , :

[0136]

[0137] By multiplying the homogeneous transformation matrices between the coordinate systems of adjacent links in the above formula, the end effector pose matrix of the robotic arm can be obtained. ;

[0138]

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

[0140]

[0141] For ease of writing, in the above formulas , , ,

[0142] , , Subsequent abbreviations with similar meanings will appear.

[0143] In harvesting scenarios, the typical process involves first determining the target point's location, then specifying the expected pose of the robotic arm's end effector, and finally controlling the joint angles to perform the harvesting operation. This embodiment uses an algebraic method to derive the analytical solution for the robotic arm's inverse kinematics:

[0144] end pose matrix Formulas multiplied on the left We can obtain:

[0145]

[0146] Performing matrix operations on the right side of the above equation yields:

[0147]

[0148] in:

[0149]

[0150] Combining the above formulas, and based on the fact that the matrix elements (2, 3) and (2, 4) at both ends are equal, we obtain the following equation:

[0151]

[0152] Simplifying the above equation, we get:

[0153]

[0154] According to trigonometric function transformations, let , , Solving for the problem yields the following:

[0155]

[0156] It can be known that: ;

[0157] Therefore, we can conclude that: ;

[0158] According to the above formula, the matrix elements (2, 1) and (2, 2) at both ends are equal, resulting in the following equation:

[0159]

[0160] From the above formula, we can obtain:

[0161]

[0162] According to the above formula, the matrix elements (1, 4) and (3, 4) at both ends are equal, resulting in the following equation:

[0163]

[0164] Eliminate the above expression , get about The equation is: ;

[0165] in, ;

[0166] Solving for the given information The expression is: ;

[0167] According to the above formula, we get:

[0168]

[0169] in: ;

[0170] Solving for the given information The expression is: ;

[0171] According to the above formula, the matrix elements (1, 3) and (3, 3) at both ends are equal, resulting in the following equation:

[0172]

[0173] Solving for the given information The expression is:

[0174]

[0175] therefore The expression is: ;

[0176] From the above expression for joint angles, it can be seen that when When joint 1 cannot be solved, the robotic arm experiences a shoulder anomaly; when When joint three cannot be solved, the robotic arm experiences an elbow singularity; when At that time, that is or Joint six cannot be solved, at which point the robotic arm experiences a wrist singularity; and , , There may be two sets of solutions for each, so a six-degree-of-freedom manipulator may have a maximum of eight sets of inverse kinematic solutions. However, due to the existence of singular positions, some solutions of the manipulator cannot be realized.

[0177] The workspace of a robotic arm refers to the set of positions reachable by the origin of the coordinate system of the robotic arm's end effector within the range of joint angle variations. The joint angle range of the harvesting robotic arm is shown in Table 2. The workspace is divided into the reachable workspace and the dexterous workspace. The reachable workspace is the set of positions reachable by the robotic arm's end effector in at least one direction, while the dexterous workspace is the set of positions reachable by the robotic arm's end effector in any direction. The dexterous workspace is a subset of the reachable workspace. When performing harvesting tasks, it is crucial to determine whether the robotic arm's end effector can reach the target fruit position in a certain posture, i.e., whether the target fruit position is within the reachable workspace. The main methods for calculating the robotic arm's workspace are analytical methods, numerical methods, and graphical methods. To provide a more intuitive observation of the robotic arm's workspace, this embodiment uses numerical and graphical methods to solve for the robotic arm's workspace. The numerical method uses the Monte Carlo method, which uses random sampling to solve for the robotic arm's workspace. 100,000 sampling points were randomly selected using Matlab to obtain the point cloud map of the robotic arm's workspace. For ease of observation, the point cloud diagram of the robotic arm's three-dimensional workspace is shown below. Figure 7 As shown, the XY, XZ, and YZ plane projection diagrams are shown.

[0178] Table 2

[0179] 1 -360～360 2 -135～135 3 -135～135 4 -175～175 5 -175～175 6 -360～360

[0180] The workspace of the robotic arm is solved using a graphical method. A side sectional view of the robotic arm's workspace is shown below. Figure 8 As shown, the shaded area represents the position that the robotic arm's end effector can reach. The process of obtaining each arc in the figure is as follows:

[0181] Arc 1: With all joints in their initial positions, the robotic arm is extended, and the end effector can reach the furthest point. Keeping the angles of other joints unchanged, joint 2 rotates from its minimum joint angle to its maximum joint angle. The arc traced by the origin of the robotic arm's end effector coordinate system is arc 1, with its center at [center missing]. , radius is .

[0182] Arc 2 and Arc 3: Place joint 2 at its maximum joint angle, with all other joint angles set to zero. Rotate joint 3 from its minimum joint angle to its maximum joint angle. The arc traced by the origin of the robotic arm's end effector coordinate system is arc 2, with its center at [center missing]. , radius is Similarly, by placing joint 2 at the minimum joint angle and setting other joint angles to zero, and rotating joint 3, arc 3 is obtained.

[0183] Arcs 4 and 5: Place joint 3 at its maximum joint angle, with all other joint angles at zero. Rotate joint 2 from its minimum joint angle to its maximum joint angle. The arc traced by the origin of the robotic arm's end effector coordinate system is arc 4, with its center at [missing information]. , radius is Similarly, by placing joint 3 at the minimum joint angle and setting other joint angles to zero, and rotating joint 3, arc 5 is obtained.

[0184] Because the elastic components such as harmonic reducers and torque sensors inside the joints of the harvesting robot arm cause the joints to exhibit a certain degree of flexibility, the desired rotation angle controlled by the motor end often cannot be fed back to the link end in a timely manner. Therefore, it is necessary to treat the joints of the harvesting robot arm as flexible joints, establish its dynamic model, and solve the dynamic model to obtain the actual joint rotation angle.

[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 connects to the motor, and the rigid wheel connects to the connecting rod. To analyze the joint dynamics of the robotic arm, this embodiment considers joint friction based on the linear torsional spring model proposed by Spong, and equates the robotic arm joint to a spring-damped model. The equivalent model diagram of the robotic arm joint is shown below. Figure 9 As shown, Let i be the rotor angle of the motor. Let be the rotation angle of link i. Let be the transmission ratio of joint i. Let be the stiffness of joint i. For the damping of joint i, Let i be the moment of inertia of the motor rotor at joint i. Let be the moment of inertia of link i. The dynamic model of joint i is:

[0186]

[0187] In the formula, The output torque of the joint i reducer, For the torque at the motor end of joint i, For the joint i motor end rotation angle, and The expression is:

[0188]

[0189] In the formula, The output torque of the motor rotor.

[0190] Assuming the origin of the motor rotor's reference coordinate system coincides with the origin of the connecting rod's coordinate system and is located at the center of mass at the motor end, and that the rotation axes of the motor rotor and the connecting rod are the same, this embodiment uses the Lagrange equation to derive the dynamic equations of the flexible joint robotic arm system. First, the connecting rod rotation angle is defined as the generalized coordinate of the connecting rod. The motor rotation angle is the motor rotor, and the motor's generalized coordinate system is the motor's rotation angle. The generalized coordinates of the two are represented as follows:

[0191]

[0192] Then, the kinetic energies of the robotic arm system's linkages and motor rotor are calculated as follows:

[0193]

[0194] In the formula, Let be the spatial inertia matrix of link i in the world coordinate system. Let be the spatial inertia matrix of the motor rotor i in the world coordinate system. Let be the spinor of link i in the world coordinate system. Let be the spinor of the motor rotor i in the world coordinate system. and The expression is:

[0195]

[0196] In the formula, Let {i} be the Jacobian matrix of the link coordinate system in the world coordinate system. In order to make The matrix whose fourth to sixth rows in the i-th column are zeros. Let represent the unit vector of the axis of rotation of motor rotor i in the world coordinate system, and .

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

[0198]

[0199] In the formula, Here is the mass matrix of the link. This is the mass matrix of the motor rotor. Let be the coupling term matrix between the connecting rod and the motor rotor. This is the moment of inertia matrix of the motor after deceleration.

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

[0201]

[0202] , Let be the masses of the i-th connecting rod and the motor rotor, respectively. , The centroids of the i-th link and the motor rotor in the world coordinate system are respectively... Representation on the axis, This is the acceleration due to gravity.

[0203] The elastic potential energy of the joint's flexible components and the dissipated energy generated by frictional damping are 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 follows: Using the Lagrange function The dynamic equations are obtained as follows:

[0208]

[0209] In the formula, , This represents the generalized force acting on the link. The generalized force acting on the motor is expressed as:

[0210]

[0211] In the formula, Given the external disturbance torque on link i, the dynamic equation of the flexible joint manipulator is obtained as follows:

[0212]

[0213] In the formula, The mass matrix of the complete kinetic equations. For Coriolis matrix, The vector of the gravity term. and These are the stiffness matrix and damping matrix of the joint, respectively. Where:

[0214]

[0215]

[0216] In the formula, It is the Coriolis matrix of the linkage.

[0217] Coupling terms It is only related to the reduction ratio and the moment of inertia at the motor end. Since the moment of inertia at the motor end of the harvesting robot studied in this embodiment is relatively small, the coupling term is made... Assuming the robotic arm's linkage is not affected by external forces, i.e. The above formula can be simplified to:

[0218]

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

[0220]

[0221] The dynamic equations of the robotic arm, expressed in terms of state equations, can be obtained as follows:

[0222]

[0223] The iterative formula for the implicit Runge-Kutta method is:

[0224]

[0225] In the formula, the subscript s represents the number of discrete elements; h represents the step size of the discrete elements.

[0226] In some embodiments, this application provides a coupled dynamics modeling system for a harvesting robotic arm, comprising:

[0227] The structural modeling module is used to obtain the actual structural composition of the harvesting robot arm and to model the structure of the harvesting robot arm to obtain the structural model of the harvesting robot arm.

[0228] The coordinate system establishment module is used to establish the link coordinate system of adjacent rotary joints of the harvesting robot arm based on the improved DH parameter method and the structural model.

[0229] The kinematic modeling module is used to establish the forward kinematic model of the harvesting robot based on the structural model and the link coordinate system, and to establish the inverse kinematic model of the harvesting robot based on the forward kinematic model using an algebraic method.

[0230] The flexible modeling module is used to acquire data on the elastic elements contained in the harvesting 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.

[0231] The coupled dynamics modeling module is used to construct a coupled dynamics model of the harvesting robot arm by introducing coupled constraints based on the dynamics model and the elastic dynamics model.

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

[0233] In some embodiments, this application provides a terminal, including:

[0234] The memory is used to store the coupled dynamics modeling program of the harvesting robot arm;

[0235] A processor is used to implement the steps of the harvesting manipulator coupling dynamics modeling method as described in the first aspect when executing the harvesting manipulator coupling dynamics modeling program.

[0236] In some embodiments, this 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 aforementioned method for modeling coupled dynamics of a picking robot arm.

[0237] It should be understood that the embodiments described in this specification are merely illustrative of the principles of the embodiments described herein. Other variations may also fall within the scope of this specification. Therefore, alternative configurations of the embodiments described herein are intended to be consistent with the teachings of this specification, rather than as examples or limitations. Accordingly, the embodiments described herein are not limited to those explicitly introduced and described herein.

Claims

1. A method for modeling coupled dynamics of a harvesting robotic arm, characterized in that, Includes the following steps: Step S1: Obtain the actual structural composition of the harvesting robot arm, model the structure of the harvesting robot arm, and obtain the structural model of the harvesting robot arm; Step S2: Establish the link coordinate system of adjacent rotary joints based on the harvesting robot arm structure model in Step S1 and the improved DH parameter method; Step S3: Based on the structural model of the harvesting robot arm in Step S1 and the linkage coordinate system of the harvesting robot arm in Step S2, establish the forward kinematic model of the harvesting robot arm. Based on the forward kinematics model of the harvesting robot and the algebraic method, the inverse kinematics model of the robot is derived and established. Step S4: Obtain the data of the elastic elements contained in the harvesting robot arm, establish a flexible deformation model for the flexible connection parts, model each link as a joint flexible element, and construct an elastic dynamic model. Step S5: Construct a dynamic model of the harvesting robotic arm. Based on the dynamic model and the elastic dynamic model, introduce coupling constraints to construct a coupled dynamic model. Based on the link coordinate system and DH parameters of the robotic arm, calculate the transformation matrix between adjacent link coordinate systems. The formula is as follows: Wherein, coordinate system {0} is the world coordinate system; link length The length of link i-1 is shaft and Common normal length of the shaft; connecting rod torsion angle For the link twist angle Indicated by The direction is the positive direction. Axis to Shaft rotation angle; joint rotation angle For The direction is the positive direction. Axis to Shaft rotation angle; connecting rod offset From and The positive direction from the intersection of the axes to the origin of the link coordinate system {i} Directed distance in a given direction; In step S5, the origin of the motor rotor reference coordinate system is assumed to coincide with the origin of the connecting rod coordinate system and be located at the centroid of the motor end. The rotation axes of the motor rotor and the connecting rod are the same. The connecting rod rotation angle is defined as the generalized coordinate of the connecting rod. The motor rotation angle is the motor rotor, and the motor's generalized coordinate system is the motor's rotation angle. The generalized coordinates of the two are represented as follows: The kinetic energies of the robotic arm system's connecting rods and motor rotor are calculated as follows: ;in, Let be the spatial inertia matrix of link i in the world coordinate system. Let be the spatial inertia matrix of the motor rotor i in the world coordinate system. Let be the spinor of link i in the world coordinate system. Let be the spinor of the motor rotor i in the world coordinate system. and The expression is: ;in, Let {i} be the Jacobian matrix of the link coordinate system in the world coordinate system. In order to make The matrix whose fourth to sixth rows in the i-th column are zeros. Let represent the unit vector of the axis of rotation of motor rotor i in the world coordinate system, and The total kinetic energy of the robotic arm system is: ;in, Here is the mass matrix of the link. The mass matrix of the motor rotor; Let be the coupling term matrix between the connecting rod and the motor rotor. Here is the moment of inertia matrix of the motor after deceleration; the gravitational potential energy of the robotic arm system is expressed as: ;in, , Let be the masses of the i-th connecting rod and the motor rotor, respectively. , The centroids of the i-th link and the motor rotor in the world coordinate system are respectively... Representation on the axis, The acceleration due to gravity; the energy dissipated by the elastic potential energy of the joint's flexible elements and frictional damping 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 follows: Using the Lagrange function The dynamic equations are obtained as follows: ;in, , This represents the generalized force acting on the link. The generalized force acting on the motor is expressed as: In the formula Given the external disturbance torque on link i, the dynamic equation of the flexible joint manipulator is obtained as follows: ;in, The mass matrix of the complete kinetic equations. For Coriolis matrix, The vector of the gravity term. and These are the stiffness matrix and damping matrix of the joint, respectively; where: , ;in, Let the link Coriolis matrix be used; let the coupling term be used. Assume that the linkage end of the robotic arm is not affected by external forces, i.e. ;get: The implicit Runge-Kutta method is used to solve the dynamic equations, with the following variable substitutions: The dynamic equations of the robotic arm, expressed in terms of state equations, can be obtained as follows: .

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

3. The method for coupled dynamics modeling of a harvesting robotic arm according to claim 2, characterized in that, The position in the reference coordinate system is represented by a three-dimensional vector, and the attitude in the reference coordinate system is represented by a third-order matrix. The form of the link coordinate system {B} relative to the reference coordinate system {A} is as follows: ;in, Let {B} represent the position vector of the origin of coordinate system {B} within {A}. , , Indicates in {A} , , Components of the three axes; Let {B} be the attitude matrix of coordinate system {A}, and let {B} be the rotation matrix relative to {A}. , , Represent the three unit axis vectors of coordinate system {B} using unit coordinate axes. , , Represented as: ;therefore: Define the homogeneous transformation matrix of coordinate system {B} relative to coordinate system {A} as: The operational rules governing the relative relationships between several coordinate systems are as follows: In the formula, Let represent the homogeneous transformation matrix of coordinate system {C} relative to coordinate system {A}. Let {C} be the homogeneous transformation matrix relative to coordinate system {B}; let {B} be the velocity state of the link, including its linear velocity and angular velocity; the three linear velocities and three angular velocities constitute the spinor of motion; let coordinate system {A} be the world coordinate system, and coordinate system {B} be the link coordinate system attached to the robotic arm link. The velocity of coordinate system {B} in coordinate system {A} is expressed as: ;in, This represents the linear velocity vector of the link, that is, the linear velocity vector of the origin of the link coordinate system {B}; This represents the angular velocity vector of the connecting rod. The unit axis of rotation representing angular velocity. The magnitude of the angular velocity is represented by: The spinor of the link in the world coordinate system is: ;in, Let {B} be the velocity Jacobian matrix of the link coordinate system in the world coordinate system.

4. The method for coupled dynamics modeling of a harvesting robotic 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 the corresponding link coordinate systems; the steps to establish the link coordinate system are as follows: Step S2-1: Confirm Axial direction, The axis of rotation of the shaft coincides with that of joint i. The axis of rotation of the shaft coincides with that of joint i-1. The positive direction of the axis is the direction of rotation of the corresponding joint; Step S2-2: Determine the origin of the coordinate system and obtain... shaft and The 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, the origin of the coordinate system should be manually selected and set 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, Confirm shaft and Axial direction; Choose the direction of the common normal as... The axis direction is determined according to the right-hand rule. Axial direction.

5. The method for coupled dynamics modeling of a harvesting robotic arm according to claim 1, characterized in that, In step S4, the robotic arm joints are modeled as equivalent to spring-damped models, and the dynamic model of joint i is: ;in, The output torque of the joint i-reducer, For the torque at the motor end of joint i, For the joint i motor end rotation angle, Let be the rotation angle of link i. Let be the stiffness of joint i. For the damping of joint i, Let i be the moment of inertia of the motor rotor at joint i. Let be the moment of inertia of link i; and The expression is: ;in, The output torque of the motor rotor; Let i be the transmission ratio of joint i; Let be the rotor angle of the motor at joint i.

6. A coupled dynamics modeling system for a harvesting robotic arm, used to execute the coupled dynamics modeling method for a harvesting robotic arm as described in claim 1, characterized in that, include: The structural modeling module is used to obtain the actual structural composition of the harvesting robot arm and to model the structure of the harvesting robot arm to obtain the structural model of the harvesting robot arm. The coordinate system establishment module is used to establish the link coordinate system of adjacent rotary joints of the harvesting robot arm based on the improved DH parameter method and the structural model. The kinematic modeling module is used to establish the forward kinematic model of the harvesting robot based on the structural model and the link coordinate system, and to establish the inverse kinematic model of the harvesting robot based on the forward kinematic model using an algebraic method. The flexible modeling module is used to acquire data on the elastic elements contained in the harvesting 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 construct a coupled dynamics model of the harvesting robot arm by introducing coupled constraints based on the dynamics model and the elastic dynamics model. The computation and processing module is used to solve the generalized coordinates of the robotic arm system based on the coupled dynamics model, establish the Lagrange dynamic equations of the system, and use the implicit Runge-Kutta method to solve the state.

7. A terminal, characterized in that, include: The memory is used to store the coupled dynamics modeling program of the harvesting robot arm; A processor is configured to implement the steps of the harvesting manipulator coupling dynamics modeling method as described in any one of claims 1-5 when executing the harvesting manipulator coupling dynamics modeling program.

8. 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 coupling dynamics modeling method for a picking robot arm as described in any one of claims 1 to 5.

Images