CN104626101B - Robot three-dimensional space gravity compensating chain device and method - Google Patents

Abstract

Robot three-dimensional space gravity compensating chain device and method, to solve the existing baroque problem of three dimensions compensation device。One end of first gimbal lever is connected with the first cradle head, the other end of first gimbal lever is connected with union joint, one end of second gimbal lever is hinged with union joint, the other end is hinged with fixed cover, one end of big armed lever is connected through fixed cover and elbow joint are fixing, the other end and the first shoulder joint connect, one end of little armed lever is hinged with elbow joint, first pulley and the first spring are each attached in first gimbal lever, second pulley and the second spring are each attached on little armed lever, 3rd shoulder joint and the 3rd cradle head are all fixing with fixed mount to be connected, one end of gimbal lever's steel wire rope is fixing with fixed mount to be connected, the other end walks around the first pulley and the fixing connection of the first spring, one end of forearm steel wire rope is fixing with second gimbal lever to be connected, the other end walks around the second pulley and the fixing connection of the second spring。The present invention is used for robot gravitational equilibrium。

Description

Device and method for three-dimensional gravity balance compensation of robot

technical field

The invention relates to a robot gravity compensation device and method, in particular to a robot three-dimensional space gravity balance compensation device and method.

Background technique

Gravity compensation devices are widely used in industrial medical and other fields. For example, in the manipulator of industrial robots, it is often necessary to balance its own gravity to achieve more precise control goals; in fields such as medical rehabilitation, gravity balance can bring rehabilitation patients more In good news, the gravity balance device can reduce or even eliminate the electric drive link, which increases the reliability of the mechanism and greatly guarantees the safety of use. Most of the existing gravity compensation devices are limited to two-dimensional space, and the structure of the existing three-dimensional space compensation device is realized by using a differential mechanism or adding a mass block. This compensation method is complicated or cumbersome and loses its practicability.

Contents of the invention

In order to solve the problem of complicated structure of the existing three-dimensional space compensation device, the present invention proposes a robot three-dimensional space gravity balance compensation device and method.

Device: The robot three-dimensional space gravity compensation balance device of the present invention includes a first balance bar, a second balance bar, a fixed sleeve, a boom bar, a small arm bar, an elbow joint, a first shoulder joint, a second shoulder joint, and a third shoulder joint. Joint, connecting head, first revolving joint, second revolving joint, third revolving joint, first pulley, first spring, second pulley, second spring, balance bar wire rope, forearm wire rope and fixed frame, first balance One end of the rod is fixedly connected to the first revolving joint, the first revolving joint is articulated to the second revolving joint, the second revolving joint is articulated to the third revolving joint, the other end of the first balance rod is fixedly connected to the connector, and the second balance rod One end of the second balance bar is hinged with the connecting head, the other end of the second balance bar is hinged with the fixed sleeve, one end of the boom rod passes through the fixed sleeve and is fixedly connected with the elbow joint, the fixed sleeve rotates around the boom rod, and the other end of the boom rod is connected to the first One shoulder joint is connected by a bearing, the first shoulder joint is hinged to the second shoulder joint, the second shoulder joint is hinged to the third shoulder joint, one end of the forearm is hinged to the elbow joint, the first pulley and the first spring are fixed on the On a balance bar, the first pulley is located on one side of the first rotating joint, the first spring is located on one side of the connecting head, the second pulley and the second spring are fixed on the forearm rod, and the second pulley is located on the elbow joint The second spring is located on the outside of the forearm rod, the third shoulder joint and the third rotation joint are fixedly connected with the fixed frame, one end of the balance bar wire rope is fixedly connected with the fixed frame, and the other end of the balance bar wire rope goes around the first A pulley is fixedly connected with the first spring, one end of the forearm wire rope is fixedly connected with the second balance bar, and the other end of the forearm wire rope is fixedly connected with the second spring around the second pulley.

Method 1: The method described is a method for realizing plane gravity balance compensation, and the steps are as follows:

Step 1: Calculate the gravitational potential energy W ₁ of the boom:

Formula 1: W ₁ =m ₁ g(l′ ₁ c ₁ +h)

Among them, m ₁ is the mass of the boom, g is the acceleration of gravity, l ₁ ′ is the length from the center of mass of the boom to the hinge point of the first shoulder joint and the second shoulder joint, c ₁ is cosθ ₁ , θ ₁ is the acute angle between the outer big arm of the parallelogram and the fixed frame, and h is the distance between the hinge point of the first shoulder joint and the hinge point of the first rotational joint;

Step 2: Calculate the gravitational potential energy W ₂ of the small arm:

Formula 2: W ₂ =m ₂ g(l ₁ c ₁ +l′ ₂ c ₁₊₂ +h)=m ₂ g(l ₁ c ₁ +l′ ₂ c ₁ c ₂ -l′ ₂ s ₁ s ₂ +h)

Among them, m ₂ is the mass of the forearm, l ₁ is the length of the main arm, l′ ₂ is the length from the center of mass of the forearm to the hinge point of the elbow joint, c ₁₊₂ is cos(θ ₁ +θ ₂ ), c ₂ is cosθ ₂ , s ₁ is sinθ ₁ , s ₂ is sinθ ₂ , θ ₂ is the acute angle between the outer forearm of the forearm and the boom;

Step 3: Calculate the gravitational potential energy W ₃ of the first balance pole:

Formula 3: W ₃ =m ₃ gl′ ₃ c ₁

Wherein, m ₃ is the quality of the first balance pole, and l' ₃ is the length from the mass center point of the first balance pole to the hinge point of the first rotating joint;

Step 4: Calculate the gravitational potential energy W ₄ of the second balance pole:

Formula 4: W ₄ ＝m ₄ g(l ₁ c ₁ +l′ ₄ )

Wherein, m ₄ is the quality of the second balance pole, and l' ₄ is the length from the mass center point of the second balance pole to the hinge point of the connector;

Step 5: Calculate the total potential energy V _g of the boom, the forearm, the first balance bar and the second balance bar:

Formula 5: V _g =W ₁ +W ₂ +W ₃ +W ₄ =

m ₁ gh+m ₂ gh+m ₄ gl′ ₄ +(m ₁ gl′ ₁ +m ₂ gl ₁ +m ₃ gl′ ₃ +m ₄ gl ₁ )c ₁ +m ₂ gl′ ₂ c ₁ c ₂ - m ₂ gl′ ₂ s ₁ s ₂

Step 6: Calculate the elongation x ₁ of the first spring:

Formula six: $x_1^2=h^2+d_1^2+2{\mathrm{hd}}_1{c}_1$

Wherein, x ₁ ² is the square of the elongation of the first spring, d ₁ is the length of the steel wire rope of the balance bar, and d ₁ ² is the square of the length of the steel wire rope of the balance bar;

Step 7: Calculate the elongation x ₂ of the second spring:

Formula seven: $x_2^2=h^2+d_2^2+2{\mathrm{hd}}_2{c}_{1+2}$

Wherein, x ₂ ² is the square of the elongation of the second spring, d ₂ is the length of the forearm steel wire rope, and d ₂ ² is the square of the length of the forearm steel wire rope;

Step 8: Calculate the elastic potential energy sum V _s of the first spring and the second spring:

Formula eight: $\begin{array}{c}{V}_{\mathrm{the\; s}}=\frac{1}{2}{k}_1{x}_1^2+\frac{1}{2}{k}_2{x}_2^2=\\ \frac{1}{2}{k}_1(h^2+d_1^2)+\frac{1}{2}{k}_2(h^2+d_2^2)+k_1{\mathrm{hd}}_1{c}_1+k_2{\mathrm{hd}}_2{c}_1{c}_2-k_2{\mathrm{hd}}_2{\mathrm{the\; s}}_1{\mathrm{the\; s}}_2\end{array}$

Wherein, k ₁ is the stiffness coefficient of the first spring, and k ₂ is the stiffness coefficient of the second spring;

Step 9: In order to make V _s +V _g = constant, eliminate the coefficient for formula 8. Since the settlement result of gravitational potential energy is a negative value, there is:

Formula 9: m ₁ gl′ ₁ +m ₂ gl ₁ +m ₃ gl′ ₃ +m ₄ gl ₁ ＝k ₁ hd ₁ The corresponding item coefficients of V _s and V _g cancel

Formula 10: m ₂ gl′ ₂ =k ₂ hd ₂ V _s and V _g corresponding item coefficients cancel

Step 10: When the system relationship satisfies the above formulas 9 and 10, the system reaches plane equilibrium;

Step 11: According to the actual mass m of the object, adjust the distance h between the hinge point of the first shoulder joint and the hinge point of the first rotation joint to realize balance compensation.

Method two: the method is to realize the space gravity balance compensation method, and its steps are as follows:

Step 1: Calculate the gravitational potential energy W ₁ of the boom:

Formula 1': W ₁ =m ₁ g(l' ₁ c ₁ +h)

Among them, m ₁ is the mass of the boom, g is the acceleration of gravity, l ₁ ′ is the length from the center of mass of the boom to the hinge point of the first shoulder joint and the second shoulder joint, c ₁ is cosθ ₁ , θ ₁ is the acute angle between the outer big arm of the parallelogram and the fixed frame, and h is the distance between the hinge point of the first shoulder joint and the hinge point of the first rotational joint;

Step 2: Calculate the gravitational potential energy W ₂ of the small arm:

Formula 2': W ₂ =m ₂ g(l ₁ c ₁ +l' ₂ c ₁ c ₂ -l' ₂ c ₀ s ₁ s ₂ +h)

Among them, m ₂ is the mass of the forearm, l ₁ is the length of the main arm, l′ ₂ is the length from the center of mass of the forearm to the hinge point of the elbow joint, c ₀ is cosθ ₀ , and c ₂ is cosθ ₂ , s ₁ is sinθ ₁ , s ₂ is sinθ ₂ , θ ₂ is the acute angle between the outer forearm of the forearm and the boom; θ ₀ is the axial rotation angle of the boom;

Step 3: Calculate the gravitational potential energy W ₃ of the first balance pole:

Formula 3': W ₃ =m ₃ gl' ₃ c ₁

Wherein, m ₃ is the quality of the first balance pole, and l' ₃ is the length from the mass center point of the first balance pole to the hinge point of the first rotating joint;

Step 4: Calculate the gravitational potential energy W ₄ of the second balance pole:

Formula 4': W ₄ =m ₄ g(l ₁ c ₁ +l' ₄ )

Wherein, m ₄ is the quality of the second balance pole, and l' ₄ is the length from the mass center point of the second balance pole to the hinge point of the connector;

Step 5: Calculate the total potential energy V _g of the boom, the forearm, the first balance bar and the second balance bar:

Formula 5': V _g =W ₁ +W ₂ +W ₃ +W ₄ =

m ₁ gh+m ₂ gh+m ₄ gl′ ₄ +(m ₁ gl′ ₁ +m ₂ gl ₁ +m ₃ gl′ ₃ +m ₄ gl ₁ )c ₁ +m ₂ gl′ ₂ c ₁ c ₂ - m ₂ gl′ ₂ c ₀ s ₁ s ₂

Step 6: Calculate the elongation x ₁ of the first spring:

Formula six': $x_1^2=h^2+d_1^2+2{\mathrm{hd}}_1{c}_1$

Wherein, x ₁ ² is the square of the elongation of the first spring, d ₁ is the length of the steel wire rope of the balance bar, and d ₁ ² is the square of the length of the steel wire rope of the balance bar;

Step 7: Calculate the elongation x ₂ of the second spring:

Formula seven': $x_2^2=h^2+d_2^2+2{\mathrm{hd}}_2(c_1{c}_2-c_0{\mathrm{the\; s}}_1{\mathrm{the\; s}}_2)$

Wherein, x ₂ ² is the square of the elongation of the second spring, d ₂ is the length of the forearm wire rope, and d ₂ ² is the square of the length of the forearm wire rope;

Step 8: Calculate the elastic potential energy sum V _s of the first spring and the second spring:

Formula Eight' $\begin{array}{c}{V}_{\mathrm{the\; s}}=\frac{1}{2}{k}_1{x}_1^2+\frac{1}{2}{k}_2{x}_2^2=\\ \frac{1}{2}{k}_1(h^2+d_1^2)+\frac{1}{2}{k}_2(h^2+d_2^2)+k_1{\mathrm{hd}}_1{c}_1+k_2{\mathrm{hd}}_2{c}_1{c}_2-k_2{\mathrm{hd}}_2{c}_0{\mathrm{the\; s}}_1{\mathrm{the\; s}}_2\end{array}$

Wherein, k ₁ is the stiffness coefficient of the first spring, and k ₂ is the stiffness coefficient of the second spring;

Step 9: In order to make V _s +V _g = constant, eliminate the coefficient for formula 8'. Since the settlement result of gravitational potential energy is a negative value, there is:

Formula 9': m ₁ gl' ₁ + m ₂ gl ₁ + m ₃ gl' ₃ + m ₄ gl ₁ = k ₁ hd ₁ V _s and V _g corresponding item coefficients cancel

Formula 10': m ₂ gl' ₂ =k ₂ hd ₂ V _s and V _g corresponding item coefficients cancel

Step 10: When the system relationship satisfies the above formula 9' and formula 10', the system reaches spatial balance;

Step eleven: According to the actual mass m of the object, adjust the distance h between the hinge point of the first shoulder joint and the hinge point of the first rotation joint to realize space compensation.

Compared with the prior art, the present invention has the following beneficial effects:

1. The device of the present invention is a structure imitating the degree of freedom of the arm. Since the fixed sleeve and the boom can rotate relatively, it will not be affected by the rotation of the boom, thereby ensuring that the parallelogram still exists in the three-dimensional space.

Two, the introduction of the parallelogram structure and the zero spring of the present invention makes the position analytical formula of each mass point and the analytical formula item type of spring elongation the same, promptly can offset, can obtain quality and spring position through calculation relation. The method of the invention effectively solves the problem of three-dimensional gravity balance. Simple structure, easy to process and use in mechanical design.

3. The application of the present invention in rehabilitation medicine can greatly increase the safety of the mechanism, reduce the complexity, and improve the practical performance of the equipment.

Description of drawings

Fig. 1 is the structural representation of the robot three-dimensional space gravity balance compensation device of the present invention;

Fig. 2 is the balance principle diagram utilizing the device of the present invention to realize the compensation method for robot plane gravity balance;

Fig. 3 is a balance schematic diagram of the method for realizing the balance compensation of robot space gravity by using the device of the present invention.

detailed description

Specific Embodiment 1: This embodiment is described with reference to FIG. 1. This embodiment includes a first balance bar 1, a second balance bar 2, a fixed sleeve 3, a boom 4, a small arm 5, an elbow joint 6, a first shoulder Joint 7, second shoulder joint 8, third shoulder joint 9, connecting head 10, first rotating joint 11, second rotating joint 12, third rotating joint 13, first pulley 14, first spring 15, second pulley 16. The second spring 17, the balance bar steel wire rope 18, the forearm steel wire rope 19 and the fixed frame 20, one end of the first balance bar 1 is fixedly connected with the first rotary joint 11, and the first rotary joint 11 is hinged with the second rotary joint 12, The first rotary joint 11 can rotate in the vertical direction, the second rotary joint 12 is hinged with the third rotary joint 13, the second rotary joint 12 can rotate in the horizontal direction, and the other end of the first balance pole 1 is fixedly connected with the connecting head 10 , one end of the second balance pole 2 is hinged with the connector 10, the second balance pole 2 can rotate in the vertical direction of the first balance pole 1, the other end of the second balance pole 2 is hinged with the fixed sleeve 3, the boom lever 4 One end passes through the fixed sleeve 3 and is fixedly connected with the elbow joint 6. The fixed sleeve 3 can rotate around the boom 4. The other end of the boom 4 is connected with the first shoulder joint 7 through a bearing. The boom 4 can be rotated in the axial direction. Autobiographical movement, the first shoulder joint 7 is hinged with the second shoulder joint 8, the second shoulder joint 8 is hinged with the third shoulder joint 9, one end of the forearm rod 5 is hinged with the elbow joint 6, the first pulley 14 and the first spring 15 Both are fixed on the first balance bar 1, and the first pulley 14 is located on one side of the first rotating joint 11, the first spring 15 is located on one side of the connector 10, and the second pulley 16 and the second spring 17 are fixed on a small On the arm bar 5, and the second pulley 16 is located on one side of the elbow joint 6, the second spring 17 is located on the outside of the forearm bar 5, the third shoulder joint 9 and the third rotating joint 13 are fixedly connected with the fixed frame 20, balanced One end of the rod wire rope 18 is fixedly connected with the fixed frame 20, the other end of the balance rod wire rope 18 is fixedly connected with the first spring 15 around the first pulley 14, and one end of the forearm wire rope 19 is fixedly connected with the second balance rod 2, and the forearm The other end of the wire rope 19 is fixedly connected with the second spring 17 around the second pulley 16 .

Embodiment 2: This embodiment is described in conjunction with FIG. 1 . The length of the boom 4 of this embodiment is the same as that of the first balance bar 1 , and the fixed frame 20 is located between the third shoulder joint 9 and the third rotation joint 13 . The distance between them is the same as the length of the second balance pole 2. Other components and connections are the same as those in the first embodiment.

Specific Embodiment 3: This embodiment is described with reference to FIG. 1 . The first balance pole 1 , the second balance pole 2 , the boom 4 and the fixing frame 20 in this embodiment form a parallelogram. Other compositions and connections are the same as those in Embodiment 1 or Embodiment 2.

Specific embodiment four: illustrate this embodiment in conjunction with Fig. 2, this embodiment is the method for realizing plane gravity balance compensation, and its steps are as follows:

Step 1: Calculate the gravitational potential energy W ₁ of the boom 4:

Formula 1: W ₁ =m ₁ g(l′ ₁ c ₁ +h)

Among them, m ₁ is the mass of the boom 4, g is the acceleration of gravity, l ₁ ′ is the length from the center of mass of the boom 4 to the hinge point of the first shoulder joint 7 and the second shoulder joint 8, c ₁ is cosθ ₁ , θ ₁ is the acute angle between the parallelogram outer boom 4 and the fixed frame 20, h is the distance between the hinge point of the first shoulder joint 7 and the hinge point of the first rotary joint 11;

Step 2: Calculate the gravitational potential energy W ₂ of the small arm 5:

Formula 2: W ₂ =m ₂ g(l ₁ c ₁ +l′ ₂ c ₁₊₂ h)=m ₂ g(l ₁ c ₁ +l′ ₂ c ₁ c ₂ -l′ ₂ s ₁ s ₂ + h)

Among them, m ₂ is the mass of the forearm 5, l ₁ is the length of the boom 4, l′ ₂ is the length from the center of mass of the forearm 5 to the hinge point of the elbow joint 6, and c ₁₊₂ is cos( θ ₁ +θ ₂ ), c ₂ is cosθ ₂ , s ₁ is sinθ ₁ , s ₂ is sinθ ₂ , θ ₂ is the acute angle between the outer forearm 5 of the forearm 5 and the boom 4;

Step 3: Calculate the gravitational potential energy W ₃ of the first balance pole 1:

Formula 3: W ₃ =m ₃ gl′ ₃ c ₁

Wherein, m ₃ is the quality of the first balance pole 1, and l' ₃ is the length from the mass center point of the first balance pole 1 to the hinge point of the first rotating joint 11;

Step 4: Calculate the gravitational potential energy W ₄ of the second balance pole 2:

Formula 4: W ₄ ＝m ₄ g(l ₁ c ₁ +l′ ₄ )

Wherein, m ₄ is the quality of the second balance pole 2, and l' ₄ is the length from the mass center point of the second balance pole 2 to the hinge point of the connecting head 10;

Step 5: Calculate the total potential energy V _g of the boom 4, the forearm 5, the first balance bar 1 and the second balance bar 2:

Formula 5: V _g =W ₁ +W ₂ +W ₃ +W ₄ =

m ₁ gh+m ₂ gh+m ₄ gh′l ₄ +(m ₁ gl′ ₁ +m ₂ gl ₁ +m ₃ gl′ ₃ +m ₄ gl ₁ )c ₁ +m ₂ gl′ ₂ c ₁ c ₂ -m ₂ gl′ ₂ s ₁ s ₂

Step 6: Calculate the elongation x ₁ of the first spring 15:

Formula six: $x_1^2=h^2+d_1^2+2{\mathrm{hd}}_1{c}_1$

Wherein, x ₁ ² is the square of the elongation of the first spring 15, d ₁ is the length of the balance bar steel wire rope 18, and d ₁ ² is the square of the length of the balance bar steel wire rope 18;

Step 7: Calculate the elongation x ₂ of the second spring 17:

Formula seven: $x_2^2=h^2+d_2^2+2{\mathrm{hd}}_2{c}_{1+2}$

Wherein, x ₂ ² is the square of the elongation of the second spring 17, d ₂ is the length of the forearm steel wire rope 19, and d ₂ ² is the square of the length of the forearm steel wire rope 19;

Step 8: Calculate the elastic potential energy sum V _s of the first spring 15 and the second spring 17:

Formula eight: $\begin{array}{c}{V}_{\mathrm{the\; s}}=\frac{1}{2}{k}_1{x}_1^2+\frac{1}{2}{k}_2{x}_2^2=\\ \frac{1}{2}{k}_1(h^2+d_1^2)+\frac{1}{2}{k}_2(h^2+d_2^2)+k_1{\mathrm{hd}}_1{c}_1+k_2{\mathrm{hd}}_2{c}_1{c}_2-k_2{\mathrm{hd}}_2{c}_0{\mathrm{the\; s}}_1{\mathrm{the\; s}}_2\end{array}$

Wherein, k ₁ is the stiffness coefficient of the first spring 15, and k ₂ is the stiffness coefficient of the second spring 17;

Step 9: In order to make V _s +V _g = constant, eliminate the coefficient for formula 8. Since the settlement result of gravitational potential energy is a negative value, there is:

Formula 9: m ₁ gl′ ₁ +m ₂ gl ₁ +m ₃ gl′ ₃ +m ₄ gl ₁ ＝k ₁ hd ₁ The corresponding item coefficients of V _s and V _g cancel

Formula 10: m ₂ gl′ ₂ =k ₂ hd ₂ V _s and V _g corresponding item coefficients cancel

Step 10: When the system relationship satisfies the above formulas 9 and 10, the system reaches plane equilibrium;

Step 11: According to the actual object mass m, adjust the distance h between the hinge point of the first shoulder joint 7 and the hinge point of the first rotation joint 11 to realize balance compensation.

Specific embodiment five: illustrate this embodiment in conjunction with Fig. 3, this embodiment is to realize the space gravity balance compensation method, and its steps are as follows:

Step 1: Calculate the gravitational potential energy W ₁ of the boom 4:

Formula 1': W ₁ =m ₁ g(l' ₁ c ₁ +h)

Among them, m ₁ is the mass of the boom 4, g is the acceleration of gravity, l ₁ ′ is the length from the center of mass of the boom 4 to the hinge point of the first shoulder joint 7 and the second shoulder joint 8, and c ₁ is cosθ ₁ , _θ1 is the acute angle between the outer big arm 4 of the parallelogram and the fixed frame 20, h is the distance between the hinge point of the first shoulder joint 7 and the hinge point of the first rotary joint 11;

Step 2: Calculate the gravitational potential energy W ₂ of the small arm 5:

Formula 2': W ₂ =m ₂ g(l ₁ c ₁ +l' ₂ c ₁ c ₂ -l' ₂ c ₀ s ₁ s ₂ +h)

Among them, m ₂ is the mass of the forearm 5, l ₁ is the length of the boom 4, l′ ₂ is the length from the center of mass of the forearm 5 to the hinge point of the elbow joint 6, c ₀ is cosθ ₀ , c ₂ is cosθ ₂ , s ₁ is sinθ ₁ , s ₂ is sinθ ₂ , θ ₂ is the acute angle between the outer forearm 5 of the forearm 5 and the boom 4 ; θ ₀ is the axial rotation of the boom 4 corner;

Step 3: Calculate the gravitational potential energy W ₃ of the first balance pole 1:

Formula 3': W ₃ =m ₃ gl' ₃ c ₁

Wherein, m ₃ is the quality of the first balance pole 1, and l' ₃ is the length from the mass center point of the first balance pole 1 to the hinge point of the first rotating joint 11;

Step 4: Calculate the gravitational potential energy W ₄ of the second balance pole 2:

Formula 4': W ₄ =m ₄ g(l ₁ c ₁ +l' ₄ )

Wherein, m ₄ is the quality of the second balance pole 2, and l' ₄ is the length from the mass center point of the second balance pole 2 to the hinge point of the connecting head 10;

Step 5: Calculate the total potential energy V _g of the boom 4, the forearm 5, the first balance bar 1 and the second balance bar 2:

Formula 5': V _g =W ₁ +W ₂ +W ₃ +W ₄ =

m ₁ gh+m ₂ gh+m ₄ gl′ ₄ +(m ₁ gl′ ₁ +m ₂ gl ₁ +m ₃ gl′ ₃ +m ₄ gl ₁ )c ₁ +m ₂ gl′ ₂ c ₁ c ₂ - m ₂ gl′ ₂ c ₀ s ₁ s ₂

Step 6: Calculate the elongation x ₁ of the first spring 15:

Formula six': $x_1^2=h^2+d_1^2+2{\mathrm{hd}}_1{c}_1$

Wherein, x ₁ ² is the square of the elongation of the first spring 15, d ₁ is the length of the balance bar steel wire rope 18, and d ₁ ² is the square of the length of the balance bar steel wire rope 18;

Step 7: Calculate the elongation x ₂ of the second spring 17:

Formula seven': $x_2^2=h^2+d_2^2+2{\mathrm{hd}}_2(c_1{c}_2-c_0{\mathrm{the\; s}}_1{\mathrm{the\; s}}_2)$

Wherein, x ₂ ² is the square of the elongation of the second spring 17, d ₂ is the length of the forearm steel wire rope 19, and d ₂ ² is the square of the length of the forearm steel wire rope 19;

Step 8: Calculate the elastic potential energy sum V _s of the first spring 15 and the second spring 17:

Formula Eight' $\begin{array}{c}{V}_{\mathrm{the\; s}}=\frac{1}{2}{k}_1{x}_1^2+\frac{1}{2}{k}_2{x}_2^2=\\ \frac{1}{2}{k}_1(h^2+d_1^2)+\frac{1}{2}{k}_2(h^2+d_2^2)+k_1{\mathrm{hd}}_1{c}_1+k_2{\mathrm{hd}}_2{c}_1{c}_2-k_2{\mathrm{hd}}_2{c}_0{\mathrm{the\; s}}_1{\mathrm{the\; s}}_2\end{array}$

Wherein, k ₁ is the stiffness coefficient of the first spring 15, and k ₂ is the stiffness coefficient of the second spring 17;

Step 9: In order to make V _s +V _g = constant, eliminate the coefficient for formula 8'. Since the settlement result of gravitational potential energy is a negative value, there is:

Formula 9': m ₁ gl' ₁ + m ₂ gl ₁ + m ₃ gl' ₃ + m ₄ gl ₁ = k ₁ hd ₁ V _s and V _g corresponding item coefficients cancel

Formula 10': m ₂ gl' ₂ =k ₂ hd ₂ V _s and V _g corresponding item coefficients cancel

Step 10: When the system relationship satisfies the above formula 9' and formula 10', the system reaches spatial balance;

Step eleven: According to the actual object mass m, adjust the distance h between the hinge point of the first shoulder joint 7 and the hinge point of the first rotation joint 11 to realize space compensation.

Claims (5)

1. a robot three-dimensional space gravity compensating chain device, it is characterized in that: described device includes first gimbal lever (1), second gimbal lever (2), fixed cover (3), big armed lever (4), little armed lever (5), elbow joint (6), first shoulder joint (7), second shoulder joint (8), 3rd shoulder joint (9), union joint (10), first cradle head (11), second cradle head (12), 3rd cradle head (13), first pulley (14), first spring (15), second pulley (16), second spring (17), gimbal lever's steel wire rope (18), forearm steel wire rope (19) and fixed mount (20), one end of first gimbal lever (1) is fixing with the first cradle head (11) to be connected, first cradle head (11) is hinged with the second cradle head (12), second cradle head (12) is hinged with the 3rd cradle head (13), the other end of first gimbal lever (1) is fixing with union joint (10) to be connected, one end of second gimbal lever (2) is hinged with union joint (10), the other end of second gimbal lever (2) is hinged with fixed cover (3), one end of big armed lever (4) is connected through fixed cover (3) and elbow joint (6) are fixing, fixed cover (3) rotates around big armed lever (4), the other end and first shoulder joint (7) of big armed lever (4) are connected by bearing, first shoulder joint (7) is hinged with the second shoulder joint (8), second shoulder joint (8) is hinged with the 3rd shoulder joint (9), one end of little armed lever (5) is hinged with elbow joint (6), first pulley (14) and the first spring (15) are each attached in first gimbal lever (1), and first pulley (14) be positioned at the side of the first cradle head (11), first spring (15) is positioned at the side of union joint (10), second pulley (16) and the second spring (17) are each attached on little armed lever (5), and second pulley (16) be positioned at the side of elbow joint (6), second spring (17) is positioned at the outside of little armed lever (5), 3rd shoulder joint (9) and the 3rd cradle head (13) are all fixing with fixed mount (20) to be connected, one end of gimbal lever's steel wire rope (18) is fixing with fixed mount (20) to be connected, the other end of gimbal lever's steel wire rope (18) walks around that the first pulley (14) and the first spring (15) are fixing to be connected, one end of forearm steel wire rope (19) is fixing with second gimbal lever (2) to be connected, the other end of forearm steel wire rope (19) walks around that the second pulley (16) and the second spring (17) are fixing to be connected。

2. robot three-dimensional space gravity compensating chain device according to claim 1, it is characterized in that: the length of described big armed lever (4) is identical with the length of first gimbal lever (1), on fixed mount (20), the distance between the 3rd shoulder joint (9) and the 3rd cradle head (13) is identical with the length of second gimbal lever (2)。

3. robot three-dimensional space gravity compensating chain device according to claim 1 or claim 2, it is characterised in that: described first gimbal lever (1), second gimbal lever (2), big armed lever (4) and fixed mount (20) constitute parallelogram。

4. one kind utilizes robot three-dimensional space gravity compensating chain device to realize the balanced compensated method of robot three-dimensional space gravity, it is characterised in that: described method is to realize the method that plane gravitational equilibrium compensates, and its step is as follows:

Step one: calculate the gravitional force W of big armed lever (4)₁:

Formula one: W₁=m₁g(l′₁c₁+h)

Wherein, wherein, m₁For the quality of big armed lever (4), g is acceleration of gravity, l '₁For the mass centre o'clock of big armed lever (4) to the length of the first shoulder joint (7) Yu the second shoulder joint (8) pin joint, c₁For cos θ₁, θ₁For the acute angle between the big armed lever in outside (4) and the fixed mount (20) of parallelogram, h is that the pin joint of the first shoulder joint (7) is to the distance between the pin joint of the first cradle head (11)；

Step 2: calculate the gravitional force W of little armed lever (5)₂:

Formula two: W₂=m₂g(l₁c₁+l′₂c₁₊₂+ h)=m₂g(l₁c₁+l′₂c₁c₂-l′₂s₁s₂+h)

Wherein, m₂For the quality of little armed lever (5), l₁For the length of big armed lever (4), l '₂For mass centre's point of little armed lever (5) to the length of elbow joint (6) pin joint, c₁₊₂For cos (θ₁+θ₂), c₂For cos θ₂, s₁For sin θ₁, s₂For sin θ₂, θ₂For the acute angle between the little armed lever in outside (5) and the big armed lever (4) of little armed lever (5)；

Step 3: calculate the gravitional force W of first gimbal lever (1)₃:

Formula three: W₃=m₃gl′₃c₁

Wherein, m₃It is the quality of first gimbal lever (1), l '₃It is the mass centre o'clock length to the first cradle head (11) pin joint of first gimbal lever (1)；

Step 4: calculate the gravitional force W of second gimbal lever (2)₄:

Formula four: W₄=m₄g(l₁c₁+l′₄)

Wherein, m4 is the quality of second gimbal lever (2), l '₄It is mass centre's point length to union joint (10) pin joint of second gimbal lever (2)；

Step 5: calculate total potential energy V of big armed lever (4), little armed lever (5), first gimbal lever (1) and second gimbal lever (2)_g:

Formula five: V_g=W₁+W₂+W₃+W₄=m₁gh+m₂gh+m₄gl′₄+(m₁gl′₁+m₂gl₁+m₃gl′₃+m₄gl₁)c₁+m₂gl′₂c₁c₂-m₂gl′₂s₁s₂

Step 6: calculate the elongation x of the first spring (15)₁:

Formula six: $x_1^2=h^2+d_1^2+2{\mathrm{hd}}_1{c}_1$

Wherein, x₁ ²Be the first spring (15) elongation square, d₁For the length of gimbal lever's steel wire rope (18), d₁ ²For gimbal lever's steel wire rope (18) length square；

Step 7: calculate the elongation x of the second spring (17)₂:

Formula seven: $x_2^2=h^2+d_2^2+2{\mathrm{hd}}_2{c}_{1+2}$

Wherein, x₂ ²Be the second spring (17) elongation square, d₂For the length of forearm steel wire rope (19), d₂ ²For forearm steel wire rope (19) length square；

Step 8: calculate elastic potential energy and the V of the first spring (15) and the second spring (17)_s:

Formula eight: $\begin{array}{c}{V}_s=\frac{1}{2}{k}_1{x}_1^2+\frac{1}{2}{k}_2{x}_2^2=\\ \frac{1}{2}{k}_1(h^2+d_1^2)+\frac{1}{2}{k}_2(h^2+d_2^2)+k_1{\mathrm{hd}}_1{c}_1+k_2{\mathrm{hd}}_2{c}_1{c}_2-k_2{\mathrm{hd}}_2{s}_1{s}_2\end{array}$

Wherein, k₁It is the stiffness factor of the first spring (15), k₂It it is the stiffness factor of the second spring (17)；

Step 9: for making V_s+V_g=constant, to formula eight rain scavenging coefficient, owing to gravitional force checkout result is negative value, therefore has:

Formula nine: m₁gl′₁+m₂gl₁+m₃gl′₃+m₄gl₁=k₁hd₁V_sAnd V_gRespective items coefficient cancellation

Formula ten: m₂gl′₂=k₂hd₂V_sAnd V_gRespective items coefficient cancellation

Step 10: when phylogenetic relationship meets above formula nine and ten, system reaches plane balancing, and namely gravity compensation completes；

Step 11: according to actual object quality m, regulates the pin joint of the first shoulder joint (7) to the distance h between the pin joint of the first cradle head (11), can realize balanced compensated。

5. one kind utilizes robot three-dimensional space gravity compensating chain device to realize the balanced compensated method of robot three-dimensional space gravity, it is characterised in that: described method is to realize the balanced compensated method of space gravity, and its step is as follows:

Step one: calculate the gravitional force W of big armed lever (4)₁:

Formula one ': W₁=m₁g(l′₁c₁+h)

Wherein, wherein, m₁For the quality of big armed lever (4), g is acceleration of gravity, l '₁For the mass centre o'clock of big armed lever (4) to the length of the first shoulder joint (7) Yu the second shoulder joint (8) pin joint, c₁For cos θ₁, θ₁For the acute angle between the big armed lever in outside (4) and the fixed mount (20) of parallelogram, h is that the pin joint of the first shoulder joint (7) is to the distance between the pin joint of the first cradle head (11)；

Step 2: calculate the gravitional force W of little armed lever (5)₂:

Formula two ': W₂=m₂g(l₁c₁+l′₂c₁c₂-l′₂c₀s₁s₂+h)

Wherein, m₂For the quality of little armed lever (5), l₁For the length of big armed lever (4), l '₂For mass centre's point of little armed lever (5) to the length of elbow joint (6) pin joint, c₀For cos θ₀, c₂For cos θ₂, s₁For sin θ₁, s₂For sin θ₂, θ₂For the acute angle between the little armed lever in outside (5) and the big armed lever (4) of little armed lever (5)；θ₀For big armed lever (4) axially angle of rotation；

Step 3: calculate the gravitional force W of first gimbal lever (1)₃:

Formula three ': W₃=m₃gl′₃c₁

Wherein, m₃It is the quality of first gimbal lever (1), l '₃It is the mass centre o'clock length to the first cradle head (11) pin joint of first gimbal lever (1)；

Step 4: calculate the gravitional force W of second gimbal lever (2)₄:

Formula four ': W₄=m₄g(l₁c₁+l′₄)

Wherein, m₄It is the quality of second gimbal lever (2), l '₄It is mass centre's point length to union joint (10) pin joint of second gimbal lever (2)；

Step 5: calculate total potential energy V of big armed lever (4), little armed lever (5), first gimbal lever (1) and second gimbal lever (2)_g:

Formula five ': V_g=W₁+W₂+W₃+W₄=m₁gh+m₂gh+m₄gl′₄+(m₁gl′₁+m₂gl₁+m₃gl′₃+m₄gl₁)c₁+m₂gl′₂c₁c₂-m₂gl′₂c₀s₁s₂

Step 6: calculate the elongation x of the first spring (15)₁:

Formula six ': $x_1^2=h^2+d_1^2+2{\mathrm{hd}}_1{c}_1$

Wherein, x₁ ²Be the first spring (15) elongation square, d₁For the length of gimbal lever's steel wire rope (18), d₁ ²For gimbal lever's steel wire rope (18) length square；

Step 7: calculate the elongation x of the second spring (17)₂:

Formula seven ': $x_2^2=h^2+d_2^2+2{\mathrm{hd}}_2(c_1{c}_2-c_0{s}_1{s}_2)$

Wherein, x₂ ²Be the second spring (17) elongation square, d₂For the length of forearm steel wire rope (19), d₂ ²For forearm steel wire rope (19) length square；

Step 8: calculate elastic potential energy and the V of the first spring (15) and the second spring (17)_s:

Formula eight ': $\begin{array}{c}{V}_s=\frac{1}{2}{k}_1{x}_1^2+\frac{1}{2}{k}_2{x}_2^2=\\ \frac{1}{2}{k}_1(h^2+d_1^2)+\frac{1}{2}{k}_2(h^2+d_2^2)+k_1{\mathrm{hd}}_1{c}_1+k_2{\mathrm{hd}}_2{c}_1{c}_2-k_2{\mathrm{hd}}_2{c}_0{s}_1{s}_2\end{array}$

Wherein, k₁It is the stiffness factor of the first spring (15), k₂It it is the stiffness factor of the second spring (17)；

Step 9: for making V_s+V_g=constant, to formula eight ' rain scavenging coefficient, owing to gravitional force checkout result is negative value, therefore has:

Formula nine ': m₁gl′₁+m₂gl₁+m₃gl′₃+m₄gl₁=k₁hd₁V_sAnd V_gRespective items coefficient cancellation

Formula ten ': m₂gl′₂=k₂hd₂V_sAnd V_gRespective items coefficient cancellation

Step 10: when meeting above formula nine ' and formula ten ' when phylogenetic relationship, system reaches spatial balance；

Step 11: according to actual object quality m, regulates the pin joint of the first shoulder joint (7) to the distance h between the pin joint of the first cradle head (11), can realize space compensation。