CN102495550B - Forward dynamic and inverse dynamic response analysis and control method of parallel robot - Google Patents

Abstract

A forward dynamic and inverse dynamic response analysis and control method of a parallel robot comprises the steps of (1) disassembling the parallel robot, namely selecting a generalized coordinate system or a user-defined coordinate system to consider each branch chain and each movable platform of the parallel robot as s subsystems which are mutually independent; (2) determining a dynamical equation of subsystems of each branch chain; (3) determining a dynamical equation of subsystems of each movable platform; (4) determining a constraint equation; (5) obtaining a forward dynamic equation through analysis; (6) obtaining an inverse dynamic equation through analysis; and (7) determining control force of a controller. The forward dynamic and inverse dynamic response analysis and control method of the parallel robot has good model sharing performance, and a dynamic unite analytical method is provided for complicatedly restrained parallel robot systems.

Description

Parallel robot just, inverse dynamics response analysis and control method

Technical field

The present invention is mainly concerned with the ROBOT CONTROL technical field, refer in particular to a kind of dynamic response analysis and control method of parallel robot, be specially adapted to contain the dynamic analysis of the parallel robot system of complicated closed loop constraint, comprise positive power and reverse dynamic response analytical approach, and be used for instructing its controller design.

Background technology

The strict difinition of parallel robot mechanism is: go up lower platform and link to each other with the branch more than 2 or 2, mechanism has the degree of freedom more than 2 or 2, and with the mechanism (referring to Fig. 1 and Fig. 2) of parallel way driving.Be a kind of have two platforms, mechanism that the centre has a plurality of poles to be formed by connecting from structure.One of them platform is fixed on reference frame or the pedestal, is called stationary platform, and another platform can arbitrary motion in its work space, is called motion platform.The form of parallel institution has multiple, comes branch that 3 link-types and 6 link-types are generally arranged by connecting the connecting rod number of going up lower platform; Come branch to have telescopic and the fixed length formula by this body structure of connecting rod; Come branch that revolute pair, moving sets, screw pair, cylindrical pair, spherical pair, planar contact pair and Hooke's hinge etc. are arranged according to the connected mode of connecting rod and stationary platform.Connected mode branch by each independent link has SPS, PSS, and RPS, forms such as PRR, wherein, S represents that spherical pair connects, and P represents that moving sets connects, and R represents the plane revolute.

Nineteen sixty-five, German Stewart has invented six-degree-of-freedom parallel connection mechanism, and is used for training flight person as flight simulator.Australian famous theory of mechanisms professor Hunt proposed parallel institution is used for robot arm in 1978.The characteristics of parallel institution: (1) is big with serial mechanism phase specific stiffness, Stability Analysis of Structures; (2) load-bearing capacity is big; (3) fine motion precision height; (4) exercise load is little; (5) find the solution in the position, the serial mechanism normal solution is easy, but anti-solution is very difficult, and the anti-solution of parallel institution normal solution difficulty is very easy to.Dynamic (dynamical) normal solution and contrary solution are two subject matters of robot dynamics.The former is acting force or the moment in each joint of known machine people, obtains displacement, speed, acceleration and the movement locus in each joint; The latter is displacement, speed, the acceleration in each joint of known machine people, obtains acting force or the moment in each joint.The 6DOF parallel institution is the big class of one in the parallel robot mechanism, is the parallel institution that Chinese scholars is studied at most, is widely used in flight simulator, 6 dimension power and fields such as torque sensor and parallel machine.But this class mechanism has a lot of guardian techniques not have or is not resolved fully, such as precision calibration of its forward kinematics solution, dynamic response analysis and parallel machine etc.

The positive inverse dynamics of parallel robot is an important branch of parallel robot research, and wherein the dynamic response analysis is the basis that parallel robot is realized control, thereby occupies an important position under study for action.The research that relevant dynamic response is analyzed has obtained very big progress in serial machine people field.Research contents to parallel robot mostly relates to mechanism and kinematics, because the complicacy of parallel institution is less relatively to the dynamics research of parallel robot.Parallel robot is the dynamical system of a complexity, has severe nonlinear, is made up of a plurality of joints and a plurality of connecting rod, has a plurality of inputs and a plurality of output, exists complicated coupled relation between them.Therefore, the dynamics of the robot that analyze and research must adopt the very method of system.Existing analytical approach has: Lagrangian method, newton-Euler's method, Gauss method, Luo Baisen-Wei Tengbao method, Kane method, spinor (dual numbers) method etc.Wherein, Newton-Euler method and Lagrangian method are to use maximum methods most.

Newton-Euler method is a kind of running balancing of power, and it need be from the kinematics of robot, obtains acceleration and with each acting force in this elimination system.This method advantage is directly perceived, understands easily.But owing between each joint variable of parallel robot stronger coupled relation is arranged, need that the physical quantity relevant with the joint done a large amount of simplification and could satisfy the various conditions that newton Euler finds the solution, be unsuitable for the modeling of actual parallel robot system.Lagrangian method only needs speed and does not need acting force in the solving system, the kinetics equation that kinetic energy and the potential energy by solving system obtains system, and this method is simply direct, can obtain result more accurately.But still there are the following problems: the computation process of (1) this method depends on Lagrange multiplier, can't obtain analytic solution; (2) numerical solution asked of numerical computation method need be unfavorable for the design of system controller through repeatedly just obtaining separating behind the iteration convergence.Because to have speed slow for numerical evaluation, efficient is low, and can not obtain shortcoming such as all possible solution, so wish to adopt analytical method to find the solution the sealing solution of parallel robot mechanism.

Summary of the invention

The technical problem to be solved in the present invention just is: at the technical matters of prior art existence, the invention provides a kind of parallel robot just, inverse dynamics response analysis and control method, this method has good model and shares property, and the parallel robot system that complexity is retrained provides dynamics to unify analytical approach.

For solving the problems of the technologies described above, the present invention by the following technical solutions:

A kind of parallel robot just, inverse dynamics response analysis and control method, its step is as follows:

(1), decomposes parallel robot: select generalized coordinate system or self-defined coordinate system that each branched chain (connecting rod) and the moving platform of parallel robot are considered as a separate s subsystem;

(2), determine the kinetics equation of each connecting rod subsystem: use the classical mechanics method to set up the kinetics equation of s-1 connecting rod subsystem and 1 moving platform subsystem, for the connecting rod subsystem, its kinetics equation is following form:

$M_i(q_i,t){\stackrel{\·\·}{q}}_i+C_i(q_i,{\stackrel{\·}{q}}_i,t)={\mathrm{\τ}}_i\left(t\right),i=\mathrm{1,2},...,s-1,$

In this area, q is system's generalized coordinate,

Be system's generalized velocity,

Be system's generalized acceleration; In the following formula, q _iBe the generalized coordinate of i mechanical arm, M _iBe i mechanical arm n _i* n _iMass matrix, C _iBe centrifugal force/Ke Liaolili and the gravity of i mechanical arm, τ _iBe i the corresponding generalized force of mechanical arm;

(3), determine the kinetics equation of moving platform subsystem: the kinetics equation of setting up following form with the classical mechanics method:

$M_s(q_s,t){\stackrel{\·\·}{q}}_s={\mathrm{\τ}}_s\left(t\right)$

Wherein, q _sBe origin and the Eulerian angle of coordinate system P with respect to coordinate system B, q _s=[x _p, y _p, z _p, ψ _p, θ _p, φ _p] ^Tn _s=6, corresponding mass matrix is diagonal matrix M _s∈ R ^{6 * 6},

$M_s=\left[\begin{array}{cccccc}{\mathrm{<figure-callout\; id="0"\; label="m\; p"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">m</figure-callout>}}_p& 0& 0& 0& 0& 0\\ 0& {\mathrm{<figure-callout\; id="0"\; label="m\; p"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">m</figure-callout>}}_p& 0& 0& 0& 0\\ 0& 0& {\mathrm{<figure-callout\; id="0"\; label="m\; p"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">m</figure-callout>}}_p& 0& 0& 0\\ 0& 0& 0& {\mathrm{<figure-callout\; id="0"\; label="I\; zp"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">I</figure-callout>}}_{\mathrm{zp}}& 0& 0\\ 0& 0& 0& 0& {\mathrm{<figure-callout\; id="0"\; label="I\; yp"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">I</figure-callout>}}_{\mathrm{yp}}& 0\\ 0& 0& 0& 0& 0& I_{\mathrm{zp}}\end{array}\right];$

(4), determine equation of constraint: set up m equation of constraint between connecting rod subsystem and motion platform, do not lead form if this equation of constraint is not second order, the time differentiate is translated into following form:

$A(q,t)\stackrel{\&CenterDot;\&CenterDot;}{q}=b(\stackrel{\&CenterDot;}{q},q,t)$

Wherein, and A (q, t)=[A _Li(q, t)] _Mxn,

(5), obtain the positive drive mechanical equation of parallel robot system by analysis:

$M(q,t)\stackrel{\&CenterDot;\&CenterDot;}{q}=\mathrm{\&tau;}\left(t\right)-C(\stackrel{\&CenterDot;}{q},q,t)+M^{\frac{1}{2}}(q,t)B^{+}(q,t)[b(\stackrel{\&CenterDot;}{q},q,t)-A(q,t)M^{-1}(q,t)(\mathrm{\&tau;}\left(t\right)-C(\stackrel{\&CenterDot;}{q},q,t))]$

Wherein,

$q=\left[\begin{array}{c}{q}_1\\ {\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">q</figure-callout>}}_2\\ .\\ .\\ .\\ q_s\end{array}\right],$ $\mathrm{\&tau;}=\left[\begin{array}{c}{\mathrm{\&tau;}}_1\\ {\mathrm{\&tau;}}_2\\ .\\ .\\ .\\ {\mathrm{\&tau;}}_{s-1}\\ {\mathrm{\&tau;}}_s\end{array}\right],$ $C=\left[\begin{array}{c}{C}_1\\ {\mathrm{<figure-callout\; id="2"\; label="C"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_2\\ .\\ .\\ .\\ C_{s-1}\\ O\end{array}\right];$ Definition $n={\mathrm{\&Sigma;}}_{i=1}^s{n}_i,$ M ∈ R is arranged ^{N * n}, q ∈ R ⁿ, τ ∈ R ⁿWith C ∈ R ⁿObtaining the acceleration equation is following form:

$\stackrel{\&CenterDot;\&CenterDot;}{q}=M^{-1}(q,t)[\mathrm{\&tau;}\left(t\right)-C(\stackrel{\&CenterDot;}{q},q,t)]+M^{-\frac{1}{2}}(q,t)B^{+}(q,t)(b(\stackrel{\&CenterDot;}{q},q,t)-B(q,t)M^{-\frac{1}{2}}(q,t)[\mathrm{\&tau;}\left(t\right)-C(\stackrel{\&CenterDot;}{q},q,t)];$

(6), inverse dynamics analysis: by the positive drive mechanical equation in the step (5), obtaining the inverse dynamics equation is following form:

$\mathrm{\&tau;}(q,\stackrel{\&CenterDot;}{q},\stackrel{\&CenterDot;\&CenterDot;}{q},t)={[I-Q(\stackrel{\&CenterDot;}{q},q,t)+M^{\frac{1}{2}}(q,t)B^{+}(q,t)A(q,t)M^{-1}(q,t)]}^{-1}\&times;[M(q,t)\stackrel{\&CenterDot;\&CenterDot;}{q}-M^{\frac{1}{2}}(q,t)B^{+}(q,t)b(\stackrel{\&CenterDot;}{q},q,t)]$

$+C(\stackrel{\&CenterDot;}{q},q,t)$

Wherein, I ∈ R ^{N * n}Be unit matrix, at the q of expection _s,

With Known, q,

To be derived by follow-up formula draws, and then the corresponding matrix M of deriving, C, A, b;

(7), the control of determining controller is:

${\mathrm{\&tau;}}_{\mathrm{ac}}={J_{\mathrm{ac}}^{+}}^T\left(q_s\right)({\mathrm{\&tau;}}_s^{\&prime;}-{\mathrm{\&tau;}}_s)+[I-{J_{\mathrm{ac}}^{+}}^T\left(q_s\right)J_{\mathrm{ac}}^T\left(q_s\right)]h.$

Wherein h is any 6 dimensional vectors.

As a further improvement on the present invention:

Classical mechanics method in described step (2) and the step (3) is Lagrangian method or Newton-Euler method.

In the described step (7), if that the motion of parallel manipulator does not exist is unusual, the equation of motion under the generalized coordinate of each subsystem can be described as:

q _i(t)＝f _i(q _i，t)， i＝1，2，…，s.

Wherein, f _i∈ R ⁿ, get f _iTo q _sPartial derivative, can obtain following Jacobi matrix:

$J\left(q_s\right)={\left[\begin{array}{cccccc}\frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_1}{{\&PartialD;x}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_1}{{\&PartialD;y}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_1}{{\&PartialD;z}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_1}{{\&PartialD;\mathrm{\&psi;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_1}{{\&PartialD;\mathrm{\&theta;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_1}{{\&PartialD;\mathrm{\&phi;}}_p}\\ \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_2}{{\&PartialD;x}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_2}{{\&PartialD;y}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_2}{{\&PartialD;z}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_2}{{\&PartialD;\mathrm{\&psi;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_2}{{\&PartialD;\mathrm{\&theta;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="f"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">f</figure-callout>}}_2}{{\&PartialD;\mathrm{\&phi;}}_p}\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ \frac{{\&PartialD;f}_s}{{\&PartialD;x}_p}& \frac{{\&PartialD;f}_s}{{\&PartialD;y}_p}& \frac{{\&PartialD;f}_s}{{\&PartialD;z}_p}& \frac{{\&PartialD;f}_s}{{\&PartialD;\mathrm{\&psi;}}_p}& \frac{{\&PartialD;f}_s}{{\&PartialD;\mathrm{\&theta;}}_p}& \frac{{\&PartialD;f}_s}{{\&PartialD;\mathrm{\&phi;}}_p}\end{array}\right]}_{(n\&times;6)}$

Wherein, (size of representing matrix of i * j),

According to the Jacobi matrix theorem, speed can be expressed as:

External force can be expressed as: τ ' _s=J ^T(q _s) τ; Wherein, τ ' _sBe total power and the moment that under the motion of expection, acts on the motion platform; Ask for the derivative of speed, obtain the following expression-form of acceleration:

To parallel robot system, establishing the generalized force that reaches the actuator that desired movement should apply is τ _Ac, τ _Ac∈ R ^τWith corresponding generalized coordinate be q _Ac, q _Ac∈ R ^τ, q _Ac=f _Ac(q _s, t), wherein, f _AcBe f in the formula _iThe combination of element, have:

$J_{\mathrm{ac}}\left(q_s\right)={\left[\begin{array}{cccccc}\frac{{\&PartialD;f}_{\mathrm{ac}}}{{\&PartialD;x}_p}& \frac{{\&PartialD;f}_{\mathrm{ac}}}{{\&PartialD;y}_p}& \frac{{\&PartialD;f}_{\mathrm{ac}}}{{\&PartialD;z}_p}& \frac{{\&PartialD;f}_{\mathrm{ac}}}{{\&PartialD;\mathrm{\&psi;}}_p}& \frac{{\&PartialD;f}_{\mathrm{ac}}}{{\&PartialD;\mathrm{\&theta;}}_p}& \frac{{\&PartialD;f}_{\mathrm{ac}}}{{\&PartialD;\mathrm{\&phi;}}_p}\end{array}\right]}_{(\mathrm{\&tau;}\&times;6)}$

So, ${\stackrel{\&CenterDot;}{q}}_{\mathrm{ac}}=J_{\mathrm{ac}}\left(q_s\right){\stackrel{\&CenterDot;}{q}}_s,$ Have ${\mathrm{\&tau;}}_s^{\&prime;}-{\mathrm{\&tau;}}_s=J_{\mathrm{ac}}^T\left(q_s\right){\mathrm{\&tau;}}_{\mathrm{ac}}.$

Compared with prior art, the invention has the advantages that: method of the present invention is decomposed into simple subsystem with complicated parallel robot mechanism and progressively analyzes, can on the basis of original parallel robot, add new branched chain (connecting rod) subsystem or the combination of subsystem arbitrarily, have good model extensibility; This method can promote the simplification of system modelling process and model to share property effectively.By adopting analysis of the present invention and control method for designing, in modeling process, can under not by the situation of any extra auxiliary variable (as Lagrange multiplier), obtain the power of constraint place, and then obtain the equation of motion of the accurate analytical form of parallel robot; The equation of motion of the accurate analytical form of parallel robot system among the present invention, but the only problem of value solution of utilizing in the Lagrangian mechanics classical way improved, provide powerful guarantee for the Control System Design of parallel robot.

Description of drawings

Fig. 1 is the parallel robot structural representation;

Fig. 2 is the parallel robot mechanism decomposing schematic representation;

Fig. 3 is the schematic flow sheet of the inventive method;

Fig. 4 is U-P-S Steward-Gough platform synoptic diagram in the specific embodiment;

Fig. 5 is U-P-S Steward-Gough platform decomposing schematic representation in the specific embodiment;

Fig. 6 is branched chain (connecting rod) generalized coordinate synoptic diagram;

Fig. 7 is the parameter synoptic diagram of motion platform;

Fig. 8 is the parameter synoptic diagram of stationary platform;

Fig. 9 is that the effect of actuator is tried hard to;

Figure 10 is the translation displacement diagram of motion platform;

Figure 11 is the angular motion displacement diagram of motion platform;

Figure 12 is the translational velocity figure of motion platform;

Figure 13 is the angular velocity figure of motion platform;

Figure 14 is the translatory acceleration figure of motion platform;

Figure 15 is the angular acceleration figure of motion platform.

Embodiment

Below with reference to Figure of description and specific embodiment the present invention is described in further details.

Though parallel robot structurally is diversified, modal in actual applications is the form of Steward platform mechanism and evolution thereof.In the present embodiment, be described in detail with a kind of U-P-S Steward-Gough platform (referring to Fig. 4 and Fig. 5), other parallel robot can be analyzed equally as stated above.Referring to Fig. 3, Fig. 6, shown in Figure 7, its idiographic flow is as follows:

1, decomposes the Steward-Gough platform.The Steward-Gough platform is decomposed into 7 groups of subsystems becomes, by 6 connecting rod subsystems and 1 motion platform subsystem, referring to Fig. 5.

2, determine the kinetics equation of each branched chain (connecting rod) subsystem.Set up the kinetics equation of the subsystem of 6 branched chain (connecting rod) respectively; Be described below respectively:

$M_i{\stackrel{\&CenterDot;\&CenterDot;}{q}}_i+C_i={\mathrm{\&tau;}}_i,(i=\mathrm{1,2},...,6)$

Wherein,

$M_i=\left[\begin{array}{ccc}{m}_{2\mathrm{<figure-callout\; id="0"\; label="i"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">i</figure-callout>}}& 0& 0\\ 0& m_{1i}{l}_{1\mathrm{<figure-callout\; id="2"\; label="i"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">i</figure-callout>}}^2+I_{1i}+m_{2i}{(l_{1i}+d_i)}^2& 0\\ 0& 0& m_{1i}{l}_{1\mathrm{<figure-callout\; id="2"\; label="i"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">i</figure-callout>}}^2+I_{\mathrm{li}}+m_{2i}{(l_{1i}+d_i)}^2\end{array}\right]$

$C_i=\left[\begin{array}{c}-m_{2i}(l_{1i}+d_i)({\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_i^2+{\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_i^2)+m_{2i}g{\sin \mathrm{\&alpha;}}_i\\ 2m_{2i}(l_{1i}+d_i){\stackrel{\&CenterDot;}{\mathrm{\&alpha;}}}_i{\stackrel{\&CenterDot;}{d}}_i+m_{1i}g\cos {\mathrm{\&alpha;}}_i+m_{2i}g\cos \mathrm{\&alpha;}+m_{2i}g\cos {\mathrm{\&alpha;}}_i(l_{1i}+d_i)\\ 2m_{2i}(l_{1i}+d_i){\stackrel{\&CenterDot;}{\mathrm{\&beta;}}}_i{\stackrel{\&CenterDot;}{d}}_i\end{array}\right]$

${\mathrm{\&tau;}}_i=\left[\begin{array}{c}{\mathrm{\&tau;}}_{{\mathrm{<figure-callout\; id="0"\; label="ac\; i"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">ac</figure-callout>}}_i}\\ 0\\ 0\end{array}\right]$

3, determine the kinetics equation of moving platform subsystem.To the moving platform subsystem, it is as follows to set up kinetics equation:

$M_7{\stackrel{\&CenterDot;\&CenterDot;}{q}}_7={\mathrm{\&tau;}}_7$

Wherein, q ₇Can be used as coordinate system P with respect to origin and the Eulerian angle of coordinate system B.

q ₇＝[x _p，y _p，z _p，ψ _p，θ _p，φ _p] ^T

Wherein, n ₇=6, corresponding mass matrix is diagonal matrix M ₇∈ R ^{6 * 6}

${\mathrm{<figure-callout\; id="7"\; label="M"\; filenames="HDA0000110620620000021.png"\; state="\{\{state\}\}">M</figure-callout>}}_7=\left[\begin{array}{cccccc}{m}_{\mathrm{<figure-callout\; id="0"\; label="p"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">p</figure-callout>}}& 0& 0& 0& 0& 0\\ 0& {\mathrm{<figure-callout\; id="0"\; label="m\; p"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">m</figure-callout>}}_p& 0& 0& 0& 0\\ 0& 0& {\mathrm{<figure-callout\; id="0"\; label="m\; p"\; filenames="HDA0000110620620000032.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">m</figure-callout>}}_p& 0& 0& 0\\ 0& 0& 0& I_{\mathrm{zp}}& 0& 0\\ 0& 0& 0& 0& I_{\mathrm{yp}}& 0\\ 0& 0& 0& 0& 0& I_{\mathrm{zp}}\end{array}\right]$

Suppose except gravity, not have other to act on power on the motion platform, then obtain:

τ ₇＝[0，0，m _pg，0，0，0] ^T

4, determine equation of constraint.Set up the equation of constraint between each branched chain (connecting rod) subsystem and the moving platform.Suppose that basis coordinates is that B places the stationary platform center, coordinate system P places motion platform center (referring to Fig. 4), represents the cut-point of i connecting rod and motion platform for the kinematic pair Pi on the motion platform.Use ^jP _iSome P on the expression subsystem j _i, X (), Y (), Z () is illustrated in the x among the coordinate system B, y, the z coordinate, x (), y (), z () is illustrated in the x among the coordinate system P, y, z coordinate.Because ball pivot only retrains 3 translation freedoms that link to each other between the junctor, therefore, to the constraint i=1 between motion platform and the connecting rod, 2 ..., 6, following relational expression is arranged:

$\left[\begin{array}{c}X\left(P_i^7\right)\\ Y\left(P_i^7\right)\\ Z\left(P_i^7\right)\end{array}\right]=\left[\begin{array}{c}X\left(P_i^i\right)\\ Y\left(P_i^i\right)\\ Z\left(P_i^i\right)\end{array}\right]$

Derivation can obtain:

$\left[\begin{array}{c}X\left(P\right)\\ Y\left(P\right)\\ Z\left(P\right)\end{array}\right]+{\left[R\right]}^T\left[\begin{array}{c}x\left(P_i^7\right)\\ y\left(P_i^7\right)\\ z\left(P_i^7\right)\end{array}\right]=\left[\begin{array}{c}X\left(B_i\right)\\ Y\left(B_i\right)\\ Z\left(B_i\right)\end{array}\right]+\left[\begin{array}{c}(l_{1i}+l_{2i}+d_i)\cos {\mathrm{\&alpha;}}_i\cos {\mathrm{\&beta;}}_i\\ (l_{1i}+l_{2i}+d_i)\cos {\mathrm{\&alpha;}}_i\sin {\mathrm{\&beta;}}_i\\ (l_{1i}+l_{2i}+d_i)\sin {\mathrm{\&alpha;}}_i\end{array}\right]$

Wherein, $\left[R\right]=\left[R_{\mathrm{\&phi;}}\right]\left[R_{\mathrm{\&theta;}}\right]\left[R_{\mathrm{\&psi;}}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\&phi;}}_i& \sin {\mathrm{\&phi;}}_i& 0\\ \sin {\mathrm{\&phi;}}_i& \cos {\mathrm{\&phi;}}_i& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\&theta;}}_i& 0& -\sin {\mathrm{\&theta;}}_i\\ 0& 1& 0\\ \sin {\mathrm{\&theta;}}_i& 0& \cos {\mathrm{\&theta;}}_i\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\&psi;}}_i& \sin {\mathrm{\&psi;}}_i& 0\\ \sin {\mathrm{\&psi;}}_i& \cos {\mathrm{\&psi;}}_i& 0\\ 0& 0& 1\end{array}\right]$

X in the following formula ( ⁷P _i), y ( ⁷P _i), z ( ⁷P _i), X (B _i), Y (B _i), Z (B _i) value be known, relevant with the physical dimension of platform, see Fig. 5 and Fig. 6.Following formula is got two rank and is led, and can obtain the equation of constraint of following form:

$A(q,t)\stackrel{\&CenterDot;\&CenterDot;}{q}=b(\stackrel{\&CenterDot;}{q},q,t)$

Wherein, A ∈ R ^{18 * 24}Be the coefficient before the second derivative, b comprises all other item.Generalized coordinate is following form:

q＝[d ₁ α ₁ β ₁ d ₂ α ₂ β ₂…d ₆ α ₆ β ₆ x _p v _p z _p ψ _p θ _p φ _p] ^T

Wherein, x _p, y _p, z _pRepresent corresponding coordinate X (P), Y (P), Z (P).

5, positive drive mechanical analysis.The positive drive mechanical equation of derivation U-P-S Steward-Gough platform.

M＝[diag(M ₁，M ₂，…，M ₇)] _(24×24)

$C={[{\mathrm{<figure-callout\; id="1"\; label="C"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_1,{\mathrm{<figure-callout\; id="2"\; label="C"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">C</figure-callout>}}_2,...,C_7]}_{(24\&times;1)}^T$

$\mathrm{\&tau;}={[{\mathrm{\&tau;}}_1,{\mathrm{\&tau;}}_2,...,{\mathrm{\&tau;}}_7]}_{(24\&times;1)}^T$

$B={\left[{\mathrm{AM}}^{-\frac{1}{2}}\right]}_{(18\&times;24)}$

Can obtain kinetics equation is following analytical form:

$\stackrel{\&CenterDot;\&CenterDot;}{q}=M^{-1}(q,t)[\mathrm{\&tau;}\left(t\right)-C(\stackrel{\&CenterDot;}{q},q,t)]+M^{-\frac{1}{2}}(q,t)B^{+}(q,t)(b(\stackrel{\&CenterDot;}{q},q,t)-B(q,t)M^{-\frac{1}{2}}(q,t)[\mathrm{\&tau;}\left(t\right)-C(\stackrel{\&CenterDot;}{q},q,t)]$

To any given exciting force τ _Ac, under given starting condition, can calculate the motion of each subsystem by separating above-mentioned ordinary differential equation.

6, inverse dynamics analysis.The inverse dynamics equation of derivation U-P-S Steward-Gough platform calculates given q ₇,

Under exciting force τ _AcTo connecting rod i=1,2 ..., 6 have following relation:

$d_i=\sqrt{{\mathrm{\&Delta;<figure-callout\; id="2"\; label="X\; i"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">X</figure-callout>}}_i^{}+{\mathrm{\&Delta;<figure-callout\; id="2"\; label="Y\; i"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">Y</figure-callout>}}_i^{}+{\mathrm{\&Delta;Z}}_i^2}-l_{1i}-l_{2i}$

${\mathrm{\&alpha;}}_i=\arctan \left(\frac{{\mathrm{\&Delta;Z}}_i}{\sqrt{{\mathrm{\&Delta;<figure-callout\; id="2"\; label="X\; i"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">X</figure-callout>}}_i^{}+{\mathrm{\&Delta;Y}}_i^2}}\right)$

${\mathrm{\&beta;}}_i=\arctan \left(\frac{{\mathrm{\&Delta;Y}}_i}{\mathrm{\&Delta;}{X}_i}\right)$

Wherein,

ΔX _i＝X( ⁷P _i)-X(B _i)

ΔY _i＝Y( ⁷P _i)-Y(B _i)

ΔZ _i＝Z( ⁷P _i)-Z(B _i)

Generalized coordinate is:

$q={\left[\begin{array}{cccccccccccccccc}{\mathrm{<figure-callout\; id="1"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_1& {\mathrm{\&alpha;}}_1& {\mathrm{\&beta;}}_1& {\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2& {\mathrm{\&alpha;}}_2& {\mathrm{\&beta;}}_2& ...& {\mathrm{<figure-callout\; id="6"\; label="d"\; filenames="HDA0000110620620000041.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">d</figure-callout>}}_6& {\mathrm{\&alpha;}}_6& {\mathrm{\&beta;}}_6& x_p& y_p& z_p& {\mathrm{\&psi;}}_p& {\mathrm{\&theta;}}_p& {\mathrm{\&phi;}}_p\end{array}\right]}_{(24\&times;1)}^T$

Obtain following Jacobi matrix:

$J_{\mathrm{ac}}={\left[\begin{array}{cccccc}\frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_1}{{\&PartialD;x}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_1}{{\&PartialD;y}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_1}{{\&PartialD;z}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_1}{{\&PartialD;\mathrm{\&psi;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_1}{{\&PartialD;\mathrm{\&theta;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="1"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_1}{{\&PartialD;\mathrm{\&phi;}}_p}\\ \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2}{{\&PartialD;x}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2}{{\&PartialD;y}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2}{{\&PartialD;z}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2}{{\&PartialD;\mathrm{\&psi;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2}{{\&PartialD;\mathrm{\&theta;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2}{{\&PartialD;\mathrm{\&phi;}}_p}\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ \frac{{\&PartialD;\mathrm{<figure-callout\; id="6"\; label="d"\; filenames="HDA0000110620620000041.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">d</figure-callout>}}_6}{{\&PartialD;x}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="6"\; label="d"\; filenames="HDA0000110620620000041.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">d</figure-callout>}}_6}{{\&PartialD;y}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="6"\; label="d"\; filenames="HDA0000110620620000041.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">d</figure-callout>}}_6}{{\&PartialD;z}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="6"\; label="d"\; filenames="HDA0000110620620000041.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">d</figure-callout>}}_6}{{\&PartialD;\mathrm{\&psi;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="6"\; label="d"\; filenames="HDA0000110620620000041.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">d</figure-callout>}}_6}{{\&PartialD;\mathrm{\&theta;}}_p}& \frac{{\&PartialD;\mathrm{<figure-callout\; id="6"\; label="d"\; filenames="HDA0000110620620000041.png,HDA0000110620620000042.png"\; state="\{\{state\}\}">d</figure-callout>}}_6}{{\&PartialD;\mathrm{\&phi;}}_p}\end{array}\right]}_{(6\&times;6)}$

Speed and acceleration can be expressed as:

$\stackrel{\&CenterDot;}{q}=J{\stackrel{\&CenterDot;}{q}}_7$

Ask for the derivative of speed, obtain the following expression-form of acceleration:

$\stackrel{\&CenterDot;\&CenterDot;}{q}=\stackrel{\&CenterDot;}{J}{\stackrel{\&CenterDot;}{q}}_7+J{\stackrel{\&CenterDot;\&CenterDot;}{q}}_7$

7, determine the control of controller.Matrix above using, generalized force can calculate following form:

$\mathrm{\&tau;}(q,\stackrel{\&CenterDot;}{q},\stackrel{\&CenterDot;\&CenterDot;}{q},t)={[I-{\mathrm{<figure-callout\; id="1"\; label="M"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">M</figure-callout>}}^{\frac{1}{2}}{B}^{+}{\mathrm{AM}}^{-1}]}^{-1}\&times;[M\stackrel{\&CenterDot;\&CenterDot;}{q}-{\mathrm{<figure-callout\; id="1"\; label="M"\; filenames="HDA0000110620620000042.png,HDA0000110620620000051.png"\; state="\{\{state\}\}">M</figure-callout>}}^{\frac{1}{2}}{B}^{+}b]+C$

The exciting force of closed loop is:

${\mathrm{\&tau;}}_{\mathrm{ac}}={J_{\mathrm{ac}}^{+}}^T(J^T\mathrm{\&tau;}-{\mathrm{\&tau;}}_7)$

If the motion platform displacement of expection is following relation:

$x_7=\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\\ {\mathrm{\&psi;}}_p\\ {\mathrm{\&theta;}}_p\\ {\mathrm{\&phi;}}_p\end{array}\right]=\left[\begin{array}{c}{r}_x\sin (\mathrm{\&omega;}\&times;t)\\ r_y\sin (\mathrm{\&omega;}\&times;t)\\ h+r_z\sin (\mathrm{\&omega;}\&times;t)\\ r_{\mathrm{\&psi;}}\sin (\mathrm{\&omega;}\&times;t)\\ r_{\mathrm{\&theta;}}\sin (\mathrm{\&omega;}\&times;t)\\ r_{\mathrm{\&phi;}}\sin (\mathrm{\&omega;}\&times;t)\end{array}\right],0\&le;t\&le;2\mathrm{\&pi;}$

Concrete parameter is as shown in the table:

Parameter	Unit	Parameter value	Parameter	Unit	Parameter value
						m 1i	kg	0.15	r p	mm	40
m 2i	kg	0.15	r b	mm	100
						m p	kg	1.0	r x	mm	30
l 1i	kg.m 2	4.0×10 -4	r y	mm	25
						l 2i	kg.m 2	4.0×10 -4	r z	mm	20
I 1i	kg.m 2	1.25×10 -4	r z	_	π/7
						I 2i	kg.m 2	1.25×10 -4	r x	_	π/6
I zp	kg.m 2	8.0×10 -4	r y	_	π/5
						I yp	kg.m 2	4.0×10 -4	ω	_	1
h	mm	140	g	m/s 2	9.81

By the calculating of above-mentioned steps, can obtain exciting force as shown in Figure 8, the motion of platform such as Fig. 9 are to shown in Figure 15.

Below only be preferred implementation of the present invention, protection scope of the present invention also not only is confined to above-described embodiment, and all technical schemes that belongs under the thinking of the present invention all belong to protection scope of the present invention.Should be pointed out that for those skilled in the art the some improvements and modifications not breaking away under the principle of the invention prerequisite should be considered as protection scope of the present invention.

Claims (3)

1. A parallel robot just, inverse dynamics response analysis and control method, it is characterized in that step is as follows:

   (1), decomposes parallel robot: select generalized coordinate system or self-defined coordinate system that each connecting rod and the moving platform of parallel robot are considered as a separate s subsystem;

   (2), determine the kinetics equation of each connecting rod subsystem: use the classical mechanics method to set up the kinetics equation of s-1 connecting rod subsystem and 1 moving platform subsystem, for the connecting rod subsystem, its kinetics equation is following form:

   $M_i(q_i,t){\stackrel{..}{q}}_i+c_i(q_i,{\stackrel{.}{q}}_i,t)={\mathrm{\&tau;}}_i\left(t\right)$ i＝1，2，…，s-1，

   Wherein, Be system's generalized velocity,

   

   Be system's generalized acceleration, q _iBe the generalized coordinate of i mechanical arm, M _iBe i mechanical arm n _i* n _iMass matrix, C _iBe centrifugal force/Ke Liaolili and the gravity of i mechanical arm, τ _iBe i the corresponding generalized force of mechanical arm;

   (3), determine the kinetics equation of moving platform subsystem: the kinetics equation of setting up following form with the classical mechanics method:

   $M_s(q_s,t){\stackrel{..}{q}}_s={\mathrm{\&tau;}}_s\left(t\right)$

   Wherein, q _sFor coordinate system P with respect to the origin of coordinate system B and the generalized coordinate of Eulerian angle, coordinate system P represents the motion platform coordinate system of parallel robot system, coordinate system B represents the stationary platform coordinate system of parallel robot system, q _s=[x _p, y _p, z _p, Ψ _p, θ _p, φ _p] ^TCorresponding mass matrix is diagonal matrix M _s∈ R ^{6 * 6},

   $M_s=\left[\begin{array}{cccccc}{m}_p& 0& 0& 0& 0& 0\\ 0& m_p& 0& 0& 0& 0\\ 0& 0& m_p& 0& 0& 0\\ 0& 0& 0& l_{\mathrm{zp}}& 0& 0\\ 0& 0& 0& 0& l_{\mathrm{yp}}& 0\\ 0& 0& 0& 0& 0& l_{\mathrm{zp}}\end{array}\right];$

   (4), determine equation of constraint: set up m equation of constraint between connecting rod subsystem and motion platform, do not lead form if this equation of constraint is not second order, the time differentiate is translated into following form:

   $A(q,t)\stackrel{..}{q}=b(\stackrel{.}{q},q,t)$

   Wherein, and A (q, t)=[A _Lt(q, t)] _{M * n},

   

   (q t) is constraint matrix to A;

   (5), obtain the positive drive mechanical equation of parallel robot system by analysis:

   $M(q,t)\stackrel{..}{q}=\mathrm{\&tau;}\left(t\right)-C\left(\stackrel{.}{q},q,t\right)+M^{\frac{1}{2}}(q,t)B+(q,t)[b(\stackrel{.}{q},q,t)-A(q,t)M^{-1}(q,t)(\mathrm{\&tau;}\left(t\right)-C(\stackrel{.}{q},q,t))]$

   Wherein,

   

   $q=\left[\begin{array}{c}{q}_1\\ q_2\\ .\\ .\\ .\\ q_s\end{array}\right],$ $\mathrm{\&tau;}=\left[\begin{array}{c}{\mathrm{\&tau;}}_1\\ {\mathrm{\&tau;}}_2\\ .\\ .\\ .\\ {\mathrm{\&tau;}}_s-1\\ {\mathrm{\&tau;}}_s\end{array}\right],$ $C=\left[\begin{array}{c}{C}_1\\ C_2\\ .\\ .\\ .\\ C_s-1\\ 0\end{array}\right];$ Definition $n={\mathrm{\&Sigma;}}_{i=1}^s{n}_{i,}$ M ∈ R is arranged ^{N * n}, q ∈ R ⁿ, τ ∈ R ⁿWith C ∈ R ⁿ

   

   Obtaining the acceleration equation is following form:

   $\stackrel{..}{q}=M^{-1}(q,t)[\mathrm{\&tau;}\left(t\right)-C(\stackrel{.}{q},q,t)]+M^{\frac{1}{2}}(q,t)B^{+}(q,t)(b(\stackrel{.}{q},q,t)-B(q,t)M^{-\frac{1}{2}}(q,t)[\mathrm{\&tau;}\left(t\right)-C(\stackrel{.}{q},q,t)];$

   (6), obtain the inverse dynamics equation by analysis: by the positive drive mechanical equation in the step (5), obtaining the inverse dynamics equation is following form:

   $\mathrm{\&tau;}(q,\stackrel{.}{q},\stackrel{..}{q},t)=[I-Q(\stackrel{.}{q},q,t)+M^{\frac{1}{2}}(q,t)B^{+}(q,t)A(q,t)M^{-1}(q,t){]}^{-1}\&times;[M(q,t)\stackrel{..}{q}-M^{\frac{1}{2}}(q,t)B^{+}(q,t)b\left(\stackrel{.}{q},q,t\right)]$

   $+C(\stackrel{.}{q},q,t)$

   Wherein, I ∈ R ^{N * n}Be unit matrix, at the q of expection _s,

   

   With

   

   Known, q,

   

   To be derived by follow-up formula draws, and then the corresponding matrix M of deriving, C, A, b;

   (7), determine the control τ of controller _AcFor:

   ${\mathrm{\&tau;}}_{\mathrm{ac}}={J_{\mathrm{ac}}^{+}}^T\left(q_s\right)({\mathrm{\&tau;}}_s^{\&prime;}-{\mathrm{\&tau;}}_s)+[I-{J_{\mathrm{ac}}^{+}}^T\left(q_s\right)J_{\mathrm{ac}}^T\left(q_s\right)]h$

   Wherein h is any 6 dimensional vectors, and J is Jacobi matrix.
2. Parallel robot according to claim 1 just, inverse dynamics response analysis and control method, it is characterized in that the classical mechanics method in described step (2) and the step (3) is Lagrangian method or newton's Euler's method.
3. Parallel robot according to claim 1 just, inverse dynamics response analysis and control method, it is characterized in that, in the described step (7), if that the motion of parallel manipulator does not exist is unusual, the equation of motion under the generalized coordinate of each subsystem is described as:

   q _i(t)＝f _i(q _i，t)，i＝1，2，…，s.

   Wherein, f _i∈ R ⁿ, get f _iTo q _sPartial derivative, obtain following Jacobi matrix:

   $J\left(q_s\right)={\left[\begin{array}{cccccc}\frac{\&PartialD;f_1}{\&PartialD;x_p}& \frac{\&PartialD;f_1}{\&PartialD;y_p}& \frac{\&PartialD;f_1}{\&PartialD;z_p}& \frac{\&PartialD;f_1}{\&PartialD;{\mathrm{\&psi;}}_p}& \frac{\&PartialD;f_1}{\&PartialD;{\mathrm{\&theta;}}_p}& \frac{\&PartialD;f_1}{\&PartialD;{\mathrm{\&phi;}}_p}\\ \frac{\&PartialD;f_2}{\&PartialD;x_p}& \frac{\&PartialD;f_2}{\&PartialD;y_p}& \frac{\&PartialD;f_2}{\&PartialD;z_p}& \frac{\&PartialD;f_2}{\&PartialD;{\mathrm{\&psi;}}_p}& \frac{\&PartialD;f_2}{\&PartialD;{\mathrm{\&theta;}}_p}& \frac{\&PartialD;f_2}{\&PartialD;{\mathrm{\&phi;}}_p}\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ \frac{\&PartialD;f_s}{\&PartialD;x_p}& \frac{\&PartialD;f_s}{\&PartialD;y_p}& \frac{\&PartialD;f_s}{\&PartialD;z_p}& \frac{\&PartialD;f_s}{\&PartialD;{\mathrm{\&psi;}}_p}& \frac{\&PartialD;f_s}{\&PartialD;{\mathrm{\&theta;}}_p}& \frac{\&PartialD;f_s}{\&PartialD;{\mathrm{\&phi;}}_p}\end{array}\right]}_{(n\&times;6)}$

   Wherein, (size of representing matrix of i * j),

   

   According to the Jacobi matrix theorem, velometer is shown:

   

   External force is expressed as: τ ' _s=J ^T(q _s) τ; Wherein, τ ' _sBe total power and the moment that under the motion of expection, acts on the motion platform; Ask for the derivative of speed, obtain the following expression-form of acceleration:

   

   To parallel robot system, establishing the generalized force that reaches the actuator that desired movement should apply is τ _Ac, τ _Ac∈ R ^rWith corresponding generalized coordinate be q _Ac, q _Ac∈ R ^r, q _Ac=f _Ac(q _s, _t), wherein, f _AcBe f in the formula _iThe combination of element, have:

   $J_{\mathrm{ac}}\left(q_s\right)={\left[\begin{array}{cccccc}\frac{\&PartialD;f_{\mathrm{ac}}}{\&PartialD;x_p}& \frac{\&PartialD;f_{\mathrm{ac}}}{\&PartialD;y_p}& \frac{\&PartialD;f_{\mathrm{ac}}}{\&PartialD;z_p}& \frac{\&PartialD;f_{\mathrm{ac}}}{\&PartialD;{\mathrm{\&psi;}}_p}& \frac{\&PartialD;f_{\mathrm{ac}}}{\&PartialD;{\mathrm{\&theta;}}_p}& \frac{\&PartialD;f_{\mathrm{ac}}}{\&PartialD;{\mathrm{\&phi;}}_p}\end{array}\right]}_{(r\&times;6)}$ So, ${\stackrel{.}{q}}_{\mathrm{ac}}=J_{\mathrm{ac}}\left(q_s\right){\stackrel{.}{q}}_{s,}$ Have ${\mathrm{\&tau;}}_s^{\&prime;}-{\mathrm{\&tau;}}_s=J_{\mathrm{ac}}^T\left(q_s\right){\mathrm{\&tau;}}_{\mathrm{ac}.}$

Images