Control method, control system, device and medium of multi-link robot arm

Abstract

This invention discloses a control method, control system, device, and medium for a multi-link robotic arm. The control method includes: acquiring the dynamic data of each link and the dynamic data of the joints connected to each link; determining the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link; and controlling each link according to the inertia matrix of the multi-link robotic arm. Through the above method, the motors of the joints can be controlled based on the inertia matrix in the Newton-Lagrange formula, thereby achieving rapid control of each link and realizing real-time control of each link.

Description

Technical Field

[0001] This invention relates to the field of robot automatic control, and specifically to a control method, control system, equipment, and medium for a multi-link robotic arm. Background Technology

[0002] In robot dynamics calculations, the establishment of rigid body motion equations must be addressed first. For robots with three degrees of freedom or less, the motion equations can usually be derived using methods such as Lagrange's equations, the principle of virtual work, Newton-Euler equations, and Kane's equations. However, for mechanisms with more than three degrees of freedom, the rigid body motion equations become extremely complex, requiring symbolic derivation through computer programming. Due to the complexity of analytical equations, previous researchers have employed numerous simplifying assumptions to obtain numerical solutions. These assumptions include neglecting the Coriolis effect, ignoring friction, assuming some elements are massless, and ignoring joint offset. The resulting numerical results are only valid within a limited operational range, and in some scenarios, it is difficult to ignore certain aspects (e.g., the Coriolis force is difficult to ignore during high-speed motion).

[0003] Current dynamics techniques have largely eliminated simplification assumptions in solving the dynamic equations, yielding widely applicable general solutions. Meanwhile, model-based control schemes have become the primary method for achieving precise robot motion. This method relies on real-time feedback calculations, thus emphasizing efficient real-time computation of robot motion equations. Furthermore, control and mechanical engineers frequently utilize dynamic analysis as a key component during development, making cost-saving computation increasingly crucial for multi-link robotic arms. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to overcome the shortcomings of high motion calculation cost and low real-time performance of multi-link robotic arms in the prior art, and to provide a control method, control system, equipment and medium for multi-link robotic arms.

[0005] The present invention solves the above-mentioned technical problems through the following technical solution:

[0006] This invention provides a control method for a multi-link robotic arm, the control method comprising:

[0007] Acquire the dynamic data of each link and the dynamic data of the joints connected to each link;

[0008] The inertia matrix of the multi-link robotic arm is determined based on the dynamic data of each link and the dynamic data of the joints connected to each link.

[0009] The individual links are controlled according to the inertia matrix of the multi-link robotic arm.

[0010] Preferably, the dynamic data includes spatial location;

[0011] The steps for obtaining the dynamic data of each link and the dynamic data of the joints connected to each link specifically include:

[0012] The coordinate transformation matrix of each link is determined based on the rotation angle and the axial deflection angle of each link.

[0013] The spatial position of each link and the spatial position of the joints connected to each link are determined based on the coordinate transformation matrix of each link, the length of each link, and the offset of each link.

[0014] Preferably, the step of determining the coordinate transformation matrix of each link based on the rotation angle and the axial deflection angle of each link includes:

[0015] The determination is made according to the following formulas (1) and (2);

[0016]

[0017]

[0018] in, The coordinate transformation matrix representing the i-th link relative to the (i-1)-th link. The coordinate transformation matrix θ represents the i-th link relative to the reference coordinate system. i The rotation angle α represents the i-th link. i The axial deflection angle of the i-th link;

[0019] The step of determining the spatial position of each link and the spatial position of the joints connected to each link based on the coordinate transformation matrix of each link, the length of each link, and the offset of each link includes:

[0020] The determination is made according to the following formulas (3) and (4):

[0021]

[0022]

[0023] in, p represents the spatial position of the i-th link. i Represents the spatial position of the i-th joint. It represents the spatial position of the i-th link in a coordinate system with the j-th joint as the origin. The transpose of the coordinate transformation matrix representing the i-th link relative to the reference coordinate system. Characterizing the spatial position of the i-th link relative to the reference coordinate system, b iCharacterizing the length of the i-th link, d i Characterizes the bias of the i-th link.

[0024] Preferably, the step of determining the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link includes:

[0025] The determination is made according to the following formula (5):

[0026]

[0027] Among them, h i,j The element in the i-th row and j-th column of the inertia matrix is ​​represented. The coordinate transformation matrix representing the Kth link relative to the reference coordinate system. The transpose of the coordinate transformation matrix of the Kth link relative to the reference coordinate system; q i q represents the spatial position of the i-th joint. j Characterizes the spatial position of the j-th joint.

[0028] Preferably, the step of determining the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link specifically includes:

[0029] Arrange multiple links from the base according to their sequential connection order, and take the last link as the target link;

[0030] The target link and all other links after the target link are treated as a single rigid body system, and the output torque of the single rigid body system is determined based on the kinematic data of each link.

[0031] The link preceding the target link is taken as the new target link, and then the target link and all other links after the target link are taken as a single rigid body system. The output torque of the single rigid body system is determined based on the kinematic data of each link, until the output torque of all single rigid body systems is determined.

[0032] The inertia matrix of the multi-link robotic arm is determined based on the output torque of each single rigid body system.

[0033] Preferably, the dynamic data of each link includes the mass of each link, the position of the center of mass of each link, the length of each link, the moment of inertia of each link, and the rotation vector of each link;

[0034] The step of determining the output torque of the single rigid body system based on the kinematic data of each link specifically includes:

[0035] The mass of each rigid body system is determined based on the mass of each link.

[0036] The position of the center of mass of each rigid body system is determined based on the mass of each rigid body system, the position of the center of mass of each link, and the length of each link.

[0037] The moment of inertia of each rigid body system is determined based on the mass of each rigid body system, the position of the center of mass of each rigid body system, the length of each link, the moment of inertia of each link, and the position of the center of mass of each link.

[0038] The resultant external force of each rigid body system and the resultant external force of each link in each rigid body system are determined based on the mass of each rigid body system and the acceleration of the center of mass of each rigid body system.

[0039] The output torque generated by each rigid body system is determined based on the moment of inertia of each rigid body system, the rotation vector of each link, the resultant external force of each rigid body system acting on each link, and the position of the center of mass of each rigid body system.

[0040] Based on the rotation vector of each link in each rigid body system and the torque exerted by each rigid body system on the link.

[0041] Preferably, the dynamic data includes mechanical data and motion data;

[0042] The step of determining the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link specifically includes:

[0043] The first output torque matrix and the second output torque matrix are determined based on the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, and the Newton-Euler formula.

[0044] Wherein, the first output torque matrix is ​​the output torque matrix corresponding to the current spatial position, current velocity and current acceleration of each link;

[0045] The second output torque matrix is ​​the output torque matrix corresponding to each link at its current spatial position, current velocity, and zero acceleration;

[0046] The final output torque matrix is ​​obtained by using the difference between the first output torque matrix and the second output torque matrix.

[0047] The inertia matrix of the multi-link robotic arm is determined based on the preset acceleration matrix and the final output torque matrix.

[0048] As a second aspect of the present invention, the present invention provides a control system for a multi-link robotic arm, the control system comprising a dynamic data acquisition module, an inertia matrix determination module, and a link control module;

[0049] The dynamic data acquisition module is used to acquire the dynamic data of each link and the dynamic data of the joints connected to each link;

[0050] The inertia matrix determination module is used to determine the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link.

[0051] The linkage control module is used to control each link according to the inertia matrix of the multi-link robotic arm.

[0052] As a third aspect of the present invention, the present invention provides an electronic device including a memory, a processor, and a computer program stored in the memory and for running on the processor, wherein the processor executes the computer program to implement the control method of the multi-link robotic arm of the first aspect of the present invention.

[0053] As a fourth aspect of the present invention, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the control method for the multi-link robotic arm of the first aspect of the present invention.

[0054] Based on common knowledge in the field, the above-mentioned preferred conditions can be combined arbitrarily to obtain various preferred embodiments of the present invention.

[0055] The positive and progressive effect of this invention is that, through the inertia matrix of the multi-link robotic arm, each link in the multi-link robotic arm can be controlled quickly and directly.

[0056] This invention provides three methods for calculating the inertia matrix of a multi-link robotic arm.

[0057] Firstly, it can reduce the spatial positions of each link and joint to the same coordinate system based on the coordinate transformation matrix, thereby reducing the amount of computation when transforming the coordinate system of each link and joint, improving the computation speed, and enhancing the real-time control performance of the multi-link robotic arm.

[0058] Secondly, by treating the multiple links in a multi-link robotic arm as a single rigid body system, the inertia matrix of the multi-link robotic arm can be determined by using the inertia matrix of the single rigid body system. This greatly reduces the computational load of the multi-link robotic arm and improves its real-time control performance.

[0059] Thirdly, by using the first output torque matrix, the second output torque matrix, and the final output torque matrix, the inertia matrix of the multi-link manipulator can be determined based on the preset acceleration matrix. The inertia matrix can be directly obtained without calculating the Coriolis force and centrifugal force terms, inertial force terms, and external force constraint terms in the Newton-Euler formula, which greatly reduces the amount of calculation and improves the real-time control performance of the multi-link manipulator. Attached Figure Description

[0060] Figure 1 This is a first flowchart illustrating the control method of the multi-link robotic arm in Embodiment 1 of the present invention.

[0061] Figure 2 This is a partial flowchart illustrating the control method of the multi-link robotic arm in Embodiment 1 of the present invention.

[0062] Figure 3 This is a schematic diagram of the first structure of the multi-link robotic arm in Embodiment 1 of the present invention.

[0063] Figure 4 This is a schematic diagram of the second structure of the multi-link robotic arm in Embodiment 1 of the present invention.

[0064] Figure 5 This is a partial flowchart illustrating the control method of the multi-link robotic arm in Embodiment 1 of the present invention.

[0065] Figure 6 This is a schematic diagram of the single rigid body system of the multi-link robotic arm in Embodiment 1 of the present invention.

[0066] Figure 7 This is a partial flowchart illustrating the control method of the multi-link robotic arm in Embodiment 1 of the present invention.

[0067] Figure 8 This is a schematic diagram of the control system of the multi-link robotic arm in Embodiment 2 of the present invention.

[0068] Figure 9 This is a schematic diagram of the electronic device in Embodiment 3 of the present invention. Detailed Implementation

[0069] The present invention will be further illustrated by way of embodiments below, but the present invention is not limited to the scope of the embodiments described herein.

[0070] Example 1

[0071] This embodiment provides a control method for a multi-link robotic arm. Please refer to [link to relevant documentation]. Figure 1 The control methods include:

[0072] S1. Obtain the dynamic data of each link and the dynamic data of the joints connected to each link;

[0073] S2. Determine the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link.

[0074] S3. Control each link according to the inertia matrix of the multi-link robotic arm.

[0075] In this embodiment, the inertia matrix in the Newton-Lagrange formula can be used as a parameter input into the control program to control the motor, thereby driving the joints to control the trajectory of each link.

[0076] The Newton-Lagrange formula mentioned above is:

[0077] Where q represents spatial location, Characterizing speed, H(q) represents acceleration, and H(q) represents the inertia matrix. G(q) characterizes the Coriolis force and centrifugal force term, and K(q) characterizes the inertial force term. T k represents the external force constraint, and τ represents the output torque. In this embodiment, q specifically represents the spatial position of each link. Specifically, it represents the speed of each link. Specifically, H(q) represents the acceleration of each link, and H(q) specifically represents the inertia matrix of the multi-link robotic arm. Specifically, the Coriolis force and centrifugal force terms, K(q), are characterized by the multi-link robotic arm. T k specifically represents the external force constraint on the multi-link robotic arm, and τ specifically represents the output torque of the multi-link robotic arm. In this embodiment, the motors at the joints can be controlled based on the inertia matrix in the Newton-Lagrange formula, thereby achieving rapid control of each link.

[0078] This embodiment provides the following three methods for calculating the inertia matrix of multi-link machines, wherein:

[0079] Method 1

[0080] Please see Figure 2 In an optional embodiment, the dynamic data includes spatial location;

[0081] Step S1 specifically includes:

[0082] S11. Determine the coordinate transformation matrix of each link based on the rotation angle and the axial deflection angle of each link.

[0083] S12. Determine the spatial position of each link and the spatial position of the joints connected to each link based on the coordinate transformation matrix of each link, the length of each link, and the offset of each link.

[0084] Please see Figure 3In general, the spatial position of the first link is established in a coordinate system with the first joint point as the origin, the spatial position of the second link is established in a coordinate system with the second joint point as the origin, and so on. During implementation, coordinate system transformation is required. However, using the coordinate transformation matrix mentioned above, the spatial position of each link can be directly determined in the reference coordinate system (usually with the base of the multi-link robot arm as the origin).

[0085] In one embodiment, see Figure 4 In this embodiment, the base of the multi-link robotic arm may include fixed links. It should be noted that the fixed links in this application should be understood as part of the base, and the fixed links can be regarded as the "base" in this embodiment.

[0086] In an optional embodiment, step S11 includes: determining according to the following formulas (1) and (2);

[0087]

[0088]

[0089] in, The coordinate transformation matrix representing the i-th link relative to the (i-1)-th link. The coordinate transformation matrix θ represents the i-th link relative to the reference coordinate system. i The rotation angle α represents the i-th link. i The axial deflection angle of the i-th link;

[0090] Step S12 includes determining the following formulas (3) and (4):

[0091]

[0092]

[0093] in, p represents the spatial position of the i-th link. i Represents the spatial position of the i-th joint. It represents the spatial position of the i-th link in a coordinate system with the j-th joint as the origin. The transpose of the coordinate transformation matrix representing the i-th link relative to the reference coordinate system. Characterizing the spatial position of the i-th link relative to the reference coordinate system, a i Characterizing the length of the i-th link, θ i Characterizing the rotation angle of the i-th link, d i Characterizing the offset of the i-th link

[0094] In this embodiment, the coordinate transformation matrix between adjacent links can be determined by formula (1) and formula (2), and the spatial position of each link and the spatial position of the joint point connected to the link can be determined by the length and offset of each link.

[0095] In an optional embodiment, step S2 includes:

[0096] The determination is made according to the following formula (5):

[0097]

[0098] Among them, h i,j The element in the i-th row and j-th column of the inertia matrix is ​​represented. The coordinate transformation matrix representing the Kth link relative to the reference coordinate system. The transpose of the coordinate transformation matrix of the Kth link relative to the reference coordinate system; q i q represents the spatial position of the i-th joint. j Characterizes the spatial position of the j-th joint.

[0099] In this method, since the elements of the inertia matrix are symmetrical along the main diagonal, it is also possible to calculate only the elements on the main diagonal and one side of the main diagonal of the inertia matrix. Based on the elements on the main diagonal and one side of the main diagonal of the inertia matrix of the multi-link manipulator, the inertia matrix of the entire multi-link manipulator can be determined, further reducing the computational load of calculating the inertia matrix of the multi-link manipulator. In this embodiment, it should be noted that tr in formula (5) Representation matrix The traces.

[0100] Method 2

[0101] In an optional embodiment, please refer to Figure 5 as well as Figure 6 Step S2 specifically includes:

[0102] S201. Sort multiple links from the base according to the order of their connection, and take the last link as the target link;

[0103] S202. Treat the target link and all other links after the target link as a single rigid body system, and determine the output torque of the single rigid body system based on the kinematic data of each link.

[0104] S203. Sequentially take the previous link of the target link as the new target link, and return to step S202 until the output torque of all single rigid body systems is determined;

[0105] S204. Determine the inertia matrix of the multi-link robotic arm based on the output torque of each single rigid body system.

[0106] Please see Figure 3 In this embodiment, for example, starting from the base, it sequentially includes a first joint, a first link, a second joint, a second link, a third joint, and a third link that are interconnected. The inertia matrix of the multi-link robotic arm is specifically as follows:

[0107]

[0108] First, we take the third link as the target link and treat it as the first single rigid body system (since the third link is the last link, it is treated as the first single rigid body system). We then calculate the output torque of this single rigid body system. Next, we calculate the output torque of the first link and the output torque of the second link under the action of the first single rigid body system (the output torques of the first and second links under the action of the first single rigid body system can be obtained using the Newton-Euler formula). At this point, the output torque matrix τ3, composed of the first single rigid body system, the output torque of the first link under the action of the first single rigid body system, and the output torque of the second link, has a dimension of 1×3. (In the formula...) In this context, the first rigid body system and the system under the first rigid body system This can be derived using the Newton-Euler formula. Furthermore, the acceleration matrix is ​​composed of the first rigid body, the output torque of the first link under the action of the first rigid body system, and the acceleration corresponding to the acceleration of the second link (i.e.,...). () is available.

[0109] At this point, the inertia matrices of the first rigid body system and the first and second links under the action of the first rigid body system can be obtained, specifically:

[0110]

[0111] h 11,3 As h 1,3 h 12,3 As h 2,3 h 13,3 As h 3,3 .

[0112] Similarly, when considering the second and third links as a second rigid body system, the output torque of the second rigid body system and the inertia matrix under the action of the second rigid body system can be obtained, specifically:

[0113]

[0114] h 21,2 As h 1,2 h 22,2 As h2,2 ;

[0115] Similarly, by treating the first, second, and third links as a third rigid body system, the inertia matrix of the third rigid body system can be obtained, specifically:

[0116] H(q)3=(h 31,1 );

[0117] h 31,1 As h 1,1 ;

[0118] At this point, since the elements of the inertia matrix are symmetric about the main diagonal, all elements in H(q) can be determined.

[0119] It should be noted that the above embodiments are based on a multi-link robotic arm with three links for illustration. The technical solution claimed in this application is based on the same principle, and the method is also applicable when the number of links is greater than three.

[0120] In one embodiment, the dynamic data of each link includes the mass of each link, the position of the center of mass of each link, the length of each link, the moment of inertia of each link, and the rotation vector of each link.

[0121] In one embodiment, see Figure 7 Step S202 specifically includes:

[0122] S2021. Determine the mass of each single rigid body system based on the mass of each link.

[0123] S2022. Determine the position of the center of mass of each rigid body system based on the mass of each rigid body system, the position of the center of mass of each link, and the length of each link.

[0124] S2023. Determine the moment of inertia of each rigid body system based on the mass of each rigid body system, the position of the center of mass of each rigid body system, the length of each link, the moment of inertia of each link, and the position of the center of mass of each link.

[0125] S2024. Determine the resultant external force of each rigid body system and the resultant external force of each link in each rigid body system based on the mass of each rigid body system and the acceleration of the center of mass of each rigid body system.

[0126] S2025. Determine the torque of each rigid body system acting on the connecting rod based on the moment of inertia of each rigid body system, the rotation vector of each rigid body system, the resultant external force of each rigid body system acting on each connecting rod, and the position of the center of mass of each rigid body system.

[0127] S2026. Determine the output torque of each rigid body system based on the rotation vector of each link in each rigid body system and the torque of each rigid body system.

[0128] In one embodiment, the multi-link robotic arm includes n links, and step S2021 can be determined according to formula (4.1):

[0129] M j =M j+1 +m j (4.1)

[0130] M j The mass m of the j-th rigid body system is represented by m. j Characterizes the mass of the (n-j+1)th link;

[0131] In one embodiment, step S2022 can be determined according to the following formula (4.2):

[0132]

[0133] c j M represents the position of the center of mass of the j-th rigid body system. j Characterizing the mass of the j-th single rigid body system, s j The position of the center of mass of the (n-j+1)th link is represented. Characterizes the length of the (n-j+1)th link;

[0134] In one embodiment, step S2023 can be determined according to formula (4.3):

[0135]

[0136] E j M represents the moment of inertia of the j-th rigid body system. j+1 The mass of the (j+1)th single rigid body system is represented by c. j Characterizes the position of the center of mass of the j-th rigid body system. I represents the length of the (n-j+1)th link, and J represents the identity matrix. j The moment of inertia of the (n-j+1)th link, s j The position of the center of mass of the (n-j+1)th link;

[0137] In one embodiment, step S2024 can be determined according to formula (4.4):

[0138]

[0139] F j M represents the net external force generated by the j-th rigid body system. j Characterize the mass of the j-th single rigid body system. The acceleration of the center of mass of the j-th rigid body system, z j-1The rotation vector representing the (n-j+1)th link (since it is considered as a single rigid body system, the rotation vector of the (n-j+1)th link can also be considered as the rotation vector of the (j-1)th single rigid body system), f j Characterizes the net external force acting on the nj-th link;

[0140] In one embodiment, step S2025 can be determined according to formula (4.5) or formula (4.6):

[0141] n j =M j +c j ×F j (4.5)

[0142] n j M represents the torque exerted by the j-th rigid body system on the nj-th link. j The mass of the j-th rigid body system is represented by c. j F represents the position of the center of mass of the j-th rigid body system. j Characterizes the net external force generated by the j-th rigid body system;

[0143] n j =n j+1 +p j ×f j (4.6)

[0144] n j f represents the torque exerted by the j-th rigid body system on the nj-th link. j p represents the force exerted by the (n-j+1)th link on the j-th rigid body system. j Characterizes the position of the center of mass of the i-th single rigid body system;

[0145] Regarding the choice of formulas (4.5) and (4.6), when the output torque of a single rigid body system is obtained, formula (4.6) can be nested to quickly obtain the output torque of other single rigid body systems.

[0146] In one embodiment, step S2026 can be determined according to formula (4.7):

[0147] τ j =z j ·n j (4.7)

[0148] τ j The output torque of the j-th rigid body system, z j The rotation vector representing the (n-j+1)th link. In the above embodiment, the rotation vector in the Newton-Lagrange formula can be determined using the Newton-Euler formula. In this case, only H(q) in the Newton-Lagrange formula is unknown. H(q) is finally determined by the acceleration of each link.

[0149] In this embodiment, the inertia matrix of each single rigid body system is determined sequentially, and the elements of the inertia matrix of each single rigid body system are determined based on the elements of the inertia matrix of each single rigid body system. Finally, the inertia matrix of the multi-link manipulator is determined based on the elements of the inertia matrix of each single rigid body system, thus controlling each link. Compared to the original method of directly calculating the inertia matrix of the multi-link manipulator, this method involves less computation, offers better real-time performance, and allows for more real-time control of each link in the multi-link manipulator.

[0150] Method 3

[0151] In an optional embodiment, step S1 includes:

[0152] Acquire the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, and the mass of each link.

[0153] Step S2 specifically includes:

[0154] The first output torque matrix and the second output torque matrix are determined based on the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, and the Newton-Euler formula.

[0155] Among them, the first output torque matrix is ​​the output torque matrix corresponding to the current spatial position, current velocity and current acceleration of each link;

[0156] The second output torque matrix is ​​the output torque matrix corresponding to each link at its current spatial position, current velocity, and zero acceleration.

[0157] The final output torque matrix is ​​obtained by using the difference between the first output torque matrix and the second output torque matrix.

[0158] The inertia matrix of the multi-link robotic arm is determined based on the preset acceleration matrix and the final output torque matrix.

[0159] It should be noted that the elements included in the first output torque matrix in this embodiment represent the output torque of each link.

[0160] In the above embodiments, according to the Newton-Lagrange formula:

[0161] The formula for the first output torque matrix is:

[0162]

[0163] As long as the acceleration of each link is... (That is, zero acceleration as mentioned above), by substituting the current spatial position and current velocity of each link into the Newton-Lagrange formula, we can obtain the second output torque matrix:

[0164] at this time, We can directly determine the acceleration of each link. (In this application, since it is a multi-link robotic arm composed of multiple links, the acceleration of each link is...) (It is calculated in the form of an acceleration matrix). For example, in a multi-link robotic arm with three links, the elements of the acceleration matrix include the acceleration of the first link. Characterizing the acceleration of the second link and the acceleration of the third link At this point, the preset acceleration matrix can be [1,0,0]. T [0,1,0] T and [0,0,1] T These three preset acceleration matrices reduce the computational burden of calculating the inertia matrix. It should be noted that the number of these preset acceleration matrices corresponds to the number of links. The elements in the preset acceleration matrices are sequentially set to 1.

[0165] It should be noted that the output torques of each link in the first and second output torque matrices can be determined according to the following Newton-Euler formulas, specifically formulas (5.1) to (5.9):

[0166]

[0167]

[0168]

[0169]

[0170]

[0171]

[0172] f i =F i +f i+1 (5.7)

[0173]

[0174] τ i =z i ·n i (5.9)

[0175] ωi Characterizing the angular velocity of the i-th link, Characterizing the velocity of the i-th joint, z i The rotation vector representing the i-th link, The acceleration characterizing the angular velocity of the i-th link. Characterizing the acceleration of the i-th joint, Characterizing the linear acceleration of the i-th link, Characterizing the position of the i-th link, Characterizing the acceleration of the center of mass of the i-th link, s i The position of the centroid of the i-th link in the coordinate axis with the origin at the i-th joint, F i Characterizing the net external force of the i-th link, m i Characterizing the mass of the i-th link, J i Characterizing the inertia tensor of the i-th link, f i The force N representing the force exerted by the (i+1)th link on the ith link. i Characterizing the torque of the i-th link; n i Characterizing the torque τ exerted by the first link on the target link i Characterizing the output torque of the i-th link, p i It represents the position of the center of mass of the i-th link.

[0176] It should be noted that in Method 2, the output torque of the single rigid body system and the output torque of the other links under the action of the single rigid body can also use the first output torque matrix and the second output torque matrix in Method 3, and determine the final output torque matrix.

[0177] The inertia matrix of the multi-link robotic arm is determined based on the preset acceleration matrix and the final output torque matrix.

[0178] The difference between the first output torque matrix in Equation 3 and the first output torque matrix used in Method 2 is that the elements of the first output torque matrix in Method 3 represent the output torque of the nth link, the output torque of the (n-1)th link, ... the output torque of the first link, respectively; while the elements of the first output torque matrix in Method 2 represent the output torque of the jth single rigid body system, the output torque of the njth link, the output torque of the (nj-1)th link, ... the output torque of the first link, respectively.

[0179] It should be noted that i, j, and n mentioned in this application are all positive integers. Here, n represents the number of links in the entire multi-link robotic arm. The terms "first," "second," and "third" used in this application are for the purpose of clearly describing each link, each joint, and each single rigid body system. Modifying the description of the "xth" link, joint, or single rigid body system using the method provided in this application is also within the scope of protection claimed in this application.

[0180] Example 2

[0181] Please see Figure 8 This embodiment provides a control system for a multi-link robotic arm, used to implement the control method of the multi-link robotic arm in Embodiment 1 of the present invention. The control system includes a dynamic data acquisition module 201, an inertia matrix determination module 202, and a link control module 203.

[0182] The dynamic data acquisition module 201 is used to acquire the dynamic data of each link and the dynamic data of the joints connected to each link;

[0183] The inertia matrix determination module 202 is used to determine the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link.

[0184] The linkage control module 203 is used to control each link according to the inertia matrix of the multi-link robotic arm.

[0185] In an optional embodiment, the dynamic data includes spatial location;

[0186] The dynamic data acquisition module 201 is used to determine the coordinate transformation matrix of each link based on the rotation angle and the shaft deflection angle of each link.

[0187] The spatial position of each link and the spatial position of the joints connected to each link are determined based on the coordinate transformation matrix of each link, the length of each link, and the offset of each link.

[0188] In an optional embodiment, the coordinate transformation matrix of each link can be determined according to the rotation angle and the axial deflection angle of each link, based on the following formulas (1) and (2);

[0189]

[0190]

[0191] The spatial position of each link and the spatial position of the joints connected to each link are determined according to the coordinate transformation matrix of each link, the length of each link, and the offset of each link. The determination is made according to the following formulas (3) and (4):

[0192]

[0193]

[0194] The inertia matrix determination module 202 is specifically used to sort multiple links from the base according to the sequential connection order of the links, and to take the last link as the target link;

[0195] Treat the target link and all other links after the target link as a single rigid body system;

[0196] Determine the output torque matrix of the single rigid body system based on the kinematic data of each link;

[0197] The link preceding the target link is taken as the new target link, and then all the remaining links after the target link are taken as single rigid body systems. The output torque matrix of the single rigid body system is determined based on the kinematic data of each link, until the output torque matrix of all single rigid body systems is determined.

[0198] The inertia matrix of the multi-link robotic arm is determined based on the output torque matrix of each single rigid body system.

[0199] In one embodiment, the inertia matrix determination module 202 can call the following formula (5) to determine the inertia matrix of the multi-link manipulator based on the dynamic data of each link and the dynamic data of the joints connected to each link;

[0200]

[0201] Among them, h i,j The element in the i-th row and j-th column of the inertia matrix is ​​represented. The coordinate transformation matrix representing the Kth link relative to the reference coordinate system. The transpose of the coordinate transformation matrix of the Kth link relative to the reference coordinate system; q i q represents the spatial position of the i-th joint. j tr represents the spatial location of the j-th joint. Representation matrix The traces.

[0202] In an optional embodiment, the dynamic data of the links include the mass of each link, the position of the center of mass of each link, the length of each link, the moment of inertia of each link, and the rotation vector of each link.

[0203] The output torque of the single rigid body system is determined based on the kinematic data of each link, including:

[0204] The mass of each rigid body system is determined based on the mass of each link.

[0205] The position of the center of mass of each rigid body system is determined based on the mass of each rigid body system, the position of the center of mass of each link, and the length of each link.

[0206] The moment of inertia of each rigid body system is determined based on the mass of each rigid body system, the position of the center of mass of each rigid body system, the length of each link, the moment of inertia of each link, and the position of the center of mass of each link.

[0207] The resultant external force of each rigid body system and the resultant external force of each link in each rigid body system are determined based on the mass of each rigid body system and the acceleration of the center of mass of each rigid body system.

[0208] The torques of each rigid body system acting on the connecting rods are determined based on the moment of inertia of each rigid body system, the rotation vector of each link, the resultant external force of each rigid body system acting on each link, and the position of the center of mass of each rigid body system.

[0209] The output torque of each rigid body system is determined based on the rotation vector of each link in each rigid body system and the torque of each rigid body system.

[0210] The mass of each rigid body system is determined based on the mass of each link, specifically according to formula (4.1):

[0211] M j =M j+1 +m j (4.1)

[0212] The position of the center of mass of each rigid body system can be determined according to the following formula (4.2): Based on the mass of each rigid body system, the position of the center of mass of each link, and the length of each link.

[0213]

[0214] The moment of inertia of each rigid body system can be determined using formula (4.3) based on the mass of each rigid body system, the position of the center of mass of each rigid body system, the length of each link, the moment of inertia of each link, and the position of the center of mass of each link.

[0215]

[0216] The net external force of each rigid body system is determined based on its mass and the acceleration of its center of mass. The net external force of each link in each rigid body system can be determined using formula (4.4).

[0217]

[0218] S2035. Based on the moment of inertia of each rigid body system, the rotation vector of each rigid body system, the resultant external force of each rigid body system acting on each link, and the position of the center of mass of each rigid body system, the torque of each rigid body system acting on the link can be determined using formulas (4.5) and (4.6).

[0219] n j =M j +c j ×F j (4.5)

[0220] n j =n j+1 +p j ×f j (4.6)

[0221] The torque of each rigid body system can be determined based on the rotation vector of each link in each rigid body system and the output torque of each rigid body system, according to formula (4.7):

[0222] τ j =z j ·n j (4.7)

[0223] In an optional embodiment, acquiring the dynamic data of each link and the dynamic data of the joints connected to each link includes:

[0224] Acquire the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, and the mass of each link.

[0225] The inertia matrix determination module is used to determine the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link. Specifically, it includes:

[0226] The first output torque matrix and the second output torque matrix are determined based on the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, and the Newton-Euler formula.

[0227] Among them, the first output torque matrix is ​​the output torque matrix corresponding to the current spatial position, current velocity and current acceleration of each link;

[0228] The second output torque matrix is ​​the output torque matrix corresponding to each link at its current spatial position, current velocity, and zero acceleration.

[0229] The final output torque matrix is ​​obtained by using the difference between the first output torque matrix and the second output torque matrix.

[0230] The inertia matrix of the multi-link robotic arm is determined based on the preset acceleration matrix and the final output torque matrix. It should be noted that the Newton-Euler formula in this embodiment specifically includes formulas (5.1) to (5.9):

[0231]

[0232]

[0233]

[0234]

[0235]

[0236]

[0237] f i =F i +f i+1 (5.7)

[0238]

[0239] τ i =z i ·n i (5.9)

[0240] In an optional embodiment, the dynamic data acquisition module 201 is specifically used to acquire the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, and the mass of each link.

[0241] The inertia matrix determination module 202 is specifically used to determine the first output torque matrix and the second output torque matrix based on the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, the mass of each link, and the Newton-Euler formula.

[0242] Among them, the first output torque matrix is ​​the output torque matrix corresponding to the current spatial position, current velocity and current acceleration of each link;

[0243] The second output torque matrix is ​​the output torque matrix corresponding to each link at its current spatial position, current velocity, and zero acceleration.

[0244] The final output torque matrix is ​​obtained by using the difference between the first output torque matrix and the second output torque matrix.

[0245] The inertia matrix of the multi-link robotic arm is determined based on the preset acceleration matrix and the final output torque matrix.

[0246] Example 3

[0247] Figure 9 This is a schematic diagram of the structure of an electronic device provided in this embodiment. The electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the control method for the multi-link robotic arm in Embodiment 1. Figure 9 The electronic device 30 shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of the present invention.

[0248] like Figure 9As shown, the electronic device 30 can be manifested as a general-purpose computing device, such as a server device. The components of the electronic device 30 may include, but are not limited to: at least one processor 31, at least one memory 32, and a bus 33 connecting different system components (including memory 32 and processor 31).

[0249] Bus 33 includes a data bus, an address bus, and a control bus.

[0250] The memory 32 may include volatile memory, such as random access memory (RAM) 321 and / or cache memory 322, and may further include read-only memory (ROM) 323.

[0251] The memory 32 may also include a program / utility 325 having a set (at least one) of program modules 324, including but not limited to: an operating system, one or more application programs, other program modules, and program data, each or some combination of these examples may include an implementation of a network environment.

[0252] The processor 31 executes various functional applications and data processing by running computer programs stored in the memory 32, such as the control method of the multi-link robotic arm in Embodiment 1.

[0253] Electronic device 30 can also communicate with one or more external devices 34 (e.g., keyboard, pointing device, etc.). This communication can be performed via input / output (I / O) interface 35. Furthermore, the model-generated device 30 can also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN), and / or public network, such as the Internet) via network adapter 36. As shown, network adapter 36 communicates with other modules of the model-generated device 30 via bus 33. It should be understood that, although not shown in the figure, other hardware and / or software modules can be used in conjunction with the model-generated device 30, including but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID (disk array) systems, tape drives, and data backup storage systems.

[0254] It should be noted that although several units / modules or sub-units / modules of the electronic device have been mentioned in the detailed description above, this division is merely exemplary and not mandatory. In fact, according to embodiments of the present invention, the features and functions of two or more units / modules described above can be embodied in one unit / module. Conversely, the features and functions of one unit / module described above can be further divided and embodied by multiple units / modules.

[0255] Example 4

[0256] This embodiment provides a computer-readable storage medium storing a computer program thereon. When the program is executed by a processor, it implements the control method for the multi-link robotic arm in Embodiment 1.

[0257] The readable storage medium may be more specifically adopted, including but not limited to: portable disk, hard disk, random access memory, read-only memory, erasable programmable read-only memory, optical storage device, magnetic storage device, or any suitable combination thereof.

[0258] In a possible implementation, the present invention can also be implemented as a program product comprising program code, which, when the program product is run on a terminal device, causes the terminal device to execute the control method for implementing the multi-link robotic arm in Embodiment 1.

[0259] The program code for executing the present invention can be written in any combination of one or more programming languages. The program code can be executed entirely on the user device, partially on the user device, as a standalone software package, partially on the user device and partially on a remote device, or entirely on a remote device.

[0260] While specific embodiments of the present invention have been described above, those skilled in the art should understand that these are merely illustrative examples, and the scope of protection of the present invention is defined by the appended claims. Those skilled in the art can make various changes or modifications to these embodiments without departing from the principles and essence of the present invention, but all such changes and modifications fall within the scope of protection of the present invention.

Claims

1. A control method for a multi-link robotic arm, characterized in that, The control method includes: Acquire the dynamic data of each link and the dynamic data of the joints connected to each link; The inertia matrix of the multi-link robotic arm is determined based on the dynamic data of each link and the dynamic data of the joints connected to each link. The inertia matrix of the multi-link robotic arm is used to control each link; The step of determining the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link includes: The steps for determining the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link specifically include: Arrange multiple links from the base according to their sequential connection order, and take the last link as the target link; The target link and all other links after the target link are treated as a single rigid body system, and the output torque of the single rigid body system is determined based on the kinematic data of each link. The link preceding the target link is taken as the new target link, and then the target link and all other links after the target link are taken as a single rigid body system. The output torque of the single rigid body system is determined based on the kinematic data of each link, until the output torque of all single rigid body systems is determined. The inertia matrix of the multi-link robotic arm is determined based on the output torque of each single rigid body system. or; The dynamic data includes mechanical data and motion data; The step of determining the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link specifically includes: The first output torque matrix and the second output torque matrix are determined based on the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, and the Newton-Euler formula. Wherein, the first output torque matrix is ​​the output torque matrix corresponding to the current spatial position, current velocity and current acceleration of each link; The second output torque matrix is ​​the output torque matrix corresponding to each link at its current spatial position, current velocity, and zero acceleration; The final output torque matrix is ​​obtained by using the difference between the first output torque matrix and the second output torque matrix. The inertia matrix of the multi-link robotic arm is determined based on the preset acceleration matrix and the final output torque matrix.

2. The control method for the multi-link robotic arm as described in claim 1, characterized in that, The dynamic data of each link includes the mass of each link, the position of the center of mass of each link, the length of each link, the moment of inertia of each link, and the rotation vector of each link. The step of determining the output torque of the single rigid body system based on the kinematic data of each link specifically includes: The mass of each rigid body system is determined based on the mass of each link. The position of the center of mass of each rigid body system is determined based on the mass of each rigid body system, the position of the center of mass of each link, and the length of each link. The moment of inertia of each rigid body system is determined based on the mass of each rigid body system, the position of the center of mass of each rigid body system, the length of each link, the moment of inertia of each link, and the position of the center of mass of each link. The resultant external force of each rigid body system and the resultant external force of each link in each rigid body system are determined based on the mass of each rigid body system and the acceleration of the center of mass of each rigid body system. The torques of each rigid body system acting on the connecting rods are determined based on the moment of inertia of each rigid body system, the rotation vector of each link, the resultant external force of each rigid body system acting on each link, and the position of the center of mass of each rigid body system. The output torque of each rigid body system is determined based on the rotation vector of each link in each rigid body system and the torque of each rigid body system.

3. A control system for a multi-link robotic arm, characterized in that, The control system includes a dynamic data acquisition module, an inertia matrix determination module, and a linkage control module; The dynamic data acquisition module is used to acquire the dynamic data of each link and the dynamic data of the joints connected to each link; The inertia matrix determination module is used to determine the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link. The linkage control module is used to control each link according to the inertia matrix of the multi-link robotic arm; The inertia matrix determination module is specifically used to sort multiple links in the base according to the sequential connection order of the links, and to take the last link as the target link; Treat the target link and all other links after the target link as a single rigid body system; Determine the output torque matrix of the single rigid body system based on the kinematic data of each link; The link preceding the target link is taken as the new target link, and then all the remaining links after the target link are taken as single rigid body systems. The output torque matrix of the single rigid body system is determined based on the kinematic data of each link, until the output torque matrix of all single rigid body systems is determined. The inertia matrix of the multi-link robotic arm is determined based on the output torque matrix of each single rigid body system. or; The dynamic data acquisition module is specifically used to acquire the mechanical data of each joint node, the motion data of each joint node, the mechanical data of each link, the motion data of each link, and the mass of each link. The inertia matrix determination module is specifically used to determine the inertia matrix of the multi-link robotic arm based on the dynamic data of each link and the dynamic data of the joints connected to each link. Specifically, this includes: The first output torque matrix and the second output torque matrix are determined based on the mechanical data of each joint, the motion data of each joint, the mechanical data of each link, the motion data of each link, and the Newton-Euler formula. Among them, the first output torque matrix is ​​the output torque matrix corresponding to the current spatial position, current velocity and current acceleration of each link; The second output torque matrix is ​​the output torque matrix corresponding to each link at its current spatial position, current velocity, and zero acceleration. The final output torque matrix is ​​obtained by using the difference between the first output torque matrix and the second output torque matrix. The inertia matrix of the multi-link robotic arm is determined based on the preset acceleration matrix and the final output torque matrix.

4. An electronic device comprising a memory, a processor, and a computer program stored in the memory and for running on the processor, characterized in that, When the processor executes the computer program, it implements the control method for the multi-link robotic arm as described in any one of claims 1 to 2.

5. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the control method of the multi-link robotic arm as described in any one of claims 1 to 2.

Images