JPH0127443B2 - - Google Patents

Description

[Detailed description of the invention] The present invention relates to a path interpolation method.

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. 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 path interpolation method for a robot hand that can move between a plurality of teaching points at high speed with simple calculations.

Therefore, the present invention gives 3 or more points,
In the method of interpolating the broken line path of the robot hand connecting these points with a straight line, the speed monotonically decreases to zero along the straight line in the direction of the intermediate point before a certain intermediate point that is the turning point of the path. Interpolate so that
Then, interpolation is performed along the next straight line from the intermediate point so that the speed reaches a predetermined speed from zero by monotonous increase, and the position for increasing the speed is placed at the position determined by the interpolation for decreasing the speed. By performing an operation of adding an increment from the intermediate point to the position determined by interpolation, a path is formed that connects the two straight lines in the vicinity of the intermediate point so that the speed does not become discontinuous. It is something.

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.

Now, it is assumed that the movement is performed at a speed v _a between the points and a speed v _b between the steps, 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 n _a = [AB/-/v _a T] ...(1) n _b = [BC/-/v _b T] ...The relationship is as shown in (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≦n _a <AB/-/v _a T+1 ...(3) Show that. 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 〓 _a , the direction from point B to point C is a unit vector 〓 _b , and the speed in the constant speed region of the line segment is
Let v _a be the velocity in the constant velocity region between the lines, and v _b be the velocity. Furthermore, the deceleration time in the vector 〓 _a direction and the acceleration time in the vector 〓 _b direction are made the same, and their value is set to t _o . Furthermore,
Vector 〓 The velocity monotonically decreases in the direction _a , with constant deceleration a,
Vector 〓 Let the velocity monotonically increase in _{the b} direction be the constant acceleration b. Under such a relationship, a and b become as follows: a=v _a /t _o ...(4) b=v _b /t _o ...(5).

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

〓 _d =〓 _b -1/2at ² _o 〓 _a =〓 _b -1/2v _a t _o 〓 _a ......(6) 〓 _e =〓 _b +1/2bt ² _o 〓 _b =〓 _b +1/2v _b t _o 〓 _b ......(7) According to the above path interpolation, on the path from A to D, movement is performed at a constant speed v _a , and on the path from D to B, it is decelerated according to the constant deceleration a, and then it reaches point B. Then the velocity becomes zero. Next, on the path B→E, the speed increases continuously from zero according to the constant acceleration b, and reaches point E.
Then, the velocity becomes constant v _b . Next, on the path E→C, it moves at a constant velocity v _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 v _a during the interval. At point D, vectors in two directions DB→, BE→ 〓 _a ,
〓 Give _b . This vector gives for time t _o .
Due to these two vectors 〓 _a , 〓 _b , the movement route does not become D → B → E → C, but rather D
It moves in the direction of E while drawing a curve following the composite vector of vectors 〓 _a and 〓 _b . t _o
After a period of time, it reaches position E and moves at a constant speed v _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
Sample E ₁ , E ₂ , E ₃ , E ₄ , E ₅ , E, and give vectors 〓 _a , 〓 _b to each composite point of each sample point to obtain a composite vector. The path formed by this composite vector (D→P ₁ →P ₂ →P ₃ →P ₄ →P ₅ →E) becomes the composite point corresponding position of each sample point. D → E in the above
The route shown in FIG.

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 〓(t) between points DE is: 〓(t)=〓 _b −1/2a(t _o −t) ² 〓 _a +1/2bt ²
〓 _b ……(8) becomes. From this, finding the speed, d〓(t)/dt=a(t _o −t)〓 _a + b+〓 _b = at _o 〓 _a + (b〓 _b −a〓 _a )t = v _a 〓 _a + (v _b 〓 _b −v _a 〓 _a ) t/t _o ...(9). The acceleration is d ² 〓(t)/dt ² = b 〓 _b − a 〓 _a = v _b 〓 _b − v _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 routes 1, 2, 3. Route 1
is an example with a path that becomes, path 2 is an example that has a path that becomes, and path 3 is an example that has a path that becomes HI. Here, point F is a point that has moved from point D to point A by a time t _a at a constant velocity v _a . Furthermore, it is assumed that point H is a point that has moved from point B to point D by 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-) Time between intervals, that is, when t<0.

The reason why t<0 is set is that point F is set at time t=0. In this section, constant speed movement is given in the direction of vector 〓 _a , and no movement vector is given in the direction of vector 〓 _b . As a result, the movement is in the direction of AF→. The position vector 〓(t) in this section is 〓(t)=〓 _b −{a/2t ² _o +v _a (t _a −t)〓 _a ...(11). The speed is d〓(t)/dt=v _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 v _a movement is given in the vector 〓 _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 + v _a (t a − _t )}〓 _a ...(13) or 〓(t)=〓 _f +v _a t〓 _a +b/2t ² 〓 _b ......(14). The speed is d〓/dt=v _a 〓 _a +v _b t/t _o 〓 _b (15).

(1-) When the corresponding interval in FG→ corresponds to the interval, that is, 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 in (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)=〓 _f + v _a t _a 〓 _a + a / 2 (t - t _a ) ² 〓 _a + b / 2t ² 〓 _b = 〓 _d + a / 2 (t - t _a ) ² 〓 _a + b / 2t ² 〓 _b ......(17) The speed is d〓(t)/dt=v _a 〓 _a + v _a ( ^ta _to )〓 _a + (v _b 〓 _b − v _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 +v _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 =v _a 〓 _a +v _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 vector 〓 _a direction, and constant velocity movement is performed in the vector 〓 _b direction. Position vector at this time = (t)
is: 〓(t)=〓 _b + {b/2t ² _o +v _b (t-t _o )}〓 _b ... (22) Or, 〓(t)=〓 _d + a/2 (t _o +t _a - t _a )〓 ² _a + b/2t ² _o 〓 _b + v _b (t-t _o )〓 _b ......(23). The speed is d〓(t)/dt=v _b 〓 _b (24).

In (1-) to (1-) above, the continuity of velocity within each section is clear, so next we will show the continuity at the boundary point of each section. Now, the velocity vector v
(t) is given, v(t)=d〓(t)/dt...(25). When the minute time is 0, v(-0)=v(+0)=v _a 〓 _a ...(26) v(t _a -0)=v(t _o +0)=v _b 〓 _b +v _a t _a /t _o 〓 _a ...(27) v(t _o +t _a -0)=v(t _o +t _a +0)=v _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) Position vector on route 3〓(t)
is as follows for each interval:

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

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

〓(t)=〓 _b −a/2(t _o −t _b −t) ² 〓 _a +b/2 t ² 〓 _b ……(33) d〓(t)/dt=v _a t _o −t _b /t _o 〓 _a +(v _b 〓 _b −v _a 〓 _a
) t/t _o ……(34) (2-) When t _o −t _b ≦t<t _o .

〓(t)=〓 _b +b/2t ² 〓 _b ......(35) d〓(t)/dt=v _b t/t _o 〓 _b ......(36) The continuity of the speed in each section above is it is obvious. There is continuity even at the boundary points of each section.

That is, v(-0)=v(+0)=v _a t _o −t _b /t _o 〓 _a ……(37
) v(t _o −t _b −0)=v(t _o −t _b +0)=v _b t _o −t _b /t _o
〓 _b ……(38) This is because it becomes.

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) It is possible to perform interpolation according to 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 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 t _l is 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 IE = t Travel time for section _b EG = 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 〓 _a direction component moves at a constant speed, and the distance of the section is 1/2 v _a t _o + v _a t _a , so if you move this distance at a constant speed v _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 2 stores arithmetic control programs 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 v _a and v _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 process 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 to operate 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 time as the time required for this deceleration, and reaches a speed v _b at time _T3 .

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

Next, the travel time will be explained.

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

Therefore, by using the route 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 v _a and moving on a line segment at a constant speed v _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 a straight line in a Cartesian coordinate system, but also to a linear drive in a coordinate system in which the displacement of a robot's actuator is taken as the coordinate axis.

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

[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)

[Claims] 1 In the method of interpolating a broken line path of a robot hand by giving 3 or more points and connecting these points with a straight line, before a certain intermediate point that is the breaking point of the path, along the straight line in the direction of the intermediate point, Interpolate so that the speed monotonically decreases to zero at the intermediate point, and interpolate so that the speed monotonically increases from zero to a predetermined speed from the intermediate point along the next straight line, and By adding an increment from the intermediate point of the position determined by interpolation to increase the speed to the position determined by interpolation for increasing the speed, the speed between the two straight lines near the intermediate point is increased. A path interpolation method for a robot hand, characterized in that a path is formed so that the paths are not discontinuous.