Kinematic attitude analysis method for mechanical arm connection assembly

Abstract

The invention belongs to the field of space assembly motion analysis, and particularly relates to a mechanical arm connection assembly attitude analysis method. The method comprises the following specific steps: 1, acquiring the position, the speed, the attitude and the angular speed of a manipulated target under a body coordinate system; 2, according to the relative position, calculating the center of mass of the combination body and establishing a combination body coordinate system parallel to the body layout system; and calculating an assembly inertia tensor under the assembly coordinate system according to the relative attitude. And 3, analyzing an angular momentum kinematics formula of the combination during relative rotation of the target, and deducing a reaction torque generated by relative rotation. And 4, analyzing the angular momentum of the combination when the configuration changes, and deducing the moment generated by the configuration changes. And 5, after the moment is superposed, the attitude angular velocity of the combination is calculated according to the inertia tensor, and integration is carried out to obtain the attitude angle of the body with the layout system parallel to the combination system. The method can improve the simulation precision and reliability of a kinematics analysis system based on angular momentum conservation, and has been applied to in-orbit satellite ground tests.

Description

Technical Field

[0001] The invention relates to a kinematic posture analysis method for a mechanical arm connection assembly, belonging to the technical field of spatial assembly motion analysis. Background Art

[0002] Space attitude control in the aerospace field requires simulation verification and analysis of various ground-based control systems and dynamic components. The ground-based simulation analysis system is huge, and its supporting kinematic analysis requires fast calculation speed and high calculation accuracy. It is difficult to match the overall dynamic interface of commercial dynamics software with the joint simulation system, and the calculation accuracy of traditional kinematic analysis methods is significantly affected by transmission data packet loss. A combination kinematic attitude analysis method with strong interface adaptability, fast calculation speed and reliable calculation results is required.

[0003] The main theories of combined body motion analysis include Newton-Euler equations, Lagrange equations, Kane method, etc. Among them, Newton-Euler equations are used for dynamic modeling based on vector mechanics. All rigid body units in the system are decoupled, the motion equations of the center of mass of each unit are derived, the kinematic equations of different units in the system are obtained, and the interaction forces between the units are recursively calculated. Lagrange equations are a dynamic method based on analytical mechanics. The spatial position of a particle system is expressed by a generalized coordinate system, and the relationship between the actions of objects is described by energy and work. Kane method improves D'Alembert principle and virtual displacement principle, adopts generalized velocity as an independent variable, and incorporates related variables such as partial velocity to solve the generalized inertial force of the system.

[0004] For assemblies with large configuration changes, high degrees of freedom of the manipulator, and body mass characteristics that are not much greater than the target, the motion process of the body and the target is strongly coupled, and existing analysis methods are difficult to accurately analyze the changes in the body's posture. The Newton-Euler method will accumulate recursive errors in the closed-loop recursive assembly motion results, and the calculation error is large. The Lagrange equation method has a large amount of calculation. During the high-degree-of-freedom motion of the manipulator, the transmission efficiency of the target posture information is high and the accuracy is greatly affected by the data quality. When the Kane method has similar masses to the target and the motion is strongly coupled, it is necessary to introduce additional constraint equations, which makes the derivation complex and the reliability of the results low. Summary of the invention

[0005] The technical problem solved by the present invention is: for the strongly coupled assembly motion with large configuration changes, high degree of freedom of the robotic arm and body mass characteristics not much greater than the target, the shortcomings of the prior art are overcome and a method for kinematic posture analysis of a robotic arm connection assembly is provided, which has simple engineering implementation and reliable calculation results.

[0006] The technical solution of the present invention is:

[0007] The present invention discloses a kinematic posture analysis method for a mechanical arm connection assembly, comprising:

[0008] The present invention discloses a kinematic posture analysis method for a mechanical arm connection assembly, comprising:

[0009] Calculate the inertia tensor of the target in a layout system parallel to the body with the target mass center as the origin;

[0010] Calculate the positions of the subject and the target relative to the center of mass of the assembly in the assembly coordinate system;

[0011] Calculating the inertia tensor of the assembly in the assembly coordinate system according to the inertia tensor and the position;

[0012] According to the inertia tensor of the assembly, calculate the rotational angular momentum of the assembly;

[0013] According to the rotational angular momentum of the assembly, the target rotational reaction torque is calculated;

[0014] According to the target rotation reaction torque and the change of the manipulator configuration, the translational angular momentum of the assembly is calculated;

[0015] According to the translational angular momentum of the assembly, the reaction torque of the configuration change is calculated;

[0016] According to the target rotation reaction torque and the configuration change reaction torque, a dynamic differential equation of the assembly is obtained;

[0017] According to the dynamic differential equation of the assembly, the angular velocity is integrated to obtain the body attitude angle parallel to the assembly coordinate system.

[0018] Furthermore, in the above method, the inertia tensor of the target is calculated in a layout system parallel to the main body with the target mass center as the origin, specifically:

[0019]

[0020] Among them, C SfromB The transformation matrix from the target centroid coordinate system to the body layout system; I is the target moment of inertia matrix in the target mass center coordinate system; T It is the inertia tensor of the system parallel to the main body with the target mass center as the origin.

[0021] Furthermore, in the above method, the positions of the body and the target relative to the center of mass of the assembly in the assembly coordinate system are calculated as follows:

[0022]

[0023] Among them, m T is the target quality; m Sis the mass of the body; P is the position vector of the body mass center pointing to the target mass center; P S P is the position vector of the center of mass of the combined body pointing to the center of mass of the main body; T The position vector of the center of mass of the assembly pointing to the center of mass of the target.

[0024] Furthermore, in the above method, the calculation of the inertia tensor of the assembly in the assembly coordinate system is specifically:

[0025]

[0026] Among them, I S I is the inertia tensor of the main body under the main body layout system; ZHT It is the inertia tensor of the assembly under the layout system with the origin at the center of mass of the assembly parallel to the body.

[0027] Furthermore, in the above method, the step of calculating the rotational angular momentum of the assembly is specifically:

[0028]

[0029] Among them, I T is the inertia tensor of the target mass center parallel to the body layout system, I ZHT is the inertia tensor of the assembly when the origin is at the center of mass of the assembly and parallel to the main body layout system; Ω T is the angular velocity of the target posture relative to the body posture, w ZHT is the angular velocity of the combination; rot is the subscript indicating the rotational component.

[0030] Furthermore, in the above method, the target rotation reaction torque is calculated as follows:

[0031]

[0032] Among them, T rot is the target rotation reaction torque.

[0033] Furthermore, in the above method, the step of calculating the translational angular momentum of the assembly is specifically:

[0034]

[0035] Among them, v T is the translational velocity of the target relative to the body, I ZHT is the inertia tensor of the assembly when the origin is at the center of mass of the assembly and parallel to the main body layout system; w ZHT is the angular velocity of the assembly; P T is the position vector of the center of mass of the assembly pointing to the center of mass of the target, m T Target quality.

[0036] Furthermore, in the above method, the calculation of the configuration change reaction torque is specifically:

[0037]

[0038] Among them, T tran is the reaction torque of configuration change.

[0039] Furthermore, in the above method, the dynamic differential equation of the assembly is specifically:

[0040]

[0041] in, is the angular momentum of the assembly during the configurational manipulation process; T rot is the target rotation reaction torque; T tran is the configuration translation reaction moment; T control +T distrurbance It is the external torque generated by the flywheel, thruster, light pressure and gravity gradient in the dynamic system.

[0042] The beneficial effects of the present invention and the prior art are:

[0043] (1) The present invention adopts a technical solution for solving the interaction torque based on the conservation of angular momentum, which achieves a simple and physically intuitive technical effect for the derivation formula of the motion of a strongly coupled assembly with large configuration changes, and solves the technical problem of high reliability requirements for motion analysis results.

[0044] (2) The present invention adopts a technical solution of decoupling analysis of translation and rotation, which achieves the technical effect of simple and rapid analysis when the robot arm is manipulated with high degrees of freedom, and solves the technical problem of high calculation accuracy when the configuration of the assembly changes rapidly.

[0045] (3) The present invention adopts a technical solution of unifying all vectors and tensors into the coordinate system of the entity and making the coordinate system of the assembly parallel to the present system, thereby achieving the technical effect of obtaining the angular acceleration of the assembly, that is, the desired angular acceleration of the entity, and solving the technical problem of combining with other dynamics in the joint simulation system. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is a flowchart of the kinematic posture analysis method of the robot arm connection assembly of the present invention. DETAILED DESCRIPTION

[0047] The present invention is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0048] like Figure 1 As shown, the present invention provides a method for analyzing the kinematic posture of a robot arm connection assembly, comprising the following steps:

[0049] like Figure 1 The figure shows a block diagram of the kinematic posture analysis method of the manipulator-connected assembly. By considering the manipulated target as a component of the assembly that can move with six degrees of freedom, its relative posture change is obtained according to the change of the joint angle of the manipulator, and then the assembly is analyzed for rotation and translation respectively. The reaction torque generated by the relative motion of the manipulated target on the assembly is calculated by the conservation of angular momentum, and the angular velocity of the assembly is obtained by superimposing other dynamic torque terms of the GNC simulation system, and then the assembly motion result is obtained. Finally, the body posture motion state in the body coordinate system parallel to the assembly coordinate system is derived, thereby realizing the posture dynamics support for the space-variable configuration assembly joint simulation system.

[0050] The specific implementation steps are as follows:

[0051] Step 1: Obtain the angular motion state of the main body robot arm joint, calculate the position P of the manipulated target mass center relative to the main body mass center in the main body coordinate system; solve the velocity v of the target mass center projected in the main body coordinate system relative to the main body mass center T ; The target posture to the main body posture conversion matrix C SfromB ; The angular velocity of the target relative to the body in the body coordinate system ΩT .

[0052] Step 2: Based on the known mass and relative position of the target and the body, the coordinates O of the center of mass of the assembly in the body layout system can be obtained. zht ; with O zht Establish O parallel to the main layout system for the origin zht X b Y b Z b Combined body coordinate system; calculate the combined body inertia in the combined body coordinate system based on the inertia of the two bodies and the relative posture. The specific algorithm is as follows:

[0053] 1. Calculate the inertia tensor of the target in a layout system parallel to the main body with the target mass center as the origin:

[0054]

[0055] in,

[0056] C SfromB : Transformation matrix from target centroid coordinate system to body layout system

[0057] Target moment of inertia matrix in target mass center coordinate system

[0058] I T : The inertia tensor of the target layout system with the target center of mass as the origin and parallel to the body

[0059] 2. Calculate the position vectors of the body and target relative to the center of mass of the assembly in the assembly coordinate system:

[0060]

[0061] in,

[0062] m T : Target quality

[0063] m S :Main body mass

[0064] P: The position vector of the body's center of mass pointing to the target's center of mass

[0065] P S : The center of mass of the combined body points to the position vector of the center of mass of the main body

[0066] P T : The center of mass of the assembly points to the position vector of the center of mass of the target

[0067] 3. Calculate the inertia tensor of the assembly:

[0068]

[0069] in,

[0070] I S : The inertia tensor of the main body under the main body layout system

[0071] I ZHT : The inertia tensor of the assembly under the layout system with the origin at the center of mass of the assembly parallel to the body

[0072] Step 3: Analyze the angular momentum of the assembly when the target is rotating relative to the target and derive the time derivative to derive the reaction torque generated by the rotation. The specific algorithm is as follows

[0073] 1. Angular momentum of the assembly H rot =(I ZHT w ZHT ) (rot) +I T Ω T

[0074] 2. Derivative of angular momentum with respect to time in an inertial system:

[0075]

[0076] 3. Put the kinematic part of the assembly on the left side of the equation, and the reaction torque on the right side:

[0077]

[0078] in,

[0079] ΩT : Angular velocity of target posture relative to the body posture.

[0080] w ZHT : Angular velocity of the combined body

[0081] Step 4: Calculate the reaction torque generated by the change of the control target on the assembly configuration. The specific algorithm is as follows:

[0082] 1. The translational angular momentum H of the particle in the assembly configuration tran =(I ZHT w ZHT ) (tran) +m T (P T ×v T )

[0083] 2. Derivative of angular momentum with respect to time in an inertial system:

[0084]

[0085] 3. Put the kinematic part of the assembly on the left side of the equation, and the reaction torque on the right side:

[0086]

[0087] in,

[0088] v T : The translational velocity of the target relative to the main body.

[0089] Step 5: Synthesize the relative rotation and translation. The specific analysis steps are as follows

[0090] 1. The torque of the assembly reacting to the relative motion of the target

[0091]

[0092] 2. Conservation of angular momentum of the assembly The dynamic differential equation of the assembly is obtained

[0093]

[0094] in,

[0095] Angular momentum during configurational manipulation of the assembly;

[0096] Target rotation reaction torque;

[0097] Configuration translation reaction torque;

[0098] T control +Tdistrurbance : External torque generated by flywheels, thrusters, light pressure, gravity gradient, etc. in dynamic systems.

[0099] Step 6: Combined coordinate system O zht X b Y b Z b Parallel to the main body layout system O b X b Y b Z b All the above vectors are projected in the coordinate system of the body layout. The angular velocity of the body is equal to the angular velocity of the assembly. The body attitude angle is obtained by solving the angular velocity according to the dynamic differential equation of the assembly and integrating it.

[0100] Although the content of the present invention has been described in detail through the above preferred embodiments, it should be appreciated that the above description should not be considered as a limitation of the present invention. After reading the above content, it will be apparent to those skilled in the art that various modifications and substitutions of the present invention will occur. Therefore, the protection scope of the present invention should be limited by the appended claims.

[0101] The contents not described in detail in the specification of the present invention belong to the common knowledge of the professionals in this field.

Claims

1. A method for analyzing the kinematic posture of a robot arm connection assembly, characterized in that: include: Calculate the inertia tensor of the target in a layout system parallel to the body with the target mass center as the origin; Calculate the positions of the subject and the target relative to the center of mass of the assembly in the assembly coordinate system; Calculating the inertia tensor of the assembly in the assembly coordinate system according to the inertia tensor and the position; According to the inertia tensor of the assembly, calculate the rotational angular momentum of the assembly; According to the rotational angular momentum of the assembly, the target rotational reaction torque is calculated; According to the target rotation reaction torque and the change of the manipulator configuration, the translational angular momentum of the assembly is calculated; According to the translational angular momentum of the assembly, the reaction torque of the configuration change is calculated; According to the target rotation reaction torque and the configuration change reaction torque, a dynamic differential equation of the assembly is obtained; According to the dynamic differential equation of the assembly, the angular velocity is integrated to obtain the body attitude angle parallel to the assembly coordinate system.

2. A method for analyzing kinematic posture of a robot arm connection assembly according to claim 1, characterized in that: The inertia tensor of the target is calculated in a layout system parallel to the main body with the target mass center as the origin, specifically: Among them, C SfromB The transformation matrix from the target centroid coordinate system to the body layout system; I is the target moment of inertia matrix in the target mass center coordinate system; T It is the inertia tensor of the system parallel to the main body with the target center of mass as the origin.

3. The kinematic posture analysis method of a robot arm connection assembly according to claim 1, characterized in that: The calculation of the positions of the subject and the target relative to the center of mass of the assembly in the assembly coordinate system is specifically: Among them, m T is the target quality; m S is the mass of the body; P is the position vector of the body mass center pointing to the target mass center; P S P is the position vector of the center of mass of the combined body pointing to the center of mass of the main body; T The position vector of the center of mass of the assembly pointing to the center of mass of the target.

4. The method for analyzing the kinematic posture of a robot arm connection assembly according to claim 1, characterized in that: The calculation of the inertia tensor of the assembly in the assembly coordinate system is specifically: Among them, I S I is the inertia tensor of the main body under the main body layout system; ZHT It is the inertia tensor of the assembly under the layout system with the origin at the center of mass of the assembly parallel to the body.

5. The method for analyzing the kinematic posture of a robot arm connection assembly according to claim 1, characterized in that: The calculation of the rotational angular momentum of the assembly is specifically as follows: Among them, I T is the inertia tensor of the target mass center parallel to the body layout system, I ZHT is the inertia tensor of the assembly when the origin is at the assembly center of mass and parallel to the main body layout system; ΩT is the angular velocity of the target posture relative to the body posture, w ZHT is the angular velocity of the combination; rot is the subscript indicating the rotational component.

6. A method for analyzing kinematic posture of a robot arm connection assembly according to claim 5, characterized in that: The target rotation reaction torque is calculated as follows: Among them, T rot is the target rotation reaction torque.

7. A method for analyzing kinematic posture of a robot arm connection assembly according to claim 1, characterized in that: The calculation of the translational angular momentum of the assembly is specifically as follows: Among them, v T is the translational velocity of the target relative to the body, I ZHT is the inertia tensor of the assembly when the origin is at the assembly center and parallel to the main body layout system; w ZHT is the angular velocity of the assembly; P T is the position vector of the center of mass of the assembly pointing to the center of mass of the target, m T Target quality.

8. A method for analyzing kinematic posture of a robot arm connection assembly according to claim 7, characterized in that: The calculation of the configuration change reaction torque is specifically: Among them, T tran is the reaction torque of configuration change.

9. The method for analyzing the kinematic posture of a robot arm connection assembly according to claim 1, characterized in that: The dynamic differential equation of the assembly is specifically: in, is the angular momentum of the assembly during the configurational manipulation process; T rot is the target rotation reaction torque; T tran is the configuration translation reaction moment; T control +T distrurbance It is the external torque generated by the flywheel, thruster, light pressure and gravity gradient in the dynamic system.

Images