WO2020034399A1 - 基于轴不变量的闭链机器人动力学建模与解算方法 - Google Patents
基于轴不变量的闭链机器人动力学建模与解算方法 Download PDFInfo
- Publication number
- WO2020034399A1 WO2020034399A1 PCT/CN2018/112583 CN2018112583W WO2020034399A1 WO 2020034399 A1 WO2020034399 A1 WO 2020034399A1 CN 2018112583 W CN2018112583 W CN 2018112583W WO 2020034399 A1 WO2020034399 A1 WO 2020034399A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- axis
- force
- equation
- moment
- constraint
- Prior art date
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2111/00—Details relating to CAD techniques
- G06F2111/04—Constraint-based CAD
Definitions
- the invention relates to a dynamic modeling and calculation method for a closed-chain robot, and belongs to the field of robot technology.
- 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.
- 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.
- the establishment of the Kane equation directly expresses the dynamic equation through the system's deflection velocity, velocity and acceleration.
- the Kane dynamics method greatly reduces the difficulty of system modeling because it omits the expression of system energy and the derivation of time.
- 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.
- 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.
- the technical problem to be solved by the present invention is to provide a dynamic modeling and calculation method of a closed-chain robot based on an axis invariant.
- the present invention adopts the following technical solutions:
- a dynamic modeling and solving method for closed-chain robots based on axis invariants which is characterized by:
- the inertial system is denoted by F [i] , A is the axis sequence, F is the rod reference system sequence, B is the rod body sequence, K is the motion pair type sequence, and NT is the sequence that constrains the axis, ie, non-tree; in addition to gravity, the combined external force and moment acting on the axis u in 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 Then the Ju-Kane dynamic equation of the closed-chain rigid body system is:
- 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.
- binding force solution steps based on the invariant of the axis are:
- 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
- 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 i ⁇ 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.
- 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;
- 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.
- Figures 3 and 4 are schematic diagrams of internal friction and viscous forces of a moving shaft.
- 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.
- 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.
- 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.
- 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.
- 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.
- [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.
- the axis invariant 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:
- 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.
- optical measurement equipment such as laser trackers can be applied to achieve accurate measurement of invariants of fixed axes.
- the cylindrical pair can be applied to simplify the MAS kinematics and dynamics analysis.
- Invariant A quantity that does not depend on a set of coordinate systems for measurement is called an invariant.
- 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;
- 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.
- the axis vector Is a specific free spiral.
- joint space The space represented by the joint natural coordinates q l is called joint 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.
- 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.
- 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.
- 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, which is referred to as the natural axis space. It is a 3D space with 1 degree of freedom.
- 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.
- the Span tree 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 motion chain is identified by a partial order set ().
- 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;
- 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 execution process execution If Then execute Otherwise, end.
- the computational complexity of l l k is O (
- 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
- ⁇ 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] .
- 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.
- 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.
- 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.
- 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.
- rotation vector / angle vector I is a free vector, that is, the vector can be freely translated
- the angular position that is, the joint angle and joint variables, are scalars
- T means transpose of ⁇ , which means transpose the collection, and do not perform transpose on the members; for example:
- 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.
- i l j represents a kinematic chain from i to j
- l l k is a kinematic chain from axis l to k
- n represents the Cartesian Cartesian system, then Is a Cartesian axis chain; if n represents a natural reference axis, then For natural shaft chains.
- 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 i r l is shown in equation (1), and equation (1) is called the point transformation of joint space and Cartesian space.
- 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.
- 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.
- 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.
- the Ju-Kane dynamics preliminary theorem is derived.
- the translational kinetic energy and rotational kinetic energy of the dynamic system D are expressed as
- 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.
- 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.
- Equation (11), Equation (14), Equation (15), and Equation (16) were substituted for Equation (11), Equation (14), Equation (15), and Equation (16) into Equation (8),
- 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 i l n can be replaced by the motion chain u L
- 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
- 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.
- [ ⁇ ] means taking rows or columns; and Is a 3 ⁇ 3 block matrix, and Is a 3D vector, and q is the joint space.
- 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).
- 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.
- Step 2 establish a kinetic equation.
- First establish the kinetic equation of the first axis. From Equation (37),
- the generalized mass matrix is obtained from equations (61), (63), and (67).
- 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.
- Equation (80) Substituting Equation (80) into Equation (85) to the right of the previous term is
- Equation (79) Substituting Equation (79) into Equation (86) to the right of the next term gives
- Equation (84) is obtained from equation (35), equation (83), and equation (89).
- Equation (84) is obtained from equation (35), equation (83), and equation (89).
- Equation (92), Equation (93), and Equation (94) are substituted into Equation (92).
- 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 i ⁇ 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.
- 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.
- Non-tree constrained pair Keep the constraint points u S and u ′ S consistent, so
- ⁇ represents increment
- 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.
- Equations (103) and (104) are obtained according to the Ju-Kane kinetic norm equation of the axis u.
- Equation (128) shows that the motion axis vector and the constraint force of the motion axis have a natural orthogonal complement.
- 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.
- 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.
- 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.
- 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.
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Computation (AREA)
- Computer Hardware Design (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
一种基于轴不变量的闭链机器人动力学与解算方法，建立了闭链刚体系统的Ju-Kane动力学方程，以关节空间自然轴链为基础的Ju-Kane闭链刚体动力学克服了笛卡尔坐标轴链空间的局限：在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副约束不能表达如齿条与齿轮、蜗轮与蜗杆等约束。所述非树约束副的约束代数方程式可表达任一种约束类形，并且物理内涵明晰；降低了系统方程求解的复杂度；保证了约束方程的准确性。
Description
本发明涉及一种闭链机器人动力学建模与解算方法，属于机器人技术领域。
拉格朗日在研究月球天平动问题时提出了拉格朗日方法，是以广义坐标表达动力学方程的基本方法；同时，也是描述量子场论的基本方法。应用拉格朗日法建立动力学方程已是一个烦琐的过程，尽管拉格朗日方程依据系统能量的不变性推导系统的动力学方程，具有理论分析上的优势；但是在工程应用中，随着系统自由度的增加，方程推导的复杂性剧增，难以得到普遍应用。凯恩方程建立过程与拉格朗日方程相比，通过系统的偏速度、速度及加速度直接表达动力学方程。故凯恩动力学方法与拉格朗日方法相比，由于省去了系统能量的表达及对时间的求导过程，极大地降低了系统建模的难度。然而，对于高自由度的系统，凯恩动力学建模方法也是难以适用。
拉格朗日方程及凯恩方程极大地推动了多体动力学的研究，以空间算子代数为基础的动力学由于应用了迭代式的过程，计算速度及精度都有了一定程度的提高。这些动力学方法无论是运动学过程还是动力学过程都需要在体空间、体子空间、系统空间及系统子空间中进行复杂的变换，建模过程及模型表达非常复杂，难以满足高自由度系统建模与控制的需求，因此，需要建立动力学模型的简洁表达式；既要保证建模的准确性，又要保证建模的实时性。没有简洁的动力学表达式，就难以保证高自由度系统动力学工程实现的可靠性与准确性。同时，传统非结构化运动学及动力学符号通过注释约定符号内涵，无法被计算机理解，导致计算机不能自主地建立及分析运动学及动力学模型。
发明内容
本发明所要解决的技术问题是提供一种基于轴不变量的闭链机器人动力学建模与解算方法。
为解决上述技术问题，本发明采用以下技术方案：
一种基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F
[i]，
A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树；除了重力外，作用于轴u的合外力及力矩在
上的分量分别记为
及
轴k的质量及质心转动惯量分别记为m
k及
轴k的重力加速度为
驱动轴u的双边驱动力及驱动力矩在
上的分量分别记为
及
环境i对轴l的作用力及作用力矩分别为
及
轴u对轴u′的广义约束力记为
则闭链刚体系统的Ju-Kane动力学方程为：
【1】轴u及轴u′的Ju-Kane动力学规范方程为
其中：
及
是3×3的分块矩阵，
及
是3D矢量；
【2】非树约束副
uk
u′的约束代数方程为
其中：
式中：
及
是3×3的分块矩阵，
及
是3D矢量；k
I表示杆k质心I；轴k的质量及质心转动惯量分别记为m
k及
为转动轴u的惯性矩阵；
为平动轴u的惯性矩阵；h
R为转动轴u的非惯性矩阵；h
P为平动轴u的非惯性矩阵；
为平动关节角速度；
为转动关节角速度。
基于轴不变量的约束力求解步骤为：
对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为
则有
上式表示运动轴矢量与运动轴约束力具有自然正交补的关系；
若
及
为运动副
的两个正交约束轴，且约束轴与运动轴正交，即
记
为约束轴轴矢量，得
其中：
根据关节加速度
由式(130)得到关节约束力大小
约束力矩大小
当
时，由式(130)得
且
式(130)中同一时刻具有相同的运动状态及内外力；仅在运动轴向上出现力及力矩的平衡；而在约束轴向，动力学方程不满足，即力与力矩不一定平衡；
由式(130)可以得到关节约束力大小
及
约束力矩大小
及
若记运动轴径向力矢量
及力矩矢量
则有
若记运动轴径向力大小为
及力矩大小为
由式(133)得
至此，完成了轴径向约束广义力的计算。
由式(130)至式(134)计算运动轴u的径向约束力大小
及约束力矩大小
时，不考虑运动轴的广义内摩擦力及粘滞力。
考虑广义内摩擦力及粘滞力的基于轴不变量的约束力求解步骤为：
在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小
及约束力矩大小
记运动轴u的内摩擦力大小及内摩擦力矩大小分别为
及
运动轴u的粘滞力及粘滞力矩大小分别为
及
则
其中：
sk
[u]─运动轴u的内摩擦系数，
ck
[u]─运动轴u的粘滞系数；sign()表示取正或负符号；
记广义内摩擦力及粘滞力的合力及合力矩分别为
由式(140)及式(141)得
其中：
sk
[u]─运动轴u的内摩擦系数，
ck
[u]─运动轴u的粘滞系数；sign()表示取正或负符号；
为转动关节速度；
为平动关节速度。
树链Ju-Kane规范型方程
其中：
及
是3×3的分块矩阵，
及
是3D矢量；
为轴u的合外力在
上的分量，
为轴u的合力矩在
上的分量；
为关节坐标；
并且，
式中，k
I表示杆k质心I；轴k的质量及质心转动惯量分别记为m
k及
为转动轴u的惯性矩阵；
为平动轴u的惯性矩阵；h
R为转动轴u的非惯性矩阵；h
P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在
上的分量分别记为
及
驱动 轴u的双边驱动力及驱动力矩在
上的分量分别记为
及
环境i对轴l的作用力及作用力矩分别为
及
iτ
l；
ll
k为取由轴l至轴k的运动链，
uL表示获得由轴u及其子树构成的闭子树。
本发明所达到的有益效果：
对于理想约束系统，建立了闭链刚体非理想约束系统的Ju-Kane动力学方程。
【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副
uk
u′∈P约束不能表达齿条与齿轮、蜗轮与蜗杆等约束。而本申请建立的非树约束副
uk
u′的约束代数方程可表达任一种约束类形，并且物理内涵明晰；
【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是6D的；而本申请建立的非树约束副的约束代数方程表示是3D非树运动副代数约束方程，从而降低了系统方程求解的复杂度；
【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是关于6D矢量空间绝对加速度的，是关于关节坐标、关节速度的迭代式，具有累积误差；而本申请建立的非树约束副的约束代数方程是关于关节加速度的，保证了约束方程的准确性。
图1自然坐标系与轴链；
图2固定轴不变量；
图3、图4为运动轴的内摩擦力及粘滞力示意图。
下面对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
闭链刚体系统具有非常广泛的应用；比如，CE3巡视器的摇臂移动系统是具有差速器的闭链，重载机械臂通常是具有四连杆的闭链系统。同时，实际的运动轴通常包含内摩擦力及粘滞力。因此研究闭链刚体系统的Ju-Kane动力学建模非常必要。
定义1自然坐标轴：称与运动轴或测量轴共轴的，具有固定原点的单位参考轴为自然坐标轴，亦称为自然参考轴。
定义2自然坐标系：如图1所示，若多轴系统D处于零位，所有笛卡尔体坐标系方向一致，且体坐标系原点位于运动轴的轴线上，则该坐标系统为自然坐标系统，简称自然坐标系。
自然坐标系优点在于：(1)坐标系统易确定；(2)零位时的关节变量为零；(3)零位时的系统姿态一致；(4)不易引入测量累积误差。
由定义2可知，在系统处于零位时，所有杆件的自然坐标系与底座或世界系的方向一致。系统处 于零位即
时，自然坐标系
绕轴矢量
转动角度
将
转至F
[l]；
在
下的坐标矢量与
在F
[l]下的坐标矢量
恒等，即有
由上式知，
或
不依赖于相邻的坐标系
及F
[l]；故称
或
为轴不变量。在不强调不变性时，可以称之为坐标轴矢量(简称轴矢量)。
或
表征的是体
与体l共有的参考单位坐标矢量，与参考点
及O
l无关。体
与体l即为杆件或轴。
轴不变量与坐标轴具有本质区别：
(1)坐标轴是具有零位及单位刻度的参考方向，可以描述沿该方向平动的位置，但不能完整描述绕该方向的转动角度，因为坐标轴自身不具有径向参考方向，即不存在表征转动的零位。在实际应用时，需要补充该轴的径向参考。例如：在笛卡尔系F
[l]中，绕lx转动，需以ly或lz为参考零位。坐标轴自身是1D的，3个正交的1D参考轴构成3D的笛卡尔标架。
(2)轴不变量是3D的空间单位参考轴，其自身就是一个标架。其自身具有径向参考轴，即参考零位。空间坐标轴及其自身的径向参考轴可以确定笛卡尔标架。空间坐标轴可以反映运动轴及测量轴的三个基本参考属性。
已有文献将无链指标的轴矢量记为
并称之为欧拉轴(Euler Axis)，相应的关节角称为欧拉角(Euler Angle)。本申请之所以不再沿用欧拉轴，而称之为轴不变量，是因为轴不变量具有以下属性：
【1】给定旋转变换阵
因其是实矩阵，其模是单位的，故其有一个实特征值λ
1及两个互为共轭的复特征值λ
2＝e
iφ及λ
3＝e
-iφ；其中：i为纯虚数。因此，|λ
1|·||λ
2||·||λ
3||＝1，得λ
1＝1。轴矢量
是实特征值λ
1＝1对应的特征矢量，是不变量；
【2】是3D参考轴，不仅具有轴向参考方向，而且具有径向参考零位，将在3.3.1节予以阐述。
【3】在自然坐标系下：
即轴不变量
是非常特殊的矢量，它对时间的导数也具有不变性，且有非常优良的数学操作性能；
对轴不变量而言，其绝对导数就是其相对导数。因轴不变量是具有不变性的自然参考轴，故其绝对导数恒为零矢量。因此，轴不变量具有对时间微分的不变性。有：
【4】在自然坐标系统中，通过轴矢量
及关节变量
可以直接描述旋转坐标阵
没有必要为除根之外的杆件建立各自的体系。同时，以唯一需要定义的根坐标系为参考，可以提高系统结构参数的测量精度；
【5】应用轴矢量
的优良操作，将建立包含拓扑结构、坐标系、极性、结构参量及力学参量的完全参数化的统一的多轴系统运动学及动力学模型。
因基矢量e
l是与F
[l]固结的任一矢量，基矢量
是与
固结的任一矢量，又
是F
[l]及
共有的单位矢量，故
是F
[l]及
共有的基矢量。因此，轴不变量
是F
[l]及
共有的参考基。轴不变量是参数化的自然坐标基，是多轴系统的基元。固定轴不变量的平动与转动与其固结的坐标系的平动与转动等价。
在系统处于零位时，以自然坐标系为参考，测量得到坐标轴矢量
在运动副
运动时，轴矢量
是不变量；轴矢量
及关节变量
唯一确定运动副
的转动关系。
因此，应用自然坐标系统，当系统处于零位时，只需确定一个公共的参考系，而不必为系统中每一杆件确定各自的体坐标系，因为它们由轴不变量及自然坐标唯一确定。当进行系统分析时，除底座系外，与杆件固结的其它自然坐标系只发生在概念上，而与实际的测量无关。自然坐标系统对于多轴系统(MAS)理论分析及工程作用在于：
(1)系统的结构参数测量需要以统一的参考系测量；否则，不仅工程测量过程烦琐，而且引入不同的体系会引入更大的测量误差。
(2)应用自然坐标系统，除根杆件外，其它杆件的自然坐标系统由结构参量及关节变量自然确定，有助于MAS系统的运动学与动力学分析。
(3)在工程上，可以应用激光跟踪仪等光学测量设备，实现对固定轴不变量的精确测量。
(4)由于运动副R及P、螺旋副H、接触副O是圆柱副C的特例，可以应用圆柱副简化MAS运动学及动力学分析。
定义3不变量：称不依赖于一组坐标系进行度量的量为不变量。
定义4转动坐标矢量：绕坐标轴矢量
转动到角位置
的坐标矢量
为
定义5平动坐标矢量：沿坐标轴矢量
平动到线位置
的坐标矢量
为
定义6自然坐标：以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为q
l，称为自然坐标；称与自然坐标一一映射的量为关节变量；其中：
定义7机械零位：对于运动副
在初始时刻t
0时，关节绝对编码器的零位
不一定为零， 该零位称为机械零位；
故关节
的控制量
为
定义8自然运动矢量：将由自然坐标轴矢量
及自然坐标q
l确定的矢量
称为自然运动矢量。其中：
自然运动矢量实现了轴平动与转动的统一表达。将由自然坐标轴矢量及关节确定的矢量，例如
称为自由运动矢量，亦称为自由螺旋。显然，轴矢量
是特定的自由螺旋。
定义9关节空间：以关节自然坐标q
l表示的空间称为关节空间。
定义10位形空间：称表达位置及姿态(简称位姿)的笛卡尔空间为位形空间，是双矢量空间或6D空间。
定义11自然关节空间：以自然坐标系为参考，通过关节变量
表示，在系统零位时必有
的关节空间，称为自然关节空间。
如图2所示，给定链节
原点O
l受位置矢量
约束的轴矢量
为固定轴矢量，记为
其中：
轴矢量
是关节自然坐标的自然参考轴。因
是轴不变量，故称
为固定轴不变量，它表征了运动副
的结构关系，即确定了自然坐标轴。固定轴不变量
是链节
结构参数的自然描述。
定义12自然坐标轴空间：以固定轴不变量作为自然参考轴，以对应的自然坐标表示的空间称为自然坐标轴空间，简称自然轴空间。它是具有1个自由度的3D空间。
如图2所示，
及
不因杆件Ω
l的运动而改变，是不变的结构参考量。
确定了轴l相对于轴
的五个结构参数；与关节变量q
l一起，完整地表达了杆件Ω
l的6D位形。给定
时，杆件固结的自然坐标系可由结构参数
及关节变量
唯一确定。称轴不变量
固定轴不变量
关节变量
及
为自然不变量。显然，由固定轴不变量
及关节变量
构成的关节自然不变量
与由坐标系
至F
[l]确定的空间位形
具有一一映射关系，即
给定多轴系统D＝{T,A,B,K,F,NT}，在系统零位时，只要建立底座系或惯性系，以及各轴上的参考点O
l，其它杆件坐标系也自然确定。本质上，只需要确定底座系或惯性系。
给定一个由运动副连接的具有闭链的结构简图，可以选定回路中任一个运动副，将组成该运动副的定子与动子分割开来；从而，获得一个无回路的树型结构，称之为Span树。T表示带方向的span树，以描述树链运动的拓扑关系。
I为结构参数；A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树。
为取轴序列
的成员。转动副R，棱柱副P，螺旋副H，接触副O是圆柱副C的特例。
描述运动链的基本拓扑符号及操作是构成运动链拓扑符号系统的基础，定义如下：
【1】运动链由偏序集合(]标识。
【2】A
[l]为取轴序列A的成员；因轴名l具有唯一的编号对应于A
[l]的序号，故A
[l]计算复杂度为O(1)。
【3】
为取轴l的父轴；轴
的计算复杂度为O(1)。计算复杂度O()表示计算过程的操作次数，通常指浮点乘与加的次数。以浮点乘与加的次数表达计算复杂度非常烦琐，故常采用算法循环过程中的主要操作次数；比如：关节位姿、速度、加速度等操作的次数。
【4】
为取轴序列
的成员；
计算复杂度为O(1)。
【5】
ll
k为取由轴l至轴k的运动链，输出表示为
且
基数记为|
ll
k|。
ll
k执行过程：执行
若
则执行
否则，结束。
ll
k计算复杂度为O(|
ll
k|)。
【6】
ll为取轴l的子。该操作表示在
中找到成员l的地址k；从而，获得轴l的子A
[k]。因
不具有偏序结构，故
ll的计算复杂度为
【7】
lL表示获得由轴l及其子树构成的闭子树，
为不含l的子树；递归执行
ll，计算复杂度为
【8】支路、子树及非树弧的增加与删除操作也是必要的组成部分；从而，通过动态Span树及动态图描述可变拓扑结构。在支路
ll
k中，若
则记
即
表示在支路中取成员m的子。
定义以下表达式或表达形式：
轴与杆件具有一一对应性；轴间的属性量
及杆件间的属性量
具有偏序性。
约定：“□”表示属性占位；若属性p或P是关于位置的，则
应理解为坐标系
的原点至F
[l] 的原点；若属性p或P是关于方向的，则
应理解为坐标系
至F
[l]。
本申请中约定：在运动链符号演算系统中，具有偏序的属性变量或常量，在名称上包含表示偏序的指标；要么包含左上角及右下角指标，要么包含右上角及右下角指标；它们的方向总是由左上角指标至右下角指标，或由右上角指标至右下角指标，本申请中为叙述简便，有时省略方向的描述，即使省略，本领域技术人员通过符号表达式也可以知道，本申请中采用的各参数，对于某种属性符，它们的方向总是由偏序指标的左上角指标至右下角指标，或由右上角指标至右下角指标。例如：
可简述为(表示由k至l)平动矢量；
表示(由k至l的)线位置；
kr
l表示(由k至l的)平动矢量；其中：r表示“平动”属性符，其余属性符对应为：属性符φ表示“转动”；属性符Q表示“旋转变换矩阵”；属性符l表示“运动链”；属性符u表示“单位矢量”；属性符ω表示“角速度”；角标为i表示惯性坐标系或大地坐标系；其他角标可以为其他字母，也可以为数字。
本申请的符号规范与约定是根据运动链的偏序性、链节是运动链的基本单位这两个原则确定的，反映了运动链的本质特征。链指标表示的是连接关系，右上指标表征参考系。采用这种符号表达简洁、准确，便于交流与书面表达。同时，它们是结构化的符号系统，包含了组成各属性量的要素及关系，便于计算机处理，为计算机自动建模奠定基础。指标的含义需要通过属性符的背景即上下文进行理解；比如：若属性符是平动类型的，则左上角指标表示坐标系的原点及方向；若属性符是转动类型的，则左上角指标表示坐标系的方向。
(1)l
S－杆件l中的点S；而S表示空间中的一点S。
(2)
－杆件k的原点O
k至杆件l的原点O
l的平动矢量；
kr
l－
在自然坐标系F
[k]下的坐标矢量，即由k至l的坐标矢量；
(3)
－原点O
k至点l
S的平动矢量；
(4)
－原点O
k至点S的平动矢量；
kr
S－
在F
[k]下的坐标矢量；
(5)
－连接杆件
及杆件l的运动副；
(6)
－沿轴
的线位置(平动位置)，
(7)左下角指标为0时，表示机械零位；如：
(8)0－三维零矩阵；1－三维单位矩阵；
(9)约定：“\”表示续行符；“□”表示属性占位；则
幂符
表示□的x次幂；右上角角标∧或
表示分隔符；如：
或
为
的x次幂。
[□]
T表示□的转置，表示对集合转置，不对成员执行转置；如：
叉乘符运算的优先级高于投影符
的优先级。投影符
的优先级高于成员访问符□
[□]或□
[□]，成员访问符□
[□]优先级高于幂符
(10)单位矢量在大地坐标系的投影矢量
单位零位矢量
(11)
－零位时由原点
至原点O
l的平动矢量，且记
表示位置结构参数。
(12)
iQ
l，相对绝对空间的旋转变换阵；
(13)以自然坐标轴矢量为参考方向，相对系统零位的角位置或线位置，记为q
l，称为自然坐标；关节变量
自然关节坐标为φ
l；
(14)对于一给定有序的集合r＝[1,4,3,2]
T，记r
[x]表示取集合r的第x行元素。常记[x]、[y]、 [z]及[w]表示取第1、2、3及4列元素。
(15)
il
j表示由i到j的运动链；
ll
k为取由轴l至轴k的运动链；
给定运动链
若n表示笛卡尔直角系，则称
为笛卡尔轴链；若n表示自然参考轴，则称
为自然轴链。
(16)Rodrigues四元数表达形式：
欧拉四元数表达形式：
不变量的四元数(也称为轴四元数)表达形式
1.建立多轴系统的拉格朗日方程
应用链符号系统建立关节空间的拉格朗日方程，考虑质点动力学系统D＝{A,K,T,NT,F,B}，首先根据牛顿力学推导自由质点
的拉格朗日方程；然后，推广至受约束的质点系统。
保守力
相对质点惯性力
具有相同的链序，即
具有正序，质点
的合力为零。质点
的能量记为
根据广义坐标序列
与笛卡尔空间位置矢量序列
关系
得
式(2)应用系统的能量及广义坐标建立系统的方程。关节变量
与坐标矢量
ir
l的关系如式(1)所示，称式(1)为关节空间与笛卡尔空间的点变换。
保守力与惯性力具有相反的链序。拉格朗日系统内的约束既可以是质点间的固结约束，又可以是质点系统间的运动约束；刚体自身是质点系统
质点能量具有可加性；刚体动能量由质心平动动能及转动动能组成。下面，就以简单运动副R/P分别建立拉格朗日方程，为后续进一步推出新的动力学理论奠定基础。
给定刚体多轴系统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；且有
【1】系统能量
动力学系统D能量
表达为
其中：
【2】多轴系统拉格朗日方程
由式(2)得多轴系统拉格朗日方程，
式(6)为轴u的控制方程，即在轴不变量
上的力平衡方程；
是合力
在
上的分量，
是合力矩
在
上的分量。
2.建立Ju-Kane动力学预备方程：
基于多轴系统拉格朗日方程(6)推导居―凯恩(Ju-Kane)动力学预备定理。先进行拉格朗日方程与凯恩方程的等价性证明；然后，计算能量对关节速度及坐标的偏速度，再对时间求导，最后给出Ju-Kane动力学预备定理。
【1】拉格朗日方程与凯恩方程的等价性证明
证明：考虑刚体k平动动能对
的偏速度对时间的导数得
考虑刚体k转动动能对
的偏速度对时间的导数得
证毕。
因
与
不相关，由式(7)及多轴系统拉格朗日方程(6)得
动力学系统D的平动动能及转动动能分别表示为
考虑式(4)及式(5)，即有
式(7)及式(8)是居―凯恩动力学预备定理证明的依据，即居―凯恩动力学预备定理本质上与拉格朗日法是等价的。同时，式(8)右侧包含了多轴系统凯恩方程；表明拉格朗日法与凯恩法的惯性力计算是一致的，即拉格朗日法与凯恩法也是等价的。式(8)表明：在拉格朗日方程(4)中存在
重复计算的问题。
【2】能量对关节速度及坐标的偏速度
【2-1】若
并考虑
及
仅与闭子树
uL相关，由式(4)及式(5)，得
【2-2】若
并考虑
及
仅与闭子树
uL相关，由式(4)及式(5)，得
至此，已完成能量对关节速度及坐标的偏速度计算。
【3】求对时间的导数
【3-1】若
由式(7)、式(9)及式(10)得
【3-2】若
由式(7)、式(12)及式(13)得
至此，已完成对时间t的求导。
【4】Ju-Kane动力学预备定理
将式(11)、式(14)、式(15)及式(16)代入式(8)，
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F
[i]，
除了重力外，作用于轴u的合外力及力矩分别记为
及
轴k的质量及质心转动惯量分别记为m
k及
轴k的重力加速度为
则轴u的Ju-Kane动力学预备方程为
式(17)具有了树链拓扑结构。k
I表示杆k质心I。因闭子树
uL中的广义力具有可加性；因此闭子树的节点有唯一一条至根的运动链，因此运动链
il
n可以被运动链
uL替换。
下面，针对Ju-Kane动力学预备方程，解决式(17)右侧
Df
k及
Dτ
τ的计算问题，从而建立树链刚体系统Ju-Kane动力学方程。
3.建立树链刚体系统Ju-Kane动力学模型
给定轴链
k∈
il
n，有以下偏速度计算公式：
对给定轴链
|
il
l|≥2，有以下加速度迭代式：
左序叉乘与转置的关系为：
根据运动学迭代式，有：
3.1外力反向迭代
给定由环境i中施力点i
S至轴l上点l
S的双边外力
及外力矩
iτ
l，它们的瞬时轴功率p
ex表示为
其中：
及
iτ
l不受
及
控制，即
及
iτ
l不依赖于
及
【1】若k∈
il
l，则有
由式(19)及式(18)得
即
式(26)中
与式(21)中
的链序不同；前者是作用力，后者是运动量，二者是对偶的，具有相反的序。
【2】若k∈
il
l，则有
由式(22)及式(25)得
即有
式(26)及式(27)表明环境作用于轴k的合外力或力矩等价于闭子树
kL对轴k的合外力或力矩，将式(26)及式(27)合写为
至此，解决了外力反向迭代的计算问题。在式(28)中，闭子树对轴k的广义力具有可加性；力的作用具有双重效应，且是反向迭代的。所谓反向迭代是指：
是需要通过链节位置矢量迭代的；
的序与前向运动学
计算的序相反。
3.2共轴驱动力反向迭代
若轴l是驱动轴，轴l的驱动力及驱动力矩分别为
及
则驱动力
及驱动力矩
产生的 功率p
ac表示为
【1】由式(18)、式(19)及式(29)得
即
若轴u与轴
共轴，则有
记
因
与
无关，由式(30)得
因
与
共轴，故有
【2】由式(19)、式(18)及式(29)得
即
若轴u与
共轴，则有
记
由式(32)得
至此，完成了共轴驱动力反向迭代计算问题。
3.3树链刚体系统Ju-Kane动力学显式模型的建立：
下面，先陈述树链刚体系统Ju-Kane动力学方程，简称Ju-Kane方程；然后，给出建立步骤。
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F
[i]，
除了重力外，作用于轴u的合外力及力矩在
上的分量分别记为
及
轴k的质量及质心转动惯量分别记为m
k及
轴k的重力加速度为
驱动轴u的双边驱动力及驱动力矩在
上的分量分别记为
及
环境i对轴l的力及力矩分别为
及
iτ
l；则轴u树链Ju-Kane动力学方程为
其中：[·]表示取行或列；
及
是3×3的分块矩阵，
及
是3D矢量，q为关节空间。且有，
其中，记
记
上述方程的建立步骤为：
记
故有
ex的能量为
p
ex为瞬时轴功率；p
ac为驱动轴的驱动力及驱动力矩产生的功率。
由式(26)、式(27)、式(31)、式(33)及式(41)得式(40)。
将偏速度计算公式式(19)，式(18)及式(20)代入Ju-Kane动力学预备方程(17)得
由式(21)得
考虑式(43)，则有
同样，考虑式(43)，得
将式(43)至式(45)代入式(42)得式(34)至式(39)。
实施例1
给定如图3所示的通用3R机械臂，A＝(i,1:3]；应用本发明的方法建立树链Ju-Kane动力学方程，并得到广义惯性矩阵。
步骤1建立基于轴不变量的迭代式运动方程。
由式(46)基于轴不变量的转动变换矩阵
得
运动学迭代式：
二阶张量投影式：
由式(48)及式(47)得
由式(49)，式(47)及式(55)得
由式(50)及式(55)得
由式(51)、式(55)及式(57)得
由式(52)及式(55)得
由式(53)及式(55)得
步骤2建立动力学方程。先建立第1轴的动力学方程。由式(37)得
由式(39)得
由式(61)及式(62)得第1轴的动力学方程，
建立第2轴的动力学方程。由式(37)得
由式(39)得
由式(64)及式(65)得第2轴的动力学方程，
最后，建立第3轴的动力学方程。由式(37)得
由式(39)得
由式(67)及式(68)得第3轴的动力学方程，
由式(61)，式(63)及式(67)得广义质量阵。
由此可知，只要程式化地将系统的拓扑、结构参数、质惯量等参数代入式(36)至式(40)就可以完成动力学建模。通过编程，很容易实现Ju-Kane动力学方程。因后续的树链Ju-Kane规范方程是以Ju-Kane动力学方程推导的，树链Ju-Kane动力学方程的有效性可由Ju-Kane规范型实例证明。
3.4树链刚体系统Ju-Kane动力学规范型
在建立系统动力学方程后，紧接着就是方程求解的问题。在动力学系统仿真时，通常给定环境作用的广义力及驱动轴的广义驱动力，需要求解动力学系统的加速度；这是动力学方程求解的正问题。在求解前，首先需要得到式(71)所示的规范方程。
规范化动力学方程，
其中：RHS–右手侧(Right hand side)
显然，规范化过程就是将所有关节加速度项进行合并的过程；从而，得到关节加速度的系数。将该问题分解为运动链的规范型及闭子树的规范型两个子问题。
3.4.1运动链的规范型方程
将式(36)及式(37)中关节加速度项的前向迭代过程转化为反向求和过程，以便后续应用；显然，其中含有6种不同类型的加速度项，分别予以处理。
【1】给定运动链
则有
上式的推导步骤为：
【2】给定运动链
则有
上式的推导步骤为：因
故得
【3】给定运动链
则有
上式可由下式而得，因
故有
【4】给定运动链
则有
上式的推导步骤为：考虑
将式(72)代入式(75)左侧得
【5】给定运动链
则有
上式的推导步骤为：考虑
将式(72)代入式(76)左侧得
【6】给定运动链
则有
上式的推导步骤为：因
故有
3.4.2闭子树的规范型方程
因闭子树
uL中的广义力具有可加性；因此闭子树的节点有唯一一条至根的运动链，式(73)至式(77)的运动链
il
n可以被
uL替换。由式(73)得
由式(74)得
由式(75)得
由式(76)得
由式(77)得
至此，已具备建立规范型的前提条件。
3.5树链刚体系统Ju-Kane动力学规范方程
下面，建立树结构刚体系统的Ju-Kane规范化动力学方程。为表达方便，首先定义
然后，应用式(78)至式(82)，将式(36)及式(37)表达为规范型。
【1】式(36)的规范型为
上式的具体建立步骤为：由式(24)及式(36)得
由式(52)及式(85)得
将式(80)代入式(85)右侧前一项得
将式(79)代入式(86)右侧后一项得
将式(87)及式(88)代入式(86)得
对于刚体k，有
由式(35)、式(83)及式(89)得式(84)。【2】式(37)的规范型为
上式的具体建立步骤为：由式(37)得
将式(78)代入式右侧前一项(91)得
将式(81)代入式(91)右侧后一项得
将式(82)代入式(91)右侧中间一项得
将式(92)，式(93)及式(94)代入式(92)得
对于刚体k，有
由式(35)，式(83)及式(95)得式(90)。
【3】应用式(84)及式(90)，将Ju-Kane方程重新表述为如下树链Ju-Kane规范型方程：
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F
[i]，
除了重力外，作用于轴u的合外力及力矩在
上的分量分别记为
及
轴k的质量及质心转动惯量分别记为m
k及
轴k的重力加速度为
驱动轴u的双边驱动力及驱动力矩在
上的分量分别记为
及
环境i对轴l的作用力及力矩分别为
及
iτ
l；则轴u的Ju-Kane动力学规范方程为
其中：
及
是3×3的分块矩阵，
及
是3D矢量；
为轴u的合外力在
上的 分量，
为轴u的合力矩在
上的分量；
为关节坐标；
并且，
式中，k
I表示杆k质心I；轴k的质量及质心转动惯量分别记为m
k及
为转动轴u的惯性矩阵；
为平动轴u的惯性矩阵；h
R为转动轴u的非惯性矩阵；h
P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在
上的分量分别记为
及
驱动轴u的双边驱动力及驱动力矩在
上的分量分别记为
及
环境i对轴l的作用力及作用力矩分别为
及
iτ
l；
ll
k为取由轴l至轴k的运动链，
uL表示获得由轴u及其子树构成的闭子树。
4.闭链刚体系统的Ju-Kane动力学方程建立
下面，先陈述闭链刚体系统的居―凯恩(简称Ju-Kane)动力学方程；然后，给出具体建模过程。
给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F
[i]，
除了重力外，作用于轴u的合外力及力矩在
上的分量分别记为
及
轴k的质量及质心转动惯量分别记为m
k及
轴k的重力加速度为
驱动轴u的双边驱动力及驱动力矩在
上的分量分别记为
及
环境i对轴l的作用力及作用力矩分别为
及
iτ
l；轴u对轴u′的广义约束力记为
则有闭链刚体系统的Ju-Kane动力学方程：
【1】轴u及轴u′的Ju-Kane动力学规范方程分别为
【2】非树约束副
uk
u′的约束代数方程为
其中：
式中：
及
是3×3的分块矩阵，
及
是3D矢量；k
I表示杆k质心I；轴k的质量及质心转动惯量分别记为m
k及
为转动轴u的惯性矩阵；
为平动轴u的惯性矩阵；h
R 为转动轴u的非惯性矩阵；h
P为平动轴u的非惯性矩阵；
为平动关节角速度；
为转动关节角速度。
具体建模过程如下：
非树约束副
保持约束点u
S及u′
S一致，故有
由式(114)得
轴u对轴u′在约束轴方向上的广义约束力
及轴u′对轴u在约束轴方向上的广义约束力
的功率分别为
由式(115)及式(116)得
由式(115)得
δ表示增量；
由式(18)及式(118)得
故有
由式(110)及式(122)得式(105)。由式(19)及式(119)得
由式(111)及式(123)得式(106)。由式(19)及式(120)得
由式(112)及式(124)得式(107)。由式(19)及式(121)得
由式(113)及式(125)得(108)。由式(18)，式(116)及式(110)得
广义约束力
及
是矢量，由式(126)及式(127)得式(109)。由此可知，偏速度主要应用于力的反向迭代。广义约束力
及
视为外力。
根据轴u的Ju-Kane动力学规范方程得式(103)及式(104)。
以关节空间自然轴链为基础的Ju-Kane闭链刚体动力学克服了笛卡尔坐标轴链空间的局限：
【1】在基于笛卡尔坐标轴链的牛顿欧拉动力学中，非树运动副
uk
u′∈P约束不能表达
及
或
及
的情形，即不能表达齿条与齿轮、蜗轮与蜗杆等约束。而本申请的非树约束副
uk
u′的约束代数方程式(105)至式(108)可表达任一种约束类形，并且物理内涵明晰；
【2】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是6D的；而式(105)至式(108)表示是3D非树运动副代数约束方程，从而降低了系统方程求解的复杂度；
【3】在基于笛卡尔坐标轴链的牛顿欧拉动力学当中，非树运动副代数约束方程是关于6D矢量空间绝对加速度的，是关于关节坐标、关节速度的迭代式，具有累积误差；而式(105)至式(108)是关于关节加速度的，保证了约束方程的准确性。
5.基于轴不变量的约束力求解
对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为
显然，有
由式(96)及式(139)计算得
式(128)表示运动轴矢量与运动轴约束力具有自然正交补的关系。
若
及
为运动副
的两个正交约束轴，且约束轴与运动轴正交，即
记
为约束轴轴矢量，
替换式(96)中
重新计算得
其中：
在完成前向动力学正解后，根据已计算的关节加速度
由式(130)可以得到关节约束力大小
约束力矩大小
当
时，由式(130)得
且
式(130)中同一时刻具有相同的运动状态及内外力。仅在运动轴向上出现力及力矩的平衡；而在约束轴向，动力学方程不满足，即力与力矩不一定平衡。
由式(130)可以得到关节约束力大小
及
约束力矩大小
及
若记运动轴径向力矢量
及力矩矢量
则有
若记运动轴径向力大小为
及力矩大小为
由式(133)得
至此，完成了轴径向约束广义力的计算。
树链刚体系统对应的关节加速度序列记
可根据下述步骤计算：
将根据运动轴类型及自然参考轴表达的刚体运动链广义惯性矩阵称为轴链刚体广义惯性矩阵，简称轴链广义惯性矩阵。
定义正交补矩阵
及对应的叉乘矩阵
给定多轴刚体系统D＝{A,K,T,NT,F,B}，
将系统中各轴动力学方程(96)按行排列；将重排后的轴驱动广义力及不可测的环境作用力记为f
C，可测的环境广义作用力记为f
i；将系统对应的关节加速度序列记为
将重排后的
记为h；考虑式(135)；则该系统动力学方程为
由式(136)得
其中，
由式(136)得
6.广义内摩擦力及粘滞力计算
在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小
及约束力矩大小
如图3、图4所示，记运动轴u的内摩擦力大小及内摩擦力矩大小分别为
及
运动轴u的粘滞力及粘滞力矩大小分别为
及
故有
其中：
sk
[u]─运动轴u的内摩擦系数，
ck
[u]─运动轴u的粘滞系数；sign()表示取正或负符号。
记广义内摩擦力及粘滞力的合力及合力矩分别为
由式(140)及式(141)得
运动轴的广义内摩擦力及粘滞力是运动轴的内力，因为它们仅存在于运动轴向上，与轴径向约束 力总是正交的。当运动轴轴向动态作用力平衡时，无论广义内摩擦力及粘滞力是否存在或大小如何，都不影响动力学系统的运动状态；故而，不影响运动轴的径向约束力。因此，由式(130)至式(134)计算运动轴u的径向约束力大小
及约束力矩大小
时，可以不考虑运动轴的广义内摩擦力及粘滞力。
Claims (5)
- 一种基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，给定多轴刚体系统D＝{A,K,T,NT,F,B}，惯性系记为F [i]，A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树；除了重力外，作用于轴u的合外力及力矩在
上的分量分别记为
及
轴k的质量及质心转动惯量分别记为m k及
轴k的重力加速度为
驱动轴u的双边驱动力及驱动力矩在
上的分量分别记为
及
环境i对轴l的作用力及作用力矩分别为
及 iτ l；轴u对轴u′的广义约束力记为
则闭链刚体系统的Ju-Kane动力学方程为：
【1】轴u及轴u′的Ju-Kane动力学规范方程为
其中：及
是3×3的分块矩阵，
及
是3D矢量；
【2】非树约束副 uk u′的约束代数方程为
其中：
式中：及
是3×3的分块矩阵，
及
是3D矢量；k I表示杆k质心I；轴k的质量及质心转动惯量分别记为m k及
为转动轴u的惯性矩阵；
为平动轴u的惯性矩阵；h R为转动轴u的非惯性矩阵；h P为平动轴u的非惯性矩阵；
为平动关节角速度；
为转动关节角速度。
- 根据权利要求1所述的基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，基于轴不变量的约束力求解步骤为：对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为则有
上式表示运动轴矢量与运动轴约束力具有自然正交补的关系；若及
为运动副
的两个正交约束轴，且约束轴与运动轴正交，即
记为约束轴轴矢量，得
其中：
根据关节加速度由式(130)得到关节约束力大小
约束力矩大小
当
时，由式(130)得
且
式(130)中同一时刻具有相同的运动状态及内外力；仅在运动轴向上出现力及力矩的平衡；而在约束轴向，动力学方程不满足，即力与力矩不一定平衡；
由式(130)可以得到关节约束力大小及
约束力矩大小
及
若记运动轴径向力矢量
及力矩矢量
则有
若记运动轴径向力大小为及力矩大小为
由式(133)得
至此，完成了轴径向约束广义力的计算。 - 根据权利要求2所述的基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，由式(130)至式(134)计算运动轴u的径向约束力大小及约束力矩大小
时，不考虑运动轴的广义内摩擦力及粘滞力。
- 根据权利要求2所述的基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，考虑广义内摩擦力及粘滞力的基于轴不变量的约束力求解步骤为：在完成轴径向约束广义力的计算后，得到运动轴u的径向约束力大小及约束力矩大小
记运动轴u的内摩擦力大小及内摩擦力矩大小分别为
及
运动轴u的粘滞力及粘滞力矩大小分别为
及
则
其中： sk [u]─运动轴u的内摩擦系数， ck [u]─运动轴u的粘滞系数；sign()表示取正或负符号；记广义内摩擦力及粘滞力的合力及合力矩分别为由式(140)及式(141)得
其中： sk [u]─运动轴u的内摩擦系数， ck [u]─运动轴u的粘滞系数；sign()表示取正或负符号；为转动关节速度；
为平动关节速度。
- 根据权利要求2所述的基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，树链Ju-Kane规范型方程
其中：及
是3×3的分块矩阵，
及
是3D矢量；
为轴u的合外力在
上的分量，
为轴u的合力矩在
上的分量；
为关节坐标；
并且，
式中，k I表示杆k质心I；轴k的质量及质心转动惯量分别记为m k及为转动轴u的惯性矩阵；
为平动轴u的惯性矩阵；h R为转动轴u的非惯性矩阵；h P为平动轴u的非惯性矩阵；作用于轴u的合外力及力矩在
上的分量分别记为
及
驱动轴u的双边驱动力及驱动力矩在
上的分量分别记为
及
环境i对轴l的作用力及作用力矩分别为
及 iτ l； ll k为取由轴l至轴k的运动链， uL表示获得由轴u及其子树构成的闭子树。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810933434.5 | 2018-08-16 | ||
| CN201810933434.5A CN109086544B (zh) | 2018-08-16 | 2018-08-16 | 基于轴不变量的闭链机器人动力学建模与解算方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2020034399A1 true WO2020034399A1 (zh) | 2020-02-20 |
Family
ID=64793412
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2018/112583 WO2020034399A1 (zh) | 2018-08-16 | 2018-10-30 | 基于轴不变量的闭链机器人动力学建模与解算方法 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN109086544B (zh) |
| WO (1) | WO2020034399A1 (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104573255A (zh) * | 2015-01-22 | 2015-04-29 | 天津理工大学 | 一种基于改进多体系统传递矩阵的机械臂建模与求解方法 |
| CN106055733A (zh) * | 2016-05-10 | 2016-10-26 | 中国人民解放军国防科学技术大学 | 多功能结构的动力学参数确定方法 |
| CN107256284A (zh) * | 2017-05-10 | 2017-10-17 | 中国北方车辆研究所 | 一种实时交互式四足机器人多步态动力学建模方法及系统 |
| CN107529630A (zh) * | 2017-06-23 | 2018-01-02 | 西北工业大学 | 一种空间机器人建立动力学模型的方法 |
| CN108038286A (zh) * | 2017-11-30 | 2018-05-15 | 长安大学 | 一种二自由度冗余驱动并联机器人的动力学建模方法 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2978844B1 (fr) * | 2011-08-04 | 2014-03-21 | Aldebaran Robotics | Robot a articulations de rigidite variable et methode de calcul de ladite rigidite optimisee |
| CN103593524B (zh) * | 2013-11-13 | 2014-10-01 | 北京航空航天大学 | 一种变体飞行器的动力学建模与分析方法 |
| CN106249616B (zh) * | 2016-07-22 | 2020-06-05 | 上海航天控制技术研究所 | 一种在轨服务机械臂动力学建模方法和系统 |
| CN106695797B (zh) * | 2017-02-22 | 2019-03-05 | 哈尔滨工业大学深圳研究生院 | 基于双臂机器人协同操作的柔顺控制方法及系统 |
| CN108372505A (zh) * | 2018-01-31 | 2018-08-07 | 国机智能技术研究院有限公司 | 一种基于动力学的机器人的碰撞检测算法及系统 |
-
2018
- 2018-08-16 CN CN201810933434.5A patent/CN109086544B/zh active Active
- 2018-10-30 WO PCT/CN2018/112583 patent/WO2020034399A1/zh active Application Filing
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104573255A (zh) * | 2015-01-22 | 2015-04-29 | 天津理工大学 | 一种基于改进多体系统传递矩阵的机械臂建模与求解方法 |
| CN106055733A (zh) * | 2016-05-10 | 2016-10-26 | 中国人民解放军国防科学技术大学 | 多功能结构的动力学参数确定方法 |
| CN107256284A (zh) * | 2017-05-10 | 2017-10-17 | 中国北方车辆研究所 | 一种实时交互式四足机器人多步态动力学建模方法及系统 |
| CN107529630A (zh) * | 2017-06-23 | 2018-01-02 | 西北工业大学 | 一种空间机器人建立动力学模型的方法 |
| CN108038286A (zh) * | 2017-11-30 | 2018-05-15 | 长安大学 | 一种二自由度冗余驱动并联机器人的动力学建模方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109086544A (zh) | 2018-12-25 |
| CN109086544B (zh) | 2019-12-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11491649B2 (en) | Axis-invariant based multi-axis robot kinematics modeling method | |
| WO2020034420A1 (zh) | 一种基于轴不变量的多轴机器人逆运动学建模与解算方法 | |
| WO2020034422A1 (zh) | 一种基于轴不变量的多轴机器人系统正运动学建模与解算方法 | |
| Park et al. | Geometric algorithms for robot dynamics: A tutorial review | |
| CN107984472A (zh) | 一种用于冗余度机械臂运动规划的变参神经求解器设计方法 | |
| CN108015763A (zh) | 一种抗噪声干扰的冗余度机械臂路径规划方法 | |
| CN109079784B (zh) | 一种基于轴不变量的多轴机器人系统建模与控制方法 | |
| Wang et al. | A multi-objective approach for the trajectory planning of a 7-DOF serial-parallel hybrid humanoid arm | |
| WO2020034404A1 (zh) | 基于轴不变量的非理想关节机器人动力学建模与解算方法 | |
| WO2020034405A1 (zh) | 基于轴不变量的树链机器人动力学建模与解算方法 | |
| WO2020034399A1 (zh) | 基于轴不变量的闭链机器人动力学建模与解算方法 | |
| WO2020034415A1 (zh) | 基于轴不变量的通用6r机械臂逆解建模与解算方法 | |
| CN112084592A (zh) | 折叠式桁架动力学分析系统、方法、装置和存储介质 | |
| WO2020034403A1 (zh) | 基于轴不变量的多轴机器人正运动学计算方法 | |
| WO2020034417A1 (zh) | 基于轴不变量多轴机器人d-h系及d-h参数确定方法 | |
| WO2020034401A1 (zh) | 基于轴不变量的动基座多轴机器人动力学建模与解算方法 | |
| WO2020034418A1 (zh) | 基于轴不变量及dh参数1r/2r/3r逆解建模方法 | |
| Li et al. | Establishing an Improved Kane Dynamic Model for the 7‐DOF Reconfigurable Modular Robot | |
| WO2020034416A1 (zh) | 基于轴不变量的通用7r机械臂逆解建模与解算方法 | |
| WO2020034402A1 (zh) | 基于轴不变量的多轴机器人结构参数精测方法 | |
| You et al. | A two-dimensional corotational beam formulation based on the local frame of special Euclidean Group SE (2) | |
| WO2024103241A1 (zh) | 一种基于物质点法的软体机器人仿真方法 | |
| Rothammer | Modeling, observing and controlling a cable-suspended aerial manipulator as a constrained system | |
| Jia et al. | Research on contact constraints linearization of dexterous hand and grasping forces optimization | |
| Wei et al. | Configuration design and performance analysis of a force feedback operating rod based on 3-RRR spherical parallel mechanism |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 32PN | Ep: public notification in the ep bulletin as address of the adressee cannot be established |
Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 16.08.2021) |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 18930452 Country of ref document: EP Kind code of ref document: A1 |