CN105729478A - Forward and inverse kinematics solution algorithm for parallel manipulator control system with two degrees of freedom - Google Patents

Abstract

The invention discloses a forward and inverse kinematics solution algorithm for a parallel manipulator control system with two degrees of freedom, and belongs to the field of technologies for controlling parallel manipulators. Forward solution includes calculating spatial attitudes of manipulators and spatial locations of working ends by the aid of rotation angles of each shaft of each manipulator and converting motor corners (theta 1, theta 2) into tail end locations (x, y); inverse solution includes calculating locations and rotation angles of various joints by the aid of the tail end locations of the manipulators, converting the tail end locations (x, y) into the motor corners (theta 1, theta 2) and converting movement of translation discs into controllable motor corners. The forward and inverse kinematics solution algorithm has the advantages that power-off keeping effects can be realized by the forward solution, and if the parallel manipulator control system is suddenly powered off, coordinates of tail ends can be calculated by the id of the motor corners recorded by absolute value encoders of servo motors after the parallel manipulator control system is powered on; the forward solution and the inverse solution are derived by the aid of plane geometry, the forward and inverse kinematics solution algorithm is clear in principle, reasonable solution which meets conditions in working planes can be acquired by the aid of the forward and inverse kinematics solution algorithm, and the problem that existing forward solution is mostly analytical solution can be solved; as proved by experiments, perfect manipulator movement effects can be ultimately realized by the aid of the forward solution and the inverse solution.

Description

A kind of kinesiology positive and negative resolving Algorithm of two-degree-of-freedoparallel parallel manipulator control system

Technical field

The invention belongs to parallel manipulator hand control technical field, relate to the control technology of a kind of two-degree-of-freedoparallel parallel mechanical hand, particularly relate to the positive and negative resolving Algorithm of kinesiology of a kind of two-degree-of-freedoparallel parallel manipulator control system.

Background technology

Forward kinematics solution is through the anglec of rotation of every, mechanical hand axle and extrapolates the spatial attitude of mechanical hand and the locus of working end, motor corner (θ 1, θ 2) it is converted into terminal position (x, y), this is the theoretical basis realizing monitor in real time end effector point bit function in systems.Inverse kinematic is through arm end dead reckoning and goes out position and the rotational angle in each joint, thus can terminal position (x, y) it is converted into motor corner (θ 1, θ 2), the movement of translational disk is converted into controlled motor corner.Normal solution has the effect that power-off keeps, if the unexpected power-off of system, can extrapolate out the coordinate of end after powering on from the motor corner of the absolute value encoder record of servomotor.Up to the present the closing solution problem of parallel manipulator forward kinematics solution is not solved comprehensively, conventional solution is to adopt the numerical solution based on Algebraic Equation set, the method weak point is that derivation is complicated, actual application exists the problem solving and accepting or rejecting more, anti-solution preocess is relatively complicated simultaneously, not intuitively.

Summary of the invention

The present invention is directed to that the derivation that above-mentioned employing exists based on the numerical solution of Algebraic Equation set is complicated, there is the deficiency solving and accepting or rejecting more, the positive and negative resolving Algorithm of kinesiology of a kind of two-degree-of-freedoparallel parallel manipulator control system is proposed, employing plane geometry is derived, directly perceived simple, the reasonable solution satisfied condition in working face can be obtained, solve the problem that current normal solution mostly is analytic solutions.

The technical scheme is that the positive and negative resolving Algorithm of kinesiology of a kind of two-degree-of-freedoparallel parallel manipulator control system, including the two-degree-of-freedoparallel parallel manipulator control system being made up of the gimbal lever, master arm, slave arm, electric magnet, left motor, right motor and translational disk；It is characterized in that, the anti-resolving Algorithm step of described two-degree-of-freedoparallel parallel manipulator control system is as follows:

(1) two-degree-of-freedoparallel parallel robot manipulator structure is reduced to plane Four-connecting-rod hinge mechanism；

(2) in the plane Four-connecting-rod hinge mechanism of step (1), if actively brachium is Lt, driven brachium is Lb, and machine shaft is A and E respectively, and the distance between axles of rotating shaft is L1, and translational disk length is L2；

(3) setting up with machine shaft for X-axis, machine shaft spacing central point is the coordinate system of initial point；

(4) set two electric machine rotation angles respectively θ 1 and θ 2, in translational disk place step (3) coordinate of coordinate system be (x, y)；

(5) by all interior to master arm and slave arm translation L2/2, translational disk being simplified to C point, distance between axles foreshortens to L1-L2；

(6) use several drawing board, scale relative to that the simplification of mechanical hand is counter solves mathematical model；

(7) A point coordinates can be drawn from step (6)The left axle of regulation rotates to be positive angle counterclockwise from 0 ° of beginning, clockwise turns to negative angle；Right axle is then contrary.

(8) setThen (x+l y), is respectively as follows: vector AC=with x-axis positive axis angle and mould

$\mathrm{\β}=arccos\frac{x+l}{\left|AC\right|}$

In Δ ABC, three length of sides are all it is known that obtained by the cosine law:

Therefore the left motor anglec of rotation is: θ₁=180 °-(β+α).

In like manner draw the right motor anglec of rotation:

θ₂=β '-α '

$\left|EC\right|=\sqrt{\sqrt{{(x-l)}^2+y^2}},{\mathrm{\β}}^{\′}=\arccos \frac{x-l}{\left|EC\right|},{\mathrm{\α}}^{\′}=\arccos \left(\frac{L_t^2+|EC{|}^2-L_b^2}{2\·L_t\·\left|EC\right|}\right)$

The normal solution algorithm steps of described two-degree-of-freedoparallel parallel manipulator control system is as follows:

(1) use several drawing board, scale relative to the simplification normal solution mathematical model of mechanical hand；

(2) the left axle of regulation rotates to be positive angle counterclockwise from 0 ° of beginning, clockwise turns to negative angle；Right axle is then contrary；

(3) orderThen:

$\left|OB\right|=\sqrt{|FO{|}^2+|BF{|}^2}=\sqrt{{(l+L_t{\mathrm{cos\θ}}_1)}^2+{\left(L_t{\mathrm{sin\θ}}_1\right)}^2}$

$\left|OD\right|=\sqrt{|GO{|}^2+|DG{|}^2}=\sqrt{{(l+L_t{\mathrm{cos\θ}}_2)}^2+{\left(L_t{\mathrm{sin\θ}}_2\right)}^2}$

${\mathrm{\α}}_1=\arcsin \frac{L_t{\mathrm{sin\θ}}_1}{\left|OB\right|}$

${\mathrm{\α}}_2=\arcsin \frac{L_t{\mathrm{sin\θ}}_2}{\left|OD\right|}$

(4) calculate in right-angled trapezium FBDG:

If straight line BD is at O point lower section, i.e. α₁+α₂>=0, as shown in Figure 4, then in Δ ABD, the cosine law obtain:

$\mathrm{\β}=\∠OBD=arccos\frac{|OB{|}^2+|BD{|}^2-|OD{|}^2}{2\left|OB\right|\·\left|BD\right|}$

If straight line BD is above O point, i.e. α₁+α₂< 0, as it is shown in figure 5, then:

$\mathrm{\&beta;}=-arccos\frac{|OB{|}^2+|BD{|}^2-|OD{|}^2}{2\left|OB\right|\&CenterDot;\left|BD\right|}$

(5) in Δ COB, the cosine law calculate:

$\left|OC\right|=\sqrt{|OB{|}^2+|BC{|}^2-2\left|OB\right|\left|BC\right|\cos \&angle;OBC}=\sqrt{|OB{|}^2+L_b^2-2|OB|L_b\cos (\mathrm{\&beta;}+\mathrm{\&gamma;})}$

(6) vector OC with the angle of x-axis positive axis is(α₁It is likely negative angle), namely working end coordinate is:

The invention have the benefit that the positive and negative resolving Algorithm of kinesiology of a kind of two-degree-of-freedoparallel parallel manipulator control system that the present invention proposes, employing plane geometry is derived, clear principle, normal solution is the locus of spatial attitude and the working end being extrapolated mechanical hand by the anglec of rotation of every, mechanical hand axle, motor corner (θ 1, θ 2) be converted into terminal position (x, y)；Anti-solution is to be gone out position and the rotational angle in each joint by arm end dead reckoning, and terminal position, (x y) is converted into motor corner (θ 1, θ 2), the movement of translational disk is converted into controlled motor corner.Normal solution has the effect that power-off keeps, if the unexpected power-off of system, can extrapolate out the coordinate of end after powering on from the motor corner of the absolute value encoder record of servomotor.The reasonable solution satisfied condition in working face can be obtained, solve the problem that current normal solution mostly is analytic solutions.Experiment proves this positive and negative solution, finally gives ideal robot movement effect.

Accompanying drawing explanation

Fig. 1 is two-degree-of-freedoparallel parallel robot manipulator structure schematic diagram of the present invention.

The lanar four rod mechanism schematic diagram that Fig. 2 two-degree-of-freedoparallel parallel mechanical hand of the present invention converts.

In Fig. 3 present invention, mechanical hand simplification is counter solves mathematical model.

In Fig. 4 present invention, mechanical hand simplifies normal solution mathematical model.

In Fig. 5 present invention, mechanical hand simplifies normal solution mathematical model special circumstances model.

In figure: the gimbal lever 1, master arm 2, slave arm 3, translational disk 4, electric magnet 5, left motor 6, right motor 7.

Detailed description of the invention

Below in conjunction with accompanying drawing, the invention will be further described:

As it is shown in figure 1, the two-freedom parallel manipulator that model is SW-300 that the Shi Qi tackling company that a kind of two-degree-of-freedoparallel parallel manipulator control system adopts produces, this device is made up of servomotor, master arm, slave arm and translational disk.Silent flatform is installed two servomotors, NJ controller issues instructions to servo-driver, drive servomotor to drive master arm rotation, the slave arm action being connected with master arm, and then control translational disk planar to make two-dimensional movement through decelerator.Meanwhile, translational disk install different institutions can carry out hands grab, the different action such as clamp and absorption.In Fig. 1, what translational disk was installed is electric magnet, when above translational disk moves to irony object, makes electric magnet energising reach to draw the requirement of target by the instruction of controller.

As Figure 2-3, the positive and negative resolving Algorithm of kinesiology of a kind of two-degree-of-freedoparallel parallel manipulator control system, including the two-degree-of-freedoparallel parallel manipulator control system being made up of the gimbal lever 1, master arm 2, slave arm 3, electric magnet 5, left motor 6, right motor 7 and translational disk 4；The anti-resolving Algorithm step of two-degree-of-freedoparallel parallel manipulator control system is as follows:

(1) two-degree-of-freedoparallel parallel robot manipulator structure is reduced to plane Four-connecting-rod hinge mechanism；

(2) in the plane Four-connecting-rod hinge mechanism of step (1), if actively brachium is Lt, driven brachium is Lb, and machine shaft is A and E respectively, and the distance between axles of rotating shaft is L1, and translational disk length is L2；

(3) setting up with machine shaft for X-axis, machine shaft spacing central point is the coordinate system of initial point；

(4) set two electric machine rotation angles respectively θ 1 and θ 2, in translational disk place step (3) coordinate of coordinate system be (x, y)；

(5) by all interior to master arm and slave arm translation L2/2, translational disk being simplified to C point, distance between axles foreshortens to L1-L2；

(6) use several drawing board, scale relative to that the simplification of mechanical hand is counter solves mathematical model；

(7) A point coordinates can be drawn from step (6)The left axle of regulation rotates to be positive angle counterclockwise from 0 ° of beginning, clockwise turns to negative angle；Right axle is then contrary.

(8) setThen (x+l y), is respectively as follows: vector AC=with x-axis positive axis angle and mould

$\mathrm{\&beta;}=arccos\frac{x+l}{\left|AC\right|}$

In Δ ABC, three length of sides are all it is known that obtained by the cosine law:

Therefore the left motor anglec of rotation is: θ₁=180 °-(β+α).

In like manner draw the right motor anglec of rotation:

θ₂=β '-α '

$\left|EC\right|=\sqrt{\sqrt{{(x-l)}^2+y^2}},{\mathrm{\&beta;}}^{\&prime;}=\arccos \frac{x-l}{\left|EC\right|},{\mathrm{\&alpha;}}^{\&prime;}=\arccos \left(\frac{L_t^2+|EC{|}^2-L_b^2}{2\&CenterDot;L_t\&CenterDot;\left|EC\right|}\right)$

As illustrated in figures 4-5, the forward kinematics solution algorithm steps of a kind of two-degree-of-freedoparallel parallel manipulator control system is as follows:

(1) use several drawing board, scale relative to the simplification normal solution mathematical model of mechanical hand；

(2) the left axle of regulation rotates to be positive angle counterclockwise from 0 ° of beginning, clockwise turns to negative angle；Right axle is then contrary；

(3) orderThen:

$\left|OB\right|=\sqrt{|FO{|}^2+|BF{|}^2}=\sqrt{{(l+L_t{\mathrm{cos\&theta;}}_1)}^2+{\left(L_t{\mathrm{sin\&theta;}}_1\right)}^2}$

$\left|OD\right|=\sqrt{|GO{|}^2+|DG{|}^2}=\sqrt{{(l+L_t{\mathrm{cos\&theta;}}_2)}^2+{\left(L_t{\mathrm{sin\&theta;}}_2\right)}^2}$

${\mathrm{\&alpha;}}_1=\arcsin \frac{L_t{\mathrm{sin\&theta;}}_1}{\left|OB\right|}$

${\mathrm{\&alpha;}}_2=\arcsin \frac{L_t{\mathrm{sin\&theta;}}_2}{\left|OD\right|}$

(4) calculate in right-angled trapezium FBDG:

If straight line BD is at O point lower section, i.e. α₁+α₂>=0, as shown in Figure 4, then in Δ ABD, the cosine law obtain:

$\mathrm{\&beta;}=\&angle;OBD=arccos\frac{|OB{|}^2+|BD{|}^2-|OD{|}^2}{2\left|OB\right|\&CenterDot;\left|BD\right|}$

If straight line BD is above O point, i.e. α₁+α₂< 0, as it is shown in figure 5, then:

$\mathrm{\&beta;}=-arccos\frac{|OB{|}^2+|BD{|}^2-|OD{|}^2}{2\left|OB\right|\&CenterDot;\left|BD\right|}$

(5) in Δ COB, the cosine law calculate:

$\left|OC\right|=\sqrt{|OB{|}^2+|BC{|}^2-2\left|OB\right|\left|BC\right|\cos \&angle;OBC}=\sqrt{|OB{|}^2+L_b^2-2|OB|L_b\cos (\mathrm{\&beta;}+\mathrm{\&gamma;})}$

(6) vector OC with the angle of x-axis positive axis is(α₁It is likely negative angle), namely working end coordinate is:

In the present invention, normal solution is the locus of spatial attitude and the working end being extrapolated mechanical hand by the anglec of rotation of every, mechanical hand axle, motor corner (θ 1, θ 2) be converted into terminal position (x, y)；Anti-solution is to be gone out position and the rotational angle in each joint by arm end dead reckoning, and terminal position, (x y) is converted into motor corner (θ 1, θ 2), the movement of translational disk is converted into controlled motor corner.Normal solution has the effect that power-off keeps, if the unexpected power-off of system, can extrapolate out the coordinate of end after powering on from the motor corner of the absolute value encoder record of servomotor.Derive simple, clear principle, adopt plane geometry to derive, it is possible to obtain the reasonable solution satisfied condition in working face, solve the problem that current normal solution mostly is analytic solutions.Experiment proves this positive and negative solution, finally gives ideal robot movement effect.

Claims (2)

1. the positive and negative resolving Algorithm of the kinesiology of two-degree-of-freedoparallel parallel manipulator control system, including the two-degree-of-freedoparallel parallel manipulator control system being made up of the gimbal lever (1), master arm (2), slave arm (3), electric magnet (5), left motor (6), right motor (7) and translational disk (4)；It is characterized in that, the anti-resolving Algorithm step of described two-degree-of-freedoparallel parallel manipulator control system is as follows:

(1) two-degree-of-freedoparallel parallel robot manipulator structure is reduced to plane Four-connecting-rod hinge mechanism；

(2) in the plane Four-connecting-rod hinge mechanism of step (1), if actively brachium is Lt, driven brachium is Lb, and machine shaft is A and E respectively, and the distance between axles of rotating shaft is L1, and translational disk length is L2；

(3) setting up with machine shaft for X-axis, machine shaft spacing central point is the coordinate system of initial point；

(4) set two electric machine rotation angles respectively θ 1 and θ 2, in translational disk place step (3) coordinate of coordinate system be (x, y)；

(5) by all interior to master arm and slave arm translation L2/2, translational disk being simplified to C point, distance between axles foreshortens to L1-L2；

(6) use several drawing board, scale relative to that the simplification of mechanical hand is counter solves mathematical model；

(7) A point coordinates can be drawn from step (6)The left axle of regulation rotates to be positive angle counterclockwise from 0 ° of beginning, clockwise turns to negative angle；Right axle is then contrary.

(8) setThen (x+l y), is respectively as follows: vector AC=with x-axis positive axis angle and mould

In Δ ABC, three length of sides are all it is known that obtained by the cosine law:Therefore the left motor anglec of rotation is: θ₁=180 °-(β+α).

In like manner draw the right motor anglec of rotation:

θ₂=β '-α '

。

2. the positive and negative resolving Algorithm of the kinesiology of two-degree-of-freedoparallel parallel manipulator control system, including the two-degree-of-freedoparallel parallel manipulator control system being made up of the gimbal lever (1), master arm (2), slave arm (3), electric magnet (5), left motor (6), right motor (7) and translational disk (4)；It is characterized in that, the normal solution algorithm steps of described two-degree-of-freedoparallel parallel manipulator control system is as follows:

(1) use several drawing board, scale relative to the simplification normal solution mathematical model of mechanical hand；

(2) the left axle of regulation rotates to be positive angle counterclockwise from 0 ° of beginning, clockwise turns to negative angle；Right axle is then contrary；

(3) orderThen:

(4) calculate in right-angled trapezium FBDG:

If straight line BD is at O point lower section, i.e. α₁+α₂>=0, as shown in Figure 4, then in Δ ABD, the cosine law obtain:

If straight line BD is above O point, i.e. α₁+α₂< 0, as it is shown in figure 5, then:

(5) in Δ COB, the cosine law calculate:

(6) vector OC with the angle of x-axis positive axis isNamely working end coordinate is: