JPH0719175B2 - Robot arm trajectory planning and wavefront propagation equipment. - Google Patents

Description

The present invention relates to trajectory planning in a robot programming system, and real-time obstacle avoidance for industrial robots, maintenance robots for inspection and maintenance in nuclear reactors, etc. It is a thing.

[Prior Art] Conventionally, trajectory planning of robots has been studied in the field of artificial intelligence, and has been performed by a large-scale general-purpose computer or an expensive dedicated computer that executes a language mainly used for artificial intelligence research such as Lisp. .

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

Generally, a robot having N degrees of freedom has N joints and moves the end effector by appropriately moving each joint. Humans observe the movement of the robot and recognize obstacles in a three-dimensional Cartesian coordinate system, but since the robot controls its posture by the joint angle of each joint, it can be expressed directly as a joint angle. Posture space (Configuration-Spac
e, hereinafter abbreviated as C-Space). The C-Space of a robot with N degrees of freedom is an N-dimensional space, and each joint is assigned to each axis. Fig. 2 shows an example of C-Space for a robot with two degrees of freedom. There is a one-to-one correspondence between the robot's posture and one point on C-Space (square black dots in Fig. 2), and moving the robot corresponds to moving the point on C-Space.

In order to avoid obstacles on the C-Space and plan trajectories, obstacles in the robot's work environment must be mapped to the C-Space (Fig. 01-05). Basically, a collision check is performed for each posture using a geometric model of a robot or an obstacle. If one of the robot arm links in the work environment interferes with an obstacle, the position on C-Space corresponding to that posture becomes an "obstacle". In the part of the work environment corresponding to the part that is not an "obstacle" in C-Space (free space: Free-Space), no link of the robot arm interferes with the obstacle, that is, such a posture is set in the work environment. It shows that you can take.

Therefore, the problem of trajectory planning can be replaced with the problem of searching a path moving through free space from the initial posture at the starting point to the final posture at the arrival point on C-Space. In order to reduce the time and effort of the search, first, the free space is divided into as large a basic region as possible, and converted into a set of a small number of regions (Fig. 2, S1-S10). Next, as shown in FIG. 3, these connection relationships are expressed as a connection graph.
Thus, the problem of trajectory planning is that the area S6 including the initial attitude is
To the region S10 including the final posture becomes a problem of finding a route on the connection graph.

Various methods of graph search have been researched, but if a simple search strategy is used, a large number of unnecessary searches are performed. Therefore, basically, a more advanced search strategy is used.
It is common to try a lean search. In order to use advanced search strategies, inevitably complicated calculations must be performed. For that purpose, a high-speed general-purpose computer is required, and of course it is impossible for the robot to carry out a trajectory planning in real time by mounting such a large-scale computer on the robot.

[Problems to be solved by the invention]

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

[Means for solving problems]

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

According to the robot arm trajectory planning method of the present invention, an obstacle N in a work environment in which the robot arm having N degrees of freedom moves and the initial posture and the final posture of the arm are first mapped.
Dimensional initial pose space (C-Space) is created, this pose space is divided into sub-spaces, and free space without obstacles is expressed as a set of these sub-spaces. It is stored in a memory that can be accessed at a large speed.

Next, the one-dimensional data fetched from this memory is processed in the form of the expansion distance by the neighborhood calculation to implement the search strategy of the maze method. That is, for the one-dimensional data sent from the memory, the sub-space near the initial posture and / or the final posture is displayed in wavefront / distance, and the subspaces near the wavefront-displayed subspace are sequentially displayed in wavefront / distance. This is the wavefront
This is repeated until the sub-spaces displayed as distances come close to each other or close to the above-mentioned initial posture or final posture. The wavefront / distance display data for the sub-space is stored, and then, from the approaching points of the wavefronts toward the initial posture and the final posture, or from the initial posture where the liquid surface approaches to the final posture (or the wavefront). The trajectory of the robot arm is planned by tracing the sub-space in the direction in which the distance becomes shorter (from the final posture in which the robot approaches to the initial posture).

In order to implement the above-described robot arm trajectory planning method according to the present invention, a delay element having 2N + 1 output terminals is connected to the above memory, and these output terminals are included in a subspace developed along the delay element. The sub-spaces, which can be neighboring points, are placed at the positions where they exist at the same time, so that the wavefront is propagated to the posture 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, C-Space, which is an N-dimensional space, is divided into sub-spaces having the same shape. Regarding the expression of free space, in the prior art, as shown in FIG. 2, the free space was expressed by dividing the area into the minimum number of simple areas and clarifying the connection relation of these areas. In the present invention, a simple space (subspace) having the same shape is divided, and the free space is expressed as a set of these regions.

Each subspace is treated as represented by the grid point that is the center of each subspace. By doing so, the route search can be performed by expanding the wavefront in the free space from the lattice points of the initial posture and the final posture of the robot.

Conceptually, it may be considered to search a path from the lattice points of the initial posture and the final posture to a point where N-dimensional spherical waves are simultaneously generated and two waves collide. In the case of a C-Space with a two-degree-of-freedom robot arm, it can be considered that a square pool has an island corresponding to an obstacle in the C-Space, and the water surface becomes a free space (No. 1- ( 1)
Figure). In this embodiment, stones are simultaneously dropped at locations corresponding to the grid points 20a in the initial posture and the grid points 20b in the final posture (first
-(2)) If you are observing the wavefront on the water surface (No. 1-
(Fig. (3)), and it can be seen that two wavefronts collide with each other at point 20c (Fig. 1- (4)). The shortest path is the path of this wavefront (Figs. 1- (5) and (6)). With a three-degree-of-freedom robot arm, C-Space, you can imagine the wavefront propagating in a spherical shape in space.

The expansion of the wavefront for the search is performed by neighborhood calculation, paying attention to the property that the wavefront moves to the adjacent lattice point. That is, the expansion or expansion of the 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 that the wavefront has not yet reached to become a wavefront, the wavefront must come to the neighboring point of the lattice point. I have to. In other words, if the wave front has reached the vicinity of the grid point of interest, and the neighboring grid point carries the information of arrival of the wave front at the grid point of interest, Information on arrival of the wavefront at the lattice points may be carried, and this may be repeated from the wave source. Specifically, a window that allows you to see only the neighboring grid points around the focused grid point (in the case of two-dimensional C-Space, you can see the four subspaces that are in contact with the focused subspace). Sweep the entire surface of C-Space with a template having a cross-shaped opening W (Fig. 1- (1))), and pick up adjacent subspaces in sequence toward the wave source (lattice points in the initial and / or final postures). The wavefront can be expanded sequentially by checking if the subspace under consideration becomes a new wavefront.

Such N-dimensional data is a one-dimensional array because it is difficult to store and process the N-dimensional data as it is. N
To convert one-dimensional data into one-dimensional data, first focus on one axis of the N dimensions, and fix this axis.
Dimensional data (for example, in the case of three-dimensional xyz space, z
Paying attention to the axis and fixing this axis, it becomes two-dimensional data on the xy plane. Thus, first, N-dimensional data can be considered as a set of N + 1-dimensional data, and if they are arrayed, N-
It becomes one-dimensional data. This N-1 dimensional data is converted into N-2 dimensional data by the same method, and this operation is repeated to obtain one dimensional data (a common example is to scan the screen in the horizontal direction to obtain two dimensional image data. There is a sweeping method in the TV technology that treats as one-dimensional data.) In order to sweep such one-dimensional data with the template, it is necessary to adopt a circuit configuration that can obtain data of neighboring points at the same time. However, since the data of C-Space is a long one-dimensional array, even the neighboring points in the N-dimensional space will be data that are located far apart on the one-dimensional array. Therefore, in the present invention, the one-dimensional array of data is made to flow sequentially from the end, and a delay element such as a shift register is used to vertically and horizontally adjoin.
We use a wavefront propagation device that can extract information on the positions corresponding to 2N gratings and obtain information on neighboring points at the same time.

The operating principle of this wavefront propagation device will be described with reference to FIG. For example, in the 2-DOF robot C-Space shown in Fig. 4- (1), four neighboring points (T1, 2,
It is necessary to detect 4, 5). Since the data of C-Space is swept in the horizontal direction and converted into one-dimensional data, the data of the left and right grid points (T2, T4) are still the front and back data even if they are converted into one-dimensional data. Upper and lower grid points (T1, T5)
The data of is at a position separated by the number of grid points in the horizontal direction. In the example of FIG. 4, since there are 30 grid points in the horizontal direction,
The number of delay stages with a length of 60 may be prepared in order to obtain information on the upper and lower grid points. At this time, as shown in FIG. 4- (2),
There are data on the upper and lower grid points at the entrance and exit of the delay element, and there is data on the grid point (T3) of interest at the 30th stage, which is the center of the delay element. The data of the left and right grid points (T2, T4) are in the 29th and 31st rows.

The information on the neighboring points thus extracted is processed as follows to expand the wavefront.

Now, it is possible to write integer data to each grid point, -1 for obstacles, 0 for free space (Free-Space), 1 for grid points of initial posture and grid point of final posture. It is written. In addition, it is written in another bit whether each wavefront comes from the initial posture or the final posture. At this time, the expansion process can be performed on the extracted information of the neighboring points as shown in FIG.

First, if the grid point of interest is an obstacle (Judgment 1: Ye
s), the current data is output regardless of the expansion of the wavefront (process 2). If the wavefront has already arrived and the wavefront / distance is displayed (decision 2: Yes), there is no need to process it again, and the current data is output (process 2). If the wavefront has not reached any of the neighboring points (decision 3: Yes), it is not possible to display the wavefront / distance of the newly focused grid point, so that data is output (process 2). If the wavefront has not arrived at the target grid point and the neighboring grid points are displayed as wavefront / distance (judgment 1, 2, 3 are all NO), the distance is calculated and the new wavefront /
The distance is displayed (process 1).

Whether or not the wavefronts collide can be monitored by detecting whether or not there are both wavefronts coming from the initial posture and the final posture in the vicinity. When the two wave fronts collide with each other, the expansion process is stopped, the data is accumulated and stored, and then the collision point is moved to a nearby point having a small value. To search for a path from the initial posture 20a to the wavefront collision point 20c, the following processing may be performed with the wavefront collision point 20c as a starting point (see 1- (5)).

-Of the neighboring points, select the point with the smallest value from the lattice points in which the value of the wavefront from the initial posture is written, and move to that point. This is continued until the lattice point 20a in the initial posture is reached.

Search for the path from the collision point 20c on the wavefront to the final attitude 20b
Similarly, the following processing may be performed using the collision point 20c of the wavefront as a starting point.

-Of the neighboring points, select the point with the smallest value from the lattice points in which the value of the wavefront from the final posture is written, and move to that point. This is continued until the grid point of the final posture is reached.

If these results are combined, the shortest path from the initial posture 10a to the final posture 20b can be obtained.

In the above embodiment, the lattice points of the initial posture and the final posture are used as the wave sources, but as another example, the lattice points of the initial posture can be used as the wave source to expand the wavefront so that the wavefront reaches the lattice points of the final posture. On the contrary, it is possible to expand the wavefront so that the wavefront reaches the lattice point in the initial posture by using the lattice point in the final posture as the wave source, but the above embodiment is advantageous in terms of shortening the calculation time.

Since the distances from the lattice points of the initial posture and the final posture are written in each lattice point, a certain number of bits are required.
When a large number of bits cannot be secured due to restrictions such as memory capacity, the coding method used in automatic wiring can be used. For example, if the distance is limited to 1, 2, and 3 and 1 is written next to 3, the path from the point where the wave front hits to the lattice point of the initial posture and the final posture is similarly traced. be able to.

FIG. 6 shows an example of an apparatus for planning the trajectory of a robot arm using a wavefront propagation apparatus according to the present invention.

In FIG. 6, 1 is a memory for holding C-Space information, 2 is a delay element, 3 is an expansion processing arithmetic unit, 4 is a wavefront expansion module composed of a delay element 2 and an arithmetic unit 3, and 5 is an expansion of the wavefront. The result holding memory, and 6 is a general process for trajectory search.

The operation will be briefly described because it overlaps with the above-described explanation of information processing. The one-dimensional data of C-Space fed from the memory 1 is expanded along the delay element 2 and the information of 2N neighboring points and the information of the point of interest appear at 2N + 1 output terminals at the same time. These pieces of information are processed by the expansion processing arithmetic unit 3 according to the flowchart of FIG. 5, and the result is stored in the memory 5. When the data stored in the memory 1 passes through the wavefront expansion module 4, it means that the entire C-Space has been swept once. This sweep is repeated until the wavefront collision (No. 1- (2))
The wavefront expansion shown in the figure is the result of two sweeps.
The wavefront expansion in the figure is the result of 6 sweeps, and
(4) The wavefront expansion in the figure shows the result of sweeping until the wavefront collision). Data resulting from the wavefront expansion (FIG. 1- (4)) up to the wavefront collision is held in the memory 5.

The 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 of FIG. 6 is that in the device of FIG. 6, the result of the expansion process is stored in the memory 5 and one wavefront expansion module 4 is used.
7 is used as many times as necessary, in the apparatus shown in FIG. 7, the wavefront expansion modules 4 are arranged in series and connected in multiple stages so that multistage expansion processing is performed at one time.
In the wavefront expansion module 4, there is a processing time delay k due to the delay element 2. C- held in memory 1
Letting l be the time required to read all the data in Space, an integer n satisfying n · k> l can be obtained. n
N · k for data to pass through the wavefront expansion module in stages
Takes time. When this time elapses, the first wavefront expansion module 4 (1) has finished the information processing held in the memory 1 and is ready to process the output of the nth stage. That is, the n wavefront expansion modules are connected in a ring shape, and the expansion processing is performed at a high speed by a pipeline method. At this time, the collision of the wavefront is monitored by the expansion arithmetic unit 3, and the result of the expansion process is read out to the memory 5 as soon as the collision is detected.

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 device of FIG. 7 is that the device of FIG. 8 has the interference check module 7 connected immediately after each expansion processing module. In the device of FIG. 7, the information of the obstacle of C-Space had to be calculated in advance and held in the memory 1, whereas in the device of FIG.
The obstacle can be detected simultaneously with the wavefront expansion process. Expansion treatment module 4 (1) with multi-stage series connection
During 4 (n), the data of one grid point is sequentially transferred. Therefore, the interference check module 7
Is provided, the joint angle corresponding to the lattice point of the robot arm is known from the data of one lattice appearing in the output of the expansion processing arithmetic unit 3, and the posture of this joint angle is taken by the robot at that time in the working environment. It is determined whether or not the obstacle and the robot interfere, and if they interfere, the lattice point is registered as an obstacle. Since the C-Space obstacle can be calculated simultaneously with the expansion of the wavefront, there is an advantage that it is not necessary to calculate the C-Space obstacle in advance.

〔The invention's effect〕

The present invention realizes a trajectory planning of a robot, which has been performed using a complicated program up to now, at a higher speed and with a smaller device by performing a simple repetitive operation called expansion processing with dedicated hardware. It is possible to improve the function of the robot.

[Brief description of drawings]

1- (1), 1- (2), 1- (3), 1- (4),
1- (5) and 1- (6) are visual illustrations of the basic operation of the present invention in the case of a robot arm having two degrees of freedom. FIG. 2 is a diagram for explaining a conventional trajectory planning method, particularly a free space expression method. FIG. 3 shows a conventional trajectory planning method, particularly a connection graph for searching a trajectory for the case of FIG. FIGS. 4- (1) and 4- (2) are diagrams for explaining a method for obtaining a neighboring point from one-dimensional data using a delay element. FIG. 5 is a flowchart showing the procedure of information processing in the expansion processing arithmetic unit. FIG. 6 is a block diagram showing an example of a trajectory planning device for implementing 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 still another example of the trajectory planning device. In the figure: 1 ... Memory for holding data of Configuration Space 2 ... Delay element 3 ... Expansion processing arithmetic unit 4 ... Expansion processing module 5 ... Memory for holding expansion processing result 6 ... General purpose Processor 7: Interference check module

Claims (2)

[Claims] 1. A posture space in which obstacles in a work environment in which a robot arm having N degrees of freedom moves and an initial posture at a starting point of the arm and a final posture at an arriving point are mapped to form a posture space; Divide into spaces; sweep the posture space divided in this way, display the sub-space near the initial posture and / or the final posture in the wavefront / distance display, and sequentially display the subspaces in the subspace near the wavefront-displayed wavefront.・ Display the distance; stop the sweep when the wave space / distance-displayed subspaces are close to each other or close to the initial posture or the final posture; from the approach point to the initial posture and / or the final posture A trajectory planning method for a robot arm, 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. Information on a posture space divided into sub-spaces by mapping an obstacle in a work environment in which an arm of a robot having N degrees of freedom moves, an initial posture at a starting point of the arm and a final posture at an arriving point, and dividing the information. A memory for accumulating; and a delay element connected to this memory and having 2N + 1 output terminals, wherein the output terminals simultaneously have subspaces that can be neighboring points among subspaces developed along the delay element A wavefront propagating device characterized in that it is arranged at a position.