WO2020034404A1 - Non-ideal articulated robot dynamics modeling and solution method based on axis invariant - Google Patents

Abstract

A non-ideal articulated robot dynamics and solution method based on an axis invariant. For a non-ideal constraint system, a Ju-Kane dynamic equation of a closed-chain rigid-body non-ideal constraint system is established. Ju-Kane closed-chain rigid-body dynamics based on a natural axis chain in a joint space overcomes the limitations of a Cartesian coordinate axis chain space: in Newton-Euler dynamics based on a Cartesian coordinate axis chain, non-tree kinematic pair constraints cannot express constraints such as rack and pinion, and worm gear and worm. In the method, the constraint algebraic equation of the non-tree constraint pair can express any kind of constraint, and the physical connotation is clear; the complexity of solving the system equation is reduced; the accuracy of the constraint equation is ensured.

Description

基于轴不变量的非理想关节机器人动力学建模与解算方法Dynamic modeling and solving method of non-ideal joint robot based on axis invariant 技术领域Technical field

本发明涉及一种非理想关节机器人动力学建模与解算方法，属于机器人技术领域。The invention relates to a non-ideal articulated robot dynamics 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 non-ideal articulated robot dynamic modeling and calculation method based on axis invariants.

为解决上述技术问题，本发明采用以下技术方案：To solve the above technical problems, the present invention adopts the following technical solutions:

一种基于轴不变量的非理想关节机器人动力学建模与解算方法，其特征是，A non-ideal articulated robot dynamics modeling and solving method based on axis invariants is characterized by:

给定多轴刚体***D＝{A,K,T,NT,F,B}，惯性系记为F ^[i]，

除了重力外，作用于轴u的合外力及力矩在

上的分量分别记为

及

轴k的质量及质心转动惯量分别记为m _k及

轴k的重力加速度为

驱动轴u的双边驱动力及驱动力矩在

上的分量分别记为

及

环境i对轴l的作用力及作用力矩分别为

及

轴u对轴u′的广义约束力记为

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 acting force and acting moment of the environment i on the shaft l are

and

The generalized binding force of axis u to axis u ′ is written as

设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为

根据建立的闭链刚体***的Ju-Kane动力学方程，计算关节加速度

后，计算径向约束力大小

及

约束力矩大小

及

再建立如下闭链刚体非理想约束***的Ju-Kane动力学方程： Let the generalized internal friction and viscous combined force and moment of the motion axis u be

Calculate joint acceleration based on the Ju-Kane dynamic equation of the closed-chain rigid body

After calculating the radial binding force

and

Constraint moment

and

Then establish the following Ju-Kane dynamic equation of the closed-chain rigid body non-ideal constraint system:

【1】轴u及轴u′的Ju-Kane动力学规范方程分别为[1] The Ju-Kane kinetic norm equations for axis u and axis u ′ are

式中：

及

是3×3的分块矩阵，

及

是3D矢量；

为转动轴u的惯性矩阵；

为平动轴u的惯性矩阵；h _R为转动轴u的非惯性矩阵；h _P为平动轴u的非惯性矩阵； In the formula:

and

Is a 3 × 3 block matrix,

and

Is a 3D vector;

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;

【2】非树约束副 ^uk _u′的约束代数方程为 [2] The constraint algebraic equation of the non-tree constrained pair ^u k _{u ′} is

闭链刚体***的Ju-Kane动力学方程：Ju-Kane dynamic equation of closed-chain rigid body system:

【1】轴u及轴u′的Ju-Kane动力学规范方程分别为[1] The Ju-Kane kinetic norm equations for axis u and axis u ′ are

其中：

及

是3×3的分块矩阵，

及

是3D矢量；k _I表示杆k质心I；轴k的质量及质心转动惯量分别记为m _k及

为转动轴u的惯性矩阵；

为平动轴u的惯性矩阵；h _R为转动轴u的非惯性力；h _P为平动轴u的非惯性力。 among them:

and

Is a 3 × 3 block matrix,

and

Is a 3D vector; 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 inertial matrix of the translation axis u; h _R is the non-inertia force of the rotation axis u; h _P is the non-inertia force of the translation axis u.

【2】非树约束副 ^uk _u′的约束代数方程为 [2] The constraint algebraic equation of the non-tree constrained pair ^u k _{u ′} is

其中：among them:

式中：

及

是3×3的分块矩阵，

及

是3D矢量；k _I表示杆k质心I；轴k的质量及质心转动惯量分别记为m _k及

为转动轴u的惯性矩阵；

为平动轴u的惯性矩阵；h _R为转动轴u的非惯性矩阵；h _P为平动轴u的非惯性矩阵；

为平动关节角速度；

为转动关节角速度。 In the formula:

and

Is a 3 × 3 block matrix,

and

Is a 3D vector; 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 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;

Is the translational joint angular velocity;

Is the angular velocity of the turning joint.

应用式(129)至式(134)计算径向约束力大小

及

约束力矩大小

及

对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为

则有 Apply equations (129) to (134) to calculate the radial restraining force

and

Constraint moment

and

For the motion axis u without power loss, record its constraint force and constraint torque vector as

Then

上式表示运动轴矢量与运动轴约束力具有自然正交补的关系；The above formula indicates that the motion axis vector and the constraint force of the motion axis have a natural orthogonal complement relationship;

若

及

为运动副

的两个正交约束轴，且约束轴与运动轴正交，即 If

and

For sports pair

Two orthogonal constraint axes, and the constraint axis is orthogonal to the motion axis, ie

记

为约束轴轴矢量，有 Remember

For the constraint axis axis vector, there is

其中：among them:

由式(130)得到关节约束力大小

及

约束力矩大小

及

若记运动轴径向力矢量

及力矩矢量

则有 Get the size of the joint restraint force by (130)

and

Constraint moment

and

If you remember the motion axis radial force vector

And torque vector

Then

若记运动轴径向力大小为

及力矩大小为

由式(133)得 If the radial force of the moving axis is

And the moment is

From Equation (133),

至此，完成了轴径向约束广义力的计算。So far, the calculation of the generalized force of axial radial restraint is completed.

由式(130)至式(134)计算运动轴u的径向约束力大小

及约束力矩大小

时，不考虑运动轴的广义内摩擦力及粘滞力。 Calculate the radial restraining force of the motion axis u from equations (130) to (134)

And restraining moment

In this case, the generalized internal friction and viscous forces of the moving axis are not considered.

考虑广义内摩擦力及粘滞力的基于轴不变量的约束力求解步骤为：Considering the generalized internal friction and viscous forces, the solution steps of the constraint force based on the axis invariant are:

在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小

及约束力矩大小

记运动轴u的内摩擦力大小及内摩擦力矩大小分别为

及

运动轴u的粘滞力及粘滞力矩大小分别为

及

则 After the calculation of the radial general constraint force of the axis is completed, the radial constraint force of the motion axis u is obtained.

And restraining moment

The magnitude of the internal friction force and the internal friction moment of the motion axis u are respectively

and

The viscous force and viscous moment of the motion axis u are

and

then

其中： _sk ^[u]─运动轴u的内摩擦系数， _ck ^[u]─运动轴u的粘滞系数；sign()表示取正或负符号； Wherein: _s k ^[u] coefficient of friction u of the movement of the shaft ─, _c k ^[u] ─ movement of the shaft of the viscosity coefficient u; Sign () indicates the sign of positive or negative;

记广义内摩擦力及粘滞力的合力及合力矩分别为

由式(140)及式(141)得 The general and internal moments of generalized internal friction and viscous forces are

From equations (140) and (141),

闭链刚体***的Ju-Kane动力学方程根据树链Ju-Kane规范型方程建立。The Ju-Kane dynamic equation of the closed-chain rigid body system is established according to the tree-chain Ju-Kane canonical equation.

树链Ju-Kane规范型方程Tree Chain Ju-Kane Canonical Equation

其中：

及

是3×3的分块矩阵，

及

是3D矢量；

为轴u的合外力在

上的分量，

为轴u的合力矩在

上的分量； among them:

and

Is a 3 × 3 block matrix,

and

Is a 3D vector;

The resulting external force for axis u is

On the weight,

The resultant moment of the shaft u is

Weight

并且，and,

式中，k _I表示杆k质心I；轴k的质量及质心转动惯量分别记为m _k及

为转动轴u的惯性矩阵；

为平动轴u的惯性矩阵；h _R为转动轴u的非惯性矩阵；h _P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在

上的分量分别记为

及

作用于轴u的合外力及力矩在

上的分量分别记为

及

驱动轴u的双边驱动力及驱动力矩在

上的分量分别记为

及

环境i对轴l的作用力及作用力矩分别为

及 ⁱτ _l； ^ll _k为取由轴l至轴k的运动链， ^uL表示获得由轴u及其子树构成的闭子树。 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 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:

对于非理想约束***，建立了闭链刚体非理想约束***的Ju-Kane动力学方程。For non-ideal constraint systems, the Ju-Kane dynamic equations of closed-chain rigid-body non-ideal constraint systems are established.

【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副 ^uk _u′∈P约束不能表达齿条与齿轮、蜗轮与蜗杆等约束。而本申请建立的非树约束副 ^uk _u′的约束代数方程可表达任一种约束类形，并且物理内涵明晰； [1] In Newton Euler dynamics based on Cartesian coordinate axis chains, non-tree motion pair ^u k _{u ′} ∈ P constraints cannot express constraints such as rack and pinion, worm gear and worm. The constraint algebra equation of the non-tree-constrained pair ^u k _{u ′} established in this application can express any kind of constraint, and the physical connotation is clear;

【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是6D的；而本申请建立的非树约束副的约束代数方程表示是3D非树运动副代数约束方程，从而降低了***方程求解的复杂度；[2] In the Newton Euler dynamics based on the Cartesian coordinate axis chain, the algebraic constraint equation of the non-tree motion pair is 6D; and the constraint algebraic equation of the non-tree motion pair established in this application is a 3D non-tree motion pair algebraic constraint Equations, thereby reducing the complexity of solving system equations;

【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是关于6D矢量空间绝对加速度的，是关于关节坐标、关节速度的迭代式，具有累积误差；而本申请建立的非树约束副的约束代数方程是关于关节加速度的，保证了约束方程的准确性。[3] In the Newton Euler dynamics based on the Cartesian coordinate axis chain, the non-tree motion algebraic constraint equation is about 6D vector space absolute acceleration, iterative formula about joint coordinates and joint speed, and it has cumulative errors The constraint algebraic equation of the non-tree-constrained pair applied for is related to joint acceleration, which guarantees the accuracy of the constraint equation.

附图说明BRIEF DESCRIPTION OF THE DRAWINGS

图1自然坐标系与轴链；Figure 1 Natural coordinate system and axis chain;

图2固定轴不变量；Figure 2 Fixed axis invariant;

图3、图4为运动轴的内摩擦力及粘滞力示意图。Figures 3 and 4 are schematic diagrams of internal friction and viscous forces of a moving shaft.

具体实施方式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.

闭链刚体***具有非常广泛的应用；比如，CE3巡视器的摇臂移动***是具有差速器的闭链，重载机械臂通常是具有四连杆的闭链***。同时，实际的运动轴通常包含内摩擦力及粘滞力。因此研究闭链刚体***的Ju-Kane动力学建模非常必要。Closed-chain rigid body systems have a wide range of applications; for example, the rocker arm movement system of the CE3 patrol is a closed-chain with a differential, and heavy-duty mechanical arms are usually closed-chain systems with four links. At the same time, the actual motion axis usually contains internal friction and viscous forces. Therefore, it is necessary to study Ju-Kane dynamic modeling of closed-chain rigid body systems.

描述运动链的基本拓扑符号及操作是构成运动链拓扑符号***的基础，定义如下：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】运动链由偏序集合(]标识。[1] The motion chain is identified by a partial order set ().

【2】A ^[l]为取轴序列A的成员；因轴名l具有唯一的编号对应于A ^[l]的序号，故A ^[l]计算复杂度为O(1)。 [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】

为取轴l的父轴；由式

可知，

计算复杂度为O(1)。 [3]

Is the parent axis of axis l;

You know,

The computational complexity is O (1).

【4】

为取轴序列

的成员；由式

可知，故

计算复杂度为O(1)。 [4]

Axis sequence

Member of

Know that

The computational complexity is O (1).

【5】 ^ll _k为取由轴l至轴k的运动链，输出表示为

且

基数记为| ^ll _k|。 ^ll _k执行过程：执行

若

则执行

否则，结束。 ^ll _k计算复杂度为O(| ^ll _k|)。 [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】 ^ll为取轴l的子。该操作表示在

中找到成员l的地址k；从而，获得轴l的子A ^[k]。因

不具有偏序结构，故 ^ll的计算复杂度为

[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】 ^lL表示获得由轴l及其子树构成的闭子树，

为不含l的子树；递归执行 ^ll，计算复杂度为

[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】支路、子树及非树弧的增加与删除操作也是必要的组成部分；从而，通过动态Span树及动态图描述可变拓扑结构。在支路 ^ll _k中，若

则记

即

表示在支路中取成员m的子。 [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.

计算复杂度O()表示计算过程的操作次数，通常指浮点乘与加的次数。以浮点乘与加的次数表达计算复杂度非常烦琐，故常采用算法循环过程中的主要操作次数；比如：关节位姿、速度、加速度等操作的次数。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, so the main operation times in the algorithm loop process are often used; for example, the number of operations such as joint pose, speed, and acceleration.

定义1自然坐标轴：称与运动轴或测量轴共轴的，具有固定原点的单位参考轴为自然坐标轴，亦 称为自然参考轴。 Definition 1 Natural coordinate axis: The unit reference axis with a fixed origin that is coaxial with the motion axis or measurement axis is called the natural coordinate axis, also known as the natural reference axis.

定义2自然坐标系：若多轴***D处于零位，所有笛卡尔体坐标系方向一致，且体坐标系原点位于运动轴的轴线上，则该坐标***为自然坐标***，简称自然坐标系。Definition 2 Natural coordinate system: 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.

自然坐标系优点在于：(1)坐标***易确定；(2)零位时的关节变量为零；(3)零位时的***姿态一致；(4)不易引入测量累积误差。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.

定义3不变量：称不依赖于一组坐标系进行度量的量为不变量。Definition 3 Invariant: A quantity that does not depend on a set of coordinate systems for measurement is called an invariant.

由定义2可知，在***处于零位时，所有杆件的自然坐标系与底座或世界系的方向一致。***处于零位即

时，自然坐标系

绕轴矢量

转动角度

将

转至F ^[l]；

在

下的坐标矢量与

在F ^[l]下的坐标矢量

恒等，即有 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

由上式知，

或

不依赖于相邻的坐标系

及F ^[l]；故称

或

为轴不变量。在不强调不变性时，可以称之为坐标轴矢量(简称轴矢量)。

或

表征的是体

与体l共有的参考单位坐标矢量，与参考点

及O _l无关。 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.

对轴不变量而言，其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴，故其绝对导数恒为零矢量。因此，轴不变量具有对时间微分的不变性。有：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转动坐标矢量：绕坐标轴矢量

转动到角位置

的坐标矢量

为 Definition 4 Rotating coordinate vector: vector around coordinate axis

Turn to angular position

Coordinate vector

for

定义5平动坐标矢量：沿坐标轴矢量

平动到线位置

的坐标矢量

为 Definition 5 translation coordinate vector: vector along the coordinate axis

Pan to line position

Coordinate vector

for

定义6自然坐标：以自然坐标轴矢量为参考方向，相对***零位的角位置或线位置，记为q _l，称为自然坐标；称与自然坐标一一映射的量为关节变量；其中： Definition 6 Natural coordinates: take the natural coordinate axis vector as the reference direction, 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;

定义7机械零位：对于运动副

在初始时刻t ₀时，关节绝对编码器的零位

不一定为零，该零位称为机械零位； 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

定义8自然运动矢量：将由自然坐标轴矢量

及自然坐标q _l确定的矢量

称为自然运动矢量。其中： Definition 8 natural motion vector: will be determined by 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.

定义9关节空间：以关节自然坐标q _l表示的空间称为关节空间。 Definition 9 joint space: The space represented by the joint natural coordinates q _l is called joint space.

定义10位形空间：称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间，是双矢量空间或6D空间。Defining a 10-dimensional space: The Cartesian space that expresses the position and pose (posture for short) is called a shape space, which is a double vector space or a 6D space.

定义11自然关节空间：以自然坐标系为参考，通过关节变量

表示，在***零位时必有

的关节空间，称为自然关节空间。 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.

给定多轴***D＝{T,A,B,K,F,NT}，在***零位时，只要建立底座系或惯性系，以及各轴上的参考点O _l，其它杆件坐标系也自然确定。本质上，只需要确定底座系或惯性系。 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.

给定一个由运动副连接的具有闭链的结构简图，可以选定回路中任一个运动副，将组成该运动副的定子与动子分割开来；从而，获得一个无回路的树型结构，称之为Span树。T表示带方向的span树，以描述树链运动的拓扑关系。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为结构参数；A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树。

为取轴序列

的成员。转动副R，棱柱副P，螺旋副H，接触副O是圆柱副C的特例。 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.

定义以下表达式或表达形式：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.

约定：“□”表示属性占位；若属性p或P是关于位置的，则

应理解为坐标系

的原点至F ^[l]的原点；若属性p或P是关于方向的，则

应理解为坐标系

至F ^[l]。 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] .

及

应分别理解为关于时间t的函数

及

且

及

是t ₀时刻的常数或常数阵列。但是正体的

及

应视为常数或常数阵列。

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.

本申请中约定：在运动链符号演算***中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指 标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便，有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标。例如：

可简述为(表示由k至l)平动矢量；

表示(由k至l的)线位置； ^kr _l表示(由k至l的)平动矢量；其中：r表示“平动”属性符，其余属性符对应为：属性符φ表示“转动”；属性符Q表示“旋转变换矩阵”；属性符l表示“运动链”；属性符u表示“单位矢量”；属性符ω表示“角速度”；属性符J表示质心转动惯量；J表示偏速度雅克比矩阵；角标为i表示惯性坐标系或大地坐标系；其他角标可以为其他字母，也可以为数字。 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); ^k r _l represents the translation vector (from k to l); where: r represents the "translation" attribute, and the rest of the attributes correspond to: attribute φ represents "rotation" Attribute Q represents the "rotation transformation matrix"; Attribute l represents the "kinematic chain"; Attribute u represents the "unit vector"; Attribute represents the "angular velocity"; Attribute J represents the centroid inertia; J represents the partial velocity Jac Ratio matrix; i indicates an inertial coordinate system or a 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－杆件l中的点S；而S表示空间中的一点S。 (1) l _S -point S in rod l; and S represents a point S in space.

(2)

－杆件k的原点O _k至杆件l的原点O _l的平动矢量； (2)

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

^kr _l－

在自然坐标系F ^[k]下的坐标矢量，即由k至l的坐标矢量； ^k r _l-

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

(3)

－原点O _k至点l _S的平动矢量； (3)

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

在F ^[k]下的坐标矢量；

Coordinate vector under F ^[k] ;

(4)

－原点O _k至点S的平动矢量； (4)

-Translation vector from origin _Ok to point S;

^kr _S－

在F ^[k]下的坐标矢量； ^k r _S-

Coordinate vector under F ^[k] ;

(5)

－连接杆件

及杆件l的运动副； (5)

－Connecting rod

And the movement pair of rod l;

－运动副

的轴矢量；

－Sports Vice

Axis vector

及

分别在

及F ^[l]下的坐标矢量；

是轴不变量，为一结构常数；

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)

－沿轴

的线位置(平动位置)， (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)左下角指标为0时，表示机械零位；如：(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－三维零矩阵；0 ₃＝[0 0 0] ^T；1－三维单位矩阵； (8) 0-three-dimensional zero matrix; 0 ₃ = [0 0 0] ^T ; 1- three-dimensional identity matrix;

(9)约定：“\”表示续行符；“□”表示属性占位；则(9) Convention: "\" means line continuation character; "□" means attribute placeholder;

幂符

表示□的x次幂；右上角角标∧或

表示分隔符；如：

或

为

的x次幂。 Power

Represents the xth power of □;

Delimiter; for example:

or

for

X power.

[□] ^T表示□的转置，表示对集合转置，不对成员执行转置；如：

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

^|□为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列，投影即是点积运算“·”；如：位置矢量

在坐标系F ^[k]中的投影矢量记为

^| 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)单位矢量在大地坐标系的投影矢量

单位零位矢量

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

Unit zero vector

(11)

－零位时由原点

至原点O _l的平动矢量，且记

表示位置结构参数。 (11)

－From zero point

Translation vector to origin O _l

Represents the position structure parameter.

(12) ⁱQ _l，相对绝对空间的旋转变换阵； (12) ⁱ Q _l , a rotation transformation matrix in relative absolute space;

(13)以自然坐标轴矢量为参考方向，相对***零位的角位置或线位置，记为q _l，称为自然坐标；关节变量

自然关节坐标为φ _l； (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)对于一给定有序的集合r＝[1,4,3,2] ^T，记r ^[x]表示取集合r的第x行元素。常记[x]、[y]、 [z]及[w]表示取第1、2、3及4列元素。 (14) For a given ordered set r = [1,4,3,2] ^T , note that r ^[x] represents taking 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表示由i到j的运动链； ^ll _k为取由轴l至轴k的运动链； (15) ⁱ l _j represents a kinematic chain from i to j; ^l l _k is a kinematic chain from axis l to k;

给定运动链

若n表示笛卡尔直角系，则称

为笛卡尔轴链；若n表示自然参考轴，则称

为自然轴链。 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.

1.建立多轴***的拉格朗日方程1. Establishing Lagrange's equation of a multi-axis system

应用链符号***建立关节空间的拉格朗日方程，考虑质点动力学***D＝{A,K,T,NT,F,B}，首先根据牛顿力学推导自由质点

的拉格朗日方程；然后，推广至受约束的质点***。 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.

保守力

相对质点惯性力

具有相同的链序，即

具有正序，质点

的合力为零。质点

的能量记为

根据广义坐标序列

与笛卡尔空间位置矢量序列{ ⁱr _l|l∈T}关系 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

式(2)应用***的能量及广义坐标建立***的方程。关节变量

与坐标矢量 ⁱr _l的关系如式(1)所示，称式(1)为关节空间与笛卡尔空间的点变换。 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.

保守力与惯性力具有相反的链序。拉格朗日***内的约束既可以是质点间的固结约束，又可以是质点***间的运动约束；刚体自身是质点***

质点能量具有可加性；刚体动能量由质心平动动能及转动动能组成。下面，就以简单运动副R/P分别建立拉格朗日方程，为后续进一步推出新的动力学理论奠定基础。 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.

给定刚体多轴***D＝{A,K,T,NT,F,B}，惯性空间记为i，

轴l的能量记为

其中平动动能为

转动动能为

引力势能为

轴l受除引力外的外部合力及合力矩分别为 ^Df _l及 ^Dτ _l；轴l的质量及质心转动惯量分别为m _l及

轴u的单位轴不变量为

环境i作用于l _I的惯性加速度记为

重力加速度

链序由i至l _I；

链序由l _I至i；且有 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】***能量[1] System energy

动力学***D能量

表达为 Kinetic system D energy

Expressed as

其中：among them:

【2】多轴***拉格朗日方程[2] Lagrange's equation of multi-axis system

由式(2)得多轴***拉格朗日方程，By the equation (2) Lagrange's equation of the multi-axis system,

式(6)为轴u的控制方程，即在轴不变量

上的力平衡方程；

是合力

在

上的分量，

是合力矩

在

上的分量。 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,

Resultant torque

in

On the weight.

2.建立Ju-Kane动力学预备方程：2. Establish the Ju-Kane kinetic equation:

基于多轴***拉格朗日方程(6)推导居―凯恩(Ju-Kane)动力学预备定理。先进行拉格朗日方程与凯恩方程的等价性证明；然后，计算能量对关节速度及坐标的偏速度，再对时间求导，最后给出Ju-Kane动力学预备定理。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】拉格朗日方程与凯恩方程的等价性证明[1] Proof of equivalence between Lagrange's equation and Kane's equation

证明：考虑刚体k平动动能对

的偏速度对时间的导数得 Proof: Consider the k-translational kinetic energy pair of rigid body

The derivative of the partial velocity with time is

考虑刚体k转动动能对

的偏速度对时间的导数得 Consider the kinetic energy of k

The derivative of the partial velocity with time is

证毕。Certificate completed.

因

与

不相关，由式(7)及多轴***拉格朗日方程(6)得 because

versus

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

动力学***D的平动动能及转动动能分别表示为The translational kinetic energy and rotational kinetic energy of the dynamic system D are expressed as

考虑式(4)及式(5)，即有Considering equations (4) and (5), we have

式(7)及式(8)是居―凯恩动力学预备定理证明的依据，即居―凯恩动力学预备定理本质上与拉格朗日法是等价的。同时，式(8)右侧包含了多轴***凯恩方程；表明拉格朗日法与凯恩法的惯性力计算是一致的，即拉格朗日法与凯恩法也是等价的。式(8)表明：在拉格朗日方程(4)中存在

重复计算的问题。 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】能量对关节速度及坐标的偏速度[2] Partial velocity of energy on joint velocity and coordinates

【2-1】若

并考虑

及

仅与闭子树 ^uL相关，由式(4)及式(5)，得 [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】若

并考虑

及

仅与闭子树 ^uL相关，由式(4)及式(5)，得 [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】求对时间的导数[3] Find the derivative of time

【3-1】若

由式(7)、式(9)及式(10)得 [3-1] If

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

【3-2】若

由式(7)、式(12)及式(13)得 [3-2] If

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

至此，已完成对时间t的求导。So far, the differentiation of time t has been completed.

【4】Ju-Kane动力学预备定理[4] Ju-Kane Dynamics Theorem

将式(11)、式(14)、式(15)及式(16)代入式(8)，Substituting Equation (11), Equation (14), Equation (15), and Equation (16) into Equation (8),

给定多轴刚体***D＝{A,K,T,NT,F,B}，惯性系记为F ^[i]，

除了重力外，作用于轴u的合外力及力矩分别记为

及

轴k的质量及质心转动惯量分别记为m _k及

轴k的重力加速度为

则轴u的Ju-Kane动力学预备方程为 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 resultant external force and moment acting on the axis u are recorded as

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 Ju-Kane dynamics equation for axis u is

式(17)具有了树链拓扑结构。k _I表示杆k质心I。因闭子树 ^uL中的广义力具有可加性；因此闭子树的节点有唯一一条至根的运动链，因此运动链 ⁱl _n可以被运动链 ^uL替换。 Equation (17) has a tree chain topology. k _I represents the centroid _{I of the} rod k. 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, so the motion chain ⁱ l _n can be replaced by the motion chain ^u L

下面，针对Ju-Kane动力学预备方程，解决式(17)右侧 ^Df _k及 ^Dτ _k的计算问题，从而建立树链刚体***Ju-Kane动力学方程。 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.建立树链刚体***Ju-Kane动力学模型3. Establish Ju-Kane dynamic model of tree chain rigid body system

给定轴链

k∈ ⁱl _n，有以下偏速度计算公式： Given shaft chain

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

对给定轴链

| ⁱl _l|≥2，有以下加速度迭代式： 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外力反向迭代3.1 External force reverse iteration

给定由环境i中施力点i _S至轴l上点l _S的双边外力

及外力矩 ⁱτ _l，它们的瞬时轴功率p _ex表示为 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

其中：

及 ⁱτ _l不受

及

控制，即

及 ⁱτ _l不依赖于

及

among them:

And ⁱ τ _{l is} not affected by

and

Control, ie

And ⁱ τ _l does not depend on

and

【1】若k∈ ⁱl _l，则有

由式(19)及式(18)得 [1] If k∈ ⁱ l _l , then

From equations (19) and (18),

即which is

式(26)中

与式(21)中

的链序不同；前者是作用力，后者是运动量，二者是对偶的，具有相反的序。 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】若k∈ ⁱl _l，则有

由式(22)及式(25)得 [2] If k ∈ ⁱ l _l , then

From equations (22) and (25),

即有That is

式(26)及式(27)表明环境作用于轴k的合外力或力矩等价于闭子树 ^kL对轴k的合外力或力矩，将式(26)及式(27)合写为 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

至此，解决了外力反向迭代的计算问题。在式(28)中，闭子树对轴k的广义力具有可加性；力的作用具有双重效应，且是反向迭代的。所谓反向迭代是指：

是需要通过链节位置矢量迭代的；

的序与前向运动学

计算的序相反。 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共轴驱动力反向迭代3.2 Coaxial driving force reverse iteration

若轴l是驱动轴，轴l的驱动力及驱动力矩分别为

及

则驱动力

及驱动力矩

产生的 功率p _ac表示为 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】由式(18)、式(19)及式(29)得[1] Obtained from formula (18), formula (19), and formula (29)

即which is

若轴u与轴

共轴，则有

记

因

与

无关，由式(30)得 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】由式(19)、式(18)及式(29)得[2] Obtained from formula (19), formula (18) and formula (29)

即which is

若轴u与

共轴，则有

记

由式(32)得 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树链刚体***Ju-Kane动力学显式模型的建立：3.3 Establishment of Ju-Kane dynamic model of tree chain rigid body system:

下面，先陈述树链刚体***Ju-Kane动力学方程，简称Ju-Kane方程；然后，给出建立步骤。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.

给定多轴刚体***D＝{A,K,T,NT,F,B}，惯性系记为F ^[i]，

除了重力外，作用于轴u的合外力及力矩在

上的分量分别记为

及

轴k的质量及质心转动惯量分别记为m _k及

轴k的重力加速度为

驱动轴u的双边驱动力及驱动力矩在

上的分量分别记为

及

环境i对轴l的力及力矩分别为

及 ⁱτ _l；则轴u树链Ju-Kane动力学方程为 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

其中：[·]表示取行或列；

及

是3×3的分块矩阵，

及

是3D矢量，q为关节空间。且有， 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

ex的能量为

p _ex为瞬时轴功率；p _ac为驱动轴的驱动力及驱动力矩产生的功率。 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.

由式(26)、式(27)、式(31)、式(33)及式(41)得式(40)。Formula (40) is obtained from formula (26), formula (27), formula (31), formula (33), and formula (41).

将偏速度计算公式式(19)，式(18)及式(20)代入Ju-Kane动力学预备方程(17)得Substituting the formula (19), (18) and (20) into the Ju-Kane kinetic equation (17)

由式(21)得From (21),

考虑式(43)，则有Considering equation (43), we have

同样，考虑式(43)，得Similarly, considering equation (43), we get

将式(43)至式(45)代入式(42)得式(34)至式(39)。Substituting equations (43) to (45) into equation (42) gives equations (34) to (39).

实施例1Example 1

给定如图3所示的通用3R机械臂，A＝(i,1:3]；应用本发明的方法建立树链Ju-Kane动力学方程，并得到广义惯性矩阵。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.

步骤1建立基于轴不变量的迭代式运动方程。 Step 1 establish an iterative motion equation based on the axis invariants.

由式(46)基于轴不变量的转动变换矩阵Rotation transformation matrix based on axis invariant by equation (46)

得Get

运动学迭代式：Kinematic iteration:

二阶张量投影式：Second-order tensor projection:

由式(48)及式(47)得From equations (48) and (47),

由式(49)，式(47)及式(55)得From equations (49), (47), and (55),

由式(50)及式(55)得From equations (50) and (55),

由式(51)、式(55)及式(57)得From equations (51), (55), and (57),

由式(52)及式(55)得From equations (52) and (55),

由式(53)及式(55)得From equations (53) and (55),

步骤2建立动力学方程。先建立第1轴的动力学方程。由式(37)得Step 2 establish a kinetic equation. First establish the kinetic equation of the first axis. From Equation (37),

由式(39)得From Equation (39)

由式(61)及式(62)得第1轴的动力学方程，From equations (61) and (62), the kinetic equation of the first axis is obtained.

建立第2轴的动力学方程。由式(37)得Establish a dynamics equation for the second axis. From Equation (37),

由式(39)得From Equation (39)

由式(64)及式(65)得第2轴的动力学方程，From equations (64) and (65), the second-axis dynamic equation is obtained.

最后，建立第3轴的动力学方程。由式(37)得Finally, the dynamic equation of the third axis is established. From Equation (37),

由式(39)得From Equation (39)

由式(67)及式(68)得第3轴的动力学方程，From equations (67) and (68), the third-axis dynamic equation is obtained.

由式(61)，式(63)及式(67)得广义质量阵。The generalized mass matrix is obtained from equations (61), (63), and (67).

由此可知，只要程式化地将***的拓扑、结构参数、质惯量等参数代入式(36)至式(40)就可以完成动力学建模。通过编程，很容易实现Ju-Kane动力学方程。因后续的树链Ju-Kane规范方程是以Ju-Kane动力学方程推导的，树链Ju-Kane动力学方程的有效性可由Ju-Kane规范型实例证明。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动力学规范型3.4 Ju-Kane Dynamic Canonical Model of Tree Chain Rigid Body System

在建立***动力学方程后，紧接着就是方程求解的问题。在动力学***仿真时，通常给定环境作用的广义力及驱动轴的广义驱动力，需要求解动力学***的加速度；这是动力学方程求解的正问题。在求解前，首先需要得到式(71)所示的规范方程。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,

其中：RHS–右手侧(Right hand side)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运动链的规范型方程3.4.1 Canonical Equations of a Motion Chain

将式(36)及式(37)中关节加速度项的前向迭代过程转化为反向求和过程，以便后续应用；显然，其中含有6种不同类型的加速度项，分别予以处理。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】给定运动链

则有 [1] Given motion chain

Then

上式的推导步骤为：The derivation steps of the above formula are:

【2】给定运动链

则有 [2] Given motion chain

Then

上式的推导步骤为：因

故得 The derivation steps of the above formula are:

Therefore

【3】给定运动链

则有 [3] Given motion chain

Then

上式可由下式而得，因

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

Therefore

【4】给定运动链

则有 [4] Given motion chain

Then

上式的推导步骤为：考虑

将式(72)代入式(75)左侧得 The derivation steps of the above formula are: consider

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

【5】给定运动链

则有 [5] Given motion chain

Then

上式的推导步骤为：考虑

将式(72)代入式(76)左侧得 The derivation steps of the above formula are: consider

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

【6】给定运动链

则有 [6] Given motion chain

Then

上式的推导步骤为：因

故有 The derivation steps of the above formula are:

Therefore

3.4.2闭子树的规范型方程3.4.2 Canonical Equations for Closed Subtrees

因闭子树 ^uL中的广义力具有可加性；因此闭子树的节点有唯一一条至根的运动链，式(73)至式(77)的运动链 ⁱl _n可以被 ^uL替换。由式(73)得 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),

由式(74)得From equation (74),

由式(75)得From equation (75)

由式(76)得From equation (76),

由式(77)得From Equation (77)

至此，已具备建立规范型的前提条件。At this point, the prerequisites for establishing a standardized model have been established.

3.5树链刚体***Ju-Kane动力学规范方程3.5 Tree Chain Rigid Body System Ju-Kane Dynamics Specification Equation

下面，建立树结构刚体***的Ju-Kane规范化动力学方程。为表达方便，首先定义Next, a Ju-Kane normalized dynamic equation of a tree-structured rigid body system is established. For convenience, first define

然后，应用式(78)至式(82)，将式(36)及式(37)表达为规范型。Then, apply equations (78) to (82) to express equations (36) and (37) as canonical.

【1】式(36)的规范型为[1] The canonical form of formula (36) is

上式的具体建立步骤为：由式(24)及式(36)得The specific establishment steps of the above formula are: from (24) and (36)

由式(52)及式(85)得From equations (52) and (85),

将式(80)代入式(85)右侧前一项得Substituting Equation (80) into Equation (85) to the right of the previous term is

将式(79)代入式(86)右侧后一项得Substituting Equation (79) into Equation (86) to the right of the next term gives

将式(87)及式(88)代入式(86)得Substituting equations (87) and (88) into equations (86) gives

对于刚体k，有

由式(35)、式(83)及式(89)得式(84)。 For rigid body k, there is

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

【2】式(37)的规范型为[2] The canonical form of formula (37) is

上式的具体建立步骤为：由式(37)得The specific establishment steps of the above formula are as follows:

将式(78)代入式右侧前一项(91)得Substituting equation (78) into the right-hand preceding term (91) gives

将式(81)代入式(91)右侧后一项得Substituting equation (81) into the right-hand side of equation (91) gives

将式(82)代入式(91)右侧中间一项得Substituting equation (82) into the right middle term of equation (91) gives

将式(92)，式(93)及式(94)代入式(92)得Substituting Equation (92), Equation (93), and Equation (94) into Equation (92) gives

对于刚体k，有

由式(35)，式(83)及式(95)得式(90)。 For rigid body k, there is

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

【3】应用式(84)及式(90)，将Ju-Kane方程重新表述为如下树链Ju-Kane规范型方程：[3] Apply equations (84) and (90) to reformulate the Ju-Kane equation as the following tree-chain Ju-Kane canonical equation:

给定多轴刚体***D＝{A,K,T,NT,F,B}，惯性系记为F ^[i]，

除了重力外，作用于轴u的合外力及力矩在

上的分量分别记为

及

轴k的质量及质心转动惯量分别记为m _k及

轴k的重力加速度为

驱动轴u的双边驱动力及驱动力矩在

上的分量分别记为

及

环境i对轴l的作用力及力矩分别为

及

则轴u的Ju-Kane动力学规范方程为 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

The Ju-Kane dynamics norm equation of axis u is

其中：

及

是3×3的分块矩阵，

及

是3D矢量。并且， among them:

and

Is a 3 × 3 block matrix,

and

Is a 3D vector. and,

式中，k _I表示杆k质心I；轴k的质量及质心转动惯量分别记为m _k及

为转动轴u的惯性矩阵；

为平动轴u的惯性矩阵；h _R为转动轴u的非惯性矩阵；h _P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在

上的分量分别记为

及

驱动轴u的双边驱动力及驱动力矩在

上的分量分别记为

及

环境i对轴l的作用力及作用力矩分别为

及 ⁱτ _l； ^ll _k为取由轴l至轴k的运动链， ^uL表示获得由轴u及其子树构成的闭子树。 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.

4.闭链刚体***的Ju-Kane动力学方程建立4. Establishment of Ju-Kane dynamic equation of closed-chain rigid body system

下面，先陈述闭链刚体***的居―凯恩(简称Ju-Kane)动力学方程；然后，给出具体建模过程。In the following, the Ju-Kane (Ju-Kane) dynamic equation of the closed-chain rigid-body system is stated first; then, the specific modeling process is given.

给定多轴刚体***D＝{A,K,T,NT,F,B}，惯性系记为F ^[i]，

除了重力外，作用于轴u的合外力及力矩在

上的分量分别记为

及

轴k的质量及质心转动惯量分别记为m _k及

轴k的重力加速度为

驱动轴u的双边驱动力及驱动力矩在

上的分量分别记为

及

环境i对轴l的作用力及作用力矩分别为

及 ⁱτ _l；轴u对轴u′的广义约束力记为

则有闭链刚体***的Ju-Kane动力学方程： 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 acting force and acting moment of the environment i on the shaft l are

And ⁱ τ _l ; the generalized binding force of axis u to axis u ′ is written as

Then there are Ju-Kane dynamic equations of closed-chain rigid body systems:

【1】轴u及轴u′的Ju-Kane动力学规范方程分别为[1] The Ju-Kane kinetic norm equations for axis u and axis u ′ are

【2】非树约束副 ^uk _u′的约束代数方程为 [2] The constraint algebraic equation of the non-tree constrained pair ^u k _{u ′} is

其中：among them:

式中：

及

是3×3的分块矩阵，

及

是3D矢量；k _I表示杆k质心I；轴k的质量及质心转动惯量分别记为m _k及

为转动轴u的惯性矩阵；

为平动轴u的惯性矩阵；h _R为转动轴u的非惯性矩阵；h _P为平动轴u的非惯性矩阵；

为平动关节角速度；

为转动关节角速度。 In the formula:

and

Is a 3 × 3 block matrix,

and

Is a 3D vector; 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 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;

Is the translational joint angular velocity;

Is the angular velocity of the turning joint.

具体建模过程如下：The specific modeling process is as follows:

非树约束副

保持约束点u _S及u′ _S一致，故有 Non-tree constrained pair

Keep the constraint points u _S and u ′ _S consistent, so

由式(114)得From equation (114),

轴u对轴u′在约束轴方向上的广义约束力

及轴u′对轴u在约束轴方向上的广义约束力

的功率分别为 Generalized constraint force of axis u to axis u ′ in the direction of the constraint axis

And the generalized binding force of axis u ′ to axis u in the direction of the constraint axis

The powers are

由式(115)及式(116)得From equations (115) and (116),

由式(115)得From equation (115),

δ表示增量；δ represents increment;

由式(18)及式(118)得From (18) and (118),

故有Therefore

由式(110)及式(122)得式(105)。由式(19)及式(119)得From formula (110) and formula (122), formula (105) is obtained. From (19) and (119),

由式(111)及式(123)得式(106)。由式(19)及式(120)得From formula (111) and formula (123), formula (106) is obtained. From equations (19) and (120),

由式(112)及式(124)得式(107)。由式(19)及式(121)得From formula (112) and formula (124), formula (107) is obtained. From equations (19) and (121),

由式(113)及式(125)得(108)。由式(18)，式(116)及式(110)得From formula (113) and formula (125), (108) is obtained. From formula (18), formula (116) and formula (110),

广义约束力

及

是矢量，由式(126)及式(127)得式(109)。由此可知，偏速度主要应用于力的反向迭代。广义约束力

及

视为外力。 Generalized binding force

and

Is a vector, and Equation (109) is obtained from Equation (126) and Equation (127). It can be seen that the bias velocity is mainly used in the reverse iteration of the force. Generalized binding force

and

Considered external force.

根据轴u的Ju-Kane动力学规范方程得式(103)及式(104)。Equations (103) and (104) are obtained according to the Ju-Kane kinetic norm equation of the axis u.

以关节空间自然轴链为基础的Ju-Kane闭链刚体动力学克服了笛卡尔坐标轴链空间的局限：Ju-Kane closed-chain rigid body dynamics based on the natural axis chain in joint space overcomes the limitations of Cartesian axis chain space:

【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副 ^uk _u′∈P约束不能表达

及

或

及

的情形，即不能表达齿条与齿轮、蜗轮与蜗杆等约束。而本申请的非树约束副 ^uk _u′的约束代数方程式(105)至式(108)可表达任一种约束类形，并且物理内涵明晰； [1] In Newton Euler dynamics based on Cartesian coordinate axis chains, non-tree motion pair ^u k _{u ′} ∈ P constraint cannot be expressed

and

or

and

In this case, constraints such as rack and pinion, worm gear and worm cannot be expressed. The constraint algebraic equations (105) to (108) of the non-tree constrained pair ^u k _{u ′} of the present application can express any kind of constraint, and the physical connotation is clear;

【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是6D的；而式(105)至式(108)表示是3D非树运动副代数约束方程，从而降低了***方程求解的复杂度；[2] In the Newton Euler dynamics based on the Cartesian coordinate axis chain, the non-tree motion auxiliary algebraic constraint equation is 6D; and equations (105) to (108) represent 3D non-tree motion auxiliary algebraic constraint equations, so that Reduce the complexity of solving system equations;

【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是关于6D矢量空间绝对加速度的，是关于关节坐标、关节速度的迭代式，具有累积误差；而式(105)至式(108)是关于关节加速度的，保证了约束方程的准确性。[3] In the Newton Euler dynamics based on the Cartesian coordinate axis chain, the algebraic constraint equation of non-tree motion pair is about the 6D vector space absolute acceleration, iterative formula about joint coordinates and joint speed, with cumulative error; (105) to (108) are about joint acceleration, which guarantees the accuracy of the constraint equation.

5.基于轴不变量的约束力求解5. Solving constraint forces based on axis invariants

对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为

显然，有 For the motion axis u without power loss, record its constraint force and constraint torque vector as

Obviously, there is

由式(96)及式(139)计算得

式(128)表示运动轴矢量与运动轴约束力具有自然正交补的关系。 Calculated from equations (96) and (139)

Equation (128) shows that the motion axis vector and the constraint force of the motion axis have a natural orthogonal complement.

若

及

为运动副

的两个正交约束轴，且约束轴与运动轴正交，即 If

and

For sports pair

Two orthogonal constraint axes, and the constraint axis is orthogonal to the motion axis, ie

记

为约束轴轴矢量，

替换式(96)中

重新计算得 Remember

To constrain the axis vector,

Replacement (96)

Recalculated

其中：among them:

在完成前向动力学正解后，根据已计算的关节加速度

由式(130)可以得到关节约束力大小

约束力矩大小

当

时，由式(130)得

且

式(130)中同一时刻具有相同的运动状态及内外力。仅在运动轴向上出现力及力矩的平衡；而在约束轴向，动力学方程不满足，即力与力矩不一定平衡。 After completing the forward dynamics solution, based on the calculated joint acceleration

The size of the joint restraint force can be obtained from equation (130)

Constraint moment

when

When, get from formula (130)

And

Equation (130) has the same motion state and internal and external forces at the same time. Equilibrium of force and moment occurs only in the axial direction of movement; while in the constraint axis, the dynamic equation is not satisfied, that is, the force and moment are not necessarily balanced.

由式(130)可以得到关节约束力大小

及

约束力矩大小

及

若记运动轴径向力矢量

及力矩矢量

则有 The size of the joint restraint force can be obtained from equation (130)

and

Constraint moment

and

If you remember the motion axis radial force vector

And torque vector

Then

若记运动轴径向力大小为

及力矩大小为

由式(133)得 If the radial force of the moving axis is

And the moment is

From Equation (133),

至此，完成了轴径向约束广义力的计算。So far, the calculation of the generalized force of axial radial restraint is completed.

树链刚体***对应的关节加速度序列记

可根据下述步骤计算： Sequence of joint acceleration corresponding to tree chain rigid body system

It can be calculated according to the following steps:

将根据运动轴类型及自然参考轴表达的刚体运动链广义惯性矩阵称为轴链刚体广义惯性矩阵，简称轴链广义惯性矩阵。The generalized inertia matrix of a rigid body motion chain expressed according to the type of the motion axis and the natural reference axis is referred to as the generalized inertia matrix of the rigid body of the axial chain, and is referred to as the generalized inertia matrix of the axial chain for short.

定义正交补矩阵

及对应的叉乘矩阵

Define orthogonal complement matrix

And the corresponding cross product matrix

给定多轴刚体***D＝{A,K,T,NT,F,B}，

将***中各轴动力学方程(96)按行排列；将重排后的轴驱动广义力及不可测的环境作用力记为f ^C，可测的环境广义作用力记为f ⁱ；将***对应的关节加速度序列记为

将重排后的

记为h；考虑式(135)；则该***动力学方程为 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 (135); then the system dynamics equation is

由式(136)得From equation (136),

其中，among them,

由式(136)得From equation (136),

6.广义内摩擦力及粘滞力计算6. Calculation of generalized internal friction and viscous forces

在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小

及约束力矩大小

如图3、图4所示，记运动轴u的内摩擦力大小及内摩擦力矩大小分别为

及

运动轴u的粘滞力及粘滞力矩大小分别为

及

After the calculation of the radial general constraint force of the axis is completed, the radial constraint force of the motion axis u is obtained.

And restraining moment

As shown in Figures 3 and 4, the internal frictional force and the internal frictional moment of the movement axis u are respectively

and

The viscous force and viscous moment of the motion axis u are

and

故有Therefore

其中： _sk ^[u]─运动轴u的内摩擦系数， _ck ^[u]─运动轴u的粘滞系数；sign()表示取正或负符号。 Wherein: _s k ^[u] ─ the coefficient of friction u of the movement of the shaft, _c k ^[u] ─ movement of the shaft of the viscosity coefficient u; Sign () takes a positive or negative sign represents.

记广义内摩擦力及粘滞力的合力及合力矩分别为

由式(140)及式(141)得 The general and internal moments of generalized internal friction and viscous forces are

From equations (140) and (141),

运动轴的广义内摩擦力及粘滞力是运动轴的内力，因为它们仅存在于运动轴向上，与轴径向约束力总是正交的。当运动轴轴向动态作用力平衡时，无论广义内摩擦力及粘滞力是否存在或大小如何，都不影响动力学***的运动状态；故而，不影响运动轴的径向约束力。因此，由式(130)至式(134)计算 运动轴u的径向约束力大小

及约束力矩大小

时，可以不考虑运动轴的广义内摩擦力及粘滞力。 The generalized internal frictional force and viscous force of the moving shaft are the internal forces of the moving shaft, because they exist only in the moving axial direction and are always orthogonal to the radial restraining force of the shaft. When the axial dynamic forces of the moving shaft are balanced, no matter the existence or magnitude of the generalized internal friction and viscous forces, it does not affect the dynamic state of the dynamic system; therefore, it does not affect the radial restraining force of the moving shaft. Therefore, the radial restraining force of the motion axis u is calculated from equations (130) to (134).

And restraining moment

In this case, the generalized internal friction and viscous forces of the moving axis can be ignored.

7.建立闭链刚体非理想约束***的Ju-Kane动力学显式模型7. Establishing Ju-Kane dynamic explicit model of closed-chain rigid body non-ideal constraint system

设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为

闭链刚体***的Ju-Kane动力学方程建立后，计算关节加速度

后，应用式(129)至式(134)计算径向约束力大小

及

约束力矩大小

及

再建立如下闭链刚体非理想约束***的Ju-Kane动力学方程： Let the generalized internal friction and viscous combined force and moment of the motion axis u be

Calculate joint acceleration after the Ju-Kane dynamic equation of a closed-chain rigid body system

Then, apply formulas (129) to (134) to calculate the radial restraint

and

Constraint moment

and

Then establish the following Ju-Kane dynamic equation of the closed-chain rigid body non-ideal constraint system:

【1】轴u及轴u′的Ju-Kane动力学规范方程分别为[1] The Ju-Kane kinetic norm equations for axis u and axis u ′ are

【2】非树约束副 ^uk _u′的约束代数方程为 [2] The constraint algebraic equation of the non-tree constrained pair ^u k _{u ′} is

其它，参见式(103)至式(113)、式(97)至式(102)。For other details, see formulas (103) to (113) and formulas (97) to (102).

建立过程为：The establishment process is:

运动轴u的内摩擦及粘滞合力

及合力矩

是运动轴u的外力，故有式(143)；运动轴u′的内摩擦及粘滞合力

及合力矩

是运动轴u′的外力，故有式(144)。其它过程与闭链刚体***的Ju-Kane动力学方程建模步骤相同。 Internal friction and viscous force of the moving shaft u

Combined torque

Is the external force of the motion axis u, so there is formula (143); the internal friction and viscous force of the motion axis u ′

Combined torque

Is the external force of the motion axis u ′, so there is equation (144). The other processes are the same as the Ju-Kane dynamic equation modeling steps of the closed-chain rigid body system.

Claims (7)

1. 一种基于轴不变量的非理想关节机器人动力学建模与解算方法，其特征是，A non-ideal articulated robot dynamics modeling and solving method based on axis invariants is characterized by:

   给定多轴刚体***D＝{A,K,T,NT,F,B}，惯性系记为F ^[i]，

   

   除了重力外，作用于轴u的合外力及力矩在

   

   上的分量分别记为

   

   及

   

   轴k的质量及质心转动惯量分别记为m _k及

   

   轴k的重力加速度为

   

   驱动轴u的双边驱动力及驱动力矩在

   

   上的分量分别记为

   

   及

   

   环境i对轴l的作用力及作用力矩分别为

   

   及 ⁱτ _l；轴u对轴u′的广义约束力记为

   

   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 acting force and acting moment of the environment i on the shaft l are

   

   And ⁱ τ _l ; the generalized binding force of axis u to axis u ′ is written as

   

   设运动轴u的广义内摩擦及粘滞的合力及合力矩分别为

   

   根据建立的闭链刚体***的Ju-Kane动力学方程，计算关节加速度

   

   后，计算径向约束力大小

   

   及

   

   约束力矩大小

   

   及

   

   再建立如下闭链刚体非理想约束***的Ju-Kane动力学方程： Let the generalized internal friction and viscous combined force and moment of the motion axis u be

   

   Calculate joint acceleration based on the Ju-Kane dynamic equation of the closed-chain rigid body

   

   After calculating the radial binding force

   

   and

   

   Constraint moment

   

   and

   

   Then establish the following Ju-Kane dynamic equation of the closed-chain rigid body non-ideal constraint system:

   【1】轴u及轴u′的Ju-Kane动力学规范方程分别为[1] The Ju-Kane kinetic norm equations for axis u and axis u ′ are

   

   

   

   

   式中：

   

   及

   

   是3×3的分块矩阵，

   

   及

   

   是3D矢量；

   

   为转动轴u的惯性矩阵；

   

   为平动轴u的惯性矩阵；h _R为转动轴u的非惯性矩阵；h _P为平动轴u的非惯性矩阵； In the formula:

   

   and

   

   Is a 3 × 3 block matrix,

   

   and

   

   Is a 3D vector;

   

   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;

   【2】非树约束副 ^uk _u′的约束代数方程为 [2] The constraint algebraic equation of the non-tree constrained pair ^u k _{u ′} is

   

   

   

   

   

   

   

   
2. 根据权利要求1所述的基于轴不变量的非理想关节机器人动力学建模与解算方法，其特征是，闭链刚体***的Ju-Kane动力学方程：The non-ideal articulated robot dynamics modeling and solving method based on axis invariant according to claim 1, characterized in that the Ju-Kane dynamic equation of the closed-chain rigid body system:

   【1】轴u及轴u′的Ju-Kane动力学规范方程分别为[1] The Ju-Kane kinetic norm equations for axis u and axis u ′ are

   

   

   

   

   其中：

   

   及

   

   是3×3的分块矩阵，

   

   及

   

   是3D矢量；k _I表示杆k质心I；轴k的质量及质心转动惯量分别记为m _k及

   

   为转动轴u的惯性矩阵；

   

   为平动轴u的惯性矩阵；h _R为转动轴u的非惯性力；h _P为平动轴u的非惯性力； among them:

   

   and

   

   Is a 3 × 3 block matrix,

   

   and

   

   Is a 3D vector; 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 inertial matrix of translation axis u; h _R is the non-inertia force of rotation axis u; h _P is the non-inertia force of translation axis u

   【2】非树约束副 ^uk _u′的约束代数方程为 [2] The constraint algebraic equation of the non-tree constrained pair ^u k _{u ′} is

   

   

   

   

   

   

   

   

   其中：among them:

   

   

   

   

   

   

   

   

   

   

   式中：

   

   及

   

   是3×3的分块矩阵，

   

   及

   

   是3D矢量；k _I表示杆k质心I；轴k的质量及质心转动惯量分别记为m _k及

   

   为转动轴u的惯性矩阵；

   

   为平动轴u的惯性矩阵；h _R为转动轴u的非惯性矩阵；h _P为平动轴u的非惯性矩阵；

   

   为平动关节角速度；

   

   为转动关节角速度。 In the formula:

   

   and

   

   Is a 3 × 3 block matrix,

   

   and

   

   Is a 3D vector; 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 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;

   

   Is the translational joint angular velocity;

   

   Is the angular velocity of the turning joint.
3. 根据权利要求2所述的基于轴不变量的非理想关节机器人动力学建模与解算方法，其特征是，The method for modeling and solving a non-ideal articulated robot based on an axis invariant according to claim 2, wherein:

   应用式(129)至式(134)计算径向约束力大小

   

   及

   

   约束力矩大小

   

   及

   

   对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为

   

   则有 Apply equations (129) to (134) to calculate the radial restraining force

   

   and

   

   Constraint moment

   

   and

   

   For the motion axis u without power loss, record its constraint force and constraint torque vector as

   

   Then

   

   

   上式表示运动轴矢量与运动轴约束力具有自然正交补的关系；The above formula indicates that the motion axis vector and the constraint force of the motion axis have a natural orthogonal complement relationship;

   若

   

   及

   

   为运动副

   

   的两个正交约束轴，且约束轴与运动轴正交，即 If

   

   and

   

   For sports pair

   

   Two orthogonal constraint axes, and the constraint axis is orthogonal to the motion axis, ie

   

   

   记

   

   为约束轴轴矢量，有 Remember

   

   For the constraint axis axis vector, there is

   

   

   其中：among them:

   

   

   

   

   由式(130)得到关节约束力大小

   

   及

   

   约束力矩大小

   

   及

   

   若记运动轴径向力矢量

   

   及力矩矢量

   

   则有 Get the size of the joint restraint force by (130)

   

   and

   

   Constraint moment

   

   and

   

   If you remember the motion axis radial force vector

   

   And torque vector

   

   Then

   

   

   若记运动轴径向力大小为

   

   及力矩大小为

   

   由式(133)得 If the radial force of the moving axis is

   

   And the moment is

   

   From Equation (133),

   

   

   至此，完成了轴径向约束广义力的计算。So far, the calculation of the generalized force of axial radial restraint is completed.
4. 根据权利要求3所述的基于轴不变量的非理想关节机器人动力学建模与解算方法，其特征是，The non-ideal articulated robot dynamics modeling and solving method based on axis invariant according to claim 3, characterized in that:

   由式(130)至式(134)计算运动轴u的径向约束力大小

   

   及约束力矩大小

   

   时，不考虑运动轴的广义内摩擦力及粘滞力。 Calculate the radial restraining force of the motion axis u from equations (130) to (134)

   

   And restraining moment

   

   In this case, the generalized internal friction and viscous forces of the moving axis are not considered.
5. 根据权利要求3所述的基于轴不变量的非理想关节机器人动力学建模与解算方法，其特征是，The non-ideal articulated robot dynamics modeling and solving method based on axis invariant according to claim 3, characterized in that:

   考虑广义内摩擦力及粘滞力的基于轴不变量的约束力求解步骤为：Considering the generalized internal friction and viscous forces, the solution steps of the constraint force based on the axis invariant are:

   在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小

   

   及约束力矩大小

   

   记运动轴u的内摩擦力大小及内摩擦力矩大小分别为

   

   及

   

   运动轴u的粘滞力及粘滞力矩大小分别为

   

   及

   

   则 After the calculation of the radial general constraint force of the axis is completed, the radial constraint force of the motion axis u is obtained.

   

   And restraining moment

   

   The magnitude of the internal friction force and the internal friction moment of the motion axis u are respectively

   

   and

   

   The viscous force and viscous moment of the motion axis u are

   

   and

   

   then

   

   

   

   

   其中： _sk ^[u]─运动轴u的内摩擦系数， _ck ^[u]─运动轴u的粘滞系数；sign()表示取正或负符号； Wherein: _s k ^[u] coefficient of friction u of the movement of the shaft ─, _c k ^[u] ─ movement of the shaft of the viscosity coefficient u; Sign () indicates the sign of positive or negative;

   记广义内摩擦力及粘滞力的合力及合力矩分别为

   

   由式(140)及式(141)得 The general and internal moments of generalized internal friction and viscous forces are

   

   From equations (140) and (141),

   

   
6. 根据权利要求1所述的基于轴不变量的非理想关节机器人动力学建模与解算方法，其特征是，The method for modeling and solving a non-ideal articulated robot based on an axis invariant according to claim 1, wherein:

   闭链刚体***的Ju-Kane动力学方程根据树链Ju-Kane规范型方程建立。The Ju-Kane dynamic equation of the closed-chain rigid body system is established according to the tree-chain Ju-Kane canonical equation.
7. 根据权利要求6所述的基于轴不变量的非理想关节机器人动力学建模与解算方法，其特征是，树链Ju-Kane规范型方程The non-ideal articulated robot dynamics modeling and solving method based on axis invariant according to claim 6, wherein the tree chain Ju-Kane canonical equation

   

   

   其中：

   

   及

   

   是3×3的分块矩阵，

   

   及

   

   是3D矢量；

   

   为轴u的合外力在

   

   上的分量，

   

   为轴u的合力矩在

   

   上的分量； among them:

   

   and

   

   Is a 3 × 3 block matrix,

   

   and

   

   Is a 3D vector;

   

   The resulting external force for axis u is

   

   On the weight,

   

   The resultant moment of the shaft u is

   

   Weight

   并且，and,

   

   

   

   

   

   

   

   

   

   

   

   

   式中，k _I表示杆k质心I；轴k的质量及质心转动惯量分别记为m _k及

   

   为转动轴u的惯性矩阵；

   

   为平动轴u的惯性矩阵；h _R为转动轴u的非惯性矩阵；h _P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在

   

   上的分量分别记为

   

   及

   

   作用于轴u的合外力及力矩在

   

   上的分量分别记为

   

   及

   

   驱动轴u的双边驱动力及驱动力矩在

   

   上的分量分别记为

   

   及

   

   环境i对轴l的作用力及作用力矩分别为

   

   及 ⁱτ _l； ^ll _k为取由轴l至轴k的运动链， ^uL表示获得由轴u及其子树构成的闭子树。 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 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