JPH0580002B2 - - Google Patents

Description

[Detailed Description of the Invention] [Field of Industrial Application] The present invention provides a method in which, when a broken line is used as the path of a robot hand, the speed of the robot hand is made continuous so that the path of the robot hand becomes curved near the bending point. The present invention relates to a route interpolation method for a robot hand.

[Conventional technology]

One of the ways to control the robot's movement path is to
There is PTP (point to point) operation. The PTP operation is an operation that specifies a finite number of passing points on a route and moves the route according to the specified passing points.

Conventionally, when performing PTP operations continuously, a method is used in which the PTP operation is performed by passing each point (position) without stopping. At this time, the broken line portion from point to point is interpolated with a circular arc. For example, between two adjacent broken lines, the two broken lines are made continuous by a circular arc having a specified radius.

[Problem to be solved by the invention]

In such conventional examples, calculation of arcs based on polygonal lines is complicated, and as a result, a large amount of calculation time is required before the robot moves along the route, making it impossible to move the robot along the route in real time. It has disadvantages. Furthermore, it has the disadvantage that it is difficult to apply joint angle motion based on the coordinates of the movement of each joint of an articulated robot to speed up PTP motion.

An object of the present invention is to provide a route interpolation method that achieves high-speed PTP operation.

[Means to solve the problem]

In order to achieve the above object, there is provided a method for interpolating a broken line path of a robot hand, which includes a method for interpolating a broken line path of a robot hand, in which the robot hand moves from a point A on the broken line path in a direction to a breaking point B, and at the breaking point B, the velocity becomes zero. A composite velocity vector of the velocity vector and a second velocity vector that monotonically increases the velocity from zero to the direction of point C on the broken line path from the breaking point B is determined for each unit time, and is determined for each unit time. The robot hand is controlled by using the resultant vector added to the position vector of the robot hand at that point as a position command amount, and the path of the robot hand is interpolated so that the path of the robot hand becomes a curved line.

Further, the route is interpolated by using the first velocity vector as a velocity vector that monotonically decreases to zero at the corner point B.

[Effect]

The composite vector of the vector in the direction that monotonically decreases the velocity in the direction of the break point so that it becomes zero at the break point, and the vector in the direction that monotonically increases the speed from zero to the next point from the break point is the vector at the break point. By applying this to the robot hand before , the speed of the robot hand becomes continuous, and a curved path can be obtained just by combining two vectors, which makes calculations easier than the conventional method that connects two straight lines with a circular arc. Since it is easy, the processing time is short. Moreover, since the obtained route is continuous and does not pass through any turning points, the time required for moving the robot hand can be shortened.

〔Example〕

Hereinafter, the present invention will be explained in detail with reference to the drawings.

In Figure 1, given three points A, B, and C, A→B→C
FIG. 2 is an explanatory diagram of an example in which a robot's hand is moved along a route. These three points A, B, and C are values taught by teaching, and are stored in the memory as teaching data.

Assume that the movement is performed at a speed ν _a in between and at a speed ν _b in the interval, and this movement is performed in units of sampling time T. Therefore, if the number of sampling points in between is n _a and the number of sampling points in between is n _b , then
The relationship is n _a = [AB/ν _a T] ...(1) n _b = [BC/ν _b T] ...(2). The meaning of the bracket here is that, for example, in equation (1), n _a is a natural number in the range of AB/v _a T≦ _na < AB/v _a T+1 . . . (3). Therefore, the target point at each sampling point is
This is the point that divides AB into n _a equal parts and divides AB into n _b equal parts.

However, when the robot's hand is driven using this method, the speed becomes discontinuous at point B, making it impossible to drive.

Therefore, in the present invention, before reaching point B, point A
Before reaching point B, the speed is monotonically decreased from point A to point B so that it becomes zero when reaching point B, and from point B to point C, the speed is monotonically increased from zero. Interpolate the route as follows.

The basic idea of the present invention will be explained with reference to FIGS. 2 and 3.

As shown in Figure 2, the direction from point A to point B is a unit vector e _a , the direction from point B to point C is a unit vector e _b , and the speed in the constant speed region of the line segment is ν _a ,
Let the velocity in the constant velocity region between the line segments be ν _b . Furthermore, the deceleration time in the direction of the vector e _a and the acceleration time in the direction of the vector e _b are made the same, and their value is set to t _o . Furthermore, the monotonous decrease in velocity in the direction of vector e _a is expressed as constant deceleration a, vector
e Let the monotonous increase in velocity in _{the b} direction be constant acceleration b. Under such a relationship, a and b are as follows: a=ν _a /t _o ...(4) b=ν _b /t _o ...(5).

If point B is the position vector 〓 _b , the deceleration start point on the line segment is point D, its position vector is 〓 _d , the acceleration end point on the line segment is point E, and its position vector is 〓 _c , then the position vector 〓 _d , 〓 _c are as follows.

〓 _d ＝〓 _b −1/2at ² _o 〓 _a ＝〓 _b −1/2ν _a t _o 〓 _a ……
(6) 〓 _c =〓 _b +1/2bt ² _o 〓 _b =〓 _b +1/2ν _b t _o 〓 _b ……
(7) According to the above-mentioned historical interpolation, the path from A to D moves at a constant speed ν _a , the path from D to B decelerates according to the constant deceleration a, and at point B the speed is zero. Become. Next, on the path B→E, the speed increases continuously from zero according to the constant acceleration b, and reaches point E.
Then, the speed becomes constant ν _b . Next, on the path E→C, it moves at a constant speed ν _b .

In the above path movement of A→D→B→E→C, the velocity is continuous, and the velocity before and after the interpolation area is also continuous, so that smooth interpolation can be achieved.

The above is an example in which velocity interpolation is performed between D→B and B→E. Next, a case in which position interpolation is performed together with velocity interpolation will be described.

Figure 3 realizes velocity interpolation and position interpolation.
FIG. 3 is an explanatory diagram of PTP operation. Time is plotted on the horizontal axis and speed is plotted on the vertical axis. The characteristic l ₁ shows the speed characteristic from B→E→C, and the characteristic l ₂ shows the speed characteristic from A→D→B. However, the characteristics l ₁ and l ₂ are taken on the same time axis, and the PTP operation is performed on the time axis shown in the figure. That is,
The hand is moved at a constant speed ν _a during the interval.
At point D, vectors in two directions DB→, BE→ 〓 _a , 〓 _b
give. This vector gives for time t _o . Due to these two vectors 〓 _a and 〓 _b , the _movement route does not become D → B → E → C, but instead goes from D to E direction without going through _B. Moves while drawing a curve following the resultant vector. After t _o time, it reaches position E and moves at a constant speed ν _b during that time.

In Figure 4, vectors in two directions DB→, BE→ at point D
The moving route from point D to point E is shown when _a , = _b is given. The movement path from point D to point E is originally continuous, but since the movement is made according to the sampling points, the path is discontinuous. The idea of sampling points is to set the interval between, for example, 6 points D ₁ ,
D ₂ , D ₃ , D ₄ , D ₅ , B samples, 6 points in between
E ₁ ; E ₂ , E ₃ , E ₄ , E ₅ , E are sampled, and vectors e _a and e _b are given to each composite point of each sample point to form a composite vector. The path D → P ₁ → P ₂ → P ₃ → P ₄ → P ₅ → E formed by this composite vector becomes the composite point corresponding position of each sample point. The above route from D to E indicates a route whose position has been interpolated.

The position vector in the above position interpolation is as follows. Now, let the time at point D be t=0. Vector 〓 The velocity component in _{the a} direction is a(t _o −t), and the velocity component in _{the b} direction of vector 〓 is bt. Therefore, the moving component after time t is -1/2a(t _o -t) ² for the vector _〓a , and 1/2bt ² for the vector _〓b . As a result, the position vector P(t) between the points DE becomes: 〓(t)=〓 _b -1/2a(t _o -t) ² 〓 _a + 1/2bt ² 〓 _b (8). From this, finding the speed, d〓(t)/dt=a(t _o -t)〓 _a + bt〓 _b = at _o 〓 _a + (b
〓 _b −a〓 _a )t=ν _a 〓 _a +(ν _b 〓 _b −ν _a 〓 _a )t/t _o ...
…(9) becomes. The acceleration is d ² 〓(t)/dt ² = b 〓 _b − a 〓 _a = ν _b 〓 _b − ν _a 〓 _a / t _o = constant (10). From these equations (9) and (10), the speed is continuous between DEs, and the speeds before and after the interpolation area are continuous.

Next, another example of positional interpolation will be explained with reference to FIG.
FIG. 5 shows three paths 1, 2, 3. Route 1
is an example that has a path that is, path 2 is an example that has a path that is, and path 3 is an example that has a path that is HI. Here, point F is from point D to A
Therefore, let it be a point that has moved for a time t _a with a constant velocity ν _a . Furthermore, point H is a point that has moved from point B to point D by a time (t _o −t _b ) with acceleration a. Point D indicates the same point as point D shown in FIG.

(1) First, on route 1 from A→F→G→C, the following control is performed for each section.

(1-) interval, that is, t<0.

The reason why t<0 is set is that point F is set at time t=0. In this interval, the vector
e Move at a constant speed in _{the a} direction, vector e do not give a movement vector in _{the b} direction. As a result, the movement
The direction is AF→. The position vector 〓(t) in this section is 〓(t)=〓 _b − {a/2t ² _o + ν _a (t _a −t)}〓 _a ...(11). The speed is d〓(t)/dt=ν _a 〓 _a ...(12).

(1-) When the corresponding interval in FG→ corresponds to the interval, that is, 0≦t<t _a .

In this section, constant velocity ν _a movement is given in the vector e _a direction, and constant acceleration motion is given in the vector 〓 _b direction. As a result, the moving point causes a curved change in the direction of FG→. The position vector 〓(t) in this section is 〓(t)=〓 _b − {a/2t ² _o + ν _a (t _a − t)}〓 _a ...(13
) or 〓(t)=〓∫＋ν _a t〓 _a + b/2t ² 〓 _b ...(14). The speed is d〓/dt=ν _a 〓 _a +ν _b t/t _o 〓 _b ...(15).

(1-) The corresponding interval in FG→ corresponding to the interval, that is, when t _a ≦ t < t _o .

In this section, constant deceleration movement is given in the vector 〓 _a direction, and constant acceleration movement is given in the vector 〓 _b direction. As a result, a composite vector composed of vectors _〓a and _〓b is obtained on path 1 as a path following the path formed by (1-).

The position vector 〓(t) in this section is 〓(t)=〓 _b − a / 2 {t _o − (t − t _a )} ² 〓 _a + b / 2t ² 〓 _b ...(16) or 〓 (t)=〓∫−ν _a t _a 〓 _a +a/2(t-t _a ) ² 〓 _a +b/2t ² 〓 _b =〓 _d +a/2(t-t _a ) ² 〓 _a +b/2t ² 〓 _b ……(17) becomes. The speed is d〓(t)/dt=ν _a 〓 _a + ν _a (t _a /t _o )〓 _a + (ν _b 〓 _b − ν _a 〓 _a ) t/t _o ... (18).

(1-) When t _o ≦ t < t _o + t _a .

In this section, constant deceleration movement is given in the direction of vector 〓 _a , and constant speed movement is given in the direction of vector 〓 _b . The position vector 〓(t) is (t)=〓 _b −a/2(t _o −(t−t _a )} ² 〓 _a + {b/2t ² _o +ν _b (t−t _o )}〓 _b ...(19) Or, 〓(t)=〓 _d - a/2 (t-t _a ) ² 〓 _a + b/2t ² _o 〓 _b ......(20) The speed is d〓(t) /dt=ν _a 〓 _a +ν _b 〓 _b −v _b t−t _a /t _o 〓 _a ...(21).

(1<) When t≧t _o +t _a .

In this section, there is no movement in the direction of the vector 〓 _a , and constant speed movement is performed in the direction of the vector 〓 _b . The position vector 〓(t) at this time is 〓(t)=〓 _b + {b/2t ² _o + ν _b (t-t _o )}〓 _b ......(22) or 〓(t)=〓 _d +a/2(t _o +t _a −t _a )〓 ² _a b/2t ² _o 〓 _b +ν _b (t−t _o }〓 _b ......(23) The speed is d〓(t)/dt ＝ν _b 〓 _b ...(24)

In the above (1-) to (1-), since the continuity of speed within each section is clear, next we will show the continuity at the boundary point of each section. Now, given the velocity vector ν(t), ν(t)=d〓(t)/dt...(25). If the minute time is 0, ν (-0) = ν (+0) = ν _a 〓 _a ... (26) ν (t _a -0) = ν (t _o +0) = ν _b 〓 _b + ν _a t _a /t _o 〓 _a ...(27) ν(t _o +t _a -0)=ν(t _o +t _a +0)=ν _b 〓 _b
...(28) becomes. As a result, the velocity exhibits continuity even at the boundary points of all sections.

(2) Next, for route 3, which is A→H→I→E→G→C, the following is true. However, point H is from point D to point B.
This is the position when the time t _b moves towards the direction of . Now, let the time at point H be t=0. At this time, the position vector of point H = _h and the position vector of position I = _i are: 〓 _h = 〓 _b − a / 2 (t _o − t _b ) ² 〓 _a ... (29) 〓 _i = 〓 _b − b/2(t _o −t _b ) ² 〓 _b ......(30) The position vector P(t) on route 3 is as follows for each section.

(2-) When −t _b ≦t<0.

〓(t)=P _b −a/2(t _o −t _b −t) ² 〓 _a ...(31) 〓d〓(t)/dt=ν _a t _o −t _b −t/t _o 〓 _a ...(32) (2-) When 0≦t<t _o −t _b .

P(t)=〓 _b −a/2(t _o −t _b −t) ² 〓 _a +b/2t ² 〓
_b ... (33) d〓 (t)／dt＝ν _a t _o −t _b ／t _o 〓 _a + (ν _b 〓 _b −ν _a 〓
_a ) t/t _o ...(34) (2‐) t _o −t _b ≦t<t _o .

(t)=〓 _b +b/2t ² 〓 _b ......(35) d〓 (t)/dt=ν _b t/t _o 〓 _b ......(36) The continuity of speed within each section above is clear. It is. There is continuity even at the boundary points of each section. That is, ν(-0)=ν(+0)=ν _a t _o −t _b /t _o e _a ...(37) ν(t _o −t _b −0)=ν(t _o −t _b +0) This is because =ν _a t _o −t _b /t _o 〓 _b ……(38).

From the above explanation, the following becomes clear. A→
In addition to the path B→C, you can also create a curve from any point on the path (in the above example, since sampling is performed, the curve is a curve based on a break point. However, if the sample interval is made smaller, a continuous curve can be created) is the same). Even if interpolation is performed from any point on the route, velocity continuity can be achieved along all points on the route. This makes it easier to drive the robot hand.

Next, from point F to point G on each route 1, 2, 3, 4
Compare the travel time to.

(1) Travel time on Route 1.

Vector 〓 In _{the a} direction component, it moves at a constant speed during the first time t _a , and moves at a constant deceleration speed during the next time t _o . Vector 〓 In _{the b} -direction component, during the first time t _o it moves with constant acceleration, and during the next time t _a it moves at a constant velocity. Therefore, the travel time is t _l
becomes t _l = t _a + t _o ... (39).

(2) Travel time on Route 2.

If we divide the route into three parts,
In the interval, during time t _a , only the component in the direction of vector 〓 _a moves at a constant speed, and in the interval t _o , the component in the direction of vector 〓 _a moves at a constant deceleration, and at the same time, the component in the direction of vector 〓 _b moves at a constant speed. It is a movement with constant acceleration. In the interval, during time t _a ,
Vector 〓 Since only _{the b-} direction component moves at a constant speed, the travel time for the entire section is the total travel time between the sections. Therefore, the travel time t _l
is, t _l = t _a + t _o + t _a = 2t _a + t _o ……(40) (3) Travel time on route 3.

Route 3 is divided into sections, , and 5
The travel time for each section is as follows: Travel time for section = t Travel time for section _a = t Travel time for section _b = t _o -t Travel time for section _b = t Travel time for section _b = t _a Therefore, the travel time t _l on route 3 is t _l =t _o +2t _a +t _b (41).

(4) Travel time on Route 4.

Divide the route into two sections. section
In FB, only the vector e _a direction component moves at a constant speed, and the distance of the section is 1/2 ν _a t _o + ν _a t _a , so if this distance is moved at a constant speed ν _a ,
The travel time is 1/2t _o +t _a . Similarly, in the section, the distance is 1/2v _b t _o +v _l t _a , and the time it takes to move at a constant speed is 1/2 t _o +t _a . Therefore, the travel time t _l for route 4 is t _l =t _o +2t _a (42).

Comparing the above four routes, if we start the transition from line segment AB to line segment BC not on the route A → B → C, but on F, which is closer to point A than the deceleration starting point D, then A → The travel time can be shorter than in the case of route 4 from B to C.

FIG. 6 shows an embodiment of the processing system of the present invention. Processor 1 connects to memory 3 via bus 2.
It interfaces with a table 4, a multiplier/divider 5, an output port 6, an input port 7, and a teaching box 8. The output port 6 and the input port 7 form an input/output section between the robot body 9 and the bus 2.

The processor 1 controls various calculations, and also controls the memory 3, table 4, multiplier/divider 5, output port 6, input port 7, and teaching box 8 via the bus.
management. The memory 3 stores programs for calculation control and various data. table 4
is a data table that stores trigonometric functions, inverse trigonometric functions, etc.

The multiplier/divider 5 is hardware dedicated to multiplication/division. The teaching box 8 is a console for man-machine interface during teaching. Teaching data instructed by the teaching box 8 and obtained from the robot's drive system or sensors is stored in the memory 3.

Which of the routes shown in FIG. 5 is selected for interpolation is set in advance based on the operation mode and each teaching data. The processor 1 instructs this selection.

Explain the operation.

First, a predetermined part of the robot body 9 is driven to a predetermined position according to instructions from the teaching box 8, the value of the displacement detector (not shown) of the actuator at this position is read, and from this value the position within the space of the hand is determined. The position is calculated and stored in the memory 3 as teaching data. During actual operation, teaching data is first read out from the memory 3 according to the operating mode. Next, according to instructions from the teaching box 8 (for example, speed instructions), interpolation processing between points of the teaching data is performed.

Interpolation processing between points will be explained using the example shown in FIG. First, constant velocities ν _a and ν _b , accelerations a and b, and a selected route are specified. This specification is made by the processor 1. The processor 1 performs interpolation processing according to the selected path based on the above specified values. This interpolation processing refers to subdividing the selected route and specifying the direction vector, its speed, etc. within the subdivided section. The multiplier/divider 5 performs interpolation calculations for each section. The interpolation calculations are the various calculation formulas described above, and relate to simple calculations consisting of combinations of multiplication, division, addition, and subtraction. The output of the multiplier/divider 5 undergoes coordinate transformation into the pairwise displacement of the hand using the contents of the table 4, and is sent to the robot body 7 through the output port 6 and is operated by the actuator.

The meaning of the coordinate transformation in this case is as follows. The teaching data is data at the detection position of the actuator displacement detector, and is different from the actual actuator position. Further, the hand operates in response to an operation input as a pair of displacements. The processing necessary to output such pairwise displacements is coordinate transformation processing. This processing is performed by the processor 1. According to this embodiment, the calculation contents are simple and continuity of speed can be maintained. Hand interpolation control is now possible in real time. Route interpolation when two line segments are on a straight line as shown in FIG. 9 will be explained. 7th
As _shown in _FIG .
A constant deceleration a and a deceleration start point _G1 are determined so that the speed becomes zero at _T2 and reaches point B. A uniform acceleration b and an acceleration end point _G2 are determined so that the vehicle accelerates in the direction of point C from time point B, which is the same as the time required for this deceleration, and reaches speed ν _b at time _T3 .

Next, the section between point G ₁ and point G ₂ in Figure 9 is defined as point G ₁ and time T ₁
If deceleration is started at constant deceleration a at the same time and acceleration is started at constant acceleration b at the same time, the speed curve becomes as shown in FIG. 8, and the point _G2 shown in FIG. ₉ is reached at time T'3.

Next, the travel time will be explained.

If the time required for deceleration is _to , then uniform deceleration a=
−ν _a /t _o , uniform acceleration b=ν _b /t _o , and the interval G ₁ B
The distance of G 2 B is 1/2 ν _a t _o , and the distance of the section G ₂ B is 1/2 ν _b t _o . Therefore, [time required to move in section G ₁ B at constant speed ν _a ] + [time required to move in section G ₂ B at constant speed ν _b ] = 1/2t _o + 1/2t _o = t _{o or} , [Time required to move the section G ₁ G ₂ with uniform acceleration (b - a)] = [Distance of section G ₁ G ₂ ] / {1/2 [uniform acceleration (b - a)] x [travel time ]} Therefore, [travel time] = √1/2(ν _a +ν _b )t _o /{1/2(ν _a /t _o +ν _b /
t _o )}=t _o .

〔Effect of the invention〕

Therefore, by using the path interpolation method of the present invention, the travel time can be made equal to the case of moving on a line segment at a constant speed ν _a and on the line segment at a constant speed ν _b .

The present invention can be applied even when there are two parallel straight lines and a relay route between the two is interpolated. Furthermore, the PTP operation can be applied not only to straight lines in the Cartesian coordinate system, but also to linear drive in a coordinate system in which the displacement of the actuator of the robot is taken as the coordinate axis.

According to the present invention, interpolation control can be achieved while maintaining velocity continuity. Moreover, since the interpolation processing contents are simple, the interpolation processing time can be shortened and interpolation control can be performed in real time.

Further, according to the present invention, two straight lines forming a broken line are used as routes, and on the straight line toward the breaking point, a vector that monotonically decreases so that the velocity becomes zero at the breaking point, and on the straight line from the breaking point, If you use linear interpolation software that interpolates to give a vector that monotonically increases the speed of the robot hand until it reaches a certain speed, the robot hand will interpolate the vector that monotonically increases the speed on the straight line from the broken line just before the break point. By giving this, you can easily use software that only performs linear interpolation.

[Brief explanation of drawings]

Fig. 1 is an explanatory diagram of PTP operation, Fig. 2 is an explanatory diagram of the principle of the present invention, Fig. 3 is an explanatory diagram of a route, Fig. 4 is an explanatory diagram of route interpolation, and Fig. 5 is an explanatory diagram of multiple route interpolation. , FIG. 6 is an embodiment of the present invention, FIG. 7, and FIG.
9 are explanatory diagrams of interpolation in PTP operation on a straight line. 1... Processor, 2... Bus, 3... Memory, 4... Table, 5... Multiplier/divider, 6... Output port, 7... Input port, 8... Teaching box, 9... Robot body .

Claims (1)

[Scope of Claims] 1. A method for interpolating a broken line path of a robot hand, which comprises: a first velocity vector that moves from point A on the broken line path to a bending point B and has a velocity of zero at the breaking point B; , find a composite velocity vector for each unit time with a second velocity vector that monotonically increases the velocity traveling from the breaking point B in the direction of the point C on the broken line path from zero, and calculate the composite vector found for each unit time. A robot hand path interpolation method characterized in that the robot hand is controlled using a position command amount added to the position vector of the robot hand at that point, and the path of the robot hand is interpolated so that the path of the robot hand becomes a curved path. . 2. A path interpolation method for a robot hand according to claim 1, wherein the first velocity vector is a velocity vector that monotonically decreases to zero at a corner point B.