CN101648376A - Method and device for controlling robot operations - Google Patents

Abstract

The invention discloses a method and a device for controlling robot operations, belonging to field of robots. The method comprises the following steps: 1. taking a kinematical equation as a target matrix, setting an expected matrix according to mission requirements, appointing a joint variable in a plurality of joint variables of a robot, and selecting a value from a joint topological space as a current value for starting iteration; 2. calculating other joint variables according to the equivalent relation of two matrixes and taking all calculated joint variables as a set of solutions of inverse kinematics; and 3. judging whether the iteration is finished or not, if so, utilizing the obtained solutions to control the robot operations, and otherwise, selecting a value from the joint topological space as a current value as an appointed joint variable according to the step length and executing the step 2. The device comprises an initiating module and an iteration module. The invention improves the solving speed of inverse kinematics and the controlling speed the robot operations, reduces the consumed time, improves the efficiency and has better universality.

Description

Control robot method of operating and device

Technical field

The present invention relates to the robot field, particularly a kind of control robot method of operating and device.

Background technology

The free degree is less than six robot and is called the lower-mobility robot.The lower-mobility robot can not carry out location, locus and attitude adjustment completely, but under some specific working condition, such as medical puncturing, arc-welding, spot welding, spraying, carrying, gluing operation etc., lower-mobility can satisfy mission requirements, and with respect to six degree of freedom even have the robot of redundant degree of freedom, the lower-mobility industrial robot is having more advantage aspect configuration design and the control.Therefore, at concrete applied environment, can comprehensively go out different mechanical arm configurations.

Robot is being carried out in the process of motion planning and TRAJECTORY CONTROL, it is the basis that inverse kinematics is found the solution.For inverse kinematics was found the solution, ideal situation was can access sealing to separate.Sealing is separated and is meant to have separating of analytical form, or refers to just can find the solution fully without iteration for the multinomial that is not higher than four times.Because with the algebraic equation solving of quadravalence (or being lower than quadravalence) is quite simple, therefore, the free degree is smaller or equal to four robot configuration complexity how no matter, and the sealing that always can obtain robot is separated.

But for the five degree of freedom or the robot of high-freedom degree more, then need to satisfy after certain configuration requires, its sealing is separated just and can be existed.As for the free degree be six and kinematic pair be the mechanical arm of revolute pair, the adequate condition that exists sealing to separate is that three adjacent joints axes intersect at a point or are parallel to each other.At present, find the solution at the inverse kinematics of robot, have a kind of general sealing to separate algorithm, still this algorithm can only be found the solution the robot architecture that analytic solutions are arranged, but then can't find the solution the robot architecture who does not have analytic solutions.In addition, also having a kind of numerical algorithm, is to search in the joint manifold according to the inverse kinematics equation, up to find separate till, this algorithm length that expends time in, efficient is lower, thereby has influenced the motion planning and the TRAJECTORY CONTROL of robot.

Summary of the invention

In order to solve the problem that prior art exists, the embodiment of the invention provides a kind of control robot method of operating and device.Described technical scheme is as follows:

A kind of control robot method of operating, described method comprises:

Step 1: with the kinematical equation of robot as objective matrix, and the expectation matrix is set according to the mission requirements of robot, in a plurality of joint variables of robot, specify a joint variable, from the manifold of joint, take out a value as the currency of specifying joint variable, the beginning iteration;

Step 2: the relation that equates with objective matrix according to described expectation matrix, calculate other joint variable in described a plurality of joint variable according to the currency of described appointment joint variable, current all joint variables of obtaining a group as inverse kinematics is separated;

Step 3: according to default step-length and joint manifold, judge whether iteration finishes, if, then use the operation of separating the control robot of the inverse kinematics that obtains, otherwise, according to described step-length once more from the manifold of joint value as the currency of described appointment joint variable, execution in step 2.

Current all joint variables of obtaining a group as inverse kinematics is separated, is specifically comprised:

When described other joint variable has trigonometric function value, judge this trigonometric function value whether in the trigonometric function manifold of joint variable, if, then current all joint variables of obtaining a group as inverse kinematics is separated, execution in step 3 then; Otherwise, direct execution in step 3.

The operation of separating the control robot with the inverse kinematics that obtains specifically comprises:

According to default required precision and working space requirement, many groups of the inverse kinematics that obtains are separated filter;

The operation of separating the control robot with the inverse kinematics that obtains after filtering.

The artificial robot with five degrees of freedom of described machine is specified a joint variable in a plurality of joint variables of robot, specifically comprise:

Make the sine value of the anglec of rotation in the 4th joint of described robot, all non-vanishing with the sine value of the anglec of rotation in the 5th joint of described robot, the anglec of rotation in described the 4th joint is set to the joint variable of appointment, and the anglec of rotation in perhaps described the 5th joint is set to the joint variable of appointment.

The artificial robot with five degrees of freedom of described machine is specified a joint variable in a plurality of joint variables of robot, specifically comprise:

The sine value of the anglec of rotation that makes the 4th joint of described robot is zero, and the anglec of rotation in the 5th joint of described robot is set to the joint variable of appointment;

Perhaps, the sine value of the anglec of rotation that makes the 5th joint of described robot is zero, and the anglec of rotation in the 4th joint of described robot is set to the joint variable of appointment.

A kind of device of controlling the robot manipulation, described device comprises:

Initialization module, be used for kinematical equation with robot as objective matrix, and the expectation matrix is set according to the mission requirements of robot, in a plurality of joint variables of robot, specify a joint variable, from the manifold of joint, take out a value as the currency of specifying joint variable, the beginning iteration;

Iteration module, be used for the relation that equates with objective matrix according to described expectation matrix, calculate other joint variable in described a plurality of joint variable according to the currency of described appointment joint variable, current all joint variables of obtaining a group as inverse kinematics is separated, according to default step-length and joint manifold, judge whether iteration finishes, if, then use the operation of separating the control robot of the inverse kinematics that obtains, otherwise, according to described step-length once more from the manifold of joint value continue iteration as the currency of described appointment joint variable.

Described iteration module is separated current all joint variables of obtaining a group as inverse kinematics, specifically comprises:

When described other joint variable has trigonometric function value, judge that this trigonometric function value is whether in the trigonometric function manifold of joint variable, if, then current all joint variables of obtaining a group as inverse kinematics is separated, carry out then and judge the step whether iteration finishes; Otherwise, directly carry out and judge the step whether iteration finishes.

The operation of separating the control robot of the inverse kinematics that obtains of described iteration module specifically comprises:

According to default required precision and working space requirement, many groups of the inverse kinematics that obtains are separated filter;

The operation of separating the control robot with the inverse kinematics that obtains after filtering.

The artificial robot with five degrees of freedom of described machine, described initialization module are specified a joint variable in a plurality of joint variables of robot, specifically comprise:

Make the sine value of the anglec of rotation in the 4th joint of described robot, all non-vanishing with the sine value of the anglec of rotation in the 5th joint of described robot, the anglec of rotation in described the 4th joint is set to the joint variable of appointment, and the anglec of rotation in perhaps described the 5th joint is set to the joint variable of appointment.

The artificial robot with five degrees of freedom of described machine, described initialization module are specified a joint variable in a plurality of joint variables of robot, specifically comprise:

The sine value of the anglec of rotation that makes the 4th joint of described robot is zero, and the anglec of rotation in the 5th joint of described robot is set to the joint variable of appointment;

Perhaps, the sine value of the anglec of rotation that makes the 5th joint of described robot is zero, and the anglec of rotation in the 4th joint of described robot is set to the joint variable of appointment.

Said method that the embodiment of the invention provides and device, adopt the mode that combines with numerical solution of finding the solution of resolving, obtain separating of robot inverse kinematics, according to the operation of solving result control robot, thereby make robot end's pose meet the requirements, its iteration direction is clear and definite, inverse kinematics speed of finding the solution and the speed of controlling the robot manipulation have been improved, reduce consumed time, improved efficient, gone for the robot of any free degree.And, overcome the defective that the kinematics of can not inverting to the robot architecture who does not have analytic solutions in the prior art is separated, and pure values algorithm search scope is big, defective long, inefficiency expends time in.In addition, the technical scheme that the embodiment of the invention provides both had been applicable to the robot architecture of analytic solutions, also was applicable to the robot architecture who does not have analytic solutions, had good versatility.

Description of drawings

Fig. 1 is the control robot method of operating flow chart that the embodiment of the invention provides;

Fig. 2 is the five degree-of-freedom manipulator structural representation that the embodiment of the invention provides;

Fig. 3 is the connecting rod coordinate schematic diagram of five degree-of-freedom manipulator shown in Figure 2;

Fig. 4 is the control robot with five degrees of freedom method of operating flow chart that the embodiment of the invention provides;

Fig. 5 is the control robot manipulation's that provides of the embodiment of the invention a structure drawing of device.

The specific embodiment

For making the purpose, technical solutions and advantages of the present invention clearer, embodiment of the present invention is described further in detail below in conjunction with accompanying drawing.

Referring to Fig. 1, the embodiment of the invention provides a kind of control robot method of operating, comprising:

Step 101: with the kinematical equation of robot as objective matrix, and the expectation matrix is set according to the free degree of robot, in a plurality of joint variables of robot, specify a joint variable, from the manifold of joint, take out a value as the currency of specifying joint variable, the beginning iteration;

Step 102: according to the relation that equates with objective matrix of expectation matrix, calculate other joint variable in a plurality of joint variables, current all joint variables of obtaining a group as inverse kinematics is separated according to the currency of appointment joint variable;

Step 103: according to default step-length and joint manifold, judge whether iteration finishes, if then execution in step 104, otherwise, execution in step 105;

Step 104: with the operation of separating the control robot of the inverse kinematics that obtains, flow process finishes.

Step 105: according to step-length once more from the manifold of joint value as the currency of specifying joint variable, execution in step 102.

Wherein, current all joint variables of obtaining a group as inverse kinematics is separated, can specifically be comprised:

When other joint variable has trigonometric function value, judge this trigonometric function value whether in the trigonometric function manifold of joint variable, if, then current all joint variables of obtaining a group as inverse kinematics is separated, execution in step 103 then; Otherwise, direct execution in step 103.

The said method that the embodiment of the invention provides can be applied to the robot of any free degree, as four-degree-of-freedom robot, robot with five degrees of freedom, six-DOF robot or the like.Preferably, be more suitable in robot with five degrees of freedom.Be example with the robot with five degrees of freedom below, specify.When to the robot application said method of other free degree, its operation principle is identical, difference only is that it is different that the positive motion of the robot of the different frees degree is learned equation, computational process is similar, it all is the relation that equates with objective matrix according to the expectation matrix, adopt numerical solution to find the solution the mode that combines and calculate, do not give unnecessary details herein with parsing.

Referring to Fig. 2, the five degree-of-freedom manipulator schematic diagram that provides for the embodiment of the invention.The connecting rod coordinate of this mechanical arm as shown in Figure 3.The D-H parameter value of this mechanical arm is as shown in table 1.

Table 1

The joint	??θ	??d	??l	??α	Joint variable
						??1	??0	??d1	??0	??0	??d 1
??2	??0	??0	??l 2	??0	??θ 2
						??3	??90	??d 3	??0	??90	??θ 3
??4	??0	??d 4	??0	??-90	??θ 4
						??5	??0	??d 5	??l 5	??90	??θ 5
??6	??0	??d 6	??0	??0	Do not have

Wherein, θ is the anglec of rotation of rod member, and d is the distance of rod member, and l is the length of rod member, and α is the torsion angle of rod member.The 6th joint in the table is the end of mechanical arm, is known in the process that the inverse kinematics of present embodiment is found the solution, and promptly carries out inverse kinematics according to the known posture information of this end and finds the solution, and obtains above-mentioned five joint variable: d ₁, θ ₂, θ ₃, θ ₄And θ ₅Except that these five joint variables, other parameter in the table all is known.Referring to Fig. 4, according to the parameter in the table 1 robot with five degrees of freedom being carried out inverse kinematics, to find the solution and control robot manipulation's process specific as follows:

Step 201: according to the parameter value in the table 1, the positive motion equation of listing this five degree-of-freedom manipulator is as follows:

$T_6_0=\left[\begin{array}{cccc}{c}_{123}{c}_4{c}_5-s_{123}{s}_5& {-c}_{123}{s}_4& c_{123}{c}_4{s}_5+s_{123}{c}_5& (c_{123}{c}_4{s}_5+s_{123}{c}_5)d_6+(c_{123}{c}_4{c}_5-s_{123}{s}_5)l_5-c_{123}{s}_4{d}_5+s_{123}{d}_4+c_{12}{l}_2\\ s_{123}{c}_4{c}_5+c_{123}{s}_5& -s_{123}{s}_4& s_{123}{c}_4{s}_5-c_{123}{c}_5& (s_{123}{c}_4{s}_5-c_{123}{c}_5)d_6+(s_{123}{c}_4{c}_5+c_{123}{s}_5)l_5-s_{123}{s}_4{d}_5-c_{123}{d}_4+s_{12}{l}_2\\ s_4{c}_5& c_4& s_4{s}_5& s_4{s}_5{d}_6+s_4{c}_5{l}_5+c_4{d}_5+d_3+d_1\\ 0& 0& 0& 1\end{array}\right]$

Above-mentioned positive motion is learned equation as objective matrix, wherein, ⁰T ₆The posture information of expression mechanical arm end, other parameter is respectively the sine or the cosine function of all angles:

c ₄＝cosθ ₄；

c ₅＝cosθ ₅；

s ₄＝sinθ ₄；

s ₅＝sinθ ₅；

s ₁₂＝sin(θ ₁+θ ₂)；

c ₁₂＝cos(θ ₁+θ ₂)；

c ₁₂₃＝cos(θ ₁+θ ₂+θ ₃)；

s ₁₂₃＝sin(θ ₁+θ ₂+θ ₃)。

Step 202: obtain robot end's posture information, comprising: position and attitude.According to the mission requirements of robot with five degrees of freedom, promptly Mo Duan expected pose is listed robot end's expectation matrix, is expressed as follows:

$X=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$

Wherein, the terminal pose of robot can know in advance according to mission requirements, as allows mechanical arm take a cup, and then the terminal pose of mechanical arm is the position and the attitude of mechanical arm correspondence when taking cup.

For robot with five degrees of freedom, can know the 3rd in above-mentioned expectation matrix row and the 4th column information according to the pose of end, therefore, according to the 3rd row and the 4th column information and expect that the relation that matrix equates with objective matrix finds the solution.

Step 203: be listed as equal relation as can be known by expectation matrix and objective matrix the third line the 3rd:

s ₄s ₅＝a _z????(1)

For convenience of calculation, present embodiment is considered two kinds of situations: i.e. s ₄s ₅Non-vanishing situation, and s ₄s ₅Be zero situation, for first kind of situation, execution in step 204; For second kind of situation, execution in step 211.

Step 204:s ₄s ₅Non-vanishing situation has multiple, as s ₄s ₅〉=10 ^-5, present embodiment is not done concrete qualification, at this moment, and can be with the anglec of rotation θ in robot the 4th joint ₄Be set to the joint variable of appointment, i.e. s ₄Be the variable of appointment, perhaps with the anglec of rotation θ in the 5th joint ₅Be set to the joint variable of appointment, i.e. s ₅Be the variable of appointment, present embodiment is particularly with s ₅For the variable of appointment describes, if with s ₄Be the variable of appointment, its computational process is identical, does not give unnecessary details herein.

According to the joint manifold, make s ₅In default range of variables, carry out value, can be according to the span of SIN function with s ₅Range of variables be set to (1,1) interval, appoint in this interval earlier and get a value as s ₅Currency, the beginning iteration.

Step 205: by s ₅Currency can directly draw the anglec of rotation in the 5th joint:

θ ₅＝±arcsin(s ₅)????(2)

Can obtain by formula (1):

$s_4=\frac{a_z}{s_5}---\left(3\right)$

According to s ₅Currency and known parameters a _z, can obtain the anglec of rotation in the 4th joint:

${\mathrm{\θ}}_4=\±\arcsin \left(\frac{a_z}{s_5}\right)---\left(4\right)$

In the present embodiment, in order to improve the accuracy that inverse kinematics is found the solution, further, can also be when joint variable have trigonometric function value, judge that this trigonometric function value is whether in the trigonometric function manifold of joint variable, if, then have legitimacy, with the joint variable of correspondence as the result who finds the solution, otherwise, do not have legitimacy, then abandon the current joint variable of obtaining.

In this step, can judge the sine value s in the 4th joint of robot earlier ₄Whether in (1,1) interval, if, then with the θ of correspondence ₄As the anglec of rotation result in the 4th joint, otherwise, the current θ that obtains abandoned ₄Value, the process of end current iteration continues next iteration.

Step 206: this step is calculated the anglec of rotation in second joint and the 3rd joint, and is specific as follows:

Equate with the 1st row the 3rd row of objective matrix according to the expectation matrix, and the 2nd row the 3rd row are equal, can list:

c ₁₂₃c ₄s ₅+s ₁₂₃c ₅＝a _x

s ₁₂₃c ₄s ₅-c ₁₂₃c ₅＝a _y????(5)

θ as shown in Table 1 ₁=0, therefore,

c ₁₂₃＝cos(θ ₁+θ ₂+θ ₃)＝cos(θ ₂+θ ₃)

s ₁₂₃＝sin(θ ₁+θ ₂+θ ₃)＝sin(θ ₂+θ ₃)????(6)

Make c ₂₃=cos (θ ₂+ θ ₃), s ₂₃=sin (θ ₂+ θ ₃), then formula (6) can be expressed as follows:

c ₁₂₃＝c ₂₃

s ₁₂₃＝s ₂₃????(7)

With formula (7) substitution formula (5), can obtain:

c ₂₃c ₄s ₅+s ₂₃c ₅＝a _x

s ₂₃c ₄s ₅-c ₂₃c ₅＝a _y????(8)

After above-mentioned two formulas merging, can obtain:

a _xc ₂₃+a _ys ₂₃＝c ₄s ₅????(9)

With above-mentioned formula divided by

And order $\sin {\mathrm{\ϵ}}_1=\frac{a_x}{\sqrt{(a_x^2+a_y^2)}},$ Wherein, ε ₁Be intermediate variable, it is worth according to a _xAnd a _yCan obtain, then can obtain:

$\sin ({\mathrm{\ϵ}}_1+{\mathrm{\θ}}_2+{\mathrm{\θ}}_3)=\frac{c_4{s}_5}{\sqrt{(a_x^2+a_y^2)}}$

${\mathrm{\θ}}_2+{\mathrm{\θ}}_3=\±\arcsin \left(\frac{c_4{s}_5}{\sqrt{(a_x^2+a_y^2)}}\right)-{\mathrm{\ϵ}}_1---\left(10\right)$

Then can obtain:

${\mathrm{<figure-callout\; id="23"\; label="s"\; filenames="A2009100923970019H2.png"\; state="\{\{state\}\}">s</figure-callout>}}_{23}=\sin ({\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3)=\sin (\&PlusMinus;\arcsin \left(\frac{c_4{s}_5}{\sqrt{(a_x^2+a_y^2)}}\right)-{\mathrm{\&epsiv;}}_1)$

${\mathrm{<figure-callout\; id="23"\; label="c"\; filenames="A2009100923970019H2.png"\; state="\{\{state\}\}">c</figure-callout>}}_{23}=\cos ({\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3)=\cos (\&PlusMinus;\arcsin \left(\frac{c_4{s}_5}{\sqrt{(a_x^2+a_y^2)}}\right)-{\mathrm{\&epsiv;}}_1)---\left(11\right)$

Equate as can be known with the 2nd row the 4th row of objective matrix by the expectation matrix:

(s ₁₂₃c ₄s ₅-c ₁₂₃c ₅)d ₆+(s ₁₂₃c ₄c ₅+c ₁₂₃s ₅)l ₅-s ₁₂₃s ₄d ₅-c ₁₂₃d ₄+s ₁₂l ₂＝p _y

And, θ _q=0, therefore

$s_2=s_{12}=\frac{p_y-(s_{123}{c}_4{s}_5-c_{123}{c}_5)d_6-(s_{123}{c}_4{c}_5+c_{123}{s}_5)l_5+s_{123}{s}_{4}d+c_{123}{d}_4}{l_2}$

$=\frac{p_y-(s_{23}{c}_4{s}_5-c_{23}{c}_5)d_6-(s_{23}{c}_4{c}_5+c_{23}{s}_5)l_5+s_{23}{s}_{4}d+c_{23}{d}_4}{l_2}$

Wherein, s ₂₃And c ₂₃Value by formula (11) as can be known, and other parameter is known value, therefore, can calculate the anglec of rotation of second joint:

${\mathrm{\&theta;}}_2=\&PlusMinus;\arcsin \left(\frac{p_y-(s_{23}{c}_4{s}_5-c_{23}{c}_5)d_6-(s_{23}{c}_4{c}_5+c_{23}{s}_5)l_5+s_{23}{s}_{4}d+c_{23}{d}_4}{l_2}\right)---\left(12\right)$

Can obtain the anglec of rotation in the 3rd joint in conjunction with formula (10):

${\mathrm{\&theta;}}_3=\&PlusMinus;\arcsin \left(\frac{c_4{s}_5}{\sqrt{(a_x^2+a_y^2)}}\right)-{\mathrm{\&epsiv;}}_1-(\&PlusMinus;\arcsin \left(\frac{p_y-(s_{23}{c}_4{s}_5-c_{23}{c}_5)d_6-(s_{23}{c}_4{c}_5+c_{23}{s}_5)l_5+s_{23}{s}_{4}d+c_{23}{d}_4}{l_2}\right))---\left(12\right)$

In this step, the process of checking with legitimacy in the step 205 is similar, also can judge the sine value s of the second joint of robot earlier ₂Whether in (1,1) interval, if, then with the θ of correspondence ₂As the anglec of rotation result of second joint, otherwise, the current θ that obtains abandoned ₂Value, the process of end current iteration continues next iteration.

Step 207:, list according to expectation matrix and the relation that the 3rd row the 4th row of objective matrix equate:

s ₄s ₅d ₆+s ₄c ₅l ₅+c ₄d ₅+d ₃+d ₁＝p _z????(13)

Therefore, can calculate the distance of the first joint rod member:

d ₁＝p _z-(s ₄s ₅d ₆+s ₄c ₅l ₅+c ₄d ₅+d ₃)????(14)

Step 208: five joint variable: d that aforementioned calculation is obtained ₁, θ ₂, θ ₃, θ ₄And θ ₅, separate as a group of inverse kinematics, can also store this group and separate, according to default step-length and joint manifold, judge whether iteration finishes then, if end, then execution in step 209, otherwise, execution in step 210.

Wherein, default step-length can be provided with and change as required, as default step-length is 1 °, 2 ° or the like, judging whether iteration finishes can be specific as follows: go out the total degree of iteration according to joint manifold and default step size computation, thereby whether the number of times of judging current iteration reaches the total degree that this calculates, if reach, then iteration finishes, otherwise, do not finish, continue iteration.According to the primary value of named variable, default step-length can be got incremental manner or the mode of successively decreasing changes.

For example, the joint manifold is (1 ,+1), and the anglec of rotation in corresponding the 5th joint is (0 a °, 360 °), and default step-length is 1 °, and then the iteration total degree is 360 times.If for the first time value is 0 °, then by step-length incremental manner value, if value is 360 ° for the first time, then by the step-length mode value of successively decreasing.

Step 209: with the operation of separating the control robot of the inverse kinematics that obtains, flow process finishes.

The robot manipulation who relates in the present embodiment mainly is meant and allows robot finish an action, as allows robot take book or the like.By the joint variable in each joint of robot of calculating, can control the action in each joint of robot, rotate 30 ° as second joint, the 3rd joint rotated 50 ° or the like, thereby can finish the operation of robot.

Further, in order to improve the precision and the efficient of robot control, before the control robot manipulation, can also be according to default required precision and working space requirement, many groups of the inverse kinematics that obtains are separated filter, filter out separating of satisfactory inverse kinematics, then with the operation of controlling robot of separating of the inverse kinematics that obtains after filtering.Wherein, separating of the inverse kinematics after the filtration can be one group, also can be many groups.Required precision and space requirement are exemplified below: require the corner in certain joint of robot must not surpass (30 °, 30 °), the distance of the connecting rod in first joint must not surpass 10cm or the like.

Step 210: get a value as s from (1,1) interval once more according to default step-length ₅Currency, continue iteration, execution in step 205.

Step 211: for s ₄s ₅Be zero situation, in the present embodiment, can comprise that also some are approximately zero situation, as s ₄s ₅＜10 ^-5Or the like, present embodiment is not done concrete qualification, and emphasis is described s ₄And s ₅Wherein have at least one to be zero situation.Wherein, make the anglec of rotation θ in the 4th joint of robot ₄Sine value be zero, i.e. s ₄=0, then with the anglec of rotation θ in the 5th joint of robot ₅Be set to the joint variable of appointment, i.e. s ₅Variable for appointment; Perhaps make the anglec of rotation θ in the 5th joint of robot ₅Sine value be zero, i.e. s ₅=0, then with the anglec of rotation θ in the 4th joint of robot ₄Be set to the joint variable of appointment, i.e. s ₄Be the variable of appointment, present embodiment is particularly with s ₄For the variable of appointment describes, if with s ₅Be the variable of appointment, its computational process is identical, does not give unnecessary details herein.

Because s ₅=0, c as can be known ₅=± 1, according to the joint manifold, make s ₄In default range of variables, carry out value, can be according to the span of SIN function with s ₄Range of variables be set to (1,1) interval, appoint in this interval earlier and get a value as s ₄Currency, the beginning iteration.

Step 212: the anglec of rotation in this step calculating robot's the 4th joint, the 5th joint, second joint and the 3rd joint, specific as follows: by s ₄Currency can directly draw the anglec of rotation in the 4th joint:

θ ₄＝±arcsin(s ₄)????(15)

According to s ₅=0, can draw the anglec of rotation in the 5th joint:

θ ₅＝±arcsin(0)????(16)

According to step 206 in identical method obtain formula (8), with above-mentioned relation s ₅=0 and c ₅=± 1 substitution formula (8) can obtain:

${\mathrm{<figure-callout\; id="23"\; label="s"\; filenames="A2009100923970019H2.png"\; state="\{\{state\}\}">s</figure-callout>}}_{23}=\frac{a_x}{c_5}---\left(17\right)$

${\mathrm{<figure-callout\; id="23"\; label="c"\; filenames="A2009100923970019H2.png"\; state="\{\{state\}\}">c</figure-callout>}}_{23}=-\frac{a_y}{c_5}$

Can obtain after above-mentioned two formulas are divided by:

${\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3=a\tan 2\frac{s_{23}}{c_{23}}=a\tan 2(-\frac{a_x}{a_y})---\left(18\right)$

The 1st row the 4th row according to expectation matrix and objective matrix equate, and the 2nd row the 4th row are equal, can list:

(c ₁₂₃c ₄s ₅+s ₁₂₃c ₅)d ₆+(c ₁₂₃c ₄c ₅-s ₁₂₃s ₅)l ₅-c ₁₂₃s ₄d ₅+s ₁₂₃d ₄+c ₁₂l ₂＝p _x

(s ₁₂₃c ₄s ₅-c ₁₂₃c ₅)d ₆+(s ₁₂₃c ₄c ₅+c ₁₂₃s ₅)l ₅-s ₁₂₃s ₄d ₅-c ₁₂₃d ₄+s ₁₂l ₂＝p _y

Because θ ₁=0, s ₅=0 and c ₅=± 1, can the substitution following formula, with formula (17) also substitution following formula, then following formula can be converted to following expression simultaneously:

$c_2=\frac{p_x-a_x{d}_6+a_y{c}_4{l}_5-\frac{a_y}{c_5}{s}_4{d}_5-\frac{a_x}{c_5}{d}_4}{l_2}---\left(19\right)$

$s_2=\frac{p_y-a_y{d}_6-a_x{c}_4{l}_5+\frac{a_x}{c_5}{s}_4{d}_5-\frac{a_y}{c_5}{d}_4}{l_2}$

Thereby, can obtain the anglec of rotation of the second joint of robot:

${\mathrm{\&theta;}}_2=a\tan 2\frac{s_2}{c_2}---\left(20\right)$

In this step, the process of checking with legitimacy in the step 205 is similar, also can judge the sine value s of the second joint of robot earlier ₂With cosine value c ₂Whether legal, particularly, can judge s ₂And c ₂Whether all in (1,1) interval, and judge s ₂ ²+ c ₂ ²Value whether equal 1, if these conditions all satisfy, then think to have legitimacy, with the θ of correspondence ₂As the anglec of rotation result of second joint, otherwise, the current θ that obtains abandoned ₂Value, the process of end current iteration continues next iteration.

Can draw the anglec of rotation in the 3rd joint of robot in conjunction with formula (18) and (20):

${\mathrm{\&theta;}}_3=({\mathrm{\&theta;}}_2+{\mathrm{\&theta;}}_3)-{\mathrm{\&theta;}}_2=a\tan 2(-\frac{a_x}{a_y})-a\tan 2\frac{s_2}{c_2}---\left(21\right)$

Step 213:, list according to expectation matrix and the relation that objective matrix the 3rd row the 4th row equate:

s ₄s ₅d ₆+s ₄c ₅l ₅+c ₄d ₅+d ₃+d ₁＝p _z

Because s ₅=0, other parameter is all known, therefore, can calculate the distance of connecting rod in first joint of robot:

d ₁＝p _z-s ₄c ₅l ₅-c ₄d ₅-d ₃????(22)

Step 214: five joint variable: d that aforementioned calculation is obtained ₁, θ ₂, θ ₃, θ ₄And θ ₅, separate as a group of inverse kinematics, can also store this group and separate, according to default step-length and joint manifold, judge whether iteration finishes then, if end, then execution in step 215, otherwise, execution in step 216.

Wherein, default step-length can be provided with and change as required, judges whether iteration finishes with the description in the step 208.According to the primary value of named variable, default step-length can be got incremental manner or the mode of successively decreasing changes.

Step 215: with the operation of separating the control robot of the inverse kinematics that obtains, flow process finishes.

Further, in order to improve the precision and the efficient of robot control, before the control robot manipulation, can also be according to default required precision and working space requirement, many groups of the inverse kinematics that obtains are separated filter, filter out the separating of inverse kinematics of symbol requirement, then with the operation of controlling robot of separating of the inverse kinematics that obtains after filtering.Wherein, separating of the inverse kinematics after the filtration can be one group, also can be many groups.Required precision and space requirement are exemplified below: require the corner in certain joint of robot must not surpass (30 °, 30 °), the distance of the connecting rod in first joint must not surpass 10cm or the like.

Step 216: get a value as s from (1,1) interval once more according to default step-length ₄Currency, continue iteration, execution in step 212.

Referring to Fig. 5, the embodiment of the invention also provides a kind of robot manipulation's of control device, comprising:

Initialization module 301, be used for kinematical equation with robot as objective matrix, and the expectation matrix is set according to the mission requirements of robot, in a plurality of joint variables of robot, specify a joint variable, from the manifold of joint, take out a value as the currency of specifying joint variable, the beginning iteration;

Iteration module 302, be used for according to the relation of expecting that matrix equates with objective matrix, calculate other joint variable in a plurality of joint variables according to the currency of specifying joint variable, current all joint variables of obtaining a group as inverse kinematics is separated, according to default step-length and joint manifold, judge whether iteration finishes, if, then use the operation of separating the control robot of the inverse kinematics that obtains, otherwise, according to step-length once more from the manifold of joint value continue iteration as the currency of specifying joint variable.

Wherein, iteration module 302 is separated current all joint variables of obtaining a group as inverse kinematics, specifically comprises:

When other joint variable has trigonometric function value, judge that this trigonometric function value is whether in the trigonometric function manifold of joint variable, if, then current all joint variables of obtaining a group as inverse kinematics is separated, carry out then and judge the step whether iteration finishes; Otherwise, directly carry out and judge the step whether iteration finishes.

In the present embodiment, the operation of separating the control robot of the inverse kinematics that iteration module 302 usefulness obtain specifically comprises:

According to default required precision and working space requirement, many groups of the inverse kinematics that obtains are separated filter;

The operation of separating the control robot with the inverse kinematics that obtains after filtering.

In the present embodiment, when the artificial robot with five degrees of freedom of machine, initialization module 301 is specified a joint variable in a plurality of joint variables of robot, can specifically comprise:

Make the sine value of the anglec of rotation in the 4th joint of robot, all non-vanishing with the sine value of the anglec of rotation in the 5th joint of robot, the anglec of rotation in the 4th joint is set to the joint variable of appointment, and perhaps the anglec of rotation in the 5th joint is set to the joint variable of appointment.

Perhaps, specifically comprise:

The sine value of the anglec of rotation that makes the 4th joint of robot is zero, and the anglec of rotation in the 5th joint of robot is set to the joint variable of appointment; Or the sine value of the anglec of rotation that makes the 5th joint of robot is zero, and the anglec of rotation in the 4th joint of robot is set to the joint variable of appointment.

Said method that the embodiment of the invention provides and device, adopt the mode that combines with numerical solution of finding the solution of resolving, obtain separating of robot inverse kinematics, according to the operation of solving result control robot, thereby make robot end's pose meet the requirements, its iteration direction is clear and definite, inverse kinematics speed of finding the solution and the speed of controlling the robot manipulation have been improved, reduce consumed time, improved efficient, gone for the robot of any free degree.And, overcome the defective that the kinematics of can not inverting to the robot architecture who does not have analytic solutions in the prior art is separated, and pure values algorithm search scope is big, defective long, inefficiency expends time in.In addition, the technical scheme that the embodiment of the invention provides both had been applicable to the robot architecture of analytic solutions, also was applicable to the robot architecture who does not have analytic solutions, had good versatility.

The all or part of of the technique scheme that the embodiment of the invention provides can be finished by the relevant hardware of programmed instruction, described program can be stored in the storage medium that can read, and this storage medium comprises: various media that can be program code stored such as ROM, RAM, magnetic disc or CD.

The above only is preferred embodiment of the present invention, and is in order to restriction the present invention, within the spirit and principles in the present invention not all, any modification of being done, is equal to replacement, improvement etc., all should be included within protection scope of the present invention.

Claims (10)

1, a kind of control robot method of operating is characterized in that described method comprises:

Step 1: with the kinematical equation of robot as objective matrix, and the expectation matrix is set according to the mission requirements of robot, in a plurality of joint variables of robot, specify a joint variable, from the manifold of joint, take out a value as the currency of specifying joint variable, the beginning iteration;

Step 2: the relation that equates with objective matrix according to described expectation matrix, calculate other joint variable in described a plurality of joint variable according to the currency of described appointment joint variable, current all joint variables of obtaining a group as inverse kinematics is separated;

Step 3: according to default step-length and joint manifold, judge whether iteration finishes, if, then use the operation of separating the control robot of the inverse kinematics that obtains, otherwise, according to described step-length once more from the manifold of joint value as the currency of described appointment joint variable, execution in step 2.

2, method according to claim 1 is characterized in that, current all joint variables of obtaining a group as inverse kinematics is separated, and specifically comprises:

When described other joint variable has trigonometric function value, judge this trigonometric function value whether in the trigonometric function manifold of joint variable, if, then current all joint variables of obtaining a group as inverse kinematics is separated, execution in step 3 then; Otherwise, direct execution in step 3.

3, method according to claim 1 is characterized in that, the operation of separating the control robot with the inverse kinematics that obtains specifically comprises:

According to default required precision and working space requirement, many groups of the inverse kinematics that obtains are separated filter;

The operation of separating the control robot with the inverse kinematics that obtains after filtering.

According to claim 1,2 or 3 described methods, it is characterized in that 4, the artificial robot with five degrees of freedom of described machine is specified a joint variable in a plurality of joint variables of robot, specifically comprise:

Make the sine value of the anglec of rotation in the 4th joint of described robot, all non-vanishing with the sine value of the anglec of rotation in the 5th joint of described robot, the anglec of rotation in described the 4th joint is set to the joint variable of appointment, and the anglec of rotation in perhaps described the 5th joint is set to the joint variable of appointment.

According to claim 1,2 or 3 described methods, it is characterized in that 5, the artificial robot with five degrees of freedom of described machine is specified a joint variable in a plurality of joint variables of robot, specifically comprise:

The sine value of the anglec of rotation that makes the 4th joint of described robot is zero, and the anglec of rotation in the 5th joint of described robot is set to the joint variable of appointment;

Perhaps, the sine value of the anglec of rotation that makes the 5th joint of described robot is zero, and the anglec of rotation in the 4th joint of described robot is set to the joint variable of appointment.

6, a kind of device of controlling the robot manipulation is characterized in that, described device comprises:

Initialization module, be used for kinematical equation with robot as objective matrix, and the expectation matrix is set according to the mission requirements of robot, in a plurality of joint variables of robot, specify a joint variable, from the manifold of joint, take out a value as the currency of specifying joint variable, the beginning iteration;

Iteration module, be used for the relation that equates with objective matrix according to described expectation matrix, calculate other joint variable in described a plurality of joint variable according to the currency of described appointment joint variable, current all joint variables of obtaining a group as inverse kinematics is separated, according to default step-length and joint manifold, judge whether iteration finishes, if, then use the operation of separating the control robot of the inverse kinematics that obtains, otherwise, according to described step-length once more from the manifold of joint value continue iteration as the currency of described appointment joint variable.

7, device according to claim 6 is characterized in that, described iteration module is separated current all joint variables of obtaining a group as inverse kinematics, specifically comprises:

When described other joint variable has trigonometric function value, judge that this trigonometric function value is whether in the trigonometric function manifold of joint variable, if, then current all joint variables of obtaining a group as inverse kinematics is separated, carry out then and judge the step whether iteration finishes; Otherwise, directly carry out and judge the step whether iteration finishes.

8, device according to claim 6 is characterized in that, the operation of separating the control robot of the inverse kinematics that obtains of described iteration module specifically comprises:

According to default required precision and working space requirement, many groups of the inverse kinematics that obtains are separated filter;

The operation of separating the control robot with the inverse kinematics that obtains after filtering.

According to claim 6,7 or 8 described devices, it is characterized in that 9, the artificial robot with five degrees of freedom of described machine, described initialization module are specified a joint variable in a plurality of joint variables of robot, specifically comprise:

Make the sine value of the anglec of rotation in the 4th joint of described robot, all non-vanishing with the sine value of the anglec of rotation in the 5th joint of described robot, the anglec of rotation in described the 4th joint is set to the joint variable of appointment, and the anglec of rotation in perhaps described the 5th joint is set to the joint variable of appointment.

According to claim 6,7 or 8 described devices, it is characterized in that 10, the artificial robot with five degrees of freedom of described machine, described initialization module are specified a joint variable in a plurality of joint variables of robot, specifically comprise:

The sine value of the anglec of rotation that makes the 4th joint of described robot is zero, and the anglec of rotation in the 5th joint of described robot is set to the joint variable of appointment;

Perhaps, the sine value of the anglec of rotation that makes the 5th joint of described robot is zero, and the anglec of rotation in the 4th joint of described robot is set to the joint variable of appointment.

Images