CN102207988B - Efficient dynamic modeling method for multi-degree of freedom (multi-DOF) mechanical arm - Google Patents

Abstract

The invention discloses an efficient modeling method for a multi-degree of freedom (multi-DOF) mechanical arm. The method mainly comprises the following steps of: describing speed, acceleration, force and moment of each joint of the mechanical arm by using the screw theory; performing inverse dynamic modeling on the mechanical arm by using the spatial operator algebraic theory; and obtaining a generalized inertial mass matrix of the mechanical arm and a factorization form of the inverse matrix of the mechanical arm by using a Kalman filter smoothing method so as to obtain an efficient lower dynamic model. According to the method, the efficient dynamic calculation problem of the multi-DOF mechanical arm is solved, and the calculation efficiency of the method is first power magnitude order of the degree of freedom of the mechanical arm; meanwhile, the method has strong theoretical property, intuitive expression form and definite physical significance.

Description

A kind of multi freedom degree mechanical arm efficient dynamic modeling method

Technical field

The present invention relates to a kind of multi freedom degree mechanical arm dynamic modeling method, belong to mechanical arm modeling technique field.

Background technology

Along with scientific and technological development, mechanical arm is widely used in the work such as the assembling, maintenance of processing, assembling and the space station of factory floor, and mechanical arm technology has become important research direction.Manipulator Dynamics is the theoretical foundation of carrying out mechanical arm Simulation Control and structural design, dynamic modeling method for mechanical arm has been tending towards ripe, yet along with the increase of mechanical arm degree of freedom, its dynamics calculation will be more complicated, be difficult to meet calculating real-time.In order to complete smoothly the real-time simulation of mechanical arm, control, the efficient dynamic calculating research of carrying out mechanical arm is very important.

At present, the dynamic modeling method for mechanical arm mainly contains the Newton-Euler method based on vector mechanics, the Lagrangian method based on analytical mechanics and Kai En (Kane) method of taking into account vector mechanics and analytical mechanics.But the counting yield of these methods is all lower, the counting yield of Newton-Euler and Lagrangian modeling method is generally the cube order of magnitude O (N of mechanical arm number of degrees of freedom, ³), the counting yield of Kane method is the O (N of mechanical arm degree of freedom ²).When mechanical arm number of degrees of freedom, increases, dynamics calculation amount is exponential increase, is difficult to reach the requirement of real-time control.In order to address the above problem, the invention provides a kind of efficient dynamic modeling method of how free mechanical arm, making dynamics calculation efficiency is mechanical arm number of degrees of freedom, object first power order of magnitude O (N).

Summary of the invention

The object of the invention is the deficiency for above-mentioned dynamics calculation method, a kind of high-level efficiency dynamic modeling method for multi freedom degree mechanical arm is provided.

The technical solution adopted in the present invention is: first utilize spinor theory to describe the speed in each joint of mechanical arm, acceleration, power and moment; Then utilize spatial operator algebra theory to set up the Inverse Dynamic Equation of mechanical arm; Finally according to Kalman Filtering smoothing method, obtain the factorization form of mechanical arm general mass matrix and inverse matrix thereof, simplify its inversion calculation, thereby obtain more efficient forward dynamics accounting equation.

Concrete modeling method is as follows:

(1) utilize spinor theory method for expressing to describe the speed V in each joint of mechanical arm (k)=[ω _k, v _k], acceleration and six-dimensional force f (k)=[N of joint _k, F _k];

(2) the inertial mass matrix M (k) of each rod member of calculating machine arm, and define the space transfer operator between the adjacent rod member of mechanical arm

$\mathrm{\φ}(k+1,k)=\left(\begin{array}{cc}{E}_I& \tilde{l}(k+1,k)\\ 0& E_I\end{array}\right)---\left(1\right)$

The transition operator of its upper triangular matrix form can be realized the inside recursion (from 0 to n) of force and moment, the transition operator of lower triangular matrix form can be realized the outside recursion (from n to 0) of speed and acceleration, according to defined transition operator, complete the speed recursion calculating of mechanical arm and the recursion of power/moment are calculated, obtain joint moment recurrence equation T (k)=H (k) .f (k) of mechanical arm;

(3) the n dimension speed operator V=[V (1) of definition mechanical arm ... V (n-1), V (n)] ^t, and be respectively α with acceleration operator, coriolis force operator, centrifugal force operator, power operator, the moment operator of same formal definition mechanical arm, and a, b, f, T, can obtain Inverse Dynamics of Manipulators accounting equation according to the recursive expression of step (2):

$T=M_G\stackrel{\·\·}{\mathrm{\θ}}+C---\left(2\right)$

Wherein:

M _G＝HφMφ ^TH ^T

C＝Hφ(Mφ ^Ta+b) (3)

(4) utilize Kalman Filtering smoothing method to obtain the factorization expression-form M of general mass matrix MG in the calculating of mechanical arm forward dynamics _g=(I+H Φ K) D (I+H Φ K) ^t, with and many factors Xiang Lian of inverse matrix take advantage of expression-form $M_G^{-1}={(I-\mathrm{H\ψK})}^T{D}^{-1}(I-\mathrm{H\ψK});$

(5) definition space operator P, D, G, K, ψ, and utilize the M (k), the φ (k+1, k) that in step (2), solve to obtain its recursion calculation expression:

P(k)＝ψ(k，k-1)P(k-1)ψ ^T(k，k-1)+M(k)

D(k)＝H(k)P(k)H ^T(k)

G(k)＝P(k)H ^T(k)D ^-1(k) (4)

K(k)＝φ(k+1，k)G(k)

ψ(k+1，k)＝φ(k+1，k)-K(k)H(k)

By the combination of formula (3) recursion result of calculation, obtain operator D, K, the expression of ψ solves form;

(6) according to the M of step (4) _gd in equation and the step of inverting (5), K, the calculation expression of ψ, obtains forward dynamics accounting equation:

$\stackrel{\·\·}{\mathrm{\θ}}={(I-\mathrm{H\ΦK})}^T{D}^{-1}(I+\mathrm{H\ψK})T_{\stackrel{\·\·}{\mathrm{\θ}}}---\left(5\right)$

Wherein joint moment for input, obtains joint acceleration according to forward dynamics accounting equation, thereby can be in the hope of mechanical arm in moment speed and angle under effect.

advantage of the present invention

The present invention relates generally to a kind of efficient dynamic modeling method of mechanical arm, spatial operator algebra theory is applied in Manipulator Dynamics Modeling Calculation, it is advantageous that (1) eliminated redundant computation, make Manipulator Dynamics computation complexity be reduced to the first power order of magnitude O (N) of mechanical arm number of degrees of freedom; (2) by factorization, improve the inversion calculation of mechanical arm general mass matrix, thereby further improved the forward dynamics counting yield of mechanical arm; (3) have clear, succinct physics expression-form, and during programming realizes, its dynamics calculation efficiency is compared traditional newton's Euler Dynamics method have been had and has increased substantially.The method is applied to 7 degree-of-freedom manipulator Dynamic Modeling, compares Nuton-Euler method, its counting yield be significantly improved (seeing embodiment 1).

Accompanying drawing explanation

Fig. 1 is any degree-of-freedom manipulator schematic diagram;

Fig. 2 is flow chart of the present invention;

Fig. 3 is the mechanical arm configuration of the embodiment of the present invention 1;

Fig. 4 is speed, the angular velocity curve map of mechanical arm tail end in the embodiment of the present invention 1, wherein,

Fig. 4-A is terminal velocity input figure;

Fig. 4-B is end turning rate input figure;

Fig. 5 is that embodiment 1 adopts after modeling of the present invention, the joint of mechanical arm moment output map obtaining, wherein,

Fig. 5-A is joint 1-4 moment output map;

Fig. 5-B is joint 5-7 moment output map;

Embodiment

The efficient dynamic computing method that the invention provides a kind of multi freedom degree mechanical arm, below in conjunction with accompanying drawing, the invention will be further described.Mechanical arm is comprised of a plurality of joints and connecting rod, and the symbol description of its representation and each rod member, joint and coordinate system as shown in Figure 1.

One, the efficient reverse dynamic calculating model of multi freedom degree mechanical arm

(1) use speed V (k)=[ω in each joint of spinor method representation mechanical arm _k, v _k], acceleration and six-dimensional force f (k)=[N of joint _k, F _k].

(2) calculate the inertial mass matrix of each rod member

$M\left(k\right)=\left(\begin{array}{cc}{I}_k& m_k.{\tilde{p}}_c\left(k\right)\\ -m_k.{\tilde{p}}_c\left(k\right)& m_k.E_I\end{array}\right)---\left(1\right)$

I wherein _kfor the inertial tensor matrix of connecting rod k with respect to k joint coordinate system, m _kfor the quality of connecting rod k, for the antisymmetric matrix of the centroid vector of connecting rod k, E _ibe +++ three-dimensional unit matrix.The state transition matrix of definition joint k is H (k)=[h ^t(k) 00 0], the rotation axis vector that wherein h (k) is joint, is three dimensional vectors.

(3) definition space transition operator, the transition operator of its upper triangular matrix form can be realized the inside recursion (from 0 to n) of force and moment, and the transition operator of lower triangular matrix form can be realized the outside recursion (from n to 0) of speed and acceleration.Definition force and moment Recursion Operator is:

$\mathrm{\φ}(k+1,k)=\left(\begin{array}{cc}{E}_I& \tilde{l}(k+1,k)\\ 0& E_I\end{array}\right)---\left(1\right)$

Definition the recurrence relation between adjacent connecting rod can be expressed as follows:

Joint velocity, the outside recursion of acceleration, k=n wherein, n-1......1:

$V\left(k\right)={\mathrm{\φ}}^T(k+1,k).V(k+1)+H^T\left(k\right).\stackrel{\·}{\mathrm{\θ}}\left(k\right)$

(3)

$\mathrm{\α}\left(k\right)={\mathrm{\φ}}^T(k+1,k).\mathrm{\α}(k+1)+H^T\left(k\right).\stackrel{\·\·}{\mathrm{\θ}}\left(k\right)+a\left(k\right)$

Joint power, the inside recursion of moment, k=1 wherein, 2 ..., n:

f(k)＝φ(k，k-1).f(k-1)+M(k).α(k)+b(k) (4)

T(k)＝H(k).f(k)

Wherein, a (k), b (k) represents respectively coriolis force and the centrifugal force of mechanical arm:

$a\left(k\right)=\left(\begin{array}{c}\mathrm{\ω}(k+1)\×h\left(k\right).\stackrel{\·}{\mathrm{\θ}}\left(k\right)\\ \mathrm{\ω}(k+1)\×[\mathrm{\ω}(k+1)\×l(k+1,k)]\end{array}\right)---\left(5\right)$

$b\left(k\right)=\left(\begin{array}{c}\mathrm{\ω}\left(k\right)\×[I\left(k\right).\mathrm{\ω}\left(k\right)]\\ m\left(k\right).\mathrm{\ω}\left(k\right)\×[\mathrm{\ω}\left(k\right)\×\mathrm{Pc}\left(k\right)]\end{array}\right)$

Said process is Inverse Dynamics of Manipulators recursion computation process, by the Recursive Solution to speed, acceleration, power and moment, obtains the efficient reverse dynamics recursion accounting equation of mechanical arm suc as formula shown in (4).Next step obtains elaboration to have more the method for the inverse dynamics accounting equation of clear and definite physical meaning.

(4) definition mechanical arm speed operator is V=[V (1) ... V (n-1), V (n)] ^t, and represent that with same form acceleration operator, coriolis force operator, centrifugal force operator, power operator, the moment operator of mechanical arm are respectively α, and a, b, f, T, formula (3), (4) can be expressed as:

$V=H^T{\mathrm{\φ}}^T\stackrel{\·}{\mathrm{\θ}}$

$\mathrm{\α}={\mathrm{\φ}}^T{H}^T\stackrel{\·\·}{\mathrm{\θ}}+{\mathrm{\φ}}^{T}a$

f＝φ(Mα+b) (6)

T＝Hf

Wherein, mechanical arm mass matrix operator, state, as projected matrix operator and space transfer operator are:

M＝diag[M(1)，...，M(n-1)，M(n)]

(7)

H＝diag[H(1)，...，H(n-1)，H(n)]

By formula (6), can derive and obtain mechanical arm torque meter formula and be:

$T=M_G\stackrel{\·\·}{\mathrm{\θ}}+C---\left(9\right)$

Wherein:

M _G＝HφMφ ^TH ^T

(10)

C＝Hφ(Mφ ^Ta+b)

So far, obtained the efficient dynamic recursion accounting equation of mechanical arm and there is the Manipulator Dynamic that clear and definite physics is expressed implication.

Two, the efficient dynamic model method for building up of multi freedom degree mechanical arm

(1) in robot for space forward dynamics calculates, to inverting by a large amount of calculating of consumption, in mass matrix M inversion process, due to H φ and φ of mass matrix M ^th ^tbe not square formation, so inverting of mass matrix can not be used H φ and φ ^th ^trepresent.The present invention utilizes Kalman Filtering smoothing method by mass matrix M _gcarry out factorization, obtain following factorization form:

M _G＝(I+HΦK)D(I+HΦK) ^T (11)

Its inverse matrix can be expressed as:

$M_G^{-1}={(I-\mathrm{H\ψK})}^T{D}^{-1}(I-\mathrm{H\ψK})---\left(12\right)$

Wherein:

(2) definition operator P, D, G, K, ψ, its recursion computation process is as follows, k=1 wherein, 2 ..., n:

P(k)＝ψ(k，k-1)P(k-1)ψ ^T(k，k-1)+M(k)

D(k)＝H(k)P(k)H ^T(k)

G(k)＝P(k)H ^T(k)D ^-1(k) (14)

K(k)＝φ(k+1，k)G(k)

ψ(k+1，k)＝φ(k+1，k)-K(k)H(k)

The ψ (k+1, k) calculating by recursion can be combined into mechanical arm operator ψ:

(3) definition mechanical arm diagonal angle Space Operators:

P＝diag[P(1)，…，P(n)]

D＝diag[D(1)，…，D(n)]

G＝diag[G(1)，…，G(n)] (16)

K＝diag[K(1)，…，K(n)]

Δφ＝diag[φ(2，1)，…，φ(n+1，n)]

Formula (14) can be write as:

D＝HPH ^T

G＝PH ^TD ^-1 (17)

K＝ΔφG

The new general mass matrix expression-form obtaining according to decomposition, derive efficient forward dynamics accounting equation:

$\stackrel{\·\·}{\mathrm{\θ}}={(I-\mathrm{H\ΦK})}^T{D}^{-1}(I+\mathrm{H\ψK})T_{\stackrel{\·\·}{\mathrm{\θ}}}---\left(18\right)$

Wherein for inputting joint moment in forward dynamics calculating, by formula (18), can calculate joint acceleration, thereby integration obtains joint velocity and joint angles, obtain the relation of joint moment and mechanical arm Position And Velocity.So far, the present invention has obtained the efficient forward dynamics computation model of multi freedom degree mechanical arm.

embodiment 1

The forward and inverse dynamic calculating model of mechanical arm of setting up according to the present invention, is that research object is launched checking for seven freedom mechanical arm as shown in Figure 3, and the D-H parameter of mechanical arm and rod member parameter are as shown in table 1, table 2.

Table 1 mechanical arm D-H parameter list

Connecting rod i	θ i(°)	d i(m)	a i-1(m)	α i-1(°)
					1	θ 7(0)	1.2	0	90°
2	θ 6(90)	0.53	0	-90°
					3	θ 5(0)	0.53	5.8	0
4	θ 4(0)	1.05	5.8	0
					5	θ 3(0)	0	0	90°
6	θ 2(-90)	0.53	0	-90°
					7	θ 1(0)	1.2	0	0

Table 2 mechanical arm rod member quality, inertia parameter

By the method described in instructions, set up seven freedom Manipulator Dynamic, the flow process that its program realizes dynamics calculation as shown in Figure 2.Set mechanical arm operational factor and the method for operation as follows:

Initial joint angle: 50 ° ,-170 °, 150 °, 60 °, 130 °, 170 °, 0};

Initial end pose: 7.78,5.24,5.9 ,-56.7 ° ,-17.6 ° ,-138.4 ° };

Stop object pose: { 9,2,4,0,0,180 ° };

Planning Model: straight line planning;

Planning time: 20s;

Acceleration time: 5s;

Planning horizon: 0.05s.

As shown in Figure 4, the joint of mechanical arm moment being calculated by the present invention as shown in Figure 5 for terminal velocity curve.By seven freedom Manipulator Dynamics is calculated and is found, under Mathmatica7.0 software platform, by the every step of method calculating machine arm inverse dynamics 14ms consuming time described in instructions, calculate the every step of forward dynamics 18ms consuming time, and utilize Nuton-Euler method to calculate inverse dynamics 36ms consuming time, forward dynamics 60ms consuming time.Result shows that the Dynamic Modeling efficiency that the present invention is directed to mechanical arm is improved significantly.

Claims (1)

1. an efficient dynamic modeling method for multi freedom degree mechanical arm, is characterized in that the method comprises the following steps:

(1) multi freedom degree mechanical arm inverse dynamic modelling, utilize spinor theory to describe the speed in each joint, acceleration and power, calculate the inertial mass matrix of each rod member and define the space transfer operator of mechanical arm, the recursion that completes speed, acceleration and the power of mechanical arm is calculated; Described multi freedom degree mechanical arm inverse dynamic modelling comprises the following steps:

1. use speed V (k)=[ω in each joint of spinor method representation mechanical arm _k, v _k], acceleration and six-dimensional force f (k)=[N of joint _k, F _k], k=1 wherein ..., n, n is mechanical arm number of degrees of freedom,, ω _kfor the angular velocity of k joint of mechanical arm, v _kfor the linear velocity of k joint of mechanical arm, N _kbe the moment of k joint, F _kbe the power of k joint;

2. the inertial mass matrix M (k) of each rod member of calculating machine arm, and define the space transfer operator φ (k+1 between the adjacent rod member of mechanical arm, k), complete the speed recursion calculating of mechanical arm and the recursion of power/moment are calculated, joint moment recurrence equation T (k)=H (k) f (k) that obtains mechanical arm, wherein H (k) is the state transition matrix of joint k;

3. define the n dimension speed operator V=[V (1) of mechanical arm ... V (n-1), V (n)] ^t, and be respectively α with acceleration operator, coriolis force operator, centrifugal force operator, power operator, the moment operator of same formal definition mechanical arm, and a, b, f, T, can obtain Inverse Dynamics of Manipulators accounting equation according to step recursive expression 2. m wherein _g=H φ M φ ^th ^tfor general mass matrix, for joint of mechanical arm angular acceleration, C=H φ (M φ ^ta+b) be mechanical arm system non-linear force matrix, comprise coriolis force and centrifugal force information, H=diag[H (1) ..., H (n-1), H (n)] be state, as projected matrix operator, M=diag[M (1) ..., M (n-1), M (n)] be mass of system matrix operator, φ is space transfer operator;

(2) the forward dynamics modeling of multi freedom degree mechanical arm, according to Kalman Filtering smoothing method, obtain the factorization form of the general mass matrix in mechanical arm forward dynamics computational item, and the many factors that calculate its finding the inverse matrix connect and take advantage of expression-form, derive on this basis the forward dynamics computation model of mechanical arm; The modeling of described multi freedom degree mechanical arm forward dynamics comprises the following steps:

1. utilize Kalman Filtering smoothing method to obtain general mass matrix M in the calculating of mechanical arm forward dynamics _gfactorization expression-form M _g=(I+H Φ K) D (I+H Φ K) ^t, with and many factors Xiang Lian of inverse matrix take advantage of expression-form wherein I is 6 dimension unit matrixs, D, and K, ψ is Space Operators, Φ is the lower triangular matrix being comprised of φ (k+1, k);

2. definition space operator P, G, and utilize M (k), H (k), the φ (k+1 obtaining in step (1), k) Recursive Solution goes out D (k), K (k), ψ (k+1, k), wherein D (k), K (k), ψ (k+1, k) be the operator of corresponding k joint, final combination obtains operator D, K, the expression formula of ψ;

3. according to above-mentioned steps M 1. _gequation and the step of inverting be middle D 2., K, and the calculation expression of ψ, obtains forward dynamics accounting equation wherein joint moment for input, obtains joint acceleration according to forward dynamics accounting equation, thereby can be in the hope of mechanical arm in moment speed and angle under effect.