WO2020034405A1 - Axis-invariant-based dynamics modeling and solving method for tree-chain robot - Google Patents

Abstract

An axis-invariant-based dynamics modeling and solving method for a tree-chain robot. A Ju-Kane dynamics model is provided and proved, is suitable for dynamics numerical computation of a tree-chain multi-axis system and is also suitable for dynamics control of the multi-axis system. The characteristics of a generalized inertia matrix of an axis-chain rigid body and a generalized inertia matrix of an axis-chain rigid body system are systematically analyzed. The principle and process of a dynamics positive solution of a multi-axis system are provided. When a GPU is used for computation, the computation has linear complexity, and when a single CPU is used for computation, the computation has quadratic complexity. The principle and process of a dynamics inverse solution of the multi-axis system are provided, and there is linear complexity. Since the inertia matrix of a system is small, the dynamics computation complexity of the multi-axis system is far lower than that of existing known dynamics systems.

Description

Dynamics Modeling and Solving Method of Tree Chain Robot Based on Axis Invariant Technical field

The invention relates to a tree chain robot dynamic modeling and calculation method, and belongs to the field of robot technology.

Background technique

Lagrangian proposed the Lagrangian method when studying the problem of lunar balance, which is a basic method for expressing dynamic equations in generalized coordinates; at the same time, it is also a basic method for describing quantum field theory. The application of Lagrange's method to establish dynamic equations is a tedious process. Although Lagrange's equations have the advantage of theoretical analysis to derive the dynamic equations of the system based on the invariance of system energy; With the increase of the degree of freedom of the system, the complexity of the derivation of the equation has increased dramatically and it is difficult to be universally applied. Compared with Lagrange's equation, the establishment of the Kane equation directly expresses the dynamic equation through the system's deflection velocity, velocity and acceleration. Therefore, compared with the Lagrangian method, the Kane dynamics method greatly reduces the difficulty of system modeling because it omits the expression of system energy and the derivation of time. However, for high-degree-of-freedom systems, the Kane dynamics modeling method is also difficult to apply.

Lagrange's equation and Kane's equation have greatly promoted the study of multibody dynamics. The dynamics based on the space operator algebra have improved the calculation speed and accuracy to a certain extent due to the application of the iterative process. These dynamic methods require complex transformations in body space, body subspace, system space, and system subspace, both in kinematics and dynamics. The modeling process and model expression are very complex, and it is difficult to meet high-degree-of-freedom systems. The need for modeling and control, therefore, a concise expression of the dynamic model needs to be established; both the accuracy of the modeling and the real-time nature of the modeling must be guaranteed. Without concise dynamic expressions, it is difficult to ensure the reliability and accuracy of dynamic engineering of high-degree-of-freedom systems. At the same time, the traditional unstructured kinematics and dynamics symbols are annotated with the meaning of the symbols, which cannot be understood by the computer. As a result, the computer cannot automatically establish and analyze the kinematics and dynamics models.

Summary of the Invention

The technical problem to be solved by the present invention is to provide a tree chain robot dynamics and calculation method based on axis invariants.

To solve the above technical problems, the present invention adopts the following technical solutions:

A tree-chain robot dynamics and calculation method based on axis invariants is characterized in that it includes the following steps:

Given a multi-axis rigid body system D = {A, K, T, NT, F, B}, the inertial system is denoted by F ^[i] ,

In addition to gravity, the combined external force and moment acting on the axis u are recorded as ^{i | D} f _u and ^{i | D} τ _{u respectively} ; the mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

The acceleration of gravity of axis k is

According to the topology, structure and mass-inertia parameters of the robot system, the Lagrange equation of joint space is established using the chain symbol system, and the Ju-Kane dynamics preliminary equation is established based on the Lagrange equation of the multi-axis system

Substituting the partial velocity into the Ju-Kane dynamics equation to establish the Ju-Kane dynamic equation of the tree chain rigid body system

Establish Ju-Kane normalized dynamic equations of tree structure rigid body system;

The Ju-Kane dynamic equation of the tree chain rigid body system is reformulated as the tree chain Ju-Kane canonical equation.

Based on the Lagrange equation of the multi-axis system, the Ju-Kane dynamics preliminary theorem is derived, and its steps are:

[1] Prove the equivalence of Lagrange's equation and Kane's equation;

[2] Based on Lagrange's equation, based on energy's partial velocity of joint velocity and coordinates;

[3] Find the derivative of the deviation speed with time;

[4] Based on the above steps, a Ju-Kane kinetics preliminary theorem is obtained.

[1] Prove the equivalence of Lagrange's equation and Kane's equation

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the rotation speed vector;

Is the translational acceleration vector;

Is the translational velocity vector;

For translational kinetic energy;

For rotational kinetic energy;

Is joint coordinates;

Joint speed

Consider the kinetic energy pair of rigid body k translation

The derivative of the partial velocity with time is

Consider the kinetic energy of k

The derivative of the partial velocity with time is

because

versus

Irrelevant, obtained by equation (7) and Lagrange's equation of multi-axis system

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the rotation speed vector;

Is the rotation acceleration vector;

Is the translational acceleration vector;

Is the translational velocity vector;

For translational kinetic energy;

For rotational kinetic energy;

Is gravitational potential energy;

Is joint coordinates;

Joint speed

The translational kinetic energy and rotational kinetic energy of the dynamic system D are expressed as

Considering equations (4) and (5), we have

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Equations (7) and (8) are the basis for the proof of the Jue-Kane dynamics theorem, that is, the Jue-Kane dynamics theorem is essentially equivalent to the Lagrangian method; at the same time, the equation (8) The right side contains the Kane equation of the multi-axis system; it shows that the calculation of the inertia force of the Lagrange method and the Kane method is consistent, that is, the Lagrange method and the Kane method are equivalent;

[2] Based on Lagrange's equation, based on energy, the velocity of joints and the deflection of coordinates

System D energy

Expressed as

among them:

[2-1] If

And consider

and

It is only related to the closed subtree ^u L. From equations (4) and (5), we get

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the rotation speed vector;

Is the translational velocity vector;

Is joint coordinates;

Joint speed

[2-2] If

And consider

and

It is only related to the closed subtree ^u L. From equations (4) and (5), we get

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the rotation speed vector;

Is the translational velocity vector;

Is the rotation joint coordinates;

For turning joint speed;

[3] Find the derivative of partial velocity with time

[3-1] If

From formula (7), formula (9) and formula (10),

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

[3-2] If

From (7), (12), and (13),

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

[4] Ju-Kane kinetics preliminary theorem based on the above steps

The Ju-Kane dynamics equation for axis u is

Equation (17) has a tree chain topology; k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are denoted as m _k and

Is the rotation speed vector;

Is the rotation acceleration vector;

Is the translational acceleration vector;

Is the translational velocity vector;

For translational joint coordinates;

Is the translational joint speed;

Is the rotation joint coordinates;

Is the joint velocity; the generalized force in the closed sub-tree ^u L is additive, and the nodes of the closed sub-tree have only one motion chain to the root. The motion chain ⁱ l _n can be replaced by the motion chain ^u L.

Given shaft chain

The formula for calculating partial speed is:

Where along the axis

Line position

Around the axis

Angular position

Axis vector

Angular velocity

Line speed

The relationship between left-order cross product and transpose is:

In the formula:

Is the rotation speed vector.

Given a bilateral external force from the force application point i _S in the environment i to the point l _S on the axis l

And external moment ⁱ τ _l , their instantaneous shaft power p _{ex is} expressed as

among them:

And ⁱ τ _{l is} not affected by

and

Control, ie

And ⁱ τ _l does not depend on

and

[1] If k∈ ⁱ l _l , then

From equations (19) and (18),

In (26)

And (21)

The chain order of is different; the former is the force, the latter is the amount of motion, and the two are dual, with opposite order

[2] If k ∈ ⁱ l _l , then

From equations (22) and (25),

Equations (26) and (27) show that the combined external force or moment acting on the axis k by the environment is equivalent to the combined external force or moment of the closed subtree ^k L on the axis k, and formulae (26) and (27) as

In Equation (28), the closed subtree has an additivity to the generalized force of axis k; the effect of the force has a dual effect and is iterative in the reverse direction; the so-called reverse iteration refers to:

It is necessary to iterate through the link position vector;

Order and forward kinematics

The order of calculation is reversed.

If the shaft l is a driving shaft, the driving force and driving torque of the shaft l are

and

Driving force

And driving torque

The generated power p _{ac is} expressed as

[1] Obtained from formula (18), formula (19), and formula (29)

If the axis u and the axis

Co-axial, then

Remember

because

versus

Irrelevant, it is obtained by equation (30)

because

versus

Co-axial

[2] Obtained from formula (18), formula (19) and formula (29)

If the axis u and

Co-axial, then

Remember

From equation (32),

The combined external force and moment acting on the shaft u are

The components on

and

The bilateral driving force and driving torque of the driving shaft u are between

The components on

and

The force and moment of the environment i on the axis l are

And ⁱ τ _l ; then the Ju-Kane dynamic equation of the axis u-tree chain is

Among them: [·] means taking rows or columns;

and

Is a 3 × 3 block matrix,

and

Is a 3D vector, q is the joint space;

And yes,

Among them, remember

Remember

k _I represents the center of mass _{I of the} rod k; the mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

Is the inertia matrix of the rotation axis u;

Is the inertial matrix of the translation axis u; h _R is the non-inertia matrix of the rotation axis u; h _P is the non-inertia matrix of the translation axis u.

definition

The Ju-Kane normalized dynamic equation of a tree-structured rigid body system is:

The canonical form of (36) is

The canonical form of (37) is

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the axis invariant;

Is joint acceleration;

For cross multiplier, vector

The cross product matrix is

To take axis i to axis

The motion chain ^u L indicates that a closed subtree consisting of the axis u and its subtrees is obtained.

The Ju-Kane dynamic equation of tree chain rigid body system is reformulated as the tree-chain Ju-Kane canonical equation:

Given a multi-axis rigid body system D = {A, K, T, NT, F, B}, the inertial system is denoted by F ^[i] ,

In addition to gravity, the combined external force and moment acting on the axis u

The components on

and

The mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

The acceleration of gravity of axis k is

The bilateral driving force and driving torque of the driving shaft u are between

The components on

and

The force and moment of the environment i on the shaft l are

And ⁱ τ _l ; the Ju-Kane dynamics norm equation of axis u is

among them:

and

Is a 3 × 3 block matrix,

and

Is a 3D vector.

and,

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the inertia matrix of the rotation axis u;

Is the inertia matrix of the translation axis u; h _R is the non-inertia matrix of the rotation axis u; h _P is the non-inertia matrix of the translation axis u; the combined external force and moment acting on the axis u are

The components on

and

The bilateral driving force and driving torque of the driving shaft u are between

The components on

and

The acting force and acting moment of the environment i on the shaft l are

And ⁱ τ _l ; ^l l _k is a kinematic chain from axis l to axis k, and ^u L means obtaining a closed subtree composed of axis u and its subtrees.

Given the generalized force acting on the environment and the generalized driving force of the drive axis, the acceleration or inertial acceleration of the dynamic system is solved to obtain the positive solution of the Ju-Kane dynamic equation of the tree chain rigid body system.

The specific steps to find the positive solution of the Ju-Kane dynamic equation of the tree chain rigid body system are:

Define orthogonal complement matrix

And the corresponding cross product matrix

The Ju-Kane dynamics norm equations of each axis in the system are arranged in rows; the rearranged axis-driven generalized force and unmeasured environmental force are denoted as f ^C , and the measurable environmental generalized force is denoted as f ⁱ ; The corresponding joint acceleration sequence is written as

Rearranged

Let it be h; then the system dynamics equation is

From equation (125),

Let | A | = a, and write the generalized inertial matrix of the system with axis a as M _{3a × 3a} ;

Generalized Inertial Matrix

Is a symmetric matrix and it is a positive definite matrix, valid

The calculation process is as follows:

[1] First, perform LDL ^T decomposition, that is, matrix decomposition,

among them,

Is the only lower triangular matrix that exists, and D _{a × a} is a diagonal matrix;

[2] Application formula (130) calculation

Substituting equation (130) into equation (128) gives

At this point, the positive solution of the Ju-Kane dynamic equation of the tree chain rigid body system is obtained.

Find the inverse solution of the Ju-Kane dynamic equation of a tree chain rigid body system.

When the joint configuration, velocity, and acceleration are known, ^{i | D} f _u and ^{i | D} τ _{u are} obtained by equation (34); when the external force and external moment are known, the driving force is solved by equation (132)

And driving torque

In the formula, the combined external force and moment acting on the shaft u are

The components on

and

The bilateral driving force and driving torque of the driving shaft u are between

The components on

and

The acting force and acting moment of the environment i on the shaft l are

And ⁱ τ _l ; ^l l _k is a kinematic chain from axis l to axis k, and ^u L means obtaining a closed subtree composed of axis u and its subtrees.

Beneficial effects achieved by the present invention:

The invention proposes and proves the Ju-Kane dynamic model, which is suitable for both the numerical calculation of tree chain multi-axis systems and the dynamic control of multi-axis systems. The characteristics of the generalized inertial matrix of the rigid body of the axial chain and the generalized inertia matrix of the rigid system of the axial chain are systematically analyzed. The principle and process of the positive solution of the dynamics of the multi-axis system are given. When GPU computing is used, it has linear complexity. Has square complexity. The principle and process of the multi-axis system dynamics inverse solution are given, which has linear complexity. Because the system inertia matrix is small, the computational complexity of multi-axis system dynamics is much lower than the existing known dynamic systems. Features are:

[1] The space of the 3D generalized inertial matrix shown in equation (114) is more compact, which is 1/4 of the size of the 6D inertial matrix;

[2] The explicit expression of the generalized inertial matrix of the system can be listed directly through the iterative equation;

[3] Linear complexity of forward and inverse dynamics calculation;

[4] At the same time, it has the basic characteristics of the Ju-Kane gauge equation.

BRIEF DESCRIPTION OF THE DRAWINGS

Figure 1 Natural coordinate system and axis chain;

Figure 2 Fixed axis invariant;

Figure 3 Universal 3R robotic arm.

detailed description

The invention is further described below. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the protection scope of the present invention.

Definition 1 Natural coordinate axis: A unit reference axis with a fixed origin that is co-axial with a motion axis or measurement axis is called a natural coordinate axis, also known as a natural reference axis.

Definition 2 Natural coordinate system: As shown in Figure 1, if the multi-axis system D is at zero position, all Cartesian body coordinate systems have the same direction and the origin of the body coordinate system is on the axis of the motion axis, then the coordinate system is a natural coordinate system. , Referred to as the natural coordinate system.

The advantages of the natural coordinate system are: (1) the coordinate system is easy to determine; (2) the joint variables at the zero position are zero; (3) the system attitude at the zero position is consistent; (4) it is not easy to introduce a measurement error.

It can be known from Definition 2 that when the system is at the zero position, the natural coordinate system of all members is consistent with the direction of the base or world system. The system is at zero

Natural coordinate system

Vector around axis

Rotation angle

will

Go to F ^[l] ;

in

Coordinate vector with

Coordinate vector under F ^[l]

Identity, that is

Knowing from the above formula,

or

Does not depend on adjacent coordinate systems

And F ^[l] ;

or

Is the axis invariant. When invariance is not emphasized, it can be called a coordinate axis vector (referred to as an axis vector).

or

Body

Reference unit coordinate vector common to volume l, and reference point

And O _l has nothing to do. body

And body l is a rod or shaft.

Axis invariants are fundamentally different from coordinate axes:

(1) The coordinate axis is a reference direction with a zero position and a unit scale. It can describe the position of translation in that direction, but it cannot fully describe the rotation angle around the direction, because the coordinate axis itself does not have a radial reference direction, that is There is a zero position that characterizes rotation. In practical applications, the radial reference of this shaft needs to be supplemented. For example: in Cartesian F ^[l] , to rotate around lx, you need to use ly or lz as the reference zero. The coordinate axis itself is 1D, and three orthogonal 1D reference axes constitute a 3D Cartesian frame.

(2) The axis invariant is a 3D spatial unit reference axis, which is itself a frame. It has its own radial reference axis, the reference mark. The spatial coordinate axis and its own radial reference axis determine the Cartesian frame. The spatial coordinate axis can reflect the three basic reference attributes of the motion axis and the measurement axis.

Existing literature records the axis vector of the unchained index as

It is called Euler Axis, and the corresponding joint angle is called Euler Angle. The reason why this application no longer uses the Euler axis and is called the axis invariant is because the axis invariant has the following properties:

[1] Given rotation transformation matrix

Because it is a real matrix and its modulus is unit, it has a real eigenvalue λ ₁ and two complex eigenvalues λ ₂ = e ^iφ and λ ₃ = e ^-iφ ; where: i is pure Imaginary number. Therefore, | λ ₁ | · || λ ₂ || · || λ ₃ || = 1, and λ ₁ = 1 is obtained. Axis vector

Is the eigenvector corresponding to the real eigenvalue λ ₁ = 1 and is an invariant;

[2] is a 3D reference axis, which not only has an axial reference direction, but also has a radial reference zero position, which will be explained in section 3.3.1.

[3] In natural coordinate system:

Axis invariant

It is a very special vector, which has invariance to the derivative of time, and has very good mathematical operation performance;

For axis invariants, its absolute derivative is its relative derivative. Since the axis invariant is a natural reference axis with invariance, its absolute derivative is always a zero vector. Therefore, the axis invariant is invariant to time differentiation. Have:

[4] In the natural coordinate system, pass the axis vector

And joint variables

Can directly describe the rotating coordinate array

There is no need to establish separate systems for members other than roots. At the same time, taking the only root coordinate system that needs to be defined as a reference can improve the measurement accuracy of system structural parameters;

[5] Application axis vector

The excellent operation of the system will establish a fully parameterized unified multi-axis system kinematics and dynamics model including topology, coordinate system, polarity, structural parameters and mechanical parameters.

The basis vector e _l is any vector consolidated with F ^[l] . The basis vector

With

Any vector of consolidation,

Is F ^[l] and

Common unit vector, so

Is F ^[l] and

Shared basis vector. So the axis is invariant

Is F ^[l] and

Common reference base. Axis invariants are parameterized natural coordinate bases and primitives for multi-axis systems. The translation and rotation of a fixed axis invariant are equivalent to the translation and rotation of a fixed coordinate system.

When the system is at the zero position, using the natural coordinate system as a reference, the coordinate axis vector is measured.

In motion

Axis vector

Is an invariant; axis vector

And joint variables

Uniquely identified sports pair

Rotation relationship.

Therefore, when applying the natural coordinate system, when the system is at the zero position, it is only necessary to determine a common reference system instead of determining the respective body coordinate system for each member in the system, because they are uniquely determined by the axis invariants and natural coordinates. . When performing a system analysis, in addition to the base system, other natural coordinate systems consolidated with the rod only occur conceptually and have nothing to do with the actual measurement. The theoretical analysis and engineering effect of natural coordinate system on multi-axis system (MAS) lies in:

(1) The structural parameter measurement of the system needs to be measured with a unified reference frame; otherwise, not only the engineering measurement process is cumbersome, but the introduction of different systems will introduce greater measurement errors.

(2) Applying the natural coordinate system, in addition to the root member, the natural coordinate system of other members is naturally determined by the structural parameters and joint variables, which is helpful for the kinematics and dynamics analysis of the MAS system.

(3) In engineering, optical measurement equipment such as laser trackers can be applied to achieve accurate measurement of invariants of fixed axes.

(4) Since the motion pair R and P, the spiral pair H, and the contact pair O are special cases of the cylindrical pair C, the cylindrical pair can be applied to simplify the MAS kinematics and dynamics analysis.

Definition 3. Invariant: A quantity that does not depend on a set of coordinate systems for measurement is called an invariant.

Definition 4 Rotating coordinate vector: vector around coordinate axis

Turn to angular position

Coordinate vector

for

Definition 5 Translation coordinate vector: vector along the coordinate axis

Pan to line position

Coordinate vector

for

Definition 6 Natural coordinates: take the natural coordinate axis vector as the reference direction, and the angular position or line position relative to the zero position of the system, and record it as q _l , which is called natural coordinates;

Definition 7 mechanical zero: for motion pairs

Zero position of joint absolute encoder at initial time t ₀

Not necessarily zero, this zero is called mechanical zero;

Old joint

Control amount

for

Definition 8 Natural motion vector: the natural axis vector

And the vector determined by natural coordinates q _l

Called the natural motion vector. among them:

The natural motion vector realizes the unified expression of axis translation and rotation. A vector to be determined by the natural axis vector and the joint, such as

Called the free motion vector, also known as the free spiral. Obviously, the axis vector

Is a specific free spiral.

Definition 9 Joint space: The space represented by the joint natural coordinates q _l is called joint space.

Definition 10 Configuration space: The Cartesian space which expresses the position and attitude (position for short) is called the configuration space, which is a double vector space or 6D space.

Definition 11 Natural joint space: using the natural coordinate system as a reference, through joint variables

Means that there must be when the system is zero

The joint space is called natural joint space.

As shown in Figure 2, given a link

Origin O _l subject to position vector

Constrained axis vector

Is the fixed axis vector, written as

among them:

Axis vector

Is the natural reference axis of the natural coordinates of the joint. because

Is an axis invariant, so

Is a fixed axis invariant, which characterizes the motion pair

The structural relationship is determined by the natural coordinate axis. Fixed axis invariant

Is a link

Natural description of structural parameters.

Definition 12 Natural coordinate axis space: The fixed axis invariant is used as the natural reference axis, and the space represented by the corresponding natural coordinate is called the natural coordinate axis space, referred to as the natural axis space. It is a 3D space with 1 degree of freedom.

as shown in picture 2,

and

Does not change due to the movement of the rod Ω _l , it is a constant structural reference quantity.

Determines the axis l relative to the axis

Together with the joint variable q _l , the 6D configuration of the rod Ω _l is fully expressed. given

The natural coordinate system of the rod consolidation can be determined by the structural parameters

And joint variables

Only OK. Axis invariant

Fixed axis invariant

Joint variable

and

Is a natural invariant. Obviously, by the fixed axis invariants

And joint variables

Natural invariants

With coordinate system

To F ^[l] determined spatial configuration

Has a one-to-one mapping relationship, that is,

Given a multi-axis system D = {T, A, B, K, F, NT}, when the system is in zero position, as long as the base system or inertial system is established, and the reference point O _l on each axis, other coordinate systems of the member Naturally ok. Essentially, only the base or inertial system needs to be determined.

Given a schematic diagram of a closed chain structure connected by a motion pair, any motion pair in the loop can be selected, and the stator and the mover constituting the motion pair can be separated; thus, a loop-free tree structure is obtained. , Call it the Span tree. T represents a span tree with directions to describe the topological relationship of the tree chain movement.

I is a structural parameter; A is an axis sequence; F is a bar reference sequence; B is a bar body sequence; K is a motion pair type sequence; NT is a sequence of constrained axes, that is, a non-tree.

Axis sequence

a member of. Rotary pair R, prism pair P, spiral pair H, and contact pair O are special examples of cylindrical pair C.

The basic topology symbols and operations describing the kinematic chain are the basis of the kinematic chain topology symbol system and are defined as follows:

[1] The motion chain is identified by a partial order set ().

[2] _A [l] for the take-up shaft member of the sequence A; l due to the shaft having a name corresponding to a unique number _A [l] of the sequence number, so that _A [l] computing complexity is O (1).

[3]

Is the parent axis of axis l; axis

The computational complexity of is O (1). The calculation complexity O () represents the number of operations in the calculation process, and usually refers to the number of floating-point multiplications and additions. It is very tedious to express the calculation complexity by the number of floating-point multiplication and addition. Therefore, the main operation times in the algorithm cycle are often used;

[4]

Axis sequence

a member of;

The computational complexity is O (1).

[5] ^l l _k is the kinematic chain from axis l to axis k, and the output is expressed as

And

The cardinality is written as | ^l l _k |. ^l l _k execution process: execution

If

Then execute

Otherwise, end. ^The computational complexity of ^l l _k is O (| ^l l _k |).

[6] ^l l is the child of axis l. The operation is expressed in

Find the address k of member l; thus, obtain child A ^{[k] of} axis l. because

Does not have a partial order structure, so the computational complexity of ^l l is

[7] ^l L means to obtain a closed subtree composed of axis l and its subtrees,

Is a subtree without l; recursively execute ^l l with a computational complexity of

[8] The addition and deletion of branches, subtrees, and non-tree arcs are also necessary components; thus, the variable topology is described by a dynamic span tree and a dynamic graph. In the branch ^l l _k , if

Rule

which is

Represents taking the child of member m in the branch.

Define the following expressions or expressions:

There is a one-to-one correspondence between the shaft and the member; the attribute amount between the shafts

And attributes between members

Partial order.

Convention: "□" means attribute placeholder; if the attribute p or P is about position, then

Should be understood as a coordinate system

To the origin of F ^[l] ; if the attribute p or P is about direction, then

Should be understood as a coordinate system

To F ^[l] .

and

Should be understood as a function of time t

and

And

and

Is a constant or constant array at time t ₀ . But formal

and

Should be considered a constant or constant array.

It is stipulated in this application that in the motion chain symbol calculation system, attribute variables or constants with partial order include the indicators indicating partial order in the name; either include the upper left corner and lower right corner indexes, or include the upper right corner and lower right corner indexes; Their directions are always from the upper left corner indicator to the lower right corner indicator, or from the upper right corner indicator to the lower right corner indicator. In this application, for simplicity of description, the description of the direction is sometimes omitted. Even if omitted, those skilled in the art can also use symbolic expressions. It is known that, for each parameter used in this application, for a certain attribute symbol, their directions are always from the upper left corner index to the lower right corner index of the partial order index, or from the upper right corner index to the lower right corner index. E.g:

Can be briefly described (represented from k to l) translational vector;

Represents the position of the line (from k to l);

Represents the translation vector (from k to l); where: r represents the "translation" attribute, and the remaining attributes correspond to: the attribute φ represents "rotation"; the attribute Q represents the "rotation transformation matrix"; the attribute l Represents "kinematic chain"; attribute u indicates "unit vector"; attribute w indicates "angular velocity"; angle label i indicates inertial coordinate system or geodetic coordinate system; other angle labels can be other letters or numbers.

The symbol specifications and conventions of this application are determined based on the two principles of the partial order of the kinematic chain and the chain link being the basic unit of the kinematic chain, reflecting the essential characteristics of the kinematic chain. The chain indicator represents the connection relationship, and the upper right indicator represents the reference system. This symbolic expression is concise and accurate, which is convenient for communication and written expression. At the same time, they are structured symbol systems that contain the elements and relationships that make up each attribute quantity, which is convenient for computer processing and lays the foundation for computer automatic modeling. The meaning of the indicator needs to be understood through the background of the attribute, that is, the context; for example: if the attribute is a translation type, the indicator at the upper left corner indicates the origin and direction of the coordinate system; if the attribute is a rotation type, the indicator at the top left The direction of the coordinate system.

(1) l _S -point S in rod l; and S represents a point S in space.

(2)

-The translation vector of the origin O _{k of the} rod _k to the origin O _l of the rod l;

The coordinate vector in the natural coordinate system F ^[k] , that is, the coordinate vector from k to l;

(3)

-Translation _vector from origin O _k to point l _S ;

Coordinate vector under F ^[k] ;

(4)

-Translation vector from origin _Ok to point S;

Coordinate vector under F ^[k] ;

(5)

－Connecting rod

And the movement pair of rod l;

－Sports Vice

Axis vector

and

Respectively

And the coordinate vector under F ^[l] ;

Is the axis invariant and is a structural constant;

For rotation vector, rotation vector / angle vector

Is a free vector, that is, the vector can be freely translated;

(6)

-Along the axis

Line position (translation position),

-Around the axis

The angular position, that is, the joint angle and joint variables, are scalars;

(7) When the index in the lower left corner is 0, it means the mechanical zero position; for example:

－Translation axis

Mechanical zero position,

－Rotating shaft

Mechanical zero

(8) 0-three-dimensional zero matrix; 1-three-dimensional unit matrix;

(9) Convention: "\" means line continuation character; "□" means attribute placeholder;

Power

Represents the xth power of □;

Delimiter; for example:

or

for

X power.

[□] ^T means transpose of □, which means transpose the collection, and do not perform transpose on the members; for example:

^| Projection symbol □ represents vector or tensor of second order group reference projection vector or projection sequence, i.e. the vector of coordinates or coordinate array, that is, the dot product projection "*"; as: position vector

The projection vector in the coordinate system F ^[k] is written as

Is a cross multiplier; for example:

Is axis invariant

Cross product matrix; given any vector

The cross product matrix is

The cross product matrix is a second-order tensor.

The cross-multiplier operation takes precedence over the projector ^| □. Projector ^| □ has higher priority than member access symbol □ _[□] or □ ^[□] , member access symbol □ ^{[□] has} higher priority than power symbol

(10) Projection vector of unit vector in the geodetic coordinate system

Unit zero vector

(11)

－From zero point

Translation vector to origin O _l

Represents the position structure parameter.

(12) ⁱ Q _l , a rotation transformation matrix in relative absolute space;

(13) Taking the natural coordinate axis vector as the reference direction, the angular position or line position relative to the zero position of the system is recorded as q _l , which is called natural coordinate; joint variable

Natural joint coordinates are φ _l ;

(14) For a given ordered set r = [[1,4,3,2]] ^T , let r ^[x] represent the x-th row element of the set r. The constants [x], [y], [z], and [w] indicate that the first, second, third, and fourth columns are taken.

(15) ⁱ l _j represents a kinematic chain from i to j; ^l l _k is a kinematic chain from axis l to k;

Given kinematic chain

If n represents the Cartesian Cartesian system, then

Is a Cartesian axis chain; if n represents a natural reference axis, then

For natural shaft chains.

(16) Rodrigues quaternion expression:

Euler quaternion expression:

Invariant quaternion (also known as axis quaternion) representation

1. Establishing Lagrange's equation of a multi-axis system

The Lagrange equation of joint space is established by using the chain symbol system. Considering the particle dynamics system D = {A, K, T, NT, F, B}, the free particle is first deduced according to Newtonian mechanics

Lagrange's equation; then, generalize to the constrained particle system.

Conservatism

Relative particle inertial force

Have the same chain order, i.e.

Positive order, particle

The resultant force is zero. Particle

Is recorded as

Sequence of generalized coordinates

Relationship with Cartesian space position vector sequence { ⁱ r _l | l∈T}

Get

Equation (2) applies the energy of the system and generalized coordinates to establish the equations of the system. Joint variable

The relationship with the coordinate vector ⁱ r _l is shown in equation (1), and equation (1) is called the point transformation of joint space and Cartesian space.

Conservative forces have inverse chain order with inertial forces. Constraints in a Lagrangian system can be either consolidation constraints between particles or motion constraints between particle systems; rigid bodies are particle systems

Particle energy is additive; rigid body kinetic energy consists of the translational kinetic energy and rotational kinetic energy of the mass center. In the following, Lagrange's equations are established separately with simple motion pairs R / P, which lays the foundation for the subsequent introduction of new dynamics theories.

Given a rigid body multi-axis system D = {A, K, T, NT, F, B}, the inertial space is recorded as i,

The energy of axis l is written as

Where translational kinetic energy is

The kinetic energy of rotation is

Gravitational potential energy is

The external resultant force and moment other than the gravitational force of the shaft l are ^D f _l and ^D τ _{l respectively} ; the mass of the shaft l and the moment of inertia of the center of mass are m _l and

The unit axis invariant of axis u is

The inertial acceleration of the environment i acting on l _I is written as

Gravitational acceleration

Chain order from i to l _I ;

Chain order from l _I to i;

[1] System energy

Kinetic system D energy

Expressed as

among them:

[2] Lagrange's equation of multi-axis system

By the equation (2) Lagrange's equation of the multi-axis system,

Equation (6) is the governing equation of the axis u, that is, the invariant on the axis

Force balance equation

Heli

in

On the weight,

^{i | D} τ _u is the resultant moment ^{i | D} τ _u

On the weight.

2. Establish the Ju-Kane kinetic equation:

Based on the Lagrange equation (6) of the multi-axis system, the Ju-Kane dynamics preliminary theorem is derived. First, prove the equivalence of Lagrange's equation and Kane's equation; then, calculate the partial velocity of energy to joint velocity and coordinates, and then derive the time, and finally give the Ju-Kane dynamics preliminary theorem.

[1] Proof of equivalence between Lagrange's equation and Kane's equation

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the rotation speed vector;

Is the translational acceleration vector;

Is the translational velocity vector;

For translational kinetic energy;

For rotational kinetic energy;

Is joint coordinates;

Joint speed

The specific establishment steps of the above formula are: consider the translational kinetic energy

The derivative of the partial velocity with time is

Consider the kinetic energy of k

The derivative of the partial velocity with time is

because

versus

Irrelevant, obtained by equation (7) and Lagrange's equation (6)

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the rotation speed vector;

Is the rotation acceleration vector;

Is the translational acceleration vector;

Is the translational velocity vector;

For translational kinetic energy;

For rotational kinetic energy;

Is gravitational potential energy;

Is joint coordinates;

Joint speed

The translational kinetic energy and rotational kinetic energy of the dynamic system D are expressed as

Considering equations (4) and (5), we have

Equations (7) and (8) are the basis for the proof of the Jue-Kane dynamics theorem, that is, the Jue-Kane dynamics theorem is essentially equivalent to the Lagrange method. At the same time, the right side of equation (8) contains the Kane equation of the multi-axis system; it shows that the calculation of the inertial force of the Lagrange method and the Kane method is consistent, that is, the Lagrange method and the Kane method are equivalent. Equation (8) shows that there exists in Lagrange equation (4)

The problem of double counting.

[2] Partial velocity of energy on joint velocity and coordinates

[2-1] If

And consider

and

It is only related to the closed subtree ^u L. From equations (4) and (5), we get

[2-2] If

And consider

and

It is only related to the closed subtree ^u L. From equations (4) and (5), we get

At this point, the calculation of energy's deflection speed on joint speed and coordinates has been completed.

[3] Find the derivative of time

[3-1] If

From formula (7), formula (9) and formula (10),

[3-2] If

From (7), (12), and (13),

So far, the differentiation of time t has been completed.

[4] Ju-Kane Dynamics Theorem

Substituting Equation (11), Equation (14), Equation (15), and Equation (16) into Equation (8),

Given a multi-axis rigid body system D = {A, K, T, NT, F, B}, the inertial system is denoted by F ^[i] ,

In addition to gravity, the combined external force and moment acting on the axis u are recorded as ^{i | D} f _u and ^{i | D} τ _{u respectively} ; the mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

The acceleration of gravity of axis k is

The Ju-Kane dynamics equation for axis u is

Equation (17) has a tree chain topology. k _I represents the center of mass _{I of the} rod k; the mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

Is the rotation speed vector;

Is the rotation acceleration vector;

Is the translational acceleration vector;

Is the translational velocity vector;

For translational joint coordinates;

Is the translational joint speed;

Is the rotation joint coordinates;

Is the joint speed; therefore, the nodes of the closed subtree have only one motion chain to the root, so the motion chain ⁱ l _n can be replaced by the motion chain ^u L.

Next, according to the Ju-Kane dynamics preparatory equation, the calculation of ^D f _k and ^D τ _k on the right side of equation (17) is solved, and the Ju-Kane dynamic equation of the tree chain rigid body system is established.

3. Establish Ju-Kane dynamic model of tree chain rigid body system

Given shaft chain

k∈ ⁱ l _n has the following formula for calculating partial velocity:

For a given axis chain

| ⁱ l _l | ≥2, with the following acceleration iteration:

The relationship between left-order cross product and transpose is:

According to the iterative kinematics, there are:

3.1 External force reverse iteration

Given a bilateral external force from the force application point i _S in the environment i to the point l _S on the axis l

And external moment ⁱ τ _l , their instantaneous shaft power p _{ex is} expressed as

among them:

And ⁱ τ _{l is} not affected by

and

Control, ie

And ⁱ τ _l does not depend on

and

[1] If k∈ ⁱ l _l , then

From equations (19) and (18),

which is

In (26)

And (21)

The chain order of is different; the former is the acting force and the latter is the amount of motion.

[2] If k ∈ ⁱ l _l , then

From equations (22) and (25),

That is

Equations (26) and (27) show that the combined external force or moment acting on the axis k by the environment is equivalent to the combined external force or moment of the closed subtree ^k L on the axis k, and formulae (26) and (27) as

At this point, the calculation of the reverse iteration of the external force is solved. In Eq. (28), the closed subtree has an additivity to the generalized force of axis k; the force has a dual effect and is iterative in reverse. The so-called reverse iteration refers to:

It is necessary to iterate through the link position vector;

Order and forward kinematics

The order of calculation is reversed.

3.2 Coaxial driving force reverse iteration

If the shaft l is a driving shaft, the driving force and driving torque of the shaft l are

and

Driving force

And driving torque

The generated power p _{ac is} expressed as

[1] Obtained from formula (18), formula (19), and formula (29)

which is

If the axis u and the axis

Co-axial, then

Remember

because

versus

Irrelevant, it is obtained by equation (30)

because

versus

Co-axial

[2] Obtained from formula (19), formula (18) and formula (29)

which is

If the axis u and

Co-axial, then

Remember

From equation (32),

At this point, the reverse iterative calculation of the coaxial driving force is completed.

3.3 Establishment of Ju-Kane dynamic model of tree chain rigid body system:

In the following, the Ju-Kane dynamic equation of the tree-chain rigid body system is first described, and the Ju-Kane equation is simply referred to; then, the establishment steps are given.

Given a multi-axis rigid body system D = {A, K, T, NT, F, B}, the inertial system is denoted by F ^[i] ,

In addition to gravity, the combined external force and moment acting on the axis u

The components on

and

The mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

The acceleration of gravity of axis k is

The bilateral driving force and driving torque of the driving shaft u are between

The components on

and

The force and moment of the environment i on the axis l are

And ⁱ τ _l ; then the Ju-Kane dynamic equation of the axis u-tree chain is

Among them: [·] means taking rows or columns;

and

Is a 3 × 3 block matrix,

and

Is a 3D vector, and q is the joint space. And yes,

Among them, remember

Remember

The establishment steps of the above equation are:

Remember

Therefore

The energy of ex is

p _ex is the instantaneous shaft power; p _ac is the power generated by the driving force and driving torque of the drive shaft.

Formula (40) is obtained from formula (26), formula (27), formula (31), formula (33), and formula (41).

Substituting the formula (19), (18) and (20) into the Ju-Kane kinetic equation (17)

From (21),

Considering equation (43), we have

Similarly, considering equation (43), we get

Substituting equations (43) to (45) into equation (42) gives equations (34) to (39).

Example 1

Given the universal 3R manipulator shown in Fig. 3, A = (i, 1: 3]; apply the method of the present invention to establish the tree chain Ju-Kane dynamic equation, and obtain the generalized inertial matrix.

Step 1. Establish an iterative motion equation based on the axis invariants.

Rotation transformation matrix based on axis invariant by equation (46)

Get

Kinematic iteration:

Second-order tensor projection:

From equations (48) and (47),

From equations (49), (47), and (55),

From equations (50) and (55),

From equations (51), (55), and (57),

From equations (52) and (55),

From equations (53) and (55),

Step 2 Establish a kinetic equation. First establish the kinetic equation of the first axis. From Equation (37),

From Equation (39)

From equations (61) and (62), the kinetic equation of the first axis is obtained.

Establish a dynamics equation for the second axis. From Equation (37),

From Equation (39)

From equations (64) and (65), the second-axis dynamic equation is obtained.

Finally, the dynamic equation of the third axis is established. From Equation (37),

From Equation (39)

From equations (67) and (68), the third-axis dynamic equation is obtained.

The generalized mass matrix is obtained from equations (61), (63), and (67).

It can be seen that as long as the topology, structural parameters, mass inertia and other parameters of the system are programmatically substituted into equations (36) to (40), dynamic modeling can be completed. By programming, it is easy to implement the Ju-Kane kinetic equation. Since the subsequent tree chain Ju-Kane gauge equation is derived from the Ju-Kane kinetic equation, the validity of the tree chain Ju-Kane kinetic equation can be proved by the Ju-Kane gauge type example.

3.4 Ju-Kane Dynamic Canonical Model of Tree Chain Rigid Body System

After the system dynamics equation is established, it is followed by the problem of solving the equation. In dynamic system simulation, given the generalized force acting on the environment and the generalized driving force of the drive shaft, it is necessary to solve the acceleration of the dynamic system; this is a positive problem in solving dynamic equations. Before solving, we first need to get the canonical equation shown in equation (71).

Normalized kinetic equations,

Of which: RHS-Right Hand Side

Obviously, the normalization process is the process of merging all joint acceleration terms; thus, the coefficient of joint acceleration is obtained. This problem is decomposed into two sub-problems, the canonical form of the kinematic chain and the canonical form of the closed subtree.

3.4.1 Canonical Equations of a Motion Chain

The forward iterative process of joint acceleration terms in equations (36) and (37) is converted into a reverse summation process for subsequent applications; obviously, there are six different types of acceleration terms, which are processed separately.

[1] Given motion chain

Then

The derivation steps of the above formula are:

[2] Given motion chain

Then

The derivation steps of the above formula are:

Therefore

[3] Given motion chain

Then

The above formula can be obtained from the following formula, because

Therefore

[4] Given motion chain

Then

The derivation steps of the above formula are: consider

Substituting Equation (72) into Equation (75) to the left

[5] Given motion chain

Then

The derivation steps of the above formula are: consider

Substituting equation (72) into equation (76) to the left

[6] Given motion chain

Then

The derivation steps of the above formula are:

Therefore

3.4.2 Canonical Equations for Closed Subtrees

Because the generalized force in the closed subtree ^u L is additive; therefore, the nodes of the closed subtree have only one motion chain to the root, and the motion chain ⁱ l _{n in} equations (73) to (77) can be replaced by ^u L . From equation (73),

From equation (74),

From equation (75)

From equation (76),

From Equation (77)

At this point, the prerequisites for establishing a standardized model have been established.

3.5 Tree Chain Rigid Body System Ju-Kane Dynamics Specification Equation

Next, a Ju-Kane normalized dynamic equation of a tree-structured rigid body system is established. For convenience, first define

Then, apply equations (78) to (82) to express equations (36) and (37) as canonical.

[1] The canonical form of formula (36) is

The specific establishment steps of the above formula are: from (24) and (36)

From equations (52) and (85),

Substituting Equation (80) into Equation (85) to the right of the previous term is

Substituting Equation (79) into Equation (86) to the right of the next term gives

Substituting equations (87) and (88) into equations (86) gives

For rigid body k, there is

Equation (84) is obtained from equation (35), equation (83), and equation (89).

[2] The canonical form of formula (37) is

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the axis invariant;

Is joint acceleration;

For cross multiplier, vector

The cross product matrix is

To take axis i to axis

The motion chain ^u L indicates that a closed subtree consisting of the axis u and its subtrees is obtained.

The specific establishment steps of the above formula are as follows:

Substituting equation (78) into the right-hand preceding term (91) gives

Substituting equation (81) into the right-hand side of equation (91) gives

Substituting equation (82) into the right middle term of equation (91) gives

Substituting Equation (92), Equation (93), and Equation (94) into Equation (92) gives

For rigid body k, there is

From formula (35), formula (83), and formula (95), formula (90) is obtained.

[3] Apply equations (84) and (90) to reformulate the Ju-Kane equation as the following tree-chain Ju-Kane canonical equation:

Given a multi-axis rigid body system D = {A, K, T, NT, F, B}, the inertial system is denoted by F ^[i] ,

In addition to gravity, the combined external force and moment acting on the axis u

The components on

and

The mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

The acceleration of gravity of axis k is

The bilateral driving force and driving torque of the driving shaft u are between

The components on

and

The force and moment of the environment i on the shaft l are

And ⁱ τ _l ; the Ju-Kane dynamics norm equation of axis u is

among them:

and

Is a 3 × 3 block matrix,

and

Is a 3D vector. and,

In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

Is the inertia matrix of the rotation axis u;

Is the inertia matrix of the translation axis u; h _R is the non-inertia matrix of the rotation axis u; h _P is the non-inertia matrix of the translation axis u; the combined external force and moment acting on the axis u are

The components on

and

The bilateral driving force and driving torque of the driving shaft u are between

The components on

and

The acting force and acting moment of the environment i on the shaft l are

And ⁱ τ _l ; ^l l _k is a kinematic chain from axis l to axis k, and ^u L means obtaining a closed subtree composed of axis u and its subtrees.

If the multi-axis rigid body system D = {A, K, T, NT, F, B} contains only the rotation axis,

Equation (101) can be simplified as

4.Solution of Tree Chain Rigid Body Ju-Kane Dynamics Equation

4.1 Rigid Body Inertia Matrix

The generalized inertia matrix of a rigid body motion chain expressed according to the type of motion axis and the 3D natural coordinate system is called an axial chain rigid body inertia matrix, and is simply referred to as an axial chain inertia matrix. From equations (116) and (119),

From equations (104) and (105), it can be seen that the above-mentioned axis-chain inertia matrix is a 3 × 3 matrix, and its size is 4 times smaller than that of the conventional 6 × 6 generalized inertia matrix; accordingly, the complexity of inversion is also 4 times smaller than the traditional inertial matrix.

Energy of the closed subtree ^u L

Expressed as

If

l∈ ^u L, then we get from formulas (18) to (20) and (106)

If

l∈ ^u L, then we get from formulas (18) to (20) and (106)

make

And have

Therefore, M ^{[u] [k]} can be written as

Where (114) M ^{[u] [k]} is a 3 × 3 axis chain inertia matrix (AGIM), and δ _k is called a motion axis attribute symbol;

4.2 Characteristics of Generalized Inertia Matrix of Rigid Body of Axial Chain

Given a multi-axis rigid body system D = {A, K, T, NT, F, B}

ⁱ l _n = (i,…, l,…, u,… n]], k∈ ^u L; the rigid body inertia matrix of the system's axis chain has symmetry under the condition that all types of motion pairs are the same, that is,

The derivation steps of the above formula are: Let u≥l. If

From equation (98),

From equations (116) and (117), if

Get

If

From equation (101),

From formula (119), formula (120) and formula

If

Get

Let | A | = a and let the generalized inertia matrix of the system with the number of axes a be M _{3a × 3a} . From equation (115),

The rigid body inertia matrix M _{3a × 3a of the} shaft chain in formula (122) has symmetry, and its element, that is, the shaft chain inertia matrix is a 3 × 3 matrix;

Given a multi-axis rigid body system D = {A, K, T, NT, F, B}

The elements of the shaft rigid body inertia matrix have the following characteristics:

[1] if

It can be known from equation (98)

and

Is a symmetric matrix;

[2] If

It can be known from equation (101)

and

Is a symmetric matrix;

[3]

or

From equations (98) and (101),

Is an antisymmetric matrix;

As can be seen from the above, the elements of the inertia matrix of the chain are not necessarily symmetrical.

Given kinematic chain

The Cartesian axis sequence is written as

among them:

Is the rotation axis sequence,

Is a translation axis sequence, and

The natural coordinate sequence is

From equation (98),

Obviously, u = ^u L and m _l = 0,

Obtained from the above formula

Obviously, the rigid body coordinate axis inertia matrix is different from the 6D inertia matrix, but they are equivalent.

4.3 Positive Solutions of Ju-Kane Dynamic Equations for Tree-Chain Rigid Body Systems

Now we will discuss how to obtain the positive solution of the Ju-Kane dynamic equation of the tree chain rigid body system. The positive solution of the dynamic equation is to solve the joint acceleration or inertial acceleration according to the dynamic equation when the driving force is given.

Define orthogonal complement matrix

And the corresponding cross product matrix

Given a multi-axis rigid body system D = {A, K, T, NT, F, B}

The dynamic equations of the axes in the system (96) are arranged in rows; the rearranged axis-driven generalized force and unmeasured environmental force are denoted as f ^C , and the measurable environmental generalized force is denoted as f ⁱ The corresponding joint acceleration sequence is written as

Rearranged

Let it be h; Consider equation (124); then the system dynamics equation is

From equation (125),

among them,

From equation (125),

Let | A | = a and let the generalized inertia matrix of the system with the number of axes a be M _{3a × 3a} . The key is how to calculate the inverse of the axis chain generalized inertia matrix in (128)

If the pivot method is used for brute force calculations

Obviously, even for a multi-axis system with a small number of axes, the calculation cost is very high. Therefore, this method should not be used.

Generalized Inertial Matrix

Is a symmetric matrix and due to system energy

Greater than zero, so it is a positive definite matrix. Effective

The calculation process is as follows:

[1] First, perform LDL ^T decomposition,

among them,

Is the only lower triangular matrix that exists, and D _{a × a} is a diagonal matrix.

[1-1] If the LDL ^T decomposition (ie, matrix decomposition) is performed by a single CPU, the decomposition complexity is O (a ² )

[1-2] If a CPU or GPU is used for parallel decomposition

The decomposition complexity is O (a);

[2] Application formula (130) calculation

Substituting equation (130) into equation (128) gives

At this point, the positive solution of the Ju-Kane dynamic equation of the tree chain rigid body system is obtained. It has the following characteristics:

[2-1] Generalized inertial matrix of the axis chain in formula (129) based on Ju-Kane normal form

Generalized inertial matrix whose size is only 6D double vector space

1/4,

LDL ^T decomposition makes the inversion speed greatly improved. At the same time, f ^C , f ⁱ and h in equation (131) are all iterative formulas about axis invariants, which can guarantee

The real-time and accuracy of the solution; Ju-Kane canonical form has an axiomatic theoretical foundation with clear physical connotation; and the dynamics of a multibody system based on 6D space operation operators is based on an integrated correlation matrix, regardless of modeling The process or the positive solution process is more abstract than the Ju-Kane canonical system modeling and solving process. In particular, the dynamic iterative method established with reference to Kalman filtering and smoothing theory lacks rigorous axiomatic analysis process.

[2-2] Generalized inertial matrix of shaft chain in equation (129)

In formula (131), f ^C , f ⁱ and h can be dynamically updated according to the system structure, which can ensure the flexibility of engineering applications.

[2-3] Generalized inertial matrix of the axis chain in equation (129)

In Formula (131), f ^C , f ⁱ and h have a simple and elegant chain index system; at the same time, they have a pseudo code function implemented by software, which can guarantee the quality of project implementation.

[2-4] Because the polarities of the coordinate system and axis can be set according to the needs of the project, the output results of the dynamic simulation analysis do not need to be converted in the middle, which improves the convenience of application and the efficiency of post-processing.

4.4 Inverse Solution of Ju-Kane Dynamic Equation of Tree-chain Rigid Body System

The inverse solution of the dynamic equation refers to the known dynamic state of motion, structural parameters, and mass inertia to solve the driving force or driving moment. Considering equations (96) and (102),

When the joint configuration, velocity, and acceleration are known, ^{i | D} f _u and ^{i | D} τ _{u are} obtained from equation (34). Further, if the external force and external moment are known, the driving force is solved by equation (132)

And driving torque

Obviously, the computational complexity of the inverse solution of the dynamic equation is proportional to the number of system axes | A |.

Although the calculation of the inverse dynamics solution is simple, it is very important for real-time force control of multi-axis systems. When the degree of freedom of a multi-axis system is high, real-time dynamic calculation is often an important bottleneck, because the dynamic response of force control usually requires 5 to 10 times higher frequency than the dynamic response of motion control. On the one hand, due to the inertial matrix of the shaft chain

Not only symmetrical, but also the size of the traditional body-chain inertia matrix

1/4, the generalized inertial matrix of the shaft chain is calculated by equation (127)

The calculation time is much smaller. On the other hand, the axial inertial force of the moving shaft is calculated by equation (126)

The amount of calculation is only 1/36 of Newton's Euler method.

Claims (13)

1. A tree-chain robot dynamics and calculation method based on axis invariants is characterized in that it includes the following steps:

   Given a multi-axis rigid body system D = {A, K, T, NT, F, B}, the inertial system is denoted by F ^[i] ,

   

   In addition to gravity, the combined external force and moment acting on the axis u are recorded as ^{i | D} f _u and ^{i | D} τ _{u respectively} ; the mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

   

   The acceleration of gravity of axis k is

   

   According to the topology, structure and mass-inertia parameters of the robot system, the Lagrange equation of joint space is established using the chain symbol system, and the Ju-Kane dynamics preliminary equation is established based on the Lagrange equation of the multi-axis system

   Substituting the partial velocity into the Ju-Kane dynamics equation to establish the Ju-Kane dynamic equation of the tree chain rigid body system

   Establish Ju-Kane normalized dynamic equations of tree structure rigid body system;

   The Ju-Kane dynamic equation of the tree chain rigid body system is reformulated as the tree chain Ju-Kane canonical equation.
2. The axis invariant-based tree chain robot dynamics and solution method according to claim 1, wherein:

   Based on the Lagrange equation of the multi-axis system, the Ju-Kane dynamics preliminary theorem is derived, and its steps are:

   [1] Prove the equivalence of Lagrange's equation and Kane's equation;

   [2] Based on Lagrange's equation, based on energy's partial velocity of joint velocity and coordinates;

   [3] Find the derivative of the deviation speed with time;

   [4] Based on the above steps, a Ju-Kane kinetics preliminary theorem is obtained.
3. The axis invariant-based tree chain robot dynamics and calculation method according to claim 2, wherein:

   [1] Prove the equivalence of Lagrange's equation and Kane's equation

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   Is the rotation speed vector;

   

   Is the translational acceleration vector;

   

   Is the translational velocity vector;

   

   For translational kinetic energy;

   

   For rotational kinetic energy;

   

   Is joint coordinates;

   

   Joint speed

   Consider the kinetic energy pair of rigid body k translation

   

   The derivative of the partial velocity with time is

   

   Consider the kinetic energy of k

   

   The derivative of the partial velocity with time is

   

   because

   

   versus

   

   Irrelevant, obtained by equation (7) and Lagrange's equation of multi-axis system

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   Is the rotation speed vector;

   

   Is the rotation acceleration vector;

   

   Is the translational acceleration vector;

   

   Is the translational velocity vector;

   

   For translational kinetic energy;

   

   For rotational kinetic energy;

   

   Is gravitational potential energy;

   

   Is joint coordinates;

   

   Joint speed

   The translational kinetic energy and rotational kinetic energy of the dynamic system D are expressed as

   

   Considering equations (4) and (5), we have

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   Equations (7) and (8) are the basis for the proof of the Jue-Kane dynamics theorem, that is, the Jue-Kane dynamics theorem is essentially equivalent to the Lagrangian method; at the same time, the equation (8) The right side contains the Kane equation of the multi-axis system; it shows that the calculation of the inertia force of the Lagrange method and the Kane method is consistent, that is, the Lagrange method and the Kane method are equivalent;

   [2] Based on Lagrange's equation, based on energy, the velocity of joints and the deflection of coordinates

   System D energy

   

   Expressed as

   

   among them:

   

   [2-1] If

   

   And consider

   

   and

   

   It is only related to the closed subtree ^u L. From equations (4) and (5), we get

   

   

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   Is the rotation speed vector;

   

   Is the translational velocity vector;

   

   Is joint coordinates;

   

   Joint speed

   [2-2] If

   

   And consider

   

   and

   

   It is only related to the closed subtree ^u L. From equations (4) and (5), we get

   

   

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   Is the rotation speed vector;

   

   Is the translational velocity vector;

   

   Is the rotation joint coordinates;

   

   For turning joint speed;

   [3] Find the derivative of partial velocity with time

   [3-1] If

   

   From formula (7), formula (9) and formula (10),

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   [3-2] If

   

   From (7), (12), and (13),

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   [4] Ju-Kane kinetics preliminary theorem based on the above steps

   The Ju-Kane dynamics equation for axis u is

   

   Equation (17) has a tree chain topology; k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are denoted as m _k and

   

   Is the rotation speed vector;

   

   Is the rotation acceleration vector;

   

   Is the translational acceleration vector;

   

   Is the translational velocity vector;

   

   For translational joint coordinates;

   

   Is the translational joint speed;

   

   Is the rotation joint coordinates;

   

   Is the joint velocity; the generalized force in the closed sub-tree ^u L is additive, and the nodes of the closed sub-tree have only one motion chain to the root. The motion chain ⁱ l _n can be replaced by the motion chain ^u L.
4. The axis invariant-based tree chain robot dynamics and solution method according to claim 1, wherein:

   Given shaft chain

   

   k∈ ⁱ l _n , the formula for calculating partial velocity is:

   

   

   

   Where along the axis

   

   Line position

   

   Around the axis

   

   Angular position

   

   Axis vector

   

   Angular velocity

   

   Line speed

   

   The relationship between left-order cross product and transpose is:

   

   In the formula:

   

   Is the rotation speed vector.
5. The axis invariant-based tree chain robot dynamics and solution method according to claim 1, wherein:

   Given a bilateral external force from the force application point i _S in the environment i to the point l _S on the axis l

   

   And external moment ⁱ τ _l , their instantaneous shaft power p _{ex is} expressed as

   

   among them:

   

   And ⁱ τ _{l is} not affected by

   

   and

   

   Control, ie

   

   And ⁱ τ _l does not depend on

   

   and

   

   [1] If k∈ ⁱ l _l , then

   

   From equations (19) and (18),

   

   In (26)

   

   And (21)

   

   The chain order of is different; the former is the force, the latter is the amount of motion, and the two are dual, with opposite order

   [2] If k ∈ ⁱ l _l , then

   

   From equations (22) and (25),

   

   Equations (26) and (27) show that the combined external force or moment acting on the axis k by the environment is equivalent to the combined external force or moment of the closed subtree ^k L on the axis k, and formulae (26) and (27) as

   

   In Equation (28), the closed subtree has an additivity to the generalized force of axis k; the effect of the force has a dual effect and is iterative in the reverse direction; the so-called reverse iteration refers to:

   

   It is necessary to iterate through the link position vector;

   

   Order and forward kinematics

   

   The order of calculation is reversed.
6. The axis invariant-based tree chain robot dynamics and solution method according to claim 1, wherein:

   If the shaft l is a driving shaft, the driving force and driving torque of the shaft l are

   

   and

   

   Driving force

   

   And driving torque

   

   The generated power p _{ac is} expressed as

   

   [1] Obtained from formula (18), formula (19), and formula (29)

   

   If the axis u and the axis

   

   Co-axial, then

   

   Remember

   

   

   because

   

   versus

   

   Irrelevant, it is obtained by equation (30)

   

   because

   

   versus

   

   Co-axial

   

   [2] Obtained from formula (18), formula (19) and formula (29)

   

   If the axis u and

   

   Co-axial, then

   

   Remember

   

   From equation (32),

   
7. The axis invariant-based tree chain robot dynamics and solution method according to claim 1, wherein:

   The combined external force and moment acting on the shaft u are

   

   The components on

   

   and

   

   The bilateral driving force and driving torque of the driving shaft u are between

   

   The components on

   

   and

   

   The force and moment of the environment i on the axis l are

   

   And ⁱ τ _l ; then the Ju-Kane dynamic equation of the axis u-tree chain is

   

   Among them: [·] means taking rows or columns;

   

   and

   

   Is a 3 × 3 block matrix,

   

   and

   

   Is a 3D vector, q is the joint space;

   And yes,

   

   

   

   

   

   

   Among them, remember

   

   Remember

   

   

   k _I represents the center of mass _{I of the} rod k; the mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

   

   Is the inertia matrix of the rotation axis u;

   

   Is the inertial matrix of the translation axis u; h _R is the non-inertia matrix of the rotation axis u; h _P is the non-inertia matrix of the translation axis u.
8. The axis invariant-based tree chain robot dynamics and calculation method according to claim 7, characterized in that:

   

   The Ju-Kane normalized dynamic equation of a tree-structured rigid body system is:

   The canonical form of (36) is

   

   The canonical form of (37) is

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   Is the axis invariant;

   

   Is joint acceleration;

   

   For cross multiplier, vector

   

   The cross product matrix is

   

   To take axis i to axis

   

   The motion chain ^u L indicates that a closed subtree consisting of the axis u and its subtrees is obtained.
9. The axis invariant-based tree chain robot dynamics and solution method according to claim 1, wherein:

   The Ju-Kane dynamic equation of tree chain rigid body system is reformulated as the tree-chain Ju-Kane canonical equation:

   Given a multi-axis rigid body system D = {A, K, T, NT, F, B}, the inertial system is denoted by F ^[i] ,

   

   In addition to gravity, the combined external force and moment acting on the axis u

   

   The components on

   

   and

   

   The mass of the axis k and the moment of inertia of the center of mass are recorded as m _k and

   

   The acceleration of gravity of axis k is

   

   The bilateral driving force and driving torque of the driving shaft u are between

   

   The components on

   

   and

   

   The force and moment of the environment i on the shaft l are

   

   And ⁱ τ _l ; the Ju-Kane dynamics norm equation of axis u is

   

   among them:

   

   and

   

   Is a 3 × 3 block matrix,

   

   and

   

   Is a 3D vector;

   and,

   

   

   

   

   

   

   In the formula, k _I represents the centroid I of the rod k; the mass of the axis k and the moment of inertia of the centroid are recorded as m _k and

   

   Is the inertia matrix of the rotation axis u;

   

   Is the inertia matrix of the translation axis u; h _R is the non-inertia matrix of the rotation axis u; h _P is the non-inertia matrix of the translation axis u; the combined external force and moment acting on the axis u are

   

   The components on

   

   and

   

   The bilateral driving force and driving torque of the driving shaft u are between

   

   The components on

   

   and

   

   The acting force and acting moment of the environment i on the shaft l are

   

   And ⁱ τ _l ; ^l l _k is a kinematic chain from axis l to axis k, and ^u L means obtaining a closed subtree composed of axis u and its subtrees.
10. The axis invariant-based tree chain robot dynamics and calculation method according to claim 9, wherein:

    Given the generalized force acting on the environment and the generalized driving force of the drive axis, the acceleration or inertial acceleration of the dynamic system is solved to obtain the positive solution of the Ju-Kane dynamic equation of the tree chain rigid body system.
11. The axis invariant-based tree chain robot dynamics and calculation method according to claim 10, wherein:

    The specific steps to find the positive solution of the Ju-Kane dynamic equation of the tree chain rigid body system are:

    Define orthogonal complement matrix

    

    And the corresponding cross product matrix

    

    

    The Ju-Kane dynamics norm equations of each axis in the system are arranged in rows; the rearranged axis-driven generalized force and unmeasured environmental force are denoted as f ^C , and the measurable environmental generalized force is denoted as f ⁱ ; The corresponding joint acceleration sequence is written as

    

    Rearranged

    

    Let it be h; then the system dynamics equation is

    

    From equation (125),

    

    Let | A | = a, and write the generalized inertial matrix of the system with axis a as M _{3a × 3a} ;

    Generalized Inertial Matrix

    

    Is a symmetric matrix and it is a positive definite matrix, valid

    

    The calculation process is as follows:

    [1] First, perform LDL ^T decomposition, that is, matrix decomposition,

    

    among them,

    

    Is the only lower triangular matrix that exists, and D _{a × a} is a diagonal matrix;

    [2] Application formula (130) calculation

    

    

    Substituting equation (130) into equation (128) gives

    

    At this point, the positive solution of the Ju-Kane dynamic equation of the tree chain rigid body system is obtained.
12. The axis invariant-based tree chain robot dynamics and calculation method according to claim 9, wherein:

    Find the inverse solution of the Ju-Kane dynamic equation of a tree chain rigid body system.
13. The axis invariant-based tree chain robot dynamics and calculation method according to claim 12, wherein:

    When the joint configuration, velocity, and acceleration are known, ^{i | D} f _u and ^{i | D} τ _{u are} obtained by equation (34); when the external force and external moment are known, the driving force is solved by equation (132)

    

    And driving torque

    

    

    In the formula, the combined external force and moment acting on the shaft u are

    

    The components on

    

    and

    

    The bilateral driving force and driving torque of the driving shaft u are between

    

    The components on

    

    and

    

    The acting force and acting moment of the environment i on the shaft l are

    

    And ⁱ τ _l ; ^l l _k is a kinematic chain from axis l to axis k, and ^u L means obtaining a closed subtree composed of axis u and its subtrees.

Images