JPS6330908A - Arm orbit planning method for robot and wave front propagating device - Google Patents

Abstract

PURPOSE:To realize the arm orbit plan of a robot with a more compact device at higher speed, by executing such a simple repeating arithmetic operation such as expansion processing, by an exclusive hardware. CONSTITUTION:The one-dimensional data of C-Space supplied from a memory 1 is developed along a delay element 2, and 2N piece of information of adjacent points and information of aimed points appear simultaneously in (2N+1) pieces of output terminals. These information is processed by an expansion processing arithmetic unit 3, and its result is reserved in a memory 5. When the data reserved in the memory 1 passes through a wave front expansion module 4, it means that a sweep is executed once extending over the whole surface of C-Space, and this sweep is repeated until the wave fronts are collided. Also, the data of the result of the wave front expansion until the wave front collision is held in the memory 5, and a general processor 6 processes the data of the memory 5 and searches an orbit. In such way, a robot can plan the orbit in a real time by using the compact hardware with an extent that is can be loaded on the robot.

Description

[Detailed Description of the Invention] [Industrial Application Field] The present invention relates to trajectory planning in robot programming systems and real-time obstacle avoidance for industrial robots and maintenance robots that perform inspection and maintenance in nuclear reactors, etc. It is something.

[Conventional technology]

Conventionally, robot trajectory planning has been researched in the field of artificial intelligence, and has been carried out on large general-purpose computers or expensive dedicated computers that run languages such as Li5p, which are mainly used in artificial intelligence research.

Here, an example of a conventional trajectory planning algorithm will be explained.

Generally, a robot with N degrees of freedom has N joints and moves an end effector by appropriately moving each joint. Humans observe the robot's movements and recognize obstacles using a three-dimensional orthogonal coordinate system, but since the robot's posture is controlled by the joint angles of each joint, the joint angles cannot be expressed directly. Posture space
Consider ion-Space, hereinafter (abbreviated as knee-5pace). l knee 3 pace of robot with N degrees of freedom
is an N-dimensional space, and each joint is assigned to each axis. An example of a C-5pace robot with two degrees of freedom is shown in FIG. There is a one-to-one correspondence between the robot's posture and a point (square black dot in FIG. 2) on the (''-8 pace), and the movement of the robot corresponds to the movement of a point on the C-3 pace.

(--In order to avoid obstacles and plan a trajectory on 8pace, it is necessary to avoid obstacles in the robot's work environment by [knee 3p
Must be mapped to ace (Figure 2 0l-05)
. Basically, interference checks are performed for each posture using a geometric model of the robot or obstacle. If even one of the robot's arm links interferes with an obstacle in the work environment, the Cu5p will adapt to that posture.
The position on the ace becomes an "obstacle". [knee 3 pace
Among them, ``The part that is not an obstacle j (free space: Free-
In the part of the working environment corresponding to 3pace), none of the links of the robot arm interfere with obstacles, indicating that it is possible to assume such a posture within the working environment.

Therefore, the problem of trajectory planning on C-3pace is
It replaces the head while searching for a path through free space from the initial pose at the starting point to the final pose at the arrival point. In order to reduce the search effort, the free space is first divided into as large basic regions as possible and converted into a set of a small number of regions (FIG. 2, 5l-510). Next, these connection relationships are expressed as a connection graph as shown in FIG. In this way, the problem of closing the trajectory plan becomes a problem of searching for a route on the connection graph from the region S6 including the initial attitude to the region 510 including the final attitude.

Various methods of graph exploration have been studied, but are simple exploration strategies wasteful? = Since a large number of searches will be performed, it is basically important to use more advanced search strategies to perform efficient searches. The use of advanced search strategies necessarily requires complex calculations. To do this, a high-speed general-purpose computer is required, and of course it is impossible to mount such a large computer on a robot and allow the robot to plan its trajectory in real time.

[Problem that the invention seeks to solve]

An object of the present invention is to provide a robot arm trajectory planning method that enables a robot to plan a trajectory in real time using hardware small enough to be mounted on the robot, and hardware for implementing the method. An object of the present invention is to provide a wavefront propagation device for use in the present invention.

[Failure to solve the problem]

In order to achieve the above object, the present invention utilizes a memory that has a large storage capacity and can be accessed at high speed, increases the amount of information to be processed, simplifies the algorithm for trajectory determination, and executes it accordingly. The scale is such that the hardware for this purpose can be mounted on a robot.

According to the robot arm trajectory planning method of the present invention, first, obstacles in the work environment in which the robot arm moves, which has N degrees of freedom, and the initial and final postures of the arm are mapped.
A dimensional posture space (C-3pace) is created, this posture space is divided into subspaces, a free space without obstacles is expressed as a set of these subspaces, and it is converted to -dimensional data, which has a large storage capacity. Store in memory that can be accessed quickly.

Next, the one-dimensional data retrieved from this memory is processed in the form of expansion processing using neighborhood calculations to perform the maze method search. That is, for the one-dimensional data retrieved from the memory, subspaces near the initial attitude and/or final attitude are displayed as wavefronts and distances, and subspaces near the subspaces displayed as wavefronts are sequentially displayed as wavefronts and distances. This can be used as a wave surface.
This process is repeated until the subspaces whose distances are displayed are close to each other or close to the initial or final posture. The wavefront/distance display data for the subspace is accumulated, and then the wavefronts are moved from the point of approach to the initial attitude and the final attitude, or from the initial attitude where the wavefronts have approached towards the final attitude (or when the wavefront is The trajectory of the robot's arm is planned by following the subspace in the direction of decreasing distance (from the final approach approach to the initial attitude).

In order to implement the trajectory planning method of the robot arm described above, the present invention connects a delay element having 2N+1 output terminals to the memory described above, and these output terminals are arranged in a subspace developed along the delay element. It is placed at a position where subspaces that can be neighboring points exist simultaneously, thereby propagating the wavefront in the attitude space.

〔Example〕

An embodiment of the trajectory planning method of the present invention will be described with reference to FIG.

As shown in FIG. 1-(1), in the present invention, the N-dimensional space C-5pace is divided into subspaces of equal shape. Regarding the representation of free space, the conventional technology
Divide into simple regions of minimum number of shapes as shown in the figure,
Whereas the free space was expressed by clarifying the connection relationships between these regions, in the present invention, simple regions of the same shape (
The free space is expressed as a set of these regions.

Each subspace is treated as being represented by a grid point, which is its center point. In this way, route searching can be performed by expanding the wavefront in free space from the grid points of the initial and final postures of the robot.

Conceptually, it is best to think that N-dimensional spherical waves are generated simultaneously from the grid points of the initial attitude and the final attitude, and the path to the point where the two waves collide is searched.

In the case of C-5pace, a robot arm with 2 degrees of freedom, there is an island in a square pool that corresponds to the obstacles in C-5pace, and the water surface becomes free space (Part 1-( 1) Figure). In this example, stones are simultaneously dropped at a location corresponding to the grid point 20a in the initial posture and the grid point 20b in the final posture (Fig. 1-(2)), and if the wave front on the water surface is observed (Fig. 1-(2)). 3) Figure), 20C
It can be seen that the two wavefronts collide at the point (1st - (4
)figure). The shortest path is the path of this wavefront (1st-(
5), (6) Figure). C- of a robot arm with 3 degrees of freedom
In the case of 3pace, it is helpful to imagine a wavefront propagating in space in a spherical shape.

The expansion of the wavefront for searching is performed by neighborhood calculations, focusing on the property that the wavefront moves to adjacent grid points. In other words, the spreading or expansion of a wavefront is considered to be a process in which the wavefront moves to neighboring points one after another, and in order for a lattice point to which the wavefront has not yet reached to become a wavefront, the wavefront must arrive at a point near that lattice point. Must be. In other words, if it is detected at the lattice point of interest that the wavefront has arrived near the lattice point of interest, and that the neighboring lattice point carries information about the arrival of the wavefront, then It is sufficient to make the lattice points carry information about the arrival of the wavefront, and repeat this process from the wave source. Specifically, a window that allows you to see only the neighboring grid points around the grid point of interest (in the case of 2D C-3pace, a window that allows you to see the four subspaces bordering the subspace of interest) Sweep the entire surface of C-5pace using a template with a cross-shaped aperture W (Fig. 1-(1)), and sequentially pick up adjacent subspaces toward the wave source (lattice point of initial attitude and/or final attitude). By checking whether the subspace of interest becomes a new wavefront, the wavefront can be expanded sequentially.

Since it is difficult to store and process such N-dimensional data with the N-dimensional structure as it is, a -dimensional array is used. N
To convert dimensional data to one-dimensional data, first focus on one axis among N dimensions and fix this axis, then N-1
It becomes dimensional data (for example, if it is a 3-dimensional xyz space,
If we focus on the Z-axis and fix this axis, it becomes two-dimensional data on the xy plane). In this way, first, N-dimensional data can be considered as a set of N-1-dimensional data, and if this is arranged, it becomes N-1-dimensional data. This N-1 dimensional data is converted into N-2 dimensional data using a similar method, and this operation is repeated to obtain 1-dimensional data (a common example is to scan the screen horizontally to obtain 2-dimensional image data). (There is a sweeping method in TV technology that treats data as one-dimensional data.) In order to sweep such -dimensional data using the template described above, it is necessary to adopt a circuit configuration that allows data of neighboring points to be obtained at the same time.

However, (-8 pace data is a long one-dimensional array, so even if it is a nearby point in the N-dimensional space, it becomes data that is located quite far away on the -dimensional array. Therefore, in the present invention, By using a configuration in which data in a one-dimensional array is passed sequentially from the end, using delay elements such as shift registers, information on positions corresponding to 2N grids adjacent to each other vertically and horizontally can be extracted, and information on neighboring points can be obtained simultaneously. A wavefront propagation device is used.

The operating principle of this wavefront propagation device will be explained with reference to FIG. For example, in the case of the robot with two degrees of freedom shown in Figure 4-(1), the four neighboring points on the top, bottom, left and right (
Detection of Tl, 2.4.5) is required.

The 1 knee 3 pace data is converted to one-dimensional data by sweeping it horizontally, so the left and right grid points (T2, T4)
Even if the data is converted into -dimensional data, it is still the previous and subsequent data. The data of the upper and lower grid points (Tl, T5) are located at positions spaced apart by the number of grid points in the horizontal direction. In the example of FIG. 4, there are 30 grid points in the Chinese direction, so it is sufficient to prepare delay stages of length 60 in order to obtain information on the upper and lower grid points. At this time, as shown in Figure 4-(2), there is data for the upper and lower lattice points in the population and exit portions of the delay element, and the 300th stage, which is the center of the delay element, is the lattice point of interest (
There is data for T3). The data of the left and right grid points (T2, T4) are in the 299th row and the 31st row.

The information on the neighboring points extracted in this way is processed as follows to expand the wavefront.

Now, suppose we can write integer data to each grid point, -1 for obstacles, and free space (Free -5 pace).
) is written as 0, and 1 is written in the grid points of the initial posture and the grid points of the final posture. Also, each wavefront has an initial attitude,
Write in a separate bit which side of the final attitude it came from. At this time, the expansion process can be performed on the information of the extracted neighboring points as shown in FIG.

First, if the grid point we are focusing on is an obstacle (judgment 14es
), it has nothing to do with the expansion of the wavefront and outputs the data of 5 as it is (processing 2). Also, the wave front has already arrived,
If the wavefront/distance is displayed (judgment 2: Yes), there is no need to process it again, and the data in 5 is output as is (
Processing 2). If the wavefront has not reached any neighboring points (
Judgment 3: Yes), change the newly focused grid point to the wavefront
Since the distance cannot be displayed, the data of 5 is output as is (processing 2). If the wavefront has not arrived at the grid point of interest and the wavefront/distance is displayed for the neighboring grid point (NO in all judgments 1, 2, and 3), calculate the distance and display the new wavefront/distance ( Processing 1).

Monitoring whether the wavefronts collide can be performed by detecting whether there are both a wavefront from the initial attitude and a wavefront from the final attitude nearby. When the two wavefronts collide, the expansion process is stopped, the data is accumulated and saved, and then the data is moved from the collision point to a nearby point with a smaller value. To search for a path from the initial attitude 20a to the wavefront collision point 20c, the following process may be performed using the wavefront collision point 20C as a starting point (see 1-(5)).

・Among the neighboring points, choose the point with the smallest value among the grid points where the wavefront values from the initial attitude are written, and move to that point. This is continued until the grid point 20a of the initial attitude is reached.

To search for a route from the wave front collision point 20c to the most forested figure u20b, the following process may be performed using the wave front collision point 20c as a starting point.

・Among the neighboring points, choose the point with the smallest value among the grid points where the value of the wavefront from the final attitude is written, and move to that point. Continue this until you reach the grid point of the final posture.

Combining these results, the initial posture is 20a to the final posture is 20.
Find the shortest path to b.

In the above embodiment, the lattice points of the initial attitude and the final attitude are used as the wave sources, but in another embodiment, the lattice point of the initial attitude can be used as the wave source and the wavefront is expanded so that the wave front reaches the lattice point of the final attitude. On the other hand, it is also possible to expand the wavefront so that the wavefront reaches the lattice point of the initial attitude using the lattice point of the final attitude as a wave source, but the above embodiment is advantageous in terms of shortening the calculation time.

A certain number of bits is required for each grid point because the distance from the grid point of the initial attitude and final attitude is written.

If a large number of bits cannot be secured due to constraints such as memory capacity, the encoding method used in automatic wiring can be used.

For example, even if the distance is limited to 1.2.3, if 1 is written next to 3, the path will be traced from the point where the wave front collided to the grid point of the initial attitude and final attitude in the same way. be able to.

An example of a device for planning the trajectory of a robot arm using a wavefront propagation device according to the present invention is shown in FIG.

In FIG. 6, 1 is a memory that holds information on C-5pace, 2 is a delay element, 3 is an expansion processing arithmetic unit, and 4 is a wavefront expansion module consisting of the delay element 2 and the arithmetic unit 3;
5 is a memory that holds the wavefront expansion results, and 6 is a general-purpose processor for trajectory search.

The operation will be described briefly as it overlaps with the information processing described above. The one-dimensional data of the knee 3 pace is read out from the memory 1 and expanded along the delay element 2, and information on 2N neighboring points and information on the point of interest appear simultaneously at 2N+1 output terminals.

These pieces of information are processed by the expansion processing arithmetic unit 3 according to the flowchart in FIG. 5, and the results are stored in the memory 5. When the data stored in the memory 1 passes through the wavefront expansion module 4, it is swept once over the entire surface of the C-5pace. This sweep is repeated until the wavefront collides (the wavefront expansion in Figure 1-(2) is the result of two sweeps, and the wavefront expansion in Figure 1-(3) is the result of six sweeps,
The wavefront expansion in Figure 1-(4) shows the result of sweeping up to the wavefront collision). Wavefront expansion until wavefront collision (1st-(4th
) The resultant data in Figure) is held in the memory 5.

A general-purpose processor 6 processes the data in the memory 5 to search for a trajectory.

FIG. 7 shows another example of a device for planning the trajectory of a robot arm using a wavefront propagation device according to the present invention.

The difference from the device shown in FIG. 6 is that in the device shown in FIG.
In contrast, in the apparatus shown in FIG. 7, the wavefront expansion modules 4 are connected in series in multiple stages to perform expansion processing in multiple stages at once.

In the wavefront expansion module 4, there is a processing time delay k caused by the delay element 2. held in memory 1 (
:j; Let β be the time required to read all the data of pace, then an integer n that satisfies nk>β is found. It takes n- time for data to pass through the n-stage wavefront expansion module. When this time has elapsed, the first wavefront expansion module 4(1) has finished processing the information held in the memory 1, and the nF9 is in a state where it can process the output of the eye. In other words, n wavefront expansion modules are connected in a ring shape, and expansion processing is performed at high speed using a pipeline method. This collision of the wave fronts is monitored in the expansion calculation device 3, and as soon as a collision is detected, the result of the expansion process is read out to the memory 5.

FIG. 8 shows yet another example of a device for planning the trajectory of a robot arm using a wavefront propagation device according to the present invention.

The difference from the apparatus shown in FIG. 7 is that the apparatus shown in FIG. 8 has an interference check module 7 connected immediately after each expansion processing module. In the apparatus shown in FIG. 7, C-3pa is
CE obstacle information had to be calculated and stored in memory 1, whereas in the device shown in Fig.
-3 pace obstacle detection can also be performed simultaneously with wavefront expansion processing.

Expansion processing modules 4(1) to 4(n) connected in multi-stage series
of.

Between them, the data for one lattice point is received and passed sequentially. Therefore, an interference check module 7 is provided,
From the data of one grid appearing in the output of the expansion processing calculation unit 3, the joint angle corresponding to that grid point of the robot's arm is known, and the robot takes the posture of this joint angle to avoid obstacles and the robot in the working environment. If there is interference, the grid point is registered as an obstacle. Since the C-5 pace obstacles can be calculated simultaneously with the expansion of the wavefront, there is an advantage that there is no need to calculate the C-3 pace obstacles in advance.

〔Effect of the invention〕

The present invention accomplishes robot trajectory planning, which has traditionally been done using complex programs, by using dedicated hardware to perform a simple repetitive operation called dilation processing, thereby achieving faster speed and smaller installations. It is possible to improve the functionality of robots.

[Brief explanation of the drawing]

1-(1), 1-(2), 1-(3), 1-(
4), 1-(5) and 1-(6) visually illustrate the basic operation of the present invention in the case of a robot arm with two degrees of freedom. FIG. 2 is a diagram illustrating a conventional trajectory planning method, particularly a free space representation method. FIG. 3 shows a conventional trajectory planning method, particularly a connection graph for searching a trajectory, for the case of FIG. 2. 4-(1) and 4-(2) are diagrams for explaining a method for obtaining neighboring points from one-dimensional data using a delay element. FIG. 5 is a flowchart showing the procedure of information processing in the expansion processing arithmetic device. FIG. 6 is a block diagram showing an example of a trajectory planning device that implements the trajectory planning method of the present invention. FIG. 7 is a block diagram showing another example of the trajectory planning device. FIG. 8 is a block diagram showing yet another example of the trajectory planning device. In the figure: 1-= ...Position space (Configuration
5 pace) memory 2... Delay element 3... Dilation processing calculation unit 4... Dilation processing module 5... Holds the results of the dilation process. Memory 6...
...General-purpose processor 7...Interference check module 0a Fig. 2 Fig. 3 Fig. 4 Delay by one line Fig. 5 Tate output Fig. 7 Fig. 8

Claims (2)

[Claims] (1) Create a posture space that maps the obstacles in the work environment in which the robot arm moves, which has N degrees of freedom, the initial posture at the starting point of the arm, and the final posture at the arrival point; turn this posture space into a subspace. Divide: Sweep the posture space divided in this way, display the wavefront/distance of the subspace near the initial attitude and/or final attitude, and then sequentially display the wavefront/distance of the subspaces near the subspace where the wavefront was displayed. display; wavefront/distance When the displayed subspaces approach each other or near the initial attitude or final attitude, the sweep is stopped; the distance from the point of approach toward the initial attitude and/or final attitude is A trajectory planning method for a robot arm, which is characterized in that the trajectory of the robot arm is planned by tracing the subspace in the direction in which the distance becomes shorter. (2) Map the obstacles in the work environment in which the arm of the robot with N degrees of freedom moves, the initial posture at the starting point of the arm, and the final posture at the arrival point, and accumulate information on the posture space divided into subspaces. a memory; and a delay element connected to this memory and having 2N-1 output terminals, where the output terminal simultaneously exists a subspace that can be a neighboring point among the subspaces developed along the delay element. A wavefront propagation device characterized by being placed at a position where