CN-122020897-A - Joint driving moment determining method for six-axis mechanical arm

CN122020897ACN 122020897 ACN122020897 ACN 122020897ACN-122020897-A

Abstract

The application discloses a joint driving moment determining method of a six-axis mechanical arm, which relates to the technical field of multi-body system dynamics and comprises the steps of determining a connecting rod centroid linear velocity jacobian matrix and a joint rotation axis angular velocity propagation matrix to determine a rigid body part mass matrix, determining a motor rotor centroid linear velocity jacobian matrix, determining a motor rotor mass matrix based on the motor rotor centroid linear velocity jacobian matrix to determine a system mass matrix to further obtain a joint inertia moment item, determining a gravity compensation moment item based on the connecting rod centroid linear velocity jacobian matrix, the connecting rod mass, the motor rotor centroid linear velocity jacobian matrix and the motor rotor mass of all mechanical arms, determining a nonlinear velocity moment item based on the system mass matrix, a joint angle vector and a joint angular velocity vector, and determining a joint driving force based on the joint inertia moment item, the gravity compensation moment item and the nonlinear velocity moment item. The method improves the accuracy of determining the joint driving moment of the six-axis mechanical arm.

Inventors

WANG QI
ZHU JUNXI
XU LINGFENG

Assignees

景德镇陶瓷大学

Dates

Publication Date: 20260512
Application Date: 20260129

Claims (10)

1. The method for determining the joint driving moment of the six-axis mechanical arm is characterized by comprising the following steps of: Determining a connecting rod mass core line speed jacobian matrix of each mechanical arm based on a joint angle vector, a connecting rod length vector, a connecting rod torsion angle vector, a connecting rod offset vector and a mechanical arm mass center position vector of the six-axis mechanical arm; determining a joint rotation axis angular velocity propagation matrix of each mechanical arm based on the joint rotation axis direction of each mechanical arm; Determining a rigid body part mass matrix of each mechanical arm based on the connecting rod mass, the connecting rod mass core line speed jacobian matrix, the moment of inertia tensor at the connecting rod mass center and the joint rotation axis angular velocity propagation matrix of each mechanical arm; Determining a jacobian matrix of the linear speed of the motor rotor mass center of each mechanical arm based on the joint angle vector and the position of the motor rotor mass center of each mechanical arm under a corresponding connecting rod coordinate system; determining a motor rotor mass matrix of each mechanical arm based on the motor rotor mass of each mechanical arm and the motor rotor centroid linear velocity jacobian matrix; determining a system mass matrix of the six-axis mechanical arm based on the rigid body part mass matrix and the motor rotor mass matrix of all the mechanical arms; based on a system mass matrix and a joint angular acceleration vector, determining a joint inertia moment item of the six-axis mechanical arm; determining a gravity compensation moment item of the six-axis mechanical arm based on the connecting rod mass center line speed jacobian matrix, the connecting rod mass, the motor rotor mass center line speed jacobian matrix and the motor rotor mass of all the mechanical arms; Based on a system mass matrix, a joint angle vector and a joint angular velocity vector, determining a nonlinear velocity moment term of the six-axis mechanical arm; And determining the joint driving force of the six-axis mechanical arm based on the joint inertia moment item, the gravity compensation moment item and the nonlinear speed moment item of the six-axis mechanical arm.
2. The method for determining the joint driving moment of the six-axis mechanical arm according to claim 1, further comprising, before determining the link mass-center-line velocity jacobian matrix for each mechanical arm based on the joint angle vector, the link length vector, the link torsion angle vector, the link offset vector, and the mechanical arm centroid position vector of the six-axis mechanical arm: Acquiring joint state parameters, connecting rod geometric structure parameters, connecting rod inertia parameters, motor inertia structure parameters and joint rotation shaft directions of the six-axis mechanical arm in the motion process; the joint state parameters comprise a joint angle vector, a joint angular velocity vector and a joint angular acceleration vector, wherein the joint angle vector comprises the joint angle of each mechanical arm, the joint angular velocity vector comprises the joint angular velocity of each mechanical arm, and the joint angular acceleration vector comprises the joint angular acceleration of each mechanical arm; The connecting rod geometric structure parameters comprise a connecting rod length vector, a connecting rod torsion angle vector and a connecting rod offset vector, wherein the connecting rod length vector comprises the connecting rod length of each mechanical arm, the connecting rod torsion angle vector comprises the connecting rod torsion angle of each mechanical arm, and the connecting rod offset vector comprises the connecting rod offset of each mechanical arm; the connecting rod inertia parameters comprise a connecting rod mass vector, a connecting rod centroid moment of inertia tensor vector and a mechanical arm centroid position vector, wherein the connecting rod mass vector comprises the connecting rod mass of each mechanical arm, the connecting rod centroid moment of inertia tensor vector comprises the moment of inertia tensor of the connecting rod centroid of each mechanical arm, and the mechanical arm centroid position vector comprises the position of the centroid of each connecting rod under a corresponding connecting rod coordinate system; The motor inertia structure parameters comprise motor rotor mass vectors and motor rotor mass center position vectors, wherein the motor rotor mass vectors comprise motor rotor masses of all mechanical arms, and the motor rotor mass center position vectors comprise positions of motor rotor mass centers of all mechanical arms under a corresponding connecting rod coordinate system.
3. The method of determining the joint driving moment of a six-axis mechanical arm according to claim 2, wherein determining the link mass center line velocity jacobian matrix of each mechanical arm based on the joint angle vector, the link length vector, the link torsion angle vector, the link offset vector, and the mechanical arm centroid position vector of the six-axis mechanical arm comprises: Determining a homogeneous transformation matrix of the connecting rod of the corresponding mechanical arm based on the joint angle, the connecting rod length, the connecting rod torsion angle and the connecting rod offset distance of each mechanical arm respectively; Determining the serial numbers of all the mechanical arms, determining any mechanical arm as a current mechanical arm, and determining the mechanical arms with serial numbers smaller than the current mechanical arm as chain type mechanical arms, wherein the serial numbers of all the mechanical arms are respectively 1,2,3,4,5 and 6; determining an accumulated transformation matrix of the current mechanical arm based on the homogeneous transformation matrix of the connecting rod of each chained mechanical arm and the homogeneous transformation matrix of the connecting rod of the current mechanical arm; determining the position of the centroid of the connecting rod of the current mechanical arm under the base coordinate system based on the cumulative transformation matrix of the current mechanical arm and the position of the centroid of the connecting rod of the mechanical arm under the corresponding connecting rod coordinate system; and determining a jacobian matrix of the connecting rod centroid linear speed of the current mechanical arm based on the position of the centroid of the connecting rod of the current mechanical arm under the base coordinate system and the joint angle vector.
4. The joint driving torque determination method of six-axis mechanical arms according to claim 3, wherein determining the joint rotation axis angular velocity propagation matrix of each mechanical arm based on the joint rotation axis direction of each mechanical arm comprises: And determining an angular velocity propagation matrix of the joint rotation shaft of the current mechanical arm based on the joint rotation shaft direction of the current mechanical arm and the joint rotation shaft direction of the chain mechanical arm.
5. The method of determining the joint driving moment of a six-axis mechanical arm according to claim 3, wherein determining the rigid body portion mass matrix of each mechanical arm based on the link mass, the link mass-line velocity jacobian matrix, the moment of inertia tensor at the link mass center, and the joint rotation axis angular velocity propagation matrix of each mechanical arm comprises: Determining a rotation matrix of the current mechanical arm based on the accumulated transformation matrix of the current mechanical arm; and determining a rigid body part mass matrix of the current mechanical arm based on the rotation matrix, the connecting rod mass core line speed jacobian matrix, the moment of inertia tensor at the connecting rod mass center and the joint rotation axis angular velocity propagation matrix of the current mechanical arm.
6. The method for determining the joint driving moment of the six-axis mechanical arm according to claim 3, wherein determining the linear velocity jacobian matrix of the motor rotor centroid of each mechanical arm based on the joint angle vector and the position of the motor rotor centroid of each mechanical arm under the corresponding link coordinate system comprises: Determining the position of the motor rotor centroid of the current mechanical arm under a base coordinate system based on the accumulated transformation matrix of the current mechanical arm and the position of the motor rotor centroid under a corresponding connecting rod coordinate system; And determining a jacobian matrix of the linear speed of the motor rotor centroid of the current mechanical arm based on the position of the motor rotor centroid of the current mechanical arm under the base coordinate system and the joint angle vector.
7. The method of determining the joint driving moment of a six-axis mechanical arm according to claim 1, wherein determining the system mass matrix of the six-axis mechanical arm based on the rigid body part mass matrix and the motor rotor mass matrix of all the mechanical arms includes: And summing the mass matrix of the rigid body part of all the mechanical arms and the mass matrix of the motor rotor to obtain a system mass matrix of the six-axis mechanical arm.
8. The method of claim 1, wherein determining the gravity compensation moment term for the six-axis mechanical arm based on the link mass center line speed jacobian matrix, the link mass, the motor rotor mass center line speed jacobian matrix, and the motor rotor mass for all mechanical arms comprises: Obtaining a connecting rod gravity item of the six-axis mechanical arm based on the connecting rod mass core line velocity jacobian matrix, the connecting rod mass and the gravity acceleration vector of all the mechanical arms; Obtaining a motor rotor gravity item of the six-axis mechanical arm based on the motor rotor centroid linear velocity jacobian matrix, the motor rotor mass and the gravity acceleration vector of all the mechanical arms; And determining a gravity compensation moment item of the six-axis mechanical arm based on the connecting rod gravity item and the motor rotor gravity item of the six-axis mechanical arm.
9. The joint driving moment determination method of a six-axis mechanical arm according to claim 1, wherein determining a nonlinear velocity moment term of the six-axis mechanical arm based on a system mass matrix, a joint angle vector, and a joint angular velocity vector comprises: determining a centrifugal force item of the six-axis mechanical arm based on the system mass matrix, the joint angle vector and the joint angular velocity vector; based on a system mass matrix and a joint angular velocity vector, determining a Coriolis force item of the six-axis mechanical arm; And determining a nonlinear speed moment term of the six-axis mechanical arm based on the centrifugal force term and the Coriolis force term of the six-axis mechanical arm.
10. The joint driving moment determining method of a six-axis mechanical arm according to claim 1, wherein determining the joint driving force of the six-axis mechanical arm based on the joint inertia moment term, the gravity compensation moment term, and the nonlinear speed moment term of the six-axis mechanical arm includes: And summing the joint inertia moment term, the gravity compensation moment term and the nonlinear speed moment term of the six-axis mechanical arm to obtain the joint driving force of the six-axis mechanical arm.

Description

Joint driving moment determining method for six-axis mechanical arm Technical Field The application relates to the technical field of multi-body system dynamics, in particular to a joint driving moment determining method of a six-axis mechanical arm. Background In the control system of intelligent equipment such as industrial robots, collaborative robots and the like, determining high-precision joint driving moment is a key precondition for realizing accurate track tracking, dynamic compensation and force control. This process typically relies on an accurate mechanical arm dynamics model. Currently, the dominant method of determining joint driving moment is based on classical Lagrangian or Newton-Euler dynamics modeling theory. However, in practical engineering applications, in order to simplify the complex modeling process, the conventional method generally treats the motor rotor driving the joint as a pure inertial element for simple superposition or completely ignores the kinetic effect thereof when constructing a kinetic model. The simplified treatment has no obvious influence when the mechanical arm is in a low-speed and steady-state working condition, but when the mechanical arm performs high-speed and high-acceleration movement, the high-speed rotation of the motor rotor and the strong dynamic coupling effect between the motor rotor and the mechanical arm connecting rod become non-negligible. The conventional method cannot systematically incorporate the mass, moment of inertia, and coupling relation of the motor rotor to the link motion into a unified dynamic equation framework, resulting in an inherent deviation of the determined joint driving moment. This defect directly restricts further improvement of torque control accuracy and dynamic response performance in high-end applications (such as precision assembly, high-speed sorting, force-controlled polishing). Therefore, there is a need for a joint drive torque determination method that more accurately reflects the effects of motor rotor dynamics. Disclosure of Invention The application aims to provide a method for determining joint driving moment of a six-axis mechanical arm, so as to improve the accuracy of determining the joint driving moment of the six-axis mechanical arm. In order to achieve the above object, the present application provides the following. The application provides a joint driving moment determining method of a six-axis mechanical arm, which comprises the following steps: Determining a connecting rod mass core line speed jacobian matrix of each mechanical arm based on a joint angle vector, a connecting rod length vector, a connecting rod torsion angle vector, a connecting rod offset vector and a mechanical arm mass center position vector of the six-axis mechanical arm; determining a joint rotation axis angular velocity propagation matrix of each mechanical arm based on the joint rotation axis direction of each mechanical arm; Determining a rigid body part mass matrix of each mechanical arm based on the connecting rod mass, the connecting rod mass core line speed jacobian matrix, the moment of inertia tensor at the connecting rod mass center and the joint rotation axis angular velocity propagation matrix of each mechanical arm; Determining a jacobian matrix of the linear speed of the motor rotor mass center of each mechanical arm based on the joint angle vector and the position of the motor rotor mass center of each mechanical arm under a corresponding connecting rod coordinate system; determining a motor rotor mass matrix of each mechanical arm based on the motor rotor mass of each mechanical arm and the motor rotor centroid linear velocity jacobian matrix; determining a system mass matrix of the six-axis mechanical arm based on the rigid body part mass matrix and the motor rotor mass matrix of all the mechanical arms; based on a system mass matrix and a joint angular acceleration vector, determining a joint inertia moment item of the six-axis mechanical arm; determining a gravity compensation moment item of the six-axis mechanical arm based on the connecting rod mass center line speed jacobian matrix, the connecting rod mass, the motor rotor mass center line speed jacobian matrix and the motor rotor mass of all the mechanical arms; Based on a system mass matrix, a joint angle vector and a joint angular velocity vector, determining a nonlinear velocity moment term of the six-axis mechanical arm; And determining the joint driving force of the six-axis mechanical arm based on the joint inertia moment item, the gravity compensation moment item and the nonlinear speed moment item of the six-axis mechanical arm. According to the specific embodiment provided by the application, the application discloses the following technical effects: The application discloses a method for determining joint driving moment of a six-axis mechanical arm, which comprehensively considers a rigid body part mass matrix corresponding to a connecting rod and