CN105550466A - Force feedback equipment optimum spring gravity compensation method - Google Patents

Abstract

The invention relates to a force feedback equipment optimum spring gravity compensation method. A simple spring gravity compensation mode is used; a spring is arranged on a driven wheel of a line transmission speed reduction mechanism of a large arm and a small arm of force feedback equipment; the influence of the connecting point position, the free length and the rigid coefficient of the spring on the gravity compensation is sufficiently considered; a gravity compensation model of the nonlinear constraint relationship is built; the stretching free length ratio is introduced; the stretching free length ratio and the connecting point position are used as optimum quantities; the average moment error is used as an optimization fitness function; iterative optimization is performed by using an improved simple particle optimization algorithm, so that a large arm mechanism and a small arm mechanism of the force feedback mechanism can obtain the optimum simple spring gravity compensation. The method has the advantages that only two springs are used; the gravity compensation of the large arm mechanism and the small arm mechanism of the force feedback equipment is realized; the design is simple; and the realization and the modification are easy.

Description

The optimum spring gravity compensation method of a kind of force feedback equipment

Technical field

The present invention relates to for the spring gravity compensation method in force feedback equipment, particularly relate to based on the optimum spring gravity compensation method of the force feedback equipment improving simple particle algorithm.

Background technology

The feedback force of virtual environment or distal environment can be passed to operator by a desirable force feedback equipment accurately, this just requires that desirable force feedback equipment should have high rear-guard, low inertia, low friction, high rigidity, suitable work space, does not have backlash and high control bandwidth etc.When operator carries out man-machine interaction by force feedback equipment and virtual environment or distal environment, the gravity suffered by force feedback equipment manipulater can reduce the rear-guard of equipment, increases the frictional influence of equipment, thus affects the force feedback performance of equipment.Therefore, in order to make force feedback equipment have good performance, gravity compensation carries out to force feedback equipment most important.

In spring gravity compensation method, use spring to increase weight and the inertia of plant equipment itself hardly, and the design of spring and processing are simple and easy to realize, spring gravity compensation method has been widely used in all kinds of plant equipment carries out gravity compensation.In force feedback equipment, also often adopt spring gravity compensation method to carry out gravity compensation to the manipulater of equipment.In existing force feedback equipment, common gravitational compensation method has: Rahman etc. propose a kind of gravitational compensation method and (refer to: Asimpletechniquetopassivelygravity-balancearticulate-edm echanisms [J] .ASMETransMechDes.1995; 117 (4): 655 ~ 58.), the method adopts one or more Hookean spring and steamboat, for compensating the system of single connecting rod and multiple connecting rod, but this spring gravity compensation method is complicated, need extra other mechanical hook-ups of interpolation, improve design cost and the complexity of system, also easily make to form mechanical interference between spring and mechanical linkage, influential system operating performance.Omega force feedback equipment (patent No.: US8,188,843B2) uses spring and in conjunction with irregular roundness cam and wire rope to carry out gravity compensation, but this compensation way needs additional designs and machining cam, which increases design cost and complexity; Secondly, radius due to irregular roundness cam is the amount of nonlinearity with angular displacement of the cam change, when force feedback equipment is at fast operating, the wire rope connecting cam steel and spring can not remain on same level always, spring and wire rope is easily caused to produce drawing gap, between this and gear, backlash is similar, and the stability of force feedback system can be caused to reduce.The method that Rahman method and Omega force feedback equipment use is integrated as the method that PHANTOMPremium1.5 devises gravity compensation and (refers to: ImprovementofDynamicTransparencyofHapticDevicebyUsingSpr ingBalance [C] .Proceedingsofthe2012IEEEInternationalConferenceonRoboti csandBiomimetics by RongfangFan, 2012:1075 ~ 1080.), but there is above-mentioned Rahman and the Omega force feedback equipment (patent No.: US8 too, 188,843B2) in problem.The gravitational compensation methods proposed based on simple spring such as foreign scholar AhmadMashayekhi (refer to: VirSense:anovelhapticdevicewithfixed-basemotorsandagravi tycompensationsystem, IndustrialRobot:AnInternationalJournal, 2014,41 (1): 37 ~ 49.).Although the simple spring gravity compensation method that AhmadMashayekhi etc. propose can compensate the gravity of connecting rod, but the method is artificial provide spring connecting point position, do not set up optimum spring gravity and compensate mathematical model, not from the optimum tie point position that spring gravity compensates, the drift of spring optimum and optimum mean rigidity coefficient angle go design spring gravity to compensate.

Known in sum, the advantage that spring gravity compensates is: spring is connected to the weight and inertia that force feedback equipment increase hardly equipment itself.But the shortcoming existing for above-mentioned spring gravity compensation method is: do not consider how devise optimum gravity compensation spring.In other words, when designing force feedback equipment spring gravity and compensating, do not taken into full account that the tie point position of spring, drift and stiffness coefficient are on the impact of gravity compensation.

Summary of the invention

The object of the invention is for existing spring gravity compensation technique defect, use and improve simple single-particle optimizing algorithm, the optimum tie point position finding spring gravity to compensate, the optimum drift of spring and optimum mean rigidity coefficient.The present invention has taken into full account that the tie point position of spring, drift and stiffness coefficient are on the impact of gravity compensation, establish the spring gravity compensation model of non-linear constrain relation, use the simple single-particle optimizing algorithm iteration optimization improved to obtain optimum spring gravity and compensate.

The present invention is realized by following technology.

Step one, large arm gravity compensation spring 4 one end are connected to large arm reduction driven and take turns 2 ends, and the other end is connected on direct current generator 5 fixed support, for compensating large arm mechanism 1 gravity;

Step 2, take into full account that large arm gravity compensation spring 4 is connected to the drift x of link position u on direct current generator 5 fixed support, large arm gravity compensation spring ₀, when large arm mechanism rotates and the stiffness coefficient K of horizontal plane angle θ and large arm gravity compensation spring 4 on the impact of large arm mechanism gravity compensation, establish the model T of the large arm gravity compensation of non-linear constrain relation _s(u, x ₀, θ, K).Wherein, the value of K and u, x ₀constitute restriction relation with θ, therefore K also can be write as K (u, x ₀, θ); The restriction relation existed in this model is the restriction relation that variable u and θ and spring elongation length x (u, θ) are formed;

Step 3, in order to reduce the complexity of the simple single-particle optimizing algorithm of improvement, reasonable minimizing optimized variable number, take into full account variable u and θ and spring elongation length x (u, restriction relation θ) formed, introduces " stretching drift ratio " LLimit, using tie point position u and " stretching drift ratio " LLimt as optimized amount, average torque error AVEE is as the fitness function optimized, wherein, spring elongation length x (u, θ) and spring free length x is defined ₀ratio as " stretching drift than " LLimit;

The simple single-particle optimizing algorithm that step 4, use are improved carries out iteration optimization, obtains optimum large arm gravity compensation spring 4 tie point position u _opt, spring optimum drift x _optwith Optimal Stiffness COEFFICIENT K _opt, realize optimum simple spring gravity Compensation Design, concrete optimizing process is described below:

A () first, carries out Initialize installation: set large arm mechanism 1 rotational angle θ scope as [ψ ₁, ψ ₂], assuming that the population that the N number of particle in the simple single-particle optimizing algorithm improved is formed is described as: X=(X ¹, X ²..., X ⁿ), wherein, X represents population set, X ⁱ(i=1,2 ..., N) and represent the position of i-th particle in this population.In this population, the flight position search volume of each particle is 2 gt, and every 1 dimension in this 2 dimension location finding space is expressed as tie point position u and " stretching drift ratio " LLimt of Hookean spring.If particle iteration total degree is M time, so, i-th particle is expressed as position in kth time iteration: X ⁱ(k)=(u ⁱ(k), LLimt ⁱ(k)) ^t, wherein, u ⁱ(k), LLimt ⁱk () be i-th the spring connecting point position of particle in kth time iteration respectively, " stretching drift ratio ".These two parameters are random numbers, and its region of search stated range minimum is respectively u _minand LLimit _min, maximal value is respectively u _maxand LLimit _max.By angle θ scope [ψ ₁, ψ ₂] discretely turn to n part equal intervals: ψ ₁=θ ₁< θ ₂< ... < θ _n< θ _n+1=ψ ₂, between adjacent two angles, difference is: Δ θ _i=(ψ ₂-ψ ₁)/n, if θ ₁, θ ₂..., θ _n, θ _n+1the moment of the corresponding Hookean spring in place is: T _s1(θ ₁), T _s2(θ ₂) ..., T _sn(θ _n), T _sn+1(θ _n+1), the required corresponding gravity torque compensated of large arm mechanism 1 is: T ₁(θ ₁), T ₂(θ ₂) ..., T _n(θ _n), T _n+1(θ _n+1), they are all substituted into the fitness function value of middle calculating particle optimized algorithm;

B () establishes X ⁱ(k)=(u ⁱ(k), LLmit ⁱ(k)) ^tthe position of each particle i in population kth time iteration, first by X ⁱk () substitutes into stiffness coefficient K (u, the x of the large arm gravity compensation spring 4 in formula (9) ₀, θ) in, for ψ ₁=θ ₁< θ ₂< ... < θ _n< θ _n+1=ψ ₂in each angle value, corresponding K can be calculated ₁, K ₂..., K _n, K _n+1value.Again by K ₁~ K _n+1get arithmetic mean after summation to be worth this value represents the stiffness coefficient of the spring in population kth time iteration corresponding to particle i, and will substitute into in solve fitness function value;

In (c) kth time iteration, N number of particles all in population is substituted into fitness function formula respectively according to (b) step in solve average torque error AVEE, then find fitness values all in all populations in whole k time (the 1st time to kth time) iteration, and find global optimum's fitness value to be: P ^g(k)=(u ^g(k), LLmit ^g(k)) ^t.Wherein, u ^g(k) and LLmit ^gk () represents global optimum's spring connecting point in whole k iteration of whole N number of particle in population and global optimum " stretching drift than " respectively.

(d) X ⁱ(k+1)=c ₁r ₁x ⁱ(k)+c ₂r ₂[P ^g(k)-X ⁱ(k)]+Θ ξ/N as evolution iteration renewal equation, represent that i-th particle in all N number of particles upgrades in kth+1 iteration, wherein, c ₁be called cognitive learning rate, generally get constant value 1.49445, c ₂be called that social learning leads, also get constant value 1.49445, r ₁and r ₂the arbitrary constants of two scopes between 0 ~ 1.Θ ξ/N=0 is called skip over, wherein, Θ is an adjustable value, between the position maximal value of its span all particles in 0 to population, ξ is a stochastic variable, and its scope is certain the random site value in the band of position specified by all particles.N is the number of population; In the present invention, we choose Θ ξ/N=0;

E () boundary condition is arranged: if the population be made up of N number of particle, after the iterative computation that kth is secondary, and the position X of i-th particle ⁱk certain the one dimension position range in () is beyond the interval of specifying: the minimum value u respectively of the minimum value region of search scope in this interval _minand LLimit _min, maximal value is respectively u _maxand LLimit _max.So position X of i-th particle ⁱk certain one dimension of () has exceeded minimum edge boundary treaty fixed condition, must redesignated as u _minor LLimit _min.If, the position X of i-th particle ⁱk certain one dimension of () has exceeded maximum boundary agreed terms, then this dimension location variable must redesignated as u _maxand LLimit _max;

If f () evolution number of iterations reaches maximum iteration time M, then finishing iteration, find the spring connecting point position u of the global optimum after all evolution iteration _optwith optimum " stretching drift ratio " LLimit _opt.Otherwise order performs after continuing to get back to (b) step;

G () is final, according to the model T of large arm gravity compensation _s(u, x ₀, θ, K), the restriction relation that variable u and θ and spring elongation length x (u, θ) is formed, the final optimum spring connecting point position u obtaining spring _opt, drift x _optwith Optimal Stiffness COEFFICIENT K _opt.

Step 5, forearm gravity compensation spring 10 one end are connected to forearm reduction driven and take turns 8 ends, the other end is connected on direct current generator 11 fixed support, for compensating little arm mechanism 7 gravity, wherein, universal joint 17 is arranged on little arm mechanism 7 end, the center of gravity of universal joint 17 concentrates on the end of little arm mechanism 7, and forearm gravity compensation spring 10 compensates little arm mechanism 7 gravity and comprises gravity suffered by gravity suffered by universal joint and forearm self;

Step 6, set up the model T of the forearm gravity compensation of non-linear constrain relation _xs(u _x, x _x0, θ _x, K _x), wherein, u _xfor forearm gravity compensation spring 10 is connected to the tie point position on direct current generator 11 fixed support, x _x0for the drift of forearm gravity compensation spring 10, θ _xfor when little arm mechanism 7 rotates and horizontal plane angle, K _xfor the stiffness coefficient of forearm gravity compensation spring 10, due to K _xvalue and u _x, x _x0and θ _xconstitute restriction relation, therefore K _xalso K can be write as _x(u _x, x _x0, θ _x); In addition, the restriction relation existed in this model is variable u _xand θ _xwith spring elongation length x _x(u _x, θ _x) restriction relation that formed;

Step 7, for forearm gravity compensation spring 10, also introduce stretching drift and compare LLimit _x, by tie point position u _xwith LLimit _xas optimized amount, average torque error AVEE _xas the fitness function optimized; Wherein, LLimit _xfor forearm gravity compensation spring 10 tensile elongation x _x(u _x, θ _x) and spring free length x _x0ratio;

The simple single-particle optimizing algorithm that step 8, use are improved carries out iteration optimization, obtains optimum forearm gravity compensation spring 10 tie point position u _xopt, spring optimum drift x _xoptwith Optimal Stiffness COEFFICIENT K _xopt, realize the simple spring gravity Compensation Design of optimum of little arm mechanism 7, concrete steps are as follows:

(aa) first, Initialize installation is carried out: establish little arm mechanism 7 rotational angle θ _xscope is [ψ _x1, ψ _x2], assuming that be also described as by the population that N number of particle is formed in the simple single-particle optimizing algorithm improved: X=(X ¹, X ²..., X ⁿ), wherein, X represents population set, X ⁱ(i=1,2 ..., N) and represent the position of i-th particle in this population; In this population, the flight position search volume of each particle is 2 gt, and every 1 dimension in this 2 dimension location finding space is expressed as the tie point position u of spring _xlLimit is compared with stretching drift _x; If particle iteration total degree is M time, so, i-th particle is expressed as position in kth time iteration: wherein, the spring connecting point position of difference i-th particle in kth time iteration and " stretching drift ratio ", these two parameters are random numbers, and its region of search stated range minimum is respectively u _xminand LLimit _xmin, maximal value is respectively u _xmaxand LLimit _xmax; By angle θ _xscope [ψ _x1, ψ _x2] uniform discrete is n part equal intervals: ψ _x1=θ _x1< θ _x2< ... < θ _xn< θ _{x (n+1)}=ψ _x2, between adjacent two angles, difference is: θ _xi=(ψ _x1-ψ _x2)/n, if θ _x1, θ _x2..., θ _xn, θ _{x (n+1)}the moment of the corresponding Hookean spring in place is: T _xs1(θ _x1), T _xs2(θ _x2) ..., T _xsn(x θ _xn), T _{xs (n+1)}(θ _{x (n+1)}), the required corresponding gravity torque compensated of little arm mechanism 7 is: T _x1(θ _x1), T _x2(θ _x2) ..., T _xn(θ _xn), T _{x (n+1)}(θ _{x (n+1)}), they are all substituted into the fitness function value of middle calculating particle optimized algorithm;

(bb) establish the position of each particle i in population kth time iteration, first by X ⁱk () substitutes into the stiffness coefficient K of forearm gravity compensation spring 10 _x(u _x, x _x0, θ _x) in, for ψ _x1=θ _x1< θ _x2< ... < θ _xn< θ _{x (n+1)}=ψ _x2in each angle value, calculate corresponding K _x1, K _x2..., K _xn, K _{x (n+1)}value; Again by K _x1~ K _{x (n+1)}get arithmetic mean after summation to be worth this value represents the stiffness coefficient of the spring in population kth time iteration corresponding to particle i, and will substitute into in solve fitness function value;

(cc) in kth time iteration, N number of particles all in population is substituted into fitness function formula respectively according to step (bb) in solve average torque error AVEE _x, then find fitness values all in all populations in whole k time (the 1st time to kth time) iteration, and find global optimum's fitness value to be: $P^g\left(k\right)={\left(u_x^g\right(k),{\mathrm{LLimt}}_x^g(k\left)\right)}^T,$ Wherein, with represent global optimum's spring connecting point in whole k iteration of whole N number of particle in population and global optimum's stretching drift ratio respectively;

(dd) X ⁱ(k+1)=c ₁r ₁x ⁱ(k)+c ₂r ₂[P ^g(k)-X ⁱ(k)]+Θ ξ/N as evolution iteration renewal equation, represent that i-th particle in all N number of particles upgrades in kth+1 iteration, wherein, c ₁be called cognitive learning rate, generally get constant value 1.49445, c ₂be called that social learning leads, also get constant value 1.49445, r ₁and r ₂the arbitrary constants of two scopes between 0 ~ 1.Θ ξ/N=0 is called skip over, wherein, Θ is an adjustable value, between the position maximal value of its span all particles in 0 to population, ξ is a stochastic variable, and its scope is certain the random site value in the band of position specified by all particles.N is the number of population; In the present invention, we choose Θ ξ/N=0;

(ee) boundary condition agreement: if the population be made up of N number of particle, after the iterative computation that kth is secondary, the position X of i-th particle ⁱk certain the one dimension position range in () is beyond the interval of specifying: the minimum value u respectively of the minimum value region of search scope in this interval _xminand LLimit _xmin, maximal value is respectively u _xmaxand LLimit _xmax.So position X of i-th particle ⁱk certain one dimension of () has exceeded minimum edge boundary treaty fixed condition, must redesignated as u _xminor LLimit _xmin.If, the position X of i-th particle ⁱk certain one dimension of () has exceeded maximum boundary agreed terms, then this dimension location variable must redesignated as u _xmaxand LLimit _xmax;

(ff) if evolution number of iterations reaches maximum iteration time M, then finishing iteration, finds the spring connecting point position u of the global optimum after all evolution iteration _xoptlLimit is compared with optimum stretching drift _xopt; Otherwise, continue to get back to step (bb) order execution afterwards;

(gg) according to forearm gravity compensation model T _xs(u _x, x _x0, θ _x, K _x), variable u _xand θ _xwith spring elongation length x _x(u _x, θ _x) restriction relation that formed, the final optimum spring connecting point position u obtaining spring _xopt, drift x _xoptwith Optimal Stiffness COEFFICIENT K _xopt.

Advantage of the present invention: taken into full account that the tie point position of spring, drift and stiffness coefficient are on the impact of gravity compensation, establish the optimum spring gravity compensation model of non-linear constrain relation, invent a kind of spring gravity compensation method based on simple single-particle optimizing, use and improve the optimum tie point position that simple particle algorithm finds force feedback equipment gravity compensation spring, the drift of spring optimum and optimum mean rigidity coefficient; Only use two springs, these two spring one end are connected on the large arm of force feedback equipment and the reducing gear engaged wheel of forearm, their other end is connected on base reducing gear engaged wheel, realize the large arm of force feedback equipment and the gravity compensation of forearm, this invention simplicity of design, easily realizes and repacking.

Accompanying drawing explanation

Fig. 1 is that spring gravity of the present invention compensates junctor composition.In figure: 1 is large arm mechanism, 2 is large arm reduction driven wheel, and 3 be large arm deceleration driving wheel 4 is large arm gravity compensation spring, 5 be direct current generator 6 is large arm rotating shaft, 7 is little arm mechanism, and 8 is forearm reduction driven wheel, and 10 is forearm gravity compensation spring, 12 is forearm rotating shaft, 13 is base reduction driven wheel, and 14 is base deceleration driving wheel, and 16 is base shaft, 17 is universal joint, and 18 is the rotating shaft of forearm reduction driven wheel.

Fig. 2 is that turn clockwise 180 ° of spring gravity of Fig. 1 compensate junctor compositions.In figure: 1 is large arm mechanism, 2 is large arm reduction driven wheel, and 7 is little arm mechanism, 8 is forearm reduction driven wheel, and 9 is forearm deceleration driving wheel, and 10 is forearm gravity compensation spring, 11 is direct current generator, 12 is forearm rotating shaft, and 13 is base reduction driven wheel, and 14 is base reducing gear driving wheel, 15 is direct current generator, 16 is base shaft, and 17 is universal joint, and 18 is the rotating shaft of forearm reducing gear engaged wheel.

Fig. 3 is invention large arm mechanism spring gravity compensation schematic diagram.

Fig. 4 is invention little arm mechanism spring gravity compensation schematic diagram.

Embodiment

The present invention will be further described by reference to the accompanying drawings.

1-4 is described as follows by reference to the accompanying drawings: as shown in Figures 1 and 2, this force feedback equipment is a serial linkage with 6 rotary joints, be similar to the arm of people, chief component is: large arm mechanism 1, large arm reduction driven wheel 2, large arm deceleration driving wheel 3, large arm gravity compensation spring 4, direct current generator 5, large arm rotating shaft 6, little arm mechanism 7, forearm reduction driven wheel 8, forearm deceleration driving wheel 9, forearm gravity compensation spring 10, direct current generator 11, forearm rotating shaft 12, base reduction driven wheel 13, base deceleration driving wheel 14, direct current generator 15, base shaft 16, universal joint 17, composition such as part such as forearm reduction driven wheel rotating shaft 18 grade.

As shown in Figures 1 and 2, large arm mechanism 1, large arm reduction driven wheel 2, large arm deceleration driving wheel 3, large arm gravity compensation spring 4, direct current generator 5, large arm rotating shaft 6, little arm mechanism 7, forearm reduction driven wheel 8, forearm deceleration driving wheel 9, forearm gravity compensation spring 10, direct current generator 11, forearm rotating shaft 12, universal joint 17, the part such as forearm reducing gear engaged wheel rotating shaft 18 all by support installing on base reducing gear engaged wheel 13, rotate around base shaft 16 together with base reducing gear engaged wheel 13.

As shown in Figures 1 and 2, base deceleration driving wheel 14 is nested on direct current generator 15, when direct current generator 15 drives base deceleration driving wheel 14, makes both coaxial rotation.Base deceleration driving wheel 14 drives base reduction driven wheel 13 by wire rope.Seat reduction driven wheel 13 rotates around base shaft 16, and pass through wire rope gearing, large arm mechanism 1, little arm mechanism 7 and universal joint 17 is driven all to rotate around base shaft 16, thus realize making force feedback equipment can produce corresponding feedback force, base reduction driven is taken turns 13 rotational angles and is measured acquisition by the photoelectric encoder on direct current generator 15.

As shown in Figures 1 and 2, large arm deceleration driving wheel 3 is nested in the rotating shaft of direct current generator 5, when direct current generator 5 drives large arm deceleration driving wheel 3, makes both coaxial rotation.Large arm deceleration driving wheel 3 drives large arm reduction driven wheel 2 by wire rope.Large arm reduction driven wheel 2 drives large arm mechanism 1, and they all rotate around large arm rotating shaft 6, thus realizes making large arm mechanism 1 can produce corresponding feedback force.Large arm mechanism 1 rotational angle is measured by the photoelectric encoder on direct current generator 5 and is obtained.Large arm gravity compensation spring 4 one end is connected to large arm reduction driven and takes turns 2 ends, and the other end is connected on direct current generator 5 fixed support, for compensating large arm mechanism 1 gravity.

As shown in Figures 1 and 2, forearm deceleration driving wheel 9 is nested in the rotating shaft of direct current generator 11, when direct current generator 11 drives forearm deceleration driving wheel 9, makes both coaxial rotation.Forearm deceleration driving wheel 9 drives forearm reduction driven wheel 8 by wire rope.Forearm reduction driven wheel 8 rotates around the rotating shaft 18 of forearm reduction driven wheel, and by wire rope gearing, drives little arm mechanism 7 to rotate around forearm rotating shaft 12, thus realizes making little arm mechanism 7 can produce corresponding feedback force.Little arm mechanism 7 rotational angle is measured by the photoelectric encoder on direct current generator 11 and is obtained.Universal joint 17 is arranged on little arm mechanism 7 end, and universal joint 17 is made up of three mutual vertical passive rotary joints of axis, and the joint angles of these three rotary joints measures the respective anglec of rotation respectively by the angular potentiometer on respective joint.Forearm gravity compensation spring 10 one end is connected to forearm reduction driven and takes turns 8 ends, and the other end is connected on direct current generator 11 fixed support, for compensating little arm mechanism 7 gravity.In addition, the center of gravity of universal joint 17 concentrates on the end of little arm mechanism 7, and therefore, gravity compensation spring 10 compensates little arm mechanism 7 gravity and also just comprises gravity suffered by gravity suffered by universal joint and forearm self.

Fig. 3 is that large arm gravity compensation spring 4 carries out the principle schematic of simple spring gravity compensation to large arm mechanism 1, large arm mechanism 1 connecting rod O ₂o ₃describe, the center of gravity due to universal joint 17 concentrates on the end of little arm mechanism 7, therefore, and little arm mechanism 7 and universal joint 17 connecting rod O ₃o ₄be described, large arm reduction driven wheel 2 uses semicircle O ₂rQ is described, and O is used in large arm rotating shaft 6 ₂describe, O is used in forearm rotating shaft 12 ₃describe, large arm mechanism 1 describes around the rotational angle angle θ of large arm rotating shaft 6, and large arm gravity compensation spring 4 tensile elongation QP describes, and one end is connected on certain 1 P on direct current generator 5 fixed support, the other end is connected to the Q point of large arm reduction driven wheel 2, and Q point is at O ₂o ₃reverse extending line and large arm reducing gear engaged wheel O ₂rQ annulus external diameter intersection, the drift of spring is x ₀, spring elongation length QP equals x, assuming that the stiffness coefficient of spring is K, spring elongation masterpiece is used in O ₂the arm of force O at place ₂e length is h, when spring tension is F _s, moment is T _s=F _sh.Connecting rod O ₂o ₃corner ∠ O ₃o ₂l=θ, the gravity torque of connecting rod is T=Bcos θ.Large arm reducing gear engaged wheel radius O ₂q length is r.PO ₀it is parallel that residing plane and base reduction driven take turns plane residing for 13, from O ₂place makes vertical line and PO ₀place plane orthogonal intersects, and intersection point is positioned at O ₀, wherein O ₂o ₀length is a, PO ₀length is u.Vertical line and PO is made from Q point ₀place plane orthogonal intersects, and intersection point is positioned at F, ∠ EO ₂n=∠ FQP=θ ₁, ∠ QO ₂n=θ.At right angle Δ O ₂in QG, QG equals rcos θ, O ₂g length equals rsin θ.Then QF length equals GO ₀length, equals O ₂o ₀subtract O ₂g, for: a-rsin θ.PF length equals FO ₀subtract PO ₀, for: rcos θ-u.Like this, in right angle Δ QFP, spring elongation length QP x (u, θ) represents, according to Pythagorean theorem, the restriction relation can obtaining itself and variable u, θ is:

$x(u,\mathrm{\&theta;})=\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}---\left(1\right)$

Like this, the tensile force of spring is:

$F_s=K(x-x_0)=K\&lsqb;\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}-x_0\&rsqb;---\left(2\right)$

Again because the extended line of PQ and O ₂n intersects at N point, and O ₂n is parallel to O ₀f, ∠ O ₂nE equals ∠ QPF, ∠ O ₂eN and ∠ QFP equals pi/2, so ∠ EO ₂n=∠ FQP=θ ₁.So the effect of spring elongation power is to O ₂the arm of force O at place ₂e length equals:

h＝rcos(θ-θ ₁)(3)

So it is F that spring gravity compensation model produces corresponding moment _sh:

$T_s(u,x_0,\mathrm{\&theta;},K)=F_{s}h=K\&lsqb;\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}-x_0\&rsqb;rcos(\mathrm{\&theta;}-{\mathrm{\&theta;}}_1)---\left(4\right)$

Wherein, in Δ QFP, the length of known QF is the length of a-rsin θ, PF is rcos θ-u, is QP again, can obtains according to spring elongation length known in formula (1):

${\mathrm{sin\&theta;}}_1=\frac{rcos\mathrm{\&theta;}-u}{\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}}---\left(5\right)$

${\mathrm{cos\&theta;}}_1=\frac{a-rsin\mathrm{\&theta;}}{\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}}---\left(6\right)$

In order to enable spring elongation moment full remuneration large arm mechanism gravity torque T=Bcos θ, then the compensation gravitational equilibrium moment that spring produces is:

$T=Bcos\mathrm{\&theta;}=F_{s}h=K\&lsqb;\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}-x_0\&rsqb;rcos(\mathrm{\&theta;}-{\mathrm{\&theta;}}_1)---\left(7\right)$

Further, can show that the stiffness coefficient of spring is:

$K=K\left(u,x_0,\mathrm{\&theta;}\right)=\frac{Bcos\mathrm{\&theta;}}{r\&lsqb;\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}-x_0\&rsqb;\cos \left(\mathrm{\&theta;}-{\mathrm{\&theta;}}_1\right)}---\left(8\right)$

Due to value and u, x of K ₀constitute restriction relation with θ, therefore also can be write K as K (u, x ₀, θ).By cos (θ-θ ₁) expand into cos θ cos θ ₁+ sin θ sin θ ₁, and formula (5) and formula (6) are substituted in expansion, then be updated to formula (8) abbreviation, draw:

$K=K(u,x_0,\mathrm{\&theta;})=\frac{B}{r\mathrm{\&phi;}(a-utan\mathrm{\&theta;})}---\left(9\right)$

Wherein,

$\mathrm{\&phi;}=\frac{\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}-x_0}{\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}}---\left(10\right)$

Like this, in formula (9), just can draw spring rate K be one along with large arm mechanism (connecting rod O ₂o ₃) corner ∠ O ₃o ₂the variable of L=θ, wherein: introduce parameter u and x ₀be used for respectively describing the tie point present position of spring and the drift of spring.

Gravity compensation is carried out, according to parameter u and x owing to adopting Hookean spring ₀change, fully examination when the tie point of Hookean spring be in diverse location and spring adopt different drifts time, the gravity compensation effect of Hookean spring.Finally, by selecting suitable position u and suitable Hookean spring drift x ₀, find the Optimal Stiffness coefficient of Hookean spring.By using the Hookean spring of this Optimal Stiffness coefficient, make this Hookean spring fully can compensate large arm mechanism (connecting rod O ₂o ₃) gravity.

The stiffness coefficient of Hookean spring is K, and Hookean spring drift is x ₀, the tie point position of corresponding Hookean spring is u.If large arm mechanism O ₂o ₃the scope of rotational angle is [ψ ₁, ψ ₂].By [ψ ₁, ψ ₂] interval be divided into n equal portions, ψ ₁=θ ₁< θ ₂< ... < θ _n< θ _n+1=ψ ₂, the length of every adjacent spaces is: Δ θ _i=(ψ ₂-ψ ₁)/n, if θ ₁, θ ₂..., θ _n, θ _n+1the moment of the corresponding Hookean spring in place is: T _s1(θ ₁), T _s2(θ ₂) ..., T _sn(θ _n), T _sn+1(θ _n+1), gravity torque corresponding to connecting rod is: T ₁(θ ₁), T ₂(θ ₂) ..., T _n(θ _n), T _n+1(θ _n+1).Therefore, at [ψ ₁, ψ ₂] angular interval, the moment average error AVEE of Hookean spring compensation link gravity can approximate representation be:

$AVEE\&ap;\frac{1}{n+1}\underset{i=1}{\overset{n+1}{\&Sigma;}}|T_{si}\left({\mathrm{\&theta;}}_i\right)-T_i\left({\mathrm{\&theta;}}_i\right)|---\left(11\right)$

Known, Δ θ _i=(ψ ₂-ψ ₁)/n, formula (11) can be rewritten as:

$AEVE\&ap;\underset{i=1}{\overset{n+1}{\&Sigma;}}\frac{\left|T_{si}\left({\mathrm{\&theta;}}_i\right)-T_i\left({\mathrm{\&theta;}}_i\right)\right|}{{\mathrm{\&psi;}}_2-{\mathrm{\&psi;}}_1}\&CenterDot;\frac{{\mathrm{\&psi;}}_2-{\mathrm{\&psi;}}_1}{n}=\underset{i=1}{\overset{n+1}{\&Sigma;}}\frac{\left|T_{si}\left({\mathrm{\&theta;}}_i\right)-T{\left({\mathrm{\&theta;}}_i\right)}_i\right|}{{\mathrm{\&psi;}}_2-{\mathrm{\&psi;}}_1}\&CenterDot;{\mathrm{\&Delta;\&theta;}}_i---\left(12\right)$

To formula (12) finding limit, then the moment average error of Hookean spring compensation link gravity is expressed as:

$AVEE=\frac{1}{{\mathrm{\&psi;}}_2-{\mathrm{\&psi;}}_1}\underset{n+1\&RightArrow;\mathrm{\&infin;}}{\lim }\underset{i=1}{\overset{n+1}{\&Sigma;}}\left|T_{si}\left({\mathrm{\&theta;}}_i\right)-T_i\left({\mathrm{\&theta;}}_i\right)\right|{\mathrm{\&Delta;\&theta;}}_i=\frac{1}{{\mathrm{\&psi;}}_2-{\mathrm{\&psi;}}_1}{\&Integral;}_{{\mathrm{\&psi;}}_1}^{{\mathrm{\&psi;}}_2}\left|T_s\left(\mathrm{\&theta;}\right)-T\left(\mathrm{\&theta;}\right)\right|d\mathrm{\&theta;}---\left(13\right)$

Formula (4) ~ (6) and connecting rod gravity torque T=Bcos θ are substituted in formula (13), can draw:

$AVEE=\frac{1}{{\mathrm{\&psi;}}_2-{\mathrm{\&psi;}}_1}{\&Integral;}_{{\mathrm{\&psi;}}_1}^{{\mathrm{\&psi;}}_2}|Bcos\mathrm{\&theta;}-\frac{\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}-x_0}{\sqrt{{(rcos\mathrm{\&theta;}-u)}^2+{(a-rsin\mathrm{\&theta;})}^2}}Kr(acos\mathrm{\&theta;}-usin\mathrm{\&theta;})|d\mathrm{\&theta;}---\left(14\right)$

In formula (14), B, a and r are the drift x of known constant, spring ₀, the tie point position u of spring rate K and spring is unknown variable.Large arm mechanism (connecting rod O is fully compensated in order to enable designed Hookean spring ₂o ₃) gravity torque, using the fitness function of formula (14) as the simple single-particle optimizing algorithm improved, also referred to as optimization aim.The spring rate K of one group of optimum is found out by the simple single-particle optimizing algorithm improved _opt, optimum spring connecting point u _opt, and the free x of the spring of optimum _0opt, this group parameter makes Hookean spring compensate large arm mechanism (connecting rod O ₂o ₃) the average torque error AVEE of gravity is minimum.

Formula (14) is rewritten as:

AVEE＝g(x ₀,K,u|θ)(15)

Wherein, g (x ₀, K, u| θ) and equal the right-hand component of formula (14) AVEE, using this function as the fitness function of the simple single-particle optimizing algorithm search improved or be called objective function, by using this fitness function, to particle evolution iteration, finally find out the spring rate K of one group of optimum _opt, optimum spring connecting point u _optwith the spring free length x of optimum _0opt.But, in order to adapt to the discrete programming of particle algorithm, usually by (15) formula discretize accepted way of doing sth (11) form, making calculating more convenient, being more suitable for the simple single-particle optimizing algorithm optimization process improved.

Be x by the spring free length of simple single-particle optimizing algorithm optimum choice improved ₀, the tie point position u of spring rate K and spring, concrete steps are described below:

(1) first, Initialize installation is carried out.As can be known from Fig. 3, if large arm mechanism (connecting rod O ₂o ₃) rotational angle range is [ψ ₁, ψ ₂], work as O ₂q and O ₂o ₀during coincidence, large arm mechanism (connecting rod O ₂o ₃) angle be pi/2, now connecting rod gravity torque is 0, if now the drift of Hookean spring is x ₀, the tensile elongation of spring is then the tensile elongation of spring with drift ratio is: referred to as " stretching drift ratio ".Spring free length x is established by " stretching drift ratio " LLimt ₀with the relation of spring connecting point position u, then Hookean spring drift assuming that in the simple single-particle optimizing algorithm improved, the population be made up of N number of particle is described as: X=(X ¹, X ²..., X ⁿ), wherein, X represents population set, X ⁱ(i=1,2 ..., N) and represent the position of i-th particle in this population.In this population, the flight position search volume of each particle is 2 gt, and every 1 dimension in this 2 dimension location finding space is expressed as tie point position u and " stretching drift ratio " LLimt of Hookean spring.If particle iteration total degree is M time, so, i-th particle is expressed as position in kth time iteration: X ⁱ(k)=(u ⁱ(k), LLimt ⁱ(k)) ^t, wherein, u ⁱ(k), LLimt ⁱk () be i-th the spring connecting point position of particle in kth time iteration respectively, " stretching drift ratio ".These two parameters are random numbers, and its region of search stated range minimum is respectively u _minand LLimit _min, maximal value is respectively u _maxand LLimit _max.By connecting rod O ₂o ₃angle θ scope [ψ ₁, ψ ₂] uniform discrete is n part equal intervals: ψ ₁=θ ₁< θ ₂< ... < θ _n< θ _n+1=ψ ₂, between adjacent two angles, difference (interval) is: Δ θ _i=(ψ ₂-ψ ₁)/n, if θ ₁, θ ₂..., θ _n, θ _n+1the moment of the corresponding Hookean spring in place is: T _s1(θ ₁), T _s2(θ ₂) ..., T _sn(θ _n), T _sn+1(θ _n+1), gravity torque corresponding to connecting rod is: T ₁(θ ₁), T ₂(θ ₂) ..., T _n(θ _n), T _n+1(θ _n+1).For the fitness letter of calculating formula (11) particle optimized algorithm.

(2) X is established ⁱ(k)=(u ⁱ(k), LLmit ⁱ(k)) ^tthe position of each particle i in population kth time iteration, first by X ⁱk () substitutes in formula (9), i.e. K=K (u, x ₀, θ) and in=B/r φ (a-utan θ), wherein, the drift of spring is passed through try to achieve.For ψ ₁=θ ₁< θ ₂< ... < θ _n< θ _n+1=ψ ₂in each angle value, corresponding K can be calculated ₁, K ₂..., K _n, K _n+1value.Again by K ₁~ K _n+1get arithmetic mean after summation to be worth this value represents the stiffness coefficient of the spring in population kth time iteration corresponding to particle i, and will substitute into fitness function g (x ₀, K, u| θ) in solve, obtain corresponding functional value.Certainly, in order to adapt to the discrete programming of particle algorithm, usually calculate AVEE again by after (15) formula discretize accepted way of doing sth (11) form.

(3) in kth time iteration, N number of particles all in population is substituted in fitness function formula (11) respectively according to (2) step and solves average torque error AVEE, then find fitness values all in all populations in whole k time (the 1st time to kth time) iteration, and find global optimum's fitness value to be: P ^g(k)=(u ^g(k), LLmit ^g(k)) ^t.Wherein, u ^g(k) and LLmit ^gk () represents the global optimum spring connecting point of whole N number of particle in whole k iteration in population respectively, global optimum's balance stem quality, global optimum's " stretching drift ratio ".

(4) X ⁱ(k+1)=c ₁r ₁x ⁱ(k)+c ₂r ₂[P ^g(k)-X ⁱ(k)]+Θ ξ/N as evolution iteration renewal equation, represent that i-th particle in all N number of particles upgrades in kth+1 iteration, wherein, c ₁be called cognitive learning rate, generally get constant value 1.49445, c ₂be called that social learning leads, also get constant value 1.49445, r ₁and r ₂the arbitrary constants of two scopes between 0 ~ 1.Θ ξ/N=0 is called skip over, wherein, Θ is an adjustable value, between the position maximal value of its span all particles in 0 to population, ξ is a stochastic variable, and its scope is certain the random site value in the band of position specified by all particles.N is the number of population; In the present invention, we choose Θ ξ/N=0;

(5) boundary condition agreement: if the population be made up of N number of particle, after the iterative computation that kth is secondary, the position X of i-th particle ⁱk certain the one dimension position range in () is beyond the interval of specifying: the minimum value u respectively of the minimum value region of search scope in this interval _minand LLimit _min, maximal value is respectively u _maxand LLimit _max.So position X of i-th particle ⁱk certain one dimension of () has exceeded minimum edge boundary treaty fixed condition, must redesignated as u _minor LLimit _min.If, the position X of i-th particle ⁱk certain one dimension of () has exceeded maximum boundary agreed terms, then this dimension location variable must redesignated as u _maxand LLimit _max.

(6) if evolution number of iterations reaches maximum iteration time M, then finishing iteration, finds the spring connecting point position u of the global optimum after all evolution iteration _optwith optimum " stretching drift ratio " LLimit _opt.Otherwise order performs after continuing to get back to (2) step;

(7) final, according to the model T of large arm gravity compensation _s(u, x ₀, θ, K), the restriction relation that variable u and θ and spring elongation length x (u, θ) is formed, the final optimum spring connecting point position u obtaining spring _opt, drift x _0optwith Optimal Stiffness COEFFICIENT K _opt.

Above-mentionedly give large arm mechanism (connecting rod O ₂o ₃) based on the optimum spring gravity compensation process of simple single-particle optimizing algorithm improved.For little arm mechanism (connecting rod O ₃o ₄) optimum spring and said process similar, detailed process is described below:

Fig. 4 is that forearm gravity compensation spring 10 carries out the principle schematic of simple spring gravity compensation to little arm mechanism 7, large arm mechanism 1 connecting rod O ₂o ₃describe, the center of gravity due to universal joint 17 concentrates on the end of little arm mechanism 7, therefore, and little arm mechanism 7 and universal joint 17 connecting rod O ₃o ₄be described.Forearm reduction driven wheel 8 uses semicircle O ₂r _xq _xbe described, although forearm reduction driven wheel 8 takes turns 2 all around O with large arm reduction driven ₂rotate, but each autokinesis is non-interference.O is used in forearm rotating shaft 12 ₃describe, little arm mechanism 7 is around the rotational angle angle θ of forearm rotating shaft 12 _xdescribe, forearm gravity compensation spring 10 tensile elongation Q _xp _xdescribe, one end is connected to certain 1 P on direct current generator 11 fixed support _xon, the other end is connected to the Q of forearm reduction driven wheel 8 _xpoint, O ₂q _xwith O ₃o ₄parallel, and with forearm reducing gear engaged wheel O ₂r _xq _xannulus external diameter intersects at Q _xpoint, the drift of forearm gravity compensation spring 10 is x _x0, this spring elongation length Q _xp _xequal x _x, assuming that the stiffness coefficient of this spring is K _x, this spring elongation masterpiece is used in O ₂the arm of force O at place ₂e _xlength is h _x, when this spring tension is F _xs, moment is T _xs=F _xsh _x.Connecting rod O ₃o ₄corner ∠ O ₄o ₃l _x=θ _x, connecting rod O ₃o ₄gravity torque be T _x=B _xcos θ _x.The radius O of forearm reducing gear engaged wheel 8 ₂q _xlength is r.P _xo _x0it is parallel that residing plane and base reduction driven take turns plane residing for 13, from O ₂place makes vertical line and P _xo _x0place plane orthogonal intersects, and intersection point is positioned at O _x0, wherein O ₂o _x0length is a, P _xo _x0length is u _x.From Q _xpoint makes vertical line and P _xo _x0place plane orthogonal intersects, and intersection point is positioned at F _x, ∠ E _xo ₂n _x=∠ F _xq _xp _x=θ _x1, ∠ Q _xo ₂n _x=θ _x.At right-angle triangle Δ O ₂q _xg _xin, Q _xg _xequal rcos θ _x, O ₂g _xlength equals rsin θ _x.Then Q _xf _xlength equals G _xo _x0length, equals O ₂o _x0subtract O ₂g _x, for: a-rsin θ _x.P _xf _xlength equals F _xo ₀subtract P _xo _x0, for: rcos θ _x-u _x.Like this, at right angle Δ Q _xf _xp _xin, spring elongation length Q _xp _xuse x _x(u _x, θ _x) represent, according to Pythagorean theorem, itself and variable u can be obtained _xand θ _xrestriction relation be:

$x_x(u_x,{\mathrm{\&theta;}}_x)=\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}---\left(16\right)$

Like this, the tensile force of spring is:

$F_{xs}=K_x(x_x-x_{x0})=K_x\&lsqb;\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}-x_{x0}\&rsqb;---\left(17\right)$

Again because P _xq _xextended line and O ₂n _xintersect at N _xpoint, and O ₂n _xbe parallel to O _x0f _x, ∠ O ₂n _xe _xequal ∠ Q _xp _xf _x, all equal pi/2, so ∠ E _xo ₂n _x=∠ F _xq _xp _x=θ _x1.So the effect of spring elongation power is to O ₂the arm of force O at place ₂e _xlength equals:

h _x＝rcos(θ _x-θ _x1)(18)

So it is F that forearm spring gravity compensation model produces corresponding moment _xsh _x:

$T_{xs}(u_x,x_{x0},{\mathrm{\&theta;}}_x,K_x)=F_{xs}{h}_x=K_x\&lsqb;\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}-x_{x0}\&rsqb;rcos({\mathrm{\&theta;}}_x-{\mathrm{\&theta;}}_{x1})---\left(19\right)$

Wherein, at Δ Q _xf _xp _xin, known Q _xf _xlength be a-rsin θ _x, P _xf _xfor rcos θ _x-u _x, be Q according to spring elongation length known in formula (16) again _xp _x, can obtain:

${\mathrm{sin\&theta;}}_{x1}=\frac{r{\mathrm{cos\&theta;}}_x-u_x}{\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}}---\left(20\right)$

${\mathrm{cos\&theta;}}_{x1}=\frac{a-r{\mathrm{sin\&theta;}}_x}{\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}}---\left(21\right)$

In order to enable spring elongation moment full remuneration little arm mechanism gravity torque T _x=B _xcos θ _x, then the compensation gravitational equilibrium moment that spring produces is:

$T_x=B_x{\mathrm{cos\&theta;}}_x=F_{xs}{h}_x=K_x\&lsqb;\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}-x_{x0}\&rsqb;rcos({\mathrm{\&theta;}}_x-{\mathrm{\&theta;}}_{x1})---\left(22\right)$

Further, can show that the stiffness coefficient of spring is:

$K_x=K_x(u_x,x_{x0},{\mathrm{\&theta;}}_x)=\frac{B_x{\mathrm{cos\&theta;}}_x}{r\&lsqb;\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}-x_{x0}\&rsqb;cos({\mathrm{\&theta;}}_x-{\mathrm{\&theta;}}_{x1})}---\left(23\right)$

Due to K _xvalue and u _x, x _x0and θ _xconstitute restriction relation, therefore also can be write K as K (u, x ₀, θ).By cos (θ _x-θ _x1) expand into cos θ _xcos θ _x1+ sin θ _xsin θ _x1, and formula (20) and formula (21) are substituted in expansion, then be updated to formula (23) abbreviation, draw:

$K_x=\frac{B_x}{{\mathrm{r\&phi;}}_x(a-u_x{\mathrm{tan\&theta;}}_x)}---\left(24\right)$

Wherein,

${\mathrm{\&phi;}}_x=\frac{\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}-x_{x0}}{\sqrt{{(r{\mathrm{cos\&theta;}}_x-u_x)}^2+{(a-r{\mathrm{sin\&theta;}}_x)}^2}}---\left(25\right)$

Known according to formula (24), forearm spring rate K _xbe one along with little arm mechanism (connecting rod O ₃o ₄) corner ∠ O ₄o ₃l _x=θ _xthe function of change, wherein: introduce parameter u _xand x _x0be used for respectively describing the tie point present position of spring and the drift of spring.Gravity compensation is carried out, according to parameter u owing to adopting Hookean spring _xand x _x0change, fully examination when the tie point of Hookean spring be in diverse location and spring adopt different drifts time, the gravity compensation effect of Hookean spring.Finally, by selecting suitable position u _xwith suitable Hookean spring drift x _x0, find the Optimal Stiffness coefficient of Hookean spring.By using the Hookean spring of this Optimal Stiffness coefficient, make this Hookean spring fully can compensate little arm mechanism (connecting rod O ₃o ₄) gravity.

If little arm mechanism (connecting rod O ₃o ₄) rotational angle θ _xscope is [ψ _x1, ψ _x2], by [ψ _x1, ψ _x2] interval be evenly divided into n equal portions, ψ _x1=θ _x1< θ _x2< ... < θ _xn< θ _{x (n+1)}=ψ _x2, between adjacent two angles, difference is: Δ θ _xi=(ψ _x1-ψ _x2)/n, if θ _x1, θ _x2..., θ _xn, θ _{x (n+1)}the moment of the corresponding Hookean spring in place is: T _xs1(θ _x1), T _xs2(θ _x2) ..., T _xsn(x θ _xn), T _{xs (n+1)}(θ _{x (n+1)}), the required corresponding gravity torque compensated of little arm mechanism 7 is: T _x1(θ _x1), T _x2(θ _x2) ..., T _xn(θ _xn), T _{x (n+1)}(θ _{x (n+1)}).Therefore, with aforementioned large arm mechanism (connecting rod O ₂o ₃) similar, in order to adapt to the discrete programming of particle algorithm, at [ψ _x1, ψ _x2] angular interval, forearm gravity compensation spring 10 compensates little arm mechanism (connecting rod O ₃o ₄) the moment average error AVEE of gravity _xcan approximate representation be:

${\mathrm{AVEE}}_x\&ap;\frac{1}{n+1}\underset{i=1}{\overset{n+1}{\&Sigma;}}|T_{xsi}\left({\mathrm{\&theta;}}_{xi}\right)-T_{xi}\left({\mathrm{\&theta;}}_{xi}\right)|---\left(26\right)$

By AVEE _xlittle arm mechanism (connecting rod O is compensated as forearm gravity compensation spring 10 ₃o ₄) gravity time, as the fitness function of simple single-particle optimizing algorithm, also referred to as optimization aim.The spring rate K of one group of optimum is found out by the simple single-particle optimizing algorithm improved _xopt, optimum spring connecting point u _xopt, and the free x of the spring of optimum _xopt, this group parameter makes Hookean spring compensate little arm mechanism (connecting rod O ₃o ₄) the average torque error AVEE of gravity _xminimum.

Be x by the spring free length of simple single-particle optimizing algorithm optimum choice improved _x0, spring rate K _xwith the tie point position u of spring _x, concrete steps are described below:

(1) first, Initialize installation is carried out: establish little arm mechanism 7 rotational angle θ _xscope is [ψ _x1, ψ _x2], assuming that be also described as by the population that N number of particle is formed in the simple single-particle optimizing algorithm improved: X=(X ¹, X ²..., X ⁿ), wherein, X represents population set, X ⁱ(i=1,2 ..., N) and represent the position of i-th particle in this population; In this population, the flight position search volume of each particle is 2 gt, and every 1 dimension in this 2 dimension location finding space is expressed as the tie point position u of spring _xlLimit is compared with stretching drift _x; If particle iteration total degree is M time, so, i-th particle is expressed as position in kth time iteration: wherein, respectively spring connecting point position in kth time iteration of i-th particle and stretching drift ratio, these two parameters are random numbers, and its region of search stated range minimum is respectively u _xminand LLimit _xmin, maximal value is respectively u _xmaxand LLimit _xmax; By angle θ _xscope [ψ _x1, ψ _x2] uniform discrete is n part equal intervals: ψ _x1=θ _x1< θ _x2< ... < θ _xn< θ _{x (n+1)}=ψ _x2, between adjacent two angles, difference is: Δ θ _xi=(ψ _x1-ψ _x2)/n, if θ _x1, θ _x2..., θ _xn, θ _{x (n+1)}the moment of the corresponding Hookean spring in place is: T _xs1(θ _x1), T _xs2(θ _x2) ..., T _xsn(x θ _xn), T _{xs (n+1)}(θ _{x (n+1)}), the required corresponding gravity torque compensated of little arm mechanism 7 is: T _x1(θ _x1), T _x2(θ _x2) ..., T _xn(θ _xn), T _{x (n+1)}(θ _{x (n+1)}), they are all substituted into the fitness function value calculating particle optimized algorithm in formula (26);

(2) establish the position of each particle i in population kth time iteration, first by X ⁱk () substitutes in formula (24), for ψ _x1=θ _x1< θ _x2< ... < θ _xn< θ _{x (n+1)}=ψ _x2in each angle value, calculate corresponding K _x1, K _x2..., K _xn, K _{x (n+1)}value; Again by K _x1~ K _{x (n+1)}get arithmetic mean after summation to be worth this value represents the stiffness coefficient of the spring in population kth time iteration corresponding to particle i, and will substitution formula solves fitness function value in (26);

(3) in kth time iteration, N number of particles all in population is substituted in fitness function formula (26) respectively according to step (2) and solves average torque error AVEE _x, then find fitness values all in all populations in whole k time (the 1st time to kth time) iteration, and find global optimum's fitness value to be: $P^g\left(k\right)={\left(u_x^g\right(k),{\mathrm{LLimt}}_x^g(k\left)\right)}^T,$ Wherein, with represent global optimum's spring connecting point in whole k iteration of whole N number of particle in population and global optimum's stretching drift ratio respectively;

(4) X ⁱ(k+1)=c ₁r ₁x ⁱ(k)+c ₂r ₂[P ^g(k)-X ⁱ(k)]+Θ ξ/N as evolution iteration renewal equation, represent that i-th particle in all N number of particles upgrades in kth+1 iteration, wherein, c ₁be called cognitive learning rate, generally get constant value 1.49445, c ₂be called that social learning leads, also get constant value 1.49445, r ₁and r ₂the arbitrary constants of two scopes between 0 ~ 1.Θ ξ/N=0 is called skip over, wherein, Θ is an adjustable value, between the position maximal value of its span all particles in 0 to population, ξ is a stochastic variable, and its scope is certain the random site value in the band of position specified by all particles.N is the number of population; In the present invention, we choose Θ ξ/N=0;

(5) boundary condition agreement: if the population be made up of N number of particle, after the iterative computation that kth is secondary, the position X of i-th particle ⁱk certain the one dimension position range in () is beyond the interval of specifying: the minimum value u respectively of the minimum value region of search scope in this interval _xminand LLimit _xmin, maximal value is respectively u _xmaxand LLimit _xmax.So position X of i-th particle ⁱk certain one dimension of () has exceeded minimum edge boundary treaty fixed condition, must redesignated as u _xminor LLimit _xmin.If, the position X of i-th particle ⁱk certain one dimension of () has exceeded maximum boundary agreed terms, then this dimension location variable must redesignated as u _xmaxand LLimit _xmax;

(6) if evolution number of iterations reaches maximum iteration time M, then finishing iteration, finds the spring connecting point position u of the global optimum after all evolution iteration _xoptlLimit is compared with optimum stretching drift _xopt; Otherwise, continue to get back to step (2) order execution afterwards;

(7) according to forearm gravity compensation model T _xs(u _x, x _x0, θ _x, K _x), variable u _xand θ _xwith spring elongation length x _x(u _x, θ _x) restriction relation that formed, the final optimum spring connecting point position u obtaining spring _xopt, drift x _xoptwith Optimal Stiffness COEFFICIENT K _xopt.

Feature of the present invention is that the simple single-particle optimizing algorithm iteration optimization by improving calculates the one group of optimum spring connecting point position finding optimum, optimum spring free length, optimum spring rate, thus complete the design of the simple spring gravity compensation optimizing of force feedback equipment large arm and forearm.

Claims (1)

1. the optimum spring gravity compensation method of force feedback equipment, is characterized in that:

Step one, large arm gravity compensation spring (4) one end are connected to large arm reduction driven wheel (2) end, the other end is connected on direct current generator (5) fixed support, for compensating large arm mechanism (1) gravity;

Step 2, set up the model T of the large arm gravity compensation of non-linear constrain relation _s(u, x ₀, θ, K), wherein, u is the tie point position that large arm gravity compensation spring (4) is connected on direct current generator (5) fixed support, x ₀for the drift of large arm gravity compensation spring (4), θ be large arm mechanism (1) rotate time and horizontal plane angle, K is the stiffness coefficient of large arm gravity compensation spring (4), due to value and u, x of K ₀constitute restriction relation with θ, therefore K also can be write as K (u, x ₀, θ); In addition, the restriction relation that exists of spring elongation length x (u, θ) and u and θ;

Step 3, for large arm gravity compensation spring (4), introduce stretching drift than LLimit, using tie point position u and LLimt as optimized amount, average torque error AVEE is as the fitness function of optimization; Wherein, LLimit is large arm gravity compensation spring (4) tensile elongation x (u, θ) and spring free length x ₀ratio;

The simple single-particle optimizing algorithm that step 4, use are improved carries out iteration optimization, obtains optimum large arm gravity compensation spring (4) tie point position u _opt, spring optimum drift x _optwith Optimal Stiffness COEFFICIENT K _opt, realize the simple spring gravity Compensation Design of optimum of large arm mechanism (1), concrete steps are as follows:

A () first, carries out Initialize installation: set large arm mechanism (1) rotational angle θ scope as [ψ ₁, ψ ₂], assuming that the population that the N number of particle in the simple single-particle optimizing algorithm improved is formed is described as: X=(X ¹, X ²..., X ⁿ), wherein, X represents population set, X ⁱ(i=1,2 ..., N) and represent the position of i-th particle in this population; In this population, the flight position search volume of each particle is 2 gt, and every 1 dimension in this 2 dimension location finding space is expressed as the tie point position u of spring and stretching drift compares LLimt; If particle iteration total degree is M time, so, i-th particle is expressed as position in kth time iteration: X ⁱ(k)=(u ⁱ(k), LLimt ⁱ(k)) ^t, wherein, u ⁱ(k), LLimt ⁱk () be spring connecting point position in kth time iteration of i-th particle and stretching drift ratio respectively, these two parameters are random numbers, and its region of search stated range minimum is respectively u _minand LLimit _min, maximal value is respectively u _maxand LLimit _max; By angle θ scope [ψ ₁, ψ ₂] uniform discrete is n part equal intervals: ψ ₁=θ ₁< θ ₂< ... < θ _n< θ _n+1=ψ ₂, between adjacent two angles, difference is: Δ θ _i=(ψ ₂-ψ ₁)/n, if θ ₁, θ ₂..., θ _n, θ _n+1the moment of the corresponding Hookean spring in place is: T _s1(θ ₁), T _s2(θ ₂) ..., T _sn(θ _n), T _sn+1(θ _n+1), the required corresponding gravity torque compensated of large arm mechanism (1) is: T ₁(θ ₁), T ₂(θ ₂) ..., T _n(θ _n), T _n+1(θ _n+1), they are all substituted into the fitness function value of middle calculating particle optimized algorithm;

B () establishes X ⁱ(k)=(u ⁱ(k), LLmit ⁱ(k)) ^tthe position of each particle i in population kth time iteration, first by X ⁱk () substitutes into stiffness coefficient K (u, the x of large arm gravity compensation spring (4) ₀, θ) in, for ψ ₁=θ ₁< θ ₂< ... < θ _n< θ _n+1=ψ ₂in each angle value, calculate corresponding K ₁, K ₂..., K _n, K _n+1value; Again by K ₁~ K _n+1get arithmetic mean after summation to be worth this value represents the stiffness coefficient of the spring in population kth time iteration corresponding to particle i, and will substitute into in solve fitness function value;

In (c) kth time iteration, N number of particles all in population is substituted into fitness function formula respectively according to step (b) in solve average torque error AVEE, then find fitness values all in all populations in whole k time (the 1st time to kth time) iteration, and find global optimum's fitness value to be P ^g(k), wherein P ^g(k)=(u ^g(k), LLmit ^g(k)) ^t, u ^g(k) and LLmit ^gk () represents global optimum's spring connecting point in population in whole k the iteration of whole N number of particle and global optimum's stretching drift ratio respectively;

(d) X ⁱ(k+1)=c ₁r ₁x ⁱ(k)+c ₂r ₂[P ^g(k)-X ⁱ(k)]+Θ ξ/N as evolution iteration renewal equation, represent that i-th particle in all N number of particles upgrades in kth+1 iteration, wherein, c ₁be called cognitive learning rate, generally get constant value 1.49445, c ₂be called that social learning leads, also get constant value 1.49445, r ₁and r ₂the arbitrary constants of two scopes between 0 ~ 1; Θ ξ/N=0 is called skip over, wherein, Θ is an adjustable value, between the position maximal value of its span all particles in 0 to population, ξ is a stochastic variable, and its scope is certain the random site value in the band of position specified by all particles; N is the number of population; In the present invention, we choose Θ ξ/N=0;

E () boundary condition is arranged: if the population be made up of N number of particle, after the iterative computation that kth is secondary, and the position X of i-th particle ⁱk certain the one dimension position range in () is beyond the interval of specifying: the minimum value u respectively of the minimum value region of search scope in this interval _minand LLimit _min, maximal value is respectively u _maxand LLimit _max; So position X of i-th particle ⁱk certain one dimension of () has exceeded minimum edge boundary treaty fixed condition, must redesignated as u _minor LLimit _min; If, the position X of i-th particle ⁱk certain one dimension of () has exceeded maximum boundary agreed terms, then this dimension location variable must redesignated as u _maxand LLimit _max;

If f () evolution number of iterations reaches maximum iteration time M, then finishing iteration, find the spring connecting point position u of the global optimum after all evolution iteration _optlLimit is compared with optimum stretching drift _opt; Otherwise, continue to get back to step (b) order execution afterwards;

G () is according to large arm gravity compensation model T _s(u, x ₀, θ, K), the restriction relation that variable u and θ and spring elongation length x (u, θ) is formed, the final optimum spring connecting point position u obtaining spring _opt, drift x _optwith Optimal Stiffness COEFFICIENT K _opt;

Step 5, forearm gravity compensation spring (10) one end are connected to forearm reduction driven wheel (8) end, the other end is connected on direct current generator (11) fixed support, for compensating little arm mechanism (7) gravity, wherein, universal joint (17) is arranged on little arm mechanism (7) end, the center of gravity of universal joint (17) concentrates on the end of little arm mechanism (7), and forearm gravity compensation spring (10) compensates little arm mechanism (7) gravity and comprises gravity suffered by gravity suffered by universal joint and forearm self;

Step 6, set up the model T of the forearm gravity compensation of non-linear constrain relation _xs(u _x, x _x0, θ _x, K _x), wherein, u _xfor forearm gravity compensation spring (10) is connected to the tie point position on direct current generator (11) fixed support, x _x0for the drift of forearm gravity compensation spring (10), θ _xfor when little arm mechanism (7) rotates and horizontal plane angle, K _xfor the stiffness coefficient of forearm gravity compensation spring (10), due to K _xvalue and u _x, x _x0and θ _xconstitute restriction relation, therefore K _xalso K can be write as _x(u _x, x _x0, θ _x); In addition, spring elongation length x _x(u _x, θ _x) and u _xand θ _xthe restriction relation existed;

Step 7, for forearm gravity compensation spring (10), also introduce stretching drift and compare LLimit _x, by tie point position u _xwith LLimit _xas optimized amount, average torque error AVEE _xas the fitness function optimized; Wherein, xLLimit is forearm gravity compensation spring (10) tensile elongation x _x(u _x, θ _x) and spring free length x _x0ratio;

The simple single-particle optimizing algorithm that step 8, use are improved carries out iteration optimization, obtains optimum forearm gravity compensation spring (10) tie point position u _xopt, spring optimum drift x _xoptwith Optimal Stiffness COEFFICIENT K _xopt, realize the simple spring gravity Compensation Design of optimum of little arm mechanism (7), concrete steps are as follows:

(aa) first, Initialize installation is carried out: establish little arm mechanism (7) rotational angle θ _xscope is [ψ _x1, ψ _x2], assuming that be also described as by the population that N number of particle is formed in the simple single-particle optimizing algorithm improved: X=(X ¹, X ²..., X ⁿ), wherein, X represents population set, X ⁱ(i=1,2 ..., N) and represent the position of i-th particle in this population; In this population, the flight position search volume of each particle is 2 gt, and every 1 dimension in this 2 dimension location finding space is expressed as the tie point position u of spring _xlLimit is compared with stretching drift _x; If particle iteration total degree is M time, so, i-th particle is expressed as position in kth time iteration: wherein, respectively spring connecting point position in kth time iteration of i-th particle and stretching drift ratio, these two parameters are random numbers, and its region of search stated range minimum is respectively u _xminand LLimit _xmin, maximal value is respectively u _xmaxand LLimit _xmax; By angle θ _xscope [ψ _x1, ψ _x2] uniform discrete is n part equal intervals: ψ _x1=θ _x1< θ _x2< ... < θ _xn< θ _{x (n+1)}=ψ _x2, between adjacent two angles, difference is: Δ θ _xi=(ψ _x1-ψ _x2)/n, if θ _x1, θ _x2..., θ _xn, θ _{x (n+1}) moment of the corresponding Hookean spring in place is: T _xs1(θ _x1), T _xs2(θ _x2) ..., T _xsn(x θ _xn), T _{xs (n+1)}(θ _{x (n+1)}), the required corresponding gravity torque compensated of little arm mechanism (7) is: T _x1(θ _x1), T _x2(θ _x2) ..., T _xn(θ _xn), T _{x (n+1)}(θ _{x (n+1)}), they are all substituted into the fitness function value of middle calculating particle optimized algorithm;

(bb) establish the position of each particle i in population kth time iteration, first by X ⁱk () substitutes into the stiffness coefficient K of forearm gravity compensation spring (10) _x(u _x, x _x0, θ _x) in, for ψ _x1=θ _x1< θ _x2< ... < θ _xn< θ _{x (n+1)}=ψ _x2in each angle value, calculate corresponding K _x1, K _x2..., K _xn, K _{x (n+1)}value; Again by K _x1~ K _{x (n+1)}get arithmetic mean after summation to be worth this value represents the stiffness coefficient of the spring in population kth time iteration corresponding to particle i, and will substitute into in solve fitness function value;

(cc) in kth time iteration, N number of particles all in population is substituted into fitness function formula respectively according to step (bb) in solve average torque error AVEE _x, then find fitness values all in all populations in whole k time (the 1st time to kth time) iteration, and find global optimum's fitness value to be P ^g(k), wherein $P^g\left(k\right)={\left(u_x^g\right(k),{\mathrm{LLimt}}_x^g(k\left)\right)}^T,$ Wherein, with represent global optimum's spring connecting point in whole k iteration of whole N number of particle in population and global optimum's stretching drift ratio respectively;

(dd) X ⁱ(k+1)=c ₁r ₁x ⁱ(k)+c ₂r ₂[P ^g(k)-X ⁱ(k)]+Θ ξ/N as evolution iteration renewal equation, represent that i-th particle in all N number of particles upgrades in kth+1 iteration, wherein, c ₁be called cognitive learning rate, generally get constant value 1.49445, c ₂be called that social learning leads, also get constant value 1.49445, r ₁and r ₂the arbitrary constants of two scopes between 0 ~ 1; Θ ξ/N=0 is called skip over, wherein, Θ is an adjustable value, between the position maximal value of its span all particles in 0 to population, ξ is a stochastic variable, and its scope is certain the random site value in the band of position specified by all particles; N is the number of population; In the present invention, we choose Θ ξ/N=0;

(ee) boundary condition agreement: if the population be made up of N number of particle, after the iterative computation that kth is secondary, the position X of i-th particle ⁱk certain the one dimension position range in () is beyond the interval of specifying: the minimum value u respectively of the minimum value region of search scope in this interval _xminand LLimit _xmin, maximal value is respectively u _xmaxand LLimit _xmax; So position X of i-th particle ⁱk certain one dimension of () has exceeded minimum edge boundary treaty fixed condition, must redesignated as u _xminor LLimit _xmin; If, the position X of i-th particle ⁱk certain one dimension of () has exceeded maximum boundary agreed terms, then this dimension location variable must redesignated as u _xmaxand LLimit _xmax;

(ff) if evolution number of iterations reaches maximum iteration time M, then finishing iteration, finds the spring connecting point position u of the global optimum after all evolution iteration _xoptlLimit is compared with optimum stretching drift _xopt; Otherwise, continue to get back to step (bb) order execution afterwards;

(gg) according to forearm gravity compensation model T _xs(u _x, x _x0, θ _x, K _x), variable u _xand θ _xwith spring elongation length x _x(u _x, θ _x) restriction relation that formed, the final optimum spring connecting point position u obtaining spring _xopt, drift x _xoptwith Optimal Stiffness COEFFICIENT K _xopt.