CN109086544A - Closed chain manipulator Dynamic Modeling and calculation method based on axis invariant - Google Patents
Closed chain manipulator Dynamic Modeling and calculation method based on axis invariant Download PDFInfo
- Publication number
- CN109086544A CN109086544A CN201810933434.5A CN201810933434A CN109086544A CN 109086544 A CN109086544 A CN 109086544A CN 201810933434 A CN201810933434 A CN 201810933434A CN 109086544 A CN109086544 A CN 109086544A
- Authority
- CN
- China
- Prior art keywords
- axis
- formula
- force
- kinematic
- constraint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
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
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
The invention discloses a kind of closed chain manipulator dynamics and calculation method based on axis invariant, establish the Ju-Kane kinetics equation of closed chain Rigid-body System, Ju-Kane closed chain dynamics of rigid bodies based on joint space nature axis chain overcomes the limitation of cartesian coordinate axes chain space: in the newton Euler Dynamics based on cartesian coordinate axes chain, non-tree constraint of kinematic pair cannot express such as rack and pinion, worm gear and worm screw constrains.And the constraint algebraic equation of the non-tree constraint pair of the application can express any constraint class shape, and physical connotation is apparent；Reduce the complexity of system equation solution；It ensure that the accuracy of constraint equation.
Description
Technical field
The present invention relates to a kind of closed chain manipulator Dynamic Modeling and calculation methods, belong to robotic technology field.
Background technique
Lagrange proposes Lagrangian method when studying lunar libration problem, is to express power with generalized coordinates
Learn the basic skills of equation；Meanwhile and description quantum field theory basic skills.Kinetics equation is established using Lagrangian method
It has been a loaded down with trivial details process, although kinetics equation of the Lagrange's equation according to the invariance derivation system of system capacity,
With the advantage on theory analysis；But in engineer application, with the increase of degree of freedom in system, the complexity of equation inference is acute
Increase, it is difficult to be widely used.Kane equation establishment process passes through the deflected velocity of system, speed compared with Lagrange's equation
And acceleration directly expresses kinetics equation.Therefore triumphant grace dynamic method is compared with Lagrangian method, due to eliminating system
The expression of energy and derivation process to the time, significantly reduce the difficulty of system modelling.However, being for high-freedom degree
System, triumphant grace dynamic modeling method is also to be difficult to be applicable in.
Lagrange's equation and kane equation have greatly pushed the research of many-body dynamics, using spatial operator algebra as base
The dynamics of plinth has a degree of raising due to applying iterative process, calculating speed and precision.These power
Method either Kinematic process or dynamic process are required in body space, body subspace, system space and system
Complicated transformation is carried out in space, modeling process and model tormulation are extremely complex, it is difficult to meet high-freedom degree system modelling and control
The demand of system, therefore, it is necessary to establish the compact expression of kinetic model；It should guarantee the accuracy of modeling, guarantee to build again
The real-time of mould.Not succinct kinetic expression is just difficult to ensure the reliable of high-freedom degree system dynamics Project Realization
Property and accuracy.Meanwhile the unstructured kinematics of tradition and dynamics symbol can not be calculated by annotation agreement symbol intension
Mechanism solution causes computer automatically cannot establish and analyze kinematics and kinetic model.
Summary of the invention
Technical problem to be solved by the invention is to provide a kind of closed chain manipulator Dynamic Modelings based on axis invariant
With calculation method.
In order to solve the above technical problems, the invention adopts the following technical scheme:
A kind of closed chain manipulator Dynamic Modeling and calculation method based on axis invariant, characterized in that
Given multiaxis Rigid-body System D={ A, K, T, NT, F, B }, inertial system is denoted as F[i],A is axis sequence, and F is rod piece referential sequence, and B is rod piece body sequence, and K is movement
Secondary type sequence, NT are the sequence, that is, non-tree for constraining axis；Other than gravity, the bonding force and torque for acting on axis u existOn
Component is denoted as respectivelyAndThe quality and mass center rotary inertia of axis k is denoted as m respectivelykAndThe acceleration of gravity of axis k isThe bilateral driving force and driving moment of drive shaft u existsOn component be denoted as respectivelyAndEnvironment i is to axis l
Active force and opplied moment be respectivelyAndiτl；Axis u is denoted as the generalized constraint force of axis u 'Then closed chain Rigid-body System
Ju-Kane kinetics equation are as follows:
[1] the Ju-Kane dynamics double average of axis u and axis u ' is
Wherein:AndIt is 3 × 3 matrix in block form,AndIt is 3D vector；
[2] non-tree constraint is secondaryuku′Constraint algebraic equation be
Wherein:
In formula:AndIt is 3 × 3 matrix in block form,AndIt is 3D vector；kIIndication rod k mass center I；Axis k's
Quality and mass center rotary inertia are denoted as m respectivelykAnd For the inertial matrix of rotation axis u；For the used of translation shaft u
Property matrix；hRFor the non-inertial matrix of rotation axis u；hPFor the non-inertial matrix of translation shaft u；For the joint angular speed that is translatable；
For cradle head angular speed.
Restraining force solution procedure based on axis invariant are as follows:
For the kinematic axis u of no power waste, remembers its restraining force and restraint moment vector is respectivelyThen have
Above formula indicates that movement axial vector and kinematic axis restraining force have the orthocomplemented relationship of nature；
IfAndFor kinematic pairTwo orthogonality constraint axis, and constrain axis it is orthogonal with kinematic axis, i.e.,
NoteTo constrain axis axial vector, obtain
Wherein:
According to joint velocityJoint constraint power size is obtained by formula (130)Restraint moment sizeWhenWhen, it is obtained by formula (130)AndSynchronization motion state having the same and interior in formula (130)
External force；Only occurs the balance of power and torque upwards in kinematic axis；And it is axial in constraint, kinetics equation is unsatisfactory for, i.e. power and power
Square not necessarily balances；
By the available joint constraint power size of formula (130)AndRestraint moment sizeAndIf the note movement diameter of axle
To force vectorAnd moment vectorThen have
If note kinematic axis radial force size isAnd torque size isIt is obtained by formula (133)
So far, the calculating of axis radial constraint generalized force is completed.
The radial constraint size of kinematic axis u is calculated by formula (130) to formula (134)And restraint moment sizeWhen, no
Consider the broad sense internal friction and viscous force of kinematic axis.
Consider the restraining force solution procedure based on axis invariant of broad sense internal friction and viscous force are as follows:
After the calculating for completing axis radial constraint generalized force, the radial constraint size of kinematic axis u is obtainedAnd restraining force
Square sizeRemember kinematic axis u internal friction size and inner friction torque size be respectivelyAndThe viscous force of kinematic axis u
And viscous moment size is respectivelyAndThen
Wherein:sk[u]- the coefficient of internal friction of kinematic axis u,ck[u]- the coefficient of viscosity of kinematic axis u；Sign () expression takes just
Or minus symbol；
Note broad sense internal friction and the resultant force and resultant moment of viscous force are respectivelyBy formula (140) and formula (141)
?
Wherein:sk[u]- the coefficient of internal friction of kinematic axis u,ck[u]- the coefficient of viscosity of kinematic axis u；Sign () expression takes just
Or minus symbol；For cradle head speed；For the joint velocity that is translatable.
Set chain Ju-Kane canonical form equation
Wherein:AndIt is 3 × 3 matrix in block form,AndIt is 3D vector；Bonding force for axis u exists
On component,Resultant moment for axis u existsOn component；For joint coordinates；
Also,
In formula, kIIndication rod k mass center I；The quality and mass center rotary inertia of axis k is denoted as m respectivelykAnd For rotation
The inertial matrix of axis u；For the inertial matrix of translation shaft u；hRFor the non-inertial matrix of rotation axis u；hPFor the non-of translation shaft u
Inertial matrix；The bonding force and torque for acting on axis u existOn component be denoted as respectivelyAnd The bilateral driving force and driving moment of drive shaft u existsOn component be denoted as respectivelyAndEnvironment i is respectively to the active force and opplied moment of axis lAndiτl；l1kTo take the fortune by axis l to axis k
Dynamic chain,uL, which indicates to obtain, closes subtree by what axis u and its subtree were constituted.
Advantageous effects of the invention:
For ideal constraint system, the Ju-Kane kinetics equation of closed chain rigid body non-ideal system system is established.
[1] in the newton Euler Dynamics based on cartesian coordinate axes chain, non-tree kinematic pairuku′∈ P constraint is unable to table
It is constrained up to rack and pinion, worm gear and worm screw etc..And the non-tree that the application establishes constrains pairuku′Constraint algebraic equation can express
Any constraint class shape, and physical connotation is apparent；
[2] in the newton Euler Dynamics based on cartesian coordinate axes chain, non-tree kinematic pair Algebraic Constraint equation is
6D's；And the constraint algebraic equation expression for the non-tree constraint pair that the application establishes is 3D non-tree kinematic pair Algebraic Constraint equation, from
And reduce the complexity of system equation solution；
[3] in the newton Euler Dynamics based on cartesian coordinate axes chain, non-tree kinematic pair Algebraic Constraint equation is
About 6D vector space absolute acceleration, be about joint coordinates, joint velocity it is iterative, have accumulated error；And this
The secondary constraint algebraic equation of the non-tree constraint that application is established is to ensure that the accuracy of constraint equation about joint velocity.
Detailed description of the invention
Fig. 1 natural system of coordinates and axis chain；
Fig. 2 fixing axle invariant；
Fig. 3, Fig. 4 are the internal friction and viscous force schematic diagram of kinematic axis.
Specific embodiment
The invention will be further described below.Following embodiment is only used for clearly illustrating technical side of the invention
Case, and not intended to limit the protection scope of the present invention.
Closed chain Rigid-body System has very extensive application；For example, the rocker arm mobile system of CE3 rover is with differential
The closed chain of device, heavy-duty machinery arm are usually the closed chain system with double leval jib.Meanwhile actual kinematic axis generally comprises interior friction
Power and viscous force.Therefore the Ju-Kane Dynamic Modeling of research closed chain Rigid-body System is very necessary.
Define 1 natural coordinates axis: title is coaxial with kinematic axis or measurement axis, and the unit reference axis with fixed origin is certainly
Right reference axis, also known as nature reference axis.
Define 2 naturals system of coordinates: as shown in Figure 1, if multiple axes system D is in zero-bit, all Descartes's body coordinate system directions
Unanimously, and body coordinate origin is located on the axis of kinematic axis, then the coordinate system is natural coordinates system, abbreviation natural coordinates
System.
Natural system of coordinates advantage is: (1) coordinate system easily determines；(2) joint variable when zero-bit is zero；(3) zero-bit
When posture it is consistent；(4) it is not easily introduced measurement accumulated error.
By definition 2 it is found that when system is in zero-bit, the natural system of coordinates and pedestal of all rod pieces or the direction of system of the world
Unanimously.System is in zero-bitWhen, natural system of coordinatesAround axial vectorRotational angleIt willGo to F[l]；?Under coordinate vector withIn F[l]Under coordinate vectorIt is identical, that is, have
Known by above formula,OrIndependent of adjacent coordinate systemAnd F[l]；Therefore claimOrFor axis invariant.
When not emphasizing invariance, coordinate vector (abbreviation axial vector) can be referred to as.OrCharacterization is bodyIt is total with body l
Some reference units coordinate vectors, with reference pointAnd OlIt is unrelated.BodyIt is rod piece or axis with body l.
Axis invariant and reference axis have essential distinction:
(1) reference axis is that have the reference direction of zero-bit and unit scales, can describe the position being translatable in the direction, but
Rotational angle around the direction cannot completely be described, because reference axis itself does not have radial reference direction, i.e., there is no characterizations
The zero-bit of rotation.In practical application, requiring supplementation with the radial reference of the axis.Such as: in Descartes system F[l]In, it is rotated around lx,
It need to be with reference to zero-bit with ly or lz.Reference axis itself is 1D, and 3 orthogonal 1D reference axis constitute Descartes's frame of 3D.
(2) axis invariant is the mikey reference axis of 3D, its own is exactly a frame.Its own has radial reference
Axis refers to zero-bit.Solid axes and the radial reference axis of its own can determine Descartes's frame.Solid axes can be with
Reflect kinematic axis and measure three of axis and refers to attribute substantially.
The axial vector of no chain index is denoted as by existing documentAnd referred to as Euler's axis (Euler Axis), corresponding joint
Angle is known as Eulerian angles (Euler Angle).Why the application no longer continues to use Euler's axis, and referred to as axis invariant, be because
Axis invariant has with properties:
[1] rotation transformation battle array is givenBecause it is real matrix, mould is unit, therefore it has a factual investigation λ1And
Two complex eigenvalue λ being conjugated each other2=eiφAnd λ3=e-iφ；Wherein: i is pure imaginary number.Therefore, | λ1|·||λ2||·||λ3||
=1, obtain λ1=1.Axial vectorIt is factual investigation λ1=1 corresponding characteristic vector, is invariant；
[2] it is 3D reference axis, not only there is axial reference direction, but also there is radial direction to refer to zero-bit, will saves and give in 3.3.1
To illustrate.
[3] under natural system of coordinates:That is axis invariantIt is very special vector, it leads the time
Number also has invariance, and has very good mathematical operations performance；
For axis invariant, absolute derivative is exactly its Relative Derivations.Because axis invariant is the nature with invariance
Reference axis, therefore its absolute derivative perseverance is zero vector.Therefore, axis invariant has the invariance to time diffusion.Have:
[4] in natural coordinates system, pass through axial vectorAnd joint variableRotational coordinates battle array can be described directlyIt is not necessary to establish respective system for the rod piece in addition to root.Meanwhile needing the root coordinate system that defines for ginseng with unique
It examines, the measurement accuracy of system structure parameter can be improved；
[5] axial vector is appliedSuperior operational, by establish include topological structure, coordinate system, polarity, structure parameter and power
Learn the unified multiple axes system kinematics and kinetic model of the risk management of parameter.
Because of base vector elIt is and F[l]Any vector of consolidation, base vectorBe withAny vector of consolidation, againIt is F[l]AndShared unit vector, thereforeIt is F[l]AndShared base vector.Therefore, axis invariantIt is F[l]AndAltogether
Some refers to base.Axis invariant is the natural coordinates base of parametrization, is the primitive of multiple axes system.The translation of fixing axle invariant with
Translation and the rotation for rotating the coordinate system consolidated with it are of equal value.
It is reference with natural system of coordinates when system is in zero-bit, measurement obtains coordinate vectorIn kinematic pair
When movement, axial vectorIt is invariant；Axial vectorAnd joint variableUniquely determine kinematic pairRotation relation.
Therefore, using natural coordinates system, when system is in zero-bit, only a public referential need to be determined, without
Respective body coordinate system must be determined for each rod piece in system, because they are uniquely determined by axis invariant and natural coordinates.When
Carry out network analysis when, in addition to pedestal system, with rod piece consolidation other naturals system of coordinates only occur in it is conceptive, and with it is actual
It measures unrelated.Natural coordinates system is multiple axes system (MAS) theory analysis and engineering effect:
(1) the structural parameters measurement of system needs to measure with unified referential；Otherwise, not only engineering survey process is tired
It is trivial, and introduce different system and can introduce bigger measurement error.
(2) natural coordinates system is applied, in addition to root rod piece, the natural coordinates system of other rod pieces is by structure parameter and joint
Variable determines naturally, facilitates the kinematics and dynamics analysis of MAS system.
(3) in engineering, it can realize using optical measuring apparatus such as laser trackers to the accurate of fixing axle invariant
Measurement.
(4) due to the special case that kinematic pair R and P, screw pair H, Contact Pair O are cylindrical pair C, can simplify using cylindrical pair
MAS kinematics and kinetics analysis.
Define 3 invariants: the amount measured independent of one group of coordinate system is referred to as invariant.
Define 4 rotational coordinates vectors: around coordinate vectorTurn to Angle PositionCoordinate vectorFor
Define 5 translation coordinate vectors: along coordinate vectorIt is translatable to line positionCoordinate vectorFor
Define 6 natural coordinates: using natural coordinates axial vector as reference direction, the Angle Position of relative system zero-bit or line position
It sets, is denoted as ql, referred to as natural coordinates；The amount mapped one by one with natural coordinates is referred to as joint variable；Wherein:
Define 7 mechanical zeros: for kinematic pairT is carved at the beginning0When, the zero-bit of joint absolute encoderIt is different
It is set to zero, which is known as mechanical zero；
Therefore jointControl amountFor
Define 8 proper motion vectors: will be by natural coordinates axial vectorAnd natural coordinates qlDetermining vectorReferred to as certainly
Right motion vector.Wherein:
Proper motion vector realizes the Unified Expression of axis translation and rotation.It will be determined by natural coordinates axial vector and joint
Vector, such asReferred to as free movement vector, also known as free spiral rotation.Obviously, axial vectorBe it is specific from
By spiral.
Define 9 joint spaces: with joint natural coordinates qlThe space of expression is known as joint space.
Define 10 configuration spaces: the cartesian space of expression position and posture (abbreviation pose) is referred to as configuration space, is double
Vector space or the space 6D.
It defines 11 natural joint spaces: being reference with natural system of coordinates, pass through joint variableIt indicates, in system zero-bit
Must haveJoint space, referred to as natural joint space.
As shown in Fig. 2, given chain linkOrigin OlBy position vectorThe axial vector of constraintFor fixed axial vector, note
ForWherein:
Axial vectorIt is the natural reference axis of joint natural coordinates.CauseIt is axis invariant, therefore claimsIt is constant for fixing axle
Amount, it characterizes kinematic pairStructural relation, that is, natural coordinates axis has been determined.Fixing axle invariantIt is chain linkStructure
The natural description of parameter.
Define 12 natural coordinates shaft spaces: using fixing axle invariant as nature reference axis, with corresponding natural coordinates table
The space shown is known as natural coordinates shaft space, referred to as natural shaft space.It is the 3d space with 1 freedom degree.
As shown in Fig. 2,AndNot because of rod piece ΩlMovement and change, be constant structural reference amount.It has determined
Axis l is relative to axisFive structural parameters；With joint variable qlTogether, rod piece Ω is completely expressedlThe position 6D shape.It is givenWhen, the natural system of coordinates of rod piece consolidation can be by structural parametersAnd joint variableIt is unique true
It is fixed.Claim axis invariantFixing axle invariantJoint variableAndFor natural invariant.Obviously, constant by fixing axle
AmountAnd joint variableThe joint nature invariant of compositionWith by coordinate systemTo F[l]Determining space bit shapeWith mapping relations one by one, i.e.,
Given multiple axes system D={ T, A, B, K, F, NT }, in system zero-bit, as long as establishing pedestal system or inertial system, with
And the reference point O on each axisl, other member coordinates also determine naturally.Substantially, it is only necessary to determine pedestal system or inertial system.
A given structure diagram with closed chain connected by kinematic pair, can select any of circuit kinematic pair,
The stator and mover that form the kinematic pair is separated；To obtain a loop-free tree, referred to as Span
Tree.T indicates the span tree with direction, to describe the topological relation of tree chain movement.
I is structural parameters；A is axis sequence, and F is rod piece referential sequence, and B is rod piece body sequence, and K is kinematic pair type sequence
Column, NT are the sequence, that is, non-tree for constraining axis.To take axis sequenceMember.Revolute pair R, prismatic pair P, screw pair H, contact
Secondary O is the special case of cylindrical pair C.
The basic topology symbol and operation for describing kinematic chain are the bases for constituting kinematic chain topology notation, and definition is such as
Under:
[1] kinematic chain by partial ordering set (] mark.
【2】A[l]For the member for taking axis sequence A；Because there is axis name l unique number to correspond to A[l]Serial number, therefore A[l]Meter
Calculation complexity is O (1).
【3】For the father's axis for taking axis l；AxisComputation complexity be O (1).Computation complexity O () indicates calculating process
Number of operations, the number for being often referred to floating multiplication and adding.With floating multiplication with plus number expression computation complexity it is very loaded down with trivial details, therefore often
Using the primary operational number in algorithm cyclic process；Such as: the number of the operations such as joint position, speed, acceleration.
【4】To take axis sequenceMember；Computation complexity is O (1).
【5】l1kTo take the kinematic chain by axis l to axis k, output is expressed asAndRadix note
For |l1k|。l1kImplementation procedure: it executesIfThen executeOtherwise, terminate.l1kComputation complexity be O (|l1k|)。
【6】l1 is the son for taking axis l.The operation indicatesIn find the address k of member l；To obtain the sub- A of axis l[k]。
CauseWithout partial order structure, thereforel1 computation complexity is
【7】lL, which indicates to obtain, closes subtree by what axis l and its subtree were constituted, l L is the subtree without l；Recurrence executesl1, it calculates
Complexity is
[8] branch, the increase of subtree and non-tree arc and delete operation are also necessary component part；To pass through dynamic
Span tree and Dynamic Graph describe primary topology.In branchl1kIn, ifThen remember
I.e.Indicate the son that member m is taken in branch.
Define following formula or expression-form:
Axis and rod piece have one-to-one correspondence property；The attribute amount of between centersAnd the attribute amount between rod pieceWith partial order.
Agreement:Indicate attribute occupy-place；If attribute p or P be about position,It is interpreted as coordinate system's
Origin is to F[l]Origin；If attribute p or P be about direction,It is interpreted as coordinate systemTo F[l]。
AndIt should be interpreted as the function about time t respectivelyAndAndAndIt is t0Moment
Constant or constant array.But romanAndIt should be regarded as constant or constant array.
Arrange in the application: in kinematic chain symbolic operation system, attribute variable or constant with partial order, nominally
Index comprising indicating partial order；Comprising the upper left corner and lower right corner index or include the upper right corner and lower right corner index；They
Direction always by upper left corner index to lower right corner index, or by upper right corner index to lower right corner index, be narration in the application
Simplicity omits the description in direction sometimes, even if omitting, those skilled in the art are by character expression it will also be appreciated that this Shen
Please in use each parameter, for certain attribute accord with, their direction is always by the upper left corner index of partial order index to the lower right corner
Index, or by upper right corner index to lower right corner index.Such as:It can sketch (to indicate the vector that is translatable by k to l)；Indicate (by
K is to l's) line position；krlIndicate (by k to l's) translation vector；Wherein: r indicates that " translation " attribute symbol, remaining attribute symbol correspond to
Are as follows: attribute, which accords with φ, indicates " rotation "；Attribute, which accords with Q, indicates " rotational transformation matrix "；Attribute symbol 1 indicates " kinematic chain "；Attribute accords with u table
Show " unit vector "；Attribute, which accords with ω, indicates " angular speed "；Footmark is that i indicates inertial coodinate system or earth coordinates；Other footmarks
It can be other letters, or number.
The specification of symbols of the application and agreement are according to the partial order of kinematic chain, chain link be kinematic chain basic unit this two
What a principle determined, reflect the substantive characteristics of kinematic chain.Chain index expression is connection relationship, the reference of upper right index characterization
System.It is succinct using this symbolic formulation, accurate, convenient for exchange and wirtiting.Meanwhile they are the notations of structuring,
The element and relationship for forming each attribute amount are contained, is convenient for computer disposal, lays the foundation for computer auto-building modle.Index
Meaning needs the background i.e. context accorded with by attribute to be understood；Such as: if attribute symbol is translation type, the upper left corner refers to
Mark origin and the direction of indicates coordinate system；If attribute symbol is rotary type, the direction of upper left corner index expression coordinate system.
(1)lSPoint S in rod piece l；And the point S in S representation space.
(2)The origin O of rod piece kkTo the origin O of rod piece llTranslation vector；
krl?In natural system of coordinates F[k]Under coordinate vector, i.e., by the coordinate vector of k to l；
(3)Origin OkTo point lSTranslation vector；
?In F[k]Under coordinate vector；
(4)Origin OkTo the translation vector of point S；
krS?In F[k]Under coordinate vector；
(5)Connecting rodAnd the kinematic pair of rod piece l；
Kinematic pairAxial vector；
And?Exist respectivelyAnd F[l]Under coordinate vector；It is axis invariant, is a structural constant；
For gyration vector, gyration vector/angle vectorIt is free vector, i.e., the vector can free shift；
(6)Along axisLine position (translation position),
Around axisAngle Position, i.e. joint angle, joint variable are scalar；
(7) when lower left corner index is 0, mechanical zero is indicated；Such as:
Translation shaftMechanical zero,
Rotation axisMechanical zero；
(8) 0- three-dimensional null matrix；1- three-dimensional unit matrix；
(9) arrange: " " indicate continuation character；Indicate attribute occupy-place；Then
Power symbolIt indicatesX power；Upper right corner footmark ∧ orIndicate separator；Such as:OrForX power.
It indicatesTransposition, indicate to set transposition, not to member execute transposition；Such as:
For projection symbol, indicate vector or second-order tensor to the projection vector or projection sequence of reference base, i.e. coordinate vector
Or coordinate array, projection are dot-product operation " "；Such as: position vectorIn coordinate system F[k]In projection vector be denoted as
For multiplication cross symbol；Such as:It is axis invariantMultiplication cross matrix；Give any vectorMultiplication cross matrix be
Multiplication cross matrix is second-order tensor.
The priority that multiplication cross accords with operation is higher than projection symbolPriority.Projection symbolPriority be higher than member access symbolOrMember accesses symbolPriority is accorded with higher than power
(10) projection vector of the unit vector in earth coordinatesUnit zero-bit vector
(11)By origin when zero-bitTo origin OlTranslation vector, and rememberIndicate position construction parameter.
(12)iQl, the rotation transformation battle array of opposite absolute space；
(13) using natural coordinates axial vector as reference direction, the Angle Position or line position of relative system zero-bit are denoted as ql, claim
For natural coordinates；Joint variableNatural joint coordinate is φl；
(14) orderly set r=[1,4,3,2] is given for oneT, remember r[x]Expression takes the xth row element of set r.Often
Note [x], [y], [z] and [ω] expression takes the column element of the 1st, 2,3 and 4.
(15)i1jIndicate the kinematic chain by i to j；l1kTo take the kinematic chain by axis l to axis k；
Given kinematic chainIf n indicates Descartes's rectangular system, claimFor cartesian axis
Chain；If n indicates nature reference axis, claimFor natural axis chain.
(16) Rodrigues quaternary number expression-form:
Euler's quaternary number expression-form:
Quaternary number (also referred to as axis quaternary number) expression-form of invariant
1. establishing the Lagrange's equation of multiple axes system
Establish the Lagrange's equation of joint space using chain notation, consider particle dynamics system D=A, K, T,
NT, F, B }, free mass point is derived according to Newtonian mechanics firstLagrange's equation；Then, controlled particle is extended to
System.
Conservative forceOpposite particle inertia forceChain sequence having the same, i.e.,With positive sequence, particleConjunction
Power is zero.ParticleEnergy be denoted asAccording to generalized coordinates sequenceWith cartesian space position vector sequence
{irl| l ∈ T } relationship
?
The energy and generalized coordinates of formula (2) application system establish the equation of system.Joint variableWith coordinate vectorirl's
Shown in relationship such as formula (1), formula (1) is referred to as the point transformation of joint space and cartesian space.
Conservative force has opposite chain sequence with inertia force.Constraint in Lagrange system is either consolidation between particle
Constraint, and can be the kinematic constraint between particle system；Rigid body itself is particle systemParticle
Energy has additive property；Kinetic energy of rigid body amount is made of mass center translational kinetic energy and rotational kinetic energy.In the following, just with simple motion secondary R/P
Lagrange's equation is established respectively, is laid the foundation for the new kinetic theory of subsequent further release.
Given rigid body multiple axes system D={ A, K, T, NT, F, B }, inertial space is denoted as i,The energy of axis l is denoted asWherein translational kinetic energy isRotational kinetic energy isGravitational potential energy isAxis l is by the external resultant force in addition to gravitation and closes
Torque is respectivelyDflAndDτl；The quality and mass center rotary inertia of axis l is respectively mlAndThe unit axis invariant of axis u isEnvironment i acts on lIInertial acceleration be denoted asAcceleration of gravityChain sequence is by i to lI；Chain sequence is by lITo i；And
Have
[1] system capacity
Dynamic system D energyIt is expressed as
Wherein:
[2] multiple axes system Lagrange's equation
Multiple axes system Lagrange's equation is obtained by formula (2),
Formula (6) is the governing equation of axis u, i.e., in axis invariantOn equilibrium equation；It is resultant forcei|Dfu
?On component,It is resultant momenti|Dτu?On component.
2. establishing Ju-Kane dynamics preparation equation:
Ju-Kai En (Ju-Kane) dynamics lemma is derived based on multiple axes system Lagrange's equation (6).First carry out
The equivalence proof of Lagrange's equation and kane equation；Then, energy is calculated to the deflected velocity of joint velocity and coordinate, then right
Time derivation finally provides Ju-Kane dynamics lemma.
[1] equivalence proof of Lagrange's equation and kane equation
It proves: considering rigid body k translational kinetic energy pairDeflected velocity the derivative of time is obtained
Consider rigid body k rotational kinetic energy pairDeflected velocity the derivative of time is obtained
Card is finished.
CauseWithIt is uncorrelated, it is obtained by formula (7) and multiple axes system Lagrange's equation (6)
The translational kinetic energy and rotational kinetic energy of dynamic system D is expressed as
Consideration formula (4) and formula (5), that is, have
Formula (7) and formula (8) are the foundations that Ju-Kai En dynamics lemma proves, i.e. Ju-Kai En dynamics preparation is fixed
Reason is substantially of equal value with Lagrangian method.Meanwhile multiple axes system kane equation is contained on the right side of formula (8)；Show that glug is bright
The calculating of the inertia force of day method and Kai Enfa is consistent, i.e., Lagrangian method and Kai Enfa are also of equal value.Formula (8) shows:
Exist in Lagrange's equation (4)The problem of computing repeatedly.
[2] deflected velocity of the energy to joint velocity and coordinate
[2-1] ifAnd consider AndOnly with close subtreeuL is related, by formula (4) and formula (5),
?
[2-2] ifAnd consider AndOnly with close subtreeuL is related, by formula (4) and formula (5),
?
So far, energy is completed to calculate the deflected velocity of joint velocity and coordinate.
[3] derivative to the time is sought
[3-1] ifIt is obtained by formula (7), formula (9) and formula (10)
[3-2] ifIt is obtained by formula (7), formula (12) and formula (13)
So far, the derivation to time t is completed.
[4] Ju-Kane dynamics lemma
Formula (11), formula (14), formula (15) and formula (16) are substituted into formula (8),
Given multiaxis Rigid-body System D={ A, K, T, NT, F, B }, inertial system is denoted as F[i],In addition to
Outside gravity, the bonding force and torque for acting on axis u are denoted as respectivelyi|DfuAndi|Dτu；The quality and mass center rotary inertia of axis k is remembered respectively
For mkAndThe acceleration of gravity of axis k isThen the Ju-Kane dynamics preparation equation of axis u is
Formula (17) is provided with tree chain topological structure.kIIndication rod k mass center I.Because closing subtreeuGeneralized force in L has and can add
Property；Therefore the node for closing subtree has a unique kinematic chain to root, therefore kinematic chaini1nIt can be by kinematic chainuL replacement.
In the following, being directed to Ju-Kane dynamics preparation equation, solve on the right side of formula (17)DfkAndDτkComputational problem, to build
Vertical tree chain Rigid-body System Ju-Kane kinetics equation.
3. establishing tree chain Rigid-body System Ju-Kane kinetic model
Give dead axle chaink∈i1n, there is following deflected velocity calculation formula:
To dead axle chain|i1l| >=2, have following acceleration iterative:
The relationship of left sequence multiplication cross and transposition are as follows:
It is iterative according to kinematics, have:
3.1 external force inverse iterations
It gives by point of application i in environment iSPoint l on to axis lSBilateral external forceAnd moment of faceiτl, their instantaneous axis
Power pexIt is expressed as
Wherein:AndiτlNot byAndControl, i.e.,AndiτlIndependent ofAnd
[1] if k ∈i1l, then haveIt is obtained by formula (19) and formula (18)
I.e.
In formula (26)In formula (21)Chain sequence it is different；The former is active force, and the latter is amount of exercise,
The two is antithesis, has opposite sequence.
[2] if k ∈i1l, then haveIt is obtained by formula (22) and formula (25)
Have
Formula (26) and formula (27) show that environmental activity is equivalent to close subtree in the bonding force or torque of axis kkConjunction of the L to axis k
Formula (26) and formula (27) conjunction are written as by external force or torque
So far, the computational problem of external force inverse iteration is solved.In formula (28), closing subtree has the generalized force of axis k
Additive property；The effect of power has double effect, and is inverse iteration.So-called inverse iteration refers to:It is to need to pass through chain link
Position vector iteration；Sequence and Forward kinematicsThe sequence of calculating is opposite.
3.2 coaxial driving force inverse iterations
If axis l is drive shaft, the driving force and driving moment of axis l is respectivelyAndThen driving forceAnd driving force
SquareThe power p of generationacIt is expressed as
[1] it is obtained by formula (18), formula (19) and formula (29)
I.e.
If axis u and axisIt is coaxial, then haveNote CauseWithIt is unrelated, it is obtained by formula (30)
CauseWithIt is coaxial, therefore have
[2] it is obtained by formula (19), formula (18) and formula (29)
I.e.
If axis u withIt is coaxial, then haveNoteBy
Formula (32)
So far, coaxial driving force inverse iteration computational problem is completed.
The foundation of 3.3 tree chain Rigid-body System Ju-Kane dynamics explicit models:
In the following, first chain Rigid-body System Ju-Kane kinetics equation, abbreviation Ju-Kane equation are set in statement；Then, it provides and builds
Vertical step.
Given multiaxis Rigid-body System D={ A, K, T, NT, F, B }, inertial system is denoted as F[i],In addition to
Outside gravity, the bonding force and torque for acting on axis u existOn component be denoted as respectivelyAndThe quality and mass center of axis k rotates
Inertia is denoted as m respectivelykAndThe acceleration of gravity of axis k isThe bilateral driving force and driving moment of drive shaft u existsOn
Component be denoted as respectivelyAndEnvironment i is respectively to the power and torque of axis lAndiτl；Then axis u tree chain Ju-
Kane kinetics equation is
Wherein: [] expression takes row or column；AndIt is 3 × 3 matrix in block form,AndIt is 3D vector, q is
Joint space.And have,
Wherein, remember
Note
The establishment step of above-mentioned equation are as follows:
NoteTherefore have
The energy of ex ispexFor instantaneous shaft power；pacThe power generated for the driving force and driving moment of drive shaft.
Formula (40) are obtained by formula (26), formula (27), formula (31), formula (33) and formula (41).
By deflected velocity calculation formula formula (19), formula (18) and formula (20) substitute into Ju-Kane dynamics preparation equation (17) and obtain
It is obtained by formula (21)
Consideration formula (43), then have
Equally, consider formula (43), obtain
Formula (43) to formula (45) is substituted into formula (42) and obtains formula (34) to formula (39).
Embodiment 1
Given general 3R mechanical arm as shown in Figure 3, A=(i, 1:3]；Tree chain Ju- is established using method of the invention
Kane kinetics equation, and obtain broad sense inertial matrix.
Step 1 establishes the iterative equation of motion based on axis invariant.
Rotational transform matrix by formula (46) based on axis invariant
?
Kinematics is iterative:
Second-order tensor projection:
It is obtained by formula (48) and formula (47)
By formula (49), formula (47) and formula (55) are obtained
It is obtained by formula (50) and formula (55)
It is obtained by formula (51), formula (55) and formula (57)
It is obtained by formula (52) and formula (55)
It is obtained by formula (53) and formula (55)
Step 2 establishes kinetics equation.First establish the kinetics equation of the 1st axis.It is obtained by formula (37)
It is obtained by formula (39)
The kinetics equation of the 1st axis is obtained by formula (61) and formula (62),
Establish the kinetics equation of the 2nd axis.It is obtained by formula (37)
It is obtained by formula (39)
The kinetics equation of the 2nd axis is obtained by formula (64) and formula (65),
Finally, establishing the kinetics equation of the 3rd axis.It is obtained by formula (37)
It is obtained by formula (39)
The kinetics equation of the 3rd axis is obtained by formula (67) and formula (68),
By formula (61), formula (63) and formula (67) obtain generalized mass matrix.
As long as it follows that stylizedly by the parameter substitution formulas such as the topology of system, structural parameters, matter inertia (36) to formula
(40) Dynamic Modeling can be completed.Pass through programming, it is easy to realize Ju-Kane kinetics equation.Because of subsequent tree chain Ju-
Kane double average is derived with Ju-Kane kinetics equation, and the validity of tree chain Ju-Kane kinetics equation can be by Ju-
Kane canonical form example proves.
3.4 tree chain Rigid-body System Ju-Kane dynamics canonical forms
After establishing system dynamics equation, the problem of being followed by equation solution.In dynamic system emulation, lead to
The often generalized driving forces of the generalized force and drive shaft of given environmental activity, need to solve the acceleration of dynamic system；This is
The direct problem that mechanical equation solves.Before solution, it is necessary first to obtain double average shown in formula (71).
Standardize kinetics equation,
Wherein: RHS-right-hand side (Right hand side)
Obviously, process of normalization is exactly the process for merging all joint velocity items；To obtain joint acceleration
The coefficient of degree.By canonical form that the PROBLEM DECOMPOSITION is kinematic chain and close the canonical form two sub-problems of subtree.
3.4.1 the canonical form equation of kinematic chain
Reversed summation process is converted by the forward recursion procedure of joint velocity item in formula (36) and formula (37), with after an action of the bowels
Continuous application；Obviously, it wherein containing 6 kinds of different types of acceleration items, is handled respectively.
[1] kinematic chain is givenThen have
The derivation step of above formula are as follows:
[2] kinematic chain is givenThen have
The derivation step of above formula are as follows: becauseTherefore
[3] kinematic chain is givenThen have
Above formula can be obtained by following formula, becauseTherefore have
[4] kinematic chain is givenThen have
The derivation step of above formula are as follows: considerFormula (72) are substituted on the left of formula (75) and are obtained
[5] kinematic chain is givenThen have
The derivation step of above formula are as follows: considerFormula (72) are substituted on the left of formula (76) and are obtained
[6] kinematic chain is givenThen have
The derivation step of above formula are as follows: becauseTherefore have
3.4.2 the canonical form equation of subtree is closed
Because closing subtreeuGeneralized force in L has additive property；Therefore the node for closing subtree has a unique movement to root
Chain, the kinematic chain of formula (73) to formula (77)i1nIt can be byuL replacement.It is obtained by formula (73)
It is obtained by formula (74)
It is obtained by formula (75)
It is obtained by formula (76)
It is obtained by formula (77)
So far, had the precondition for establishing canonical form.
3.5 tree chain Rigid-body System Ju-Kane dynamics double averages
In the following, establishing the Ju-Kane standardization kinetics equation of tree construction Rigid-body System.It is convenient for expression, it defines first
Then, formula (36) and formula (37) are expressed as canonical form to formula (82) by applying equation (78).
[1] canonical form of formula (36) is
The specific establishment step of above formula are as follows: obtained by formula (24) and formula (36)
It is obtained by formula (52) and formula (85)
Formula (80) are substituted into previous item on the right side of formula (85) to obtain
Formula (79) are substituted into latter on the right side of formula (86) to obtain
Formula (87) and formula (88) are substituted into formula (86) to obtain
For rigid body k, haveFormula (84) are obtained by formula (35), formula (83) and formula (89).
[2] canonical form of formula (37) is
The specific establishment step of above formula are as follows: obtained by formula (37)
Formula (78) are substituted into previous item (91) on the right side of formula to obtain
Formula (81) are substituted into latter on the right side of formula (91) to obtain
Formula (82) are substituted into intermediate one of formula (91) right side to obtain
By formula (92), formula (93) and formula (94) substitute into formula (92) and obtain
For rigid body k, haveBy formula (35), formula (83) and formula (95) obtain formula (90).
Ju-Kane equation is restated as setting the canonical form side chain Ju-Kane as follows by [3] applying equation (84) and formula (90)
Journey:
Given multiaxis Rigid-body System D={ A, K, T, NT, F, B }, inertial system is denoted as F[i],In addition to
Outside gravity, the bonding force and torque for acting on axis u existOn component be denoted as respectivelyAndThe quality and mass center of axis k rotates
Inertia is denoted as m respectivelykAndThe acceleration of gravity of axis k isThe bilateral driving force and driving moment of drive shaft u existsOn
Component be denoted as respectivelyAndEnvironment i is respectively to the active force and torque of axis lAndiτl；The then Ju- of axis u
Kane dynamics double average is
Wherein:AndIt is 3 × 3 matrix in block form,AndIt is 3D vector；Bonding force for axis u exists
On component,Resultant moment for axis u existsOn component；For joint coordinates；
Also,
In formula, kIIndication rod k mass center I；The quality and mass center rotary inertia of axis k is denoted as m respectivelykAnd For rotation
The inertial matrix of axis u；For the inertial matrix of translation shaft u；hRFor the non-inertial matrix of rotation axis u；hPFor the non-of translation shaft u
Inertial matrix；The bonding force and torque for acting on axis u existOn component be denoted as respectivelyAnd The bilateral driving force and driving moment of drive shaft u existsOn component be denoted as respectivelyAndEnvironment i is respectively to the active force and opplied moment of axis lAndiτl；l1kTo take the fortune by axis l to axis k
Dynamic chain,uL, which indicates to obtain, closes subtree by what axis u and its subtree were constituted.
4. the Ju-Kane kinetics equation of closed chain Rigid-body System is established
In the following, first stating Ju-Kai En (abbreviation Ju-Kane) kinetics equation of closed chain Rigid-body System；Then, to providing
Volume modeling process.
Given multiaxis Rigid-body System D={ A, K, T, NT, F, B }, inertial system is denoted as F[i],Other than gravity, the bonding force and torque for acting on axis u existOn component point
It is not denoted asAndThe quality and mass center rotary inertia of axis k is denoted as m respectivelykAndThe acceleration of gravity of axis k isIt drives
The bilateral driving force and driving moment of moving axis u existsOn component be denoted as respectivelyAndWork of the environment i to axis l
Firmly and opplied moment is respectivelyAndiτl；Axis u is denoted as the generalized constraint force of axis u 'Then there is closed chain Rigid-body System
Ju-Kane kinetics equation:
[1] the Ju-Kane dynamics double average of axis u and axis u ' is respectively
[2] non-tree constraint is secondaryuku′Constraint algebraic equation be
Wherein:
In formula:AndIt is 3 × 3 matrix in block form,AndIt is 3D vector；kIIndication rod k mass center I；Axis k's
Quality and mass center rotary inertia are denoted as m respectivelykAnd For the inertial matrix of rotation axis u；For the used of translation shaft u
Property matrix；hRFor the non-inertial matrix of rotation axis u；hPFor the non-inertial matrix of translation shaft u；For the joint angular speed that is translatable；
For cradle head angular speed.
Specific modeling process is as follows:
Non-tree constraint is secondaryKeep obligatory point uSAnd u 'SUnanimously, therefore have
It is obtained by formula (114)
Generalized constraint force of the axis u to axis u ' in constraint axis directionAnd broad sense of the axis u ' to axis u in constraint axis direction
Restraining forcePower be respectively
It is obtained by formula (115) and formula (116)
It is obtained by formula (115)
δ indicates increment；
It is obtained by formula (18) and formula (118)
Therefore have
Formula (105) are obtained by formula (110) and formula (122).It is obtained by formula (19) and formula (119)
Formula (106) are obtained by formula (111) and formula (123).It is obtained by formula (19) and formula (120)
Formula (107) are obtained by formula (112) and formula (124).It is obtained by formula (19) and formula (121)
(108) are obtained by formula (113) and formula (125).By formula (18), formula (116) and formula (110) are obtained
Generalized constraint forceAndIt is vector, formula (109) is obtained by formula (126) and formula (127).It follows that partially fast
Degree is mainly used in the inverse iteration of power.Generalized constraint forceAndIt is considered as external force.
Formula (103) and formula (104) are obtained according to the Ju-Kane dynamics double average of axis u.
It is empty that Ju-Kane closed chain dynamics of rigid bodies based on joint space nature axis chain overcomes cartesian coordinate axes chain
Between limitation:
[1] in the newton Euler Dynamics based on cartesian coordinate axes chain, non-tree kinematic pairuku′∈ P constraint is unable to table
It reachesAndOrAndSituation, i.e., cannot express rack and pinion, worm gear and worm screw etc. about
Beam.And the non-tree of the application constrains pairuku′Constraint algebraic equation (105) to formula (108) any constraint class shape can be expressed,
And physical connotation is apparent；
[2] in the newton Euler Dynamics based on cartesian coordinate axes chain, non-tree kinematic pair Algebraic Constraint equation is
6D's；And formula (105) to formula (108) indicate to be 3D non-tree kinematic pair Algebraic Constraint equation, to reduce system equation solution
Complexity；
[3] in the newton Euler Dynamics based on cartesian coordinate axes chain, non-tree kinematic pair Algebraic Constraint equation is
About 6D vector space absolute acceleration, be about joint coordinates, joint velocity it is iterative, have accumulated error；And formula
It (105) is that ensure that the accuracy of constraint equation about joint velocity to formula (108).
5. the restraining force based on axis invariant solves
For the kinematic axis u of no power waste, remembers its restraining force and restraint moment vector is respectivelyObviously,
Have
It is calculated by formula (96) and formula (139)Formula (128) indicates that movement axial vector and kinematic axis restraining force have nature
Orthocomplemented relationship.
IfAndFor kinematic pairTwo orthogonality constraint axis, and constrain axis it is orthogonal with kinematic axis, i.e.,
NoteTo constrain axis axial vector,In alternate form (96)It recalculates
Wherein:
After completing forward dynamics normal solution, according to calculated joint velocityBy formula (130) available joint
Restraining force sizeRestraint moment sizeWhenWhen, it is obtained by formula (130)AndIn formula (130)
Synchronization motion state having the same and interior external force.Only occurs the balance of power and torque upwards in kinematic axis；And it is constraining
Axial, kinetics equation is unsatisfactory for, i.e., power is not necessarily balanced with torque.
By the available joint constraint power size of formula (130)AndRestraint moment sizeAndIf remembering kinematic axis
Radial force vectorAnd moment vectorThen have
If note kinematic axis radial force size isAnd torque size isIt is obtained by formula (133)
So far, the calculating of axis radial constraint generalized force is completed.
Set the corresponding joint velocity sequence note of chain Rigid-body SystemIt can be calculated according to following step:
It will be known as axis chain rigid body according to the rigid motion chain broad sense inertial matrix of kinematic axis type and natural reference axis expression
Broad sense inertial matrix, abbreviation axis chain broad sense inertial matrix.
Define orthogonal complement matrixAnd corresponding multiplication cross matrix
Given multiaxis Rigid-body System D={ A, K, T, NT, F, B },Axis kinetics equation (96) each in system is pressed
Row arrangement；Generalized force and immesurable environment force is driven to be denoted as f on the axis after rearrangementC, measurable environment broad sense active force
It is denoted as fi；The corresponding joint velocity sequence of system is denoted asAfter rearrangementIt is denoted as h；Consideration formula (135)；Then this is
System kinetics equation be
It is obtained by formula (136)
Wherein,
It is obtained by formula (136)
6. broad sense internal friction and viscous force calculate
After the calculating for completing axis radial constraint generalized force, the radial constraint size of kinematic axis u is obtainedAnd restraining force
Square sizeAs shown in Figure 3, Figure 4, remember the internal friction size of kinematic axis u and inner friction torque size is respectivelyAnd
The viscous force and viscous moment size of kinematic axis u be respectivelyAnd
Therefore have
Wherein:sk[u]- the coefficient of internal friction of kinematic axis u,ck[u]- the coefficient of viscosity of kinematic axis u；Sign () expression takes just
Or minus symbol.
Note broad sense internal friction and the resultant force and resultant moment of viscous force are respectivelyBy formula (140) and formula (141)
?
The broad sense internal friction and viscous force of kinematic axis are the internal force of kinematic axis, because they exist only in movement axially
On, it is always orthogonal with axis radial constraint.When kinematic axis axial direction dynamic action dynamic balance, no matter broad sense internal friction and viscous
Stagnant power whether there is or how is size, does not affect the motion state of dynamic system；So not influencing the radial direction of kinematic axis about
Beam force.Therefore, the radial constraint size of kinematic axis u is calculated by formula (130) to formula (134)And restraint moment sizeWhen,
It can not consider the broad sense internal friction and viscous force of kinematic axis.
Claims (5)
1. a kind of closed chain manipulator Dynamic Modeling and calculation method based on axis invariant, characterized in that
Given multiaxis Rigid-body System D={ A, K, T, NT, F, B }, inertial system is denoted as F[i],A is axis sequence, and F is rod piece referential sequence, and B is rod piece body sequence, and K is fortune
Dynamic pair type sequence, NT are the sequence, that is, non-tree for constraining axis；Other than gravity, the bonding force and torque for acting on axis u existOn
Component be denoted as respectivelyAndThe quality and mass center rotary inertia of axis k is denoted as m respectivelykAndThe acceleration of gravity of axis k isThe bilateral driving force and driving moment of drive shaft u existsOn component be denoted as respectivelyAndEnvironment i is to axis
The active force and opplied moment of l be respectivelyAndAxis u is denoted as the generalized constraint force of axis u 'Then closed chain Rigid-body System
Ju-Kane kinetics equation are as follows:
[1] the Ju-Kane dynamics double average of axis u and axis u ' is
Wherein:AndIt is 3 × 3 matrix in block form,AndIt is 3D vector；
[2] non-tree constraint is secondaryuku′Constraint algebraic equation be
Wherein:
In formula:AndIt is 3 × 3 matrix in block form,AndIt is 3D vector；kIIndication rod k mass center I；The quality of axis k
And mass center rotary inertia is denoted as m respectivelykAnd For the inertial matrix of rotation axis u；For the moment of inertia of translation shaft u
Battle array；hRFor the non-inertial matrix of rotation axis u；hPFor the non-inertial matrix of translation shaft u；For the joint angular speed that is translatable；To turn
Movable joint angular speed.
2. the closed chain manipulator Dynamic Modeling and calculation method according to claim 1 based on axis invariant, feature
It is,
Restraining force solution procedure based on axis invariant are as follows:
For the kinematic axis u of no power waste, remembers its restraining force and restraint moment vector is respectivelyThen have
Above formula indicates that movement axial vector and kinematic axis restraining force have the orthocomplemented relationship of nature；
IfAndFor kinematic pairTwo orthogonality constraint axis, and constrain axis it is orthogonal with kinematic axis, i.e.,
NoteTo constrain axis axial vector, obtain
Wherein:
According to joint velocityJoint constraint power size is obtained by formula (130)Restraint moment sizeWhen
When, it is obtained by formula (130)AndSynchronization motion state having the same and interior external force in formula (130)；Only exist
There is the balance of power and torque upwards in kinematic axis；And it is axial in constraint, kinetics equation is unsatisfactory for, i.e., power with torque is different allocates
Weighing apparatus；
By the available joint constraint power size of formula (130)AndRestraint moment sizeAndIf remembering kinematic axis radial force
VectorAnd moment vectorThen have
If note kinematic axis radial force size isAnd torque size isIt is obtained by formula (133)
So far, the calculating of axis radial constraint generalized force is completed.
3. the closed chain manipulator Dynamic Modeling and calculation method according to claim 2 based on axis invariant, feature
It is,
The radial constraint size of kinematic axis u is calculated by formula (130) to formula (134)And restraint moment sizeWhen, do not consider
The broad sense internal friction and viscous force of kinematic axis.
4. the closed chain manipulator Dynamic Modeling and calculation method according to claim 2 based on axis invariant, feature
It is,
Consider the restraining force solution procedure based on axis invariant of broad sense internal friction and viscous force are as follows:
After the calculating for completing axis radial constraint generalized force, the radial constraint size of kinematic axis u is obtainedAnd restraint moment is big
It is smallRemember kinematic axis u internal friction size and inner friction torque size be respectivelyAndThe viscous force of kinematic axis u and
Viscous moment size is respectivelyAndThen
Wherein:sk[u]- the coefficient of internal friction of kinematic axis u,ck[u]- the coefficient of viscosity of kinematic axis u；Sign () expression takes positive or negative
Symbol；
Note broad sense internal friction and the resultant force and resultant moment of viscous force are respectivelyIt is obtained by formula (140) and formula (141)
Wherein:sk[u]- the coefficient of internal friction of kinematic axis u,ck[u]- the coefficient of viscosity of kinematic axis u；Sign () expression takes positive or negative
Symbol；For cradle head speed；For the joint velocity that is translatable.
5. the closed chain manipulator Dynamic Modeling and calculation method according to claim 2 based on axis invariant, feature
It is,
Set chain Ju-Kane canonical form equation
Wherein:AndIt is 3 × 3 matrix in block form,AndIt is 3D vector；Bonding force for axis u existsOn
Component,Resultant moment for axis u existsOn component；For joint coordinates；
Also,
In formula, kIIndication rod k mass center I；The quality and mass center rotary inertia of axis k is denoted as m respectivelykAnd For rotation axis u
Inertial matrix；For the inertial matrix of translation shaft u；hRFor the non-inertial matrix of rotation axis u；hPFor the non-used of translation shaft u
Property matrix；The bonding force and torque for acting on axis u existOn component be denoted as respectivelyAnd The bilateral driving force and driving moment of drive shaft u existsOn component be denoted as respectivelyAndEnvironment i is respectively to the active force and opplied moment of axis lAnd llkTo take the fortune by axis l to axis k
Dynamic chain,uL, which indicates to obtain, closes subtree by what axis u and its subtree were constituted.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810933434.5A CN109086544B (en) | 2018-08-16 | 2018-08-16 | Closed-chain robot dynamics modeling and resolving method based on axis invariant |
| PCT/CN2018/112583 WO2020034399A1 (en) | 2018-08-16 | 2018-10-30 | Closed-chain robot dynamics modeling and solution method based on axis invariant |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810933434.5A CN109086544B (en) | 2018-08-16 | 2018-08-16 | Closed-chain robot dynamics modeling and resolving method based on axis invariant |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109086544A true CN109086544A (en) | 2018-12-25 |
| CN109086544B CN109086544B (en) | 2019-12-24 |
Family
ID=64793412
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810933434.5A Active CN109086544B (en) | 2018-08-16 | 2018-08-16 | Closed-chain robot dynamics modeling and resolving method based on axis invariant |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN109086544B (en) |
| WO (1) | WO2020034399A1 (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103593524A (en) * | 2013-11-13 | 2014-02-19 | 北京航空航天大学 | Dynamics modeling and analyzing method for aerospace vehicle |
| US20150343633A1 (en) * | 2011-08-04 | 2015-12-03 | Aldebaran Robotics S.A | Robot with joints of variable rigidity and method for calculating said optimized rigidity |
| CN106249616A (en) * | 2016-07-22 | 2016-12-21 | 上海航天控制技术研究所 | One server mechanical arm dynamic modeling method and system in-orbit |
| CN106695797A (en) * | 2017-02-22 | 2017-05-24 | 哈尔滨工业大学深圳研究生院 | Compliance control method and system based on collaborative operation of double-arm robot |
| CN108372505A (en) * | 2018-01-31 | 2018-08-07 | 国机智能技术研究院有限公司 | A kind of collision detection algorithm and system based on dynamic (dynamical) robot |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104573255B (en) * | 2015-01-22 | 2017-10-31 | 天津理工大学 | It is a kind of to be modeled and method for solving based on the mechanical arm for improving multi-body system transfer matrix |
| CN106055733B (en) * | 2016-05-10 | 2017-06-09 | 中国人民解放军国防科学技术大学 | The kinetic parameter of multifunction structure determines method |
| CN107256284B (en) * | 2017-05-10 | 2022-06-21 | 中国北方车辆研究所 | Real-time interactive multi-gait dynamics modeling method and system for quadruped robot |
| CN107529630B (en) * | 2017-06-23 | 2021-06-29 | 西北工业大学 | Method for building dynamic model of space robot |
| CN108038286B (en) * | 2017-11-30 | 2021-12-03 | 长安大学 | Dynamics modeling method of two-degree-of-freedom redundant drive parallel robot |
-
2018
- 2018-08-16 CN CN201810933434.5A patent/CN109086544B/en active Active
- 2018-10-30 WO PCT/CN2018/112583 patent/WO2020034399A1/en active Application Filing
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150343633A1 (en) * | 2011-08-04 | 2015-12-03 | Aldebaran Robotics S.A | Robot with joints of variable rigidity and method for calculating said optimized rigidity |
| CN103593524A (en) * | 2013-11-13 | 2014-02-19 | 北京航空航天大学 | Dynamics modeling and analyzing method for aerospace vehicle |
| CN106249616A (en) * | 2016-07-22 | 2016-12-21 | 上海航天控制技术研究所 | One server mechanical arm dynamic modeling method and system in-orbit |
| CN106695797A (en) * | 2017-02-22 | 2017-05-24 | 哈尔滨工业大学深圳研究生院 | Compliance control method and system based on collaborative operation of double-arm robot |
| CN108372505A (en) * | 2018-01-31 | 2018-08-07 | 国机智能技术研究院有限公司 | A kind of collision detection algorithm and system based on dynamic (dynamical) robot |
Non-Patent Citations (3)
| Title |
|---|
| HEHUA JU ET AL.: ""A Dynamics method for 2-dof manipulator based on ju-kane dynamics method"", 《"I-CREATE 2018 PROCEEDINGS OF 12TH INTERNATIONAL CONVENTION ON REHABILITATION ENGINEERING AND ASSISTIVE TECHNOLOGY"》 * |
| 居鹤华 等: ""月面巡视器实时动力学建模与牵引控制"", 《宇航学报》 * |
| 马树全: "" 含多闭链结构工业机器人动力学建模分析及结构优化"", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109086544B (en) | 2019-12-24 |
| WO2020034399A1 (en) | 2020-02-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11491649B2 (en) | Axis-invariant based multi-axis robot kinematics modeling method | |
| WO2020034420A1 (en) | Axis-invariant-based inverse kinematics modeling and solving method for multi-axis robot | |
| WO2020034422A1 (en) | Axis-invariant-based forward kinematics modeling and solving method for multi-axis robot system | |
| Kecskeméthy et al. | An object-oriented approach for an effective formulation of multibody dynamics | |
| CN109079784B (en) | A kind of multi-axis robot system modelling and control method based on axis invariant | |
| Nelson et al. | Simplified kinematic analysis of bevel epicyclic gear trains with application to power-flow and efficiency analyses | |
| CN108959829A (en) | Non-ideal articulated robot Dynamic Modeling and calculation method based on axis invariant | |
| CN109015641B (en) | The inverse solution modeling of general 6R mechanical arm based on axis invariant and calculation method | |
| CN109117451A (en) | Tree chain Dynamic Modeling in Robotics and calculation method based on axis invariant | |
| CN109086544A (en) | Closed chain manipulator Dynamic Modeling and calculation method based on axis invariant | |
| CN108803350A (en) | Moving base multi-axis robot Dynamic Modeling and calculation method based on axis invariant | |
| CN109079850A (en) | Based on axis invariant multi-axis robot D-H system and D-H parameter determination method | |
| CN109291047B (en) | Based on axis invariant and the inverse solution modeling method of DH parameter 1R/2R/3R | |
| Song et al. | Analysis of dynamic modeling and solution of 3-RPS parallel mechanism based on conformal geometric algebra | |
| CN108942943B (en) | Positive kinematics calculation method of multi-axis robot based on axis invariants | |
| CN108959828A (en) | The inverse solution modeling of general 3R mechanical arm based on axis invariant and calculation method | |
| Guo et al. | An innovative joint-space explicit dynamics symbolic computation model for closed-chain mechanisms | |
| CN109033688B (en) | Inverse solution modeling and resolving method for universal 7R mechanical arm based on axis invariant | |
| CN108927825B (en) | Multi-axis robot structural parameters accurate measurement method based on axis invariant | |
| Fang et al. | Design and implementation of a novel fuzzy controller with DSP for rotary inverted pendulum | |
| Han et al. | Technology-oriented synchronous optimal design of a 4-degrees-of-freedom high-speed parallel robot | |
| Pyne | Model-based digital twin of a heavy duty machinery | |
| Li et al. | Simulation and trajectory optimization of articulated robots via spectral variational integrators | |
| Wang et al. | Friction Identification of Heavy-Duty Manipulator CMOR Using Markov Chain Monte Carlo Method | |
| Jia et al. | Research on contact constraints linearization of dexterous hand and grasping forces optimization |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |