JP5775279B2 - Route generator - Google Patents

Description

The present invention relates to a path generation equipment of the robot arm, in particular about the route generation equipment of the robot arm to perform the operation plan to avoid the obstacle by using the configuration space.

In general, when working in a place where human access is difficult, such as a nuclear reactor, remote control is performed by incorporating a CCD camera, distance sensor, contact sensor, etc., inspection of the reactor, measurement of the shape, repair work, etc. In many cases, a master / slave system is used.
In particular, in a nuclear reactor, work using an articulated robot that can automatically control the operation path selection is performed. As a method for realizing such automatic control and path generation, an operation path using a configuration space is used. An operation route generation method using a probabilistic method such as a generation method, a maze search method, or an RRT (Rapidly-exploring Random Trees) method is disclosed (Patent Documents 1 to 3).

The configuration space (C space) is a virtual space having the same number of dimensions as the number of degrees of freedom of the manipulator (robot arm). On this C space, the configuration (posture) of the robot arm is the robot arm configuration. When the parameter representing the degree of freedom is a coordinate axis value, it can be represented by a point (configuration point).
For example, for all configurations of the robot arm, the presence or absence of interference with the obstacle is investigated, and a space (free space) in which the robot arm does not interfere with the obstacle is constructed on the C space, and the movement path of the robot arm in that space Is generated.

JP-A-4-235606 JP 2008-105132 A JP 2010-32326 A

  However, the path generation method using the C space has a problem that the calculation amount and the necessary storage capacity become enormous as the degree of freedom of the robot arm increases, and it is unrealistic to apply to a multi-degree-of-freedom robot. .

  Furthermore, as a solution to the path generation method using the C space, the amount of calculation is reduced by constructing the C space so as to widen the wavefront from the initial configuration point and the target configuration point on the C space. Although a method for generating motion in a free space where wavefronts are overlapped is also disclosed, there has been a problem of constructing an extra free space in order to uniformly expand the wavefront in all directions (Patent Document 1).

  The route generation method using the maze search method connects the initial configuration point and the target configuration point on the C space with a straight line, and traces the boundary of the obstacle on the C space that intersects the straight line and the straight line. Although a route is generated, a two-dimensional maze is premised, and the boundary of an obstacle in a three-dimensional or higher C space appears as a plane or a solid.

  In addition, the route generation method using the RRT method is a configuration in which a configuration point advanced by a certain distance toward a configuration point selected at random is taken into the route search tree to grow the search tree, and the configuration taken into the search tree. Although a path is generated by following a point, there is a problem that it is difficult to estimate a calculation time because a free space is constructed using random numbers.

The present invention has been made in view of such a background, to eliminate the excess calculated in configuration space, to provide a route generating equipment capable of reducing the operation plan steps of the robot arm Let it be an issue.

The invention according to claim 1 of the present invention has a control device including an arithmetic processing unit and a storage device, and adopts a new configuration point so as to avoid an obstacle from the initial configuration point to the target configuration point. A path generation device using a configuration space for generating an operation path of a robot arm, wherein the control device stores initial position storage means for storing the initial configuration point, and a target position for storing the target configuration point. Storage means; current position detection means for detecting the current position of the robot arm; path storage means for storing each configuration point constituting a path from the initial configuration point to the current position; Config adjacent to the current position adjacent to the position First candidate determining means for determining an interference point; second candidate determining means for determining an interference point adjacent configuration point adjacent to an interference point with the obstacle among the current position adjacent configuration points; and the current position adjacent configuration the Deployment point as the target, the current position and adjacent configuration interference checking means configuration point to determine whether the interference point of the obstacle, the target configuration point from the current position of the current position adjacent configuration point Distance candidate judging means for selecting a configuration point so as to be closest to the target configuration point from the current position in a direction along the virtual straight line route to the current position , and the interference check means is further adjacent to the current position configuration point Wherein at the current position on the inner said operating path metric distance candidate configuration points which are selected by the candidate determining means determines whether the interference point of the obstacle, the interference pre Ki距 away candidate configuration point If it is determined by the checking means that it is not the interference point, the distance candidate configuration point is adopted as the new configuration point, stored in the route storage means, and a straight-ahead mode route that proceeds to the new configuration point is generated. and, before when Ki距 away candidate configuration point is determined to be the point of interference by the interference checking means, among the interference point adjacent configuration point obtained by the second candidate determination means, said distance The target configuration is determined from the current position by means of candidate determination. A boundary that adopts the new configuration point so as to be closest to the target configuration point from the current position in the direction along the virtual straight line path to the configuration point , stores the new configuration point in the path storage unit, and proceeds to the new configuration point A tracking mode path is generated, and thus an operation path from the initial configuration point to a target configuration point is generated while sequentially generating the straight traveling mode path and the boundary tracking mode path, and the current position adjacent configuration is generated. A deadlock avoiding unit that excludes a new configuration point from being selected by referring to a path configuration configuration point stored in the path storage unit that configures a path that has passed to the current position among the configuration points Special To.

According to the present invention, the first candidate determination unit and the second candidate determination unit obtain the interference point adjacent configuration point adjacent to the interference point with the obstacle among the current position adjacent configuration points, thereby freeing the robot arm. Rather than determining the presence or absence of interference for all configuration points corresponding to the degree, the construction of the configuration space (C space) is limited to the current position of the robot arm and the surroundings of the interference points to be a partial one. Thus, excessive calculations in C space can be eliminated.
In this way, the present invention can effectively reduce the operation planning man-hour by limiting the construction of the C space to a partial one without using the probabilistic method.

  Then, when it is determined that the distance candidate configuration point is not the interference point at the current position in the operation path, a straight travel mode path is generated, so that the target configuration point can be quickly reached until it is adjacent to the interference point. Therefore, it is possible to reduce useless motion and generate an efficient motion path of the robot arm.

  In addition, when it is determined that the distance candidate configuration point is the interference point, a path that follows the boundary of the obstacle is adopted by generating a boundary tracking mode path. It is possible to generate an operation path while reliably avoiding an interference area with an object.

  As described above, the path generation device according to the present invention generates an operation path from the initial configuration point to the target configuration point while sequentially generating the straight traveling mode path and the boundary following mode path, thereby obstructing the obstacle. Thus, it is possible to reduce the operation planning man-hours of the robot arm by eliminating excessive calculation in the C space while reliably avoiding the interference area.

  For this reason, the route generation device according to the present invention can be suitably applied even to an articulated robot having a high degree of freedom. For example, when generating the movement path of an articulated robot arm in a narrow work space where shrouds and multiple pipes coexist in nuclear power related equipment where safety is important, the interference area Therefore, it is possible to quickly simulate an operation path that can surely avoid the above-described problem, and therefore, a high utility value as a path generation tool is expected.

According to the present invention, the distance candidate determination means moves from the current position to the target configuration point in a direction along a virtual straight line path from the current position to the target configuration point among the current position adjacent configuration points . By selecting the configuration points so as to be closest, it is possible to generate an operation path while reliably avoiding an interference area with an obstacle even in a narrow space.

According to the present invention , the configuration point encountered up to the current position is excluded, and the route search is always performed for a new configuration point. Therefore, the route is returned and the same route is reciprocated or the same route is circulated. It is possible to avoid deadlock.

According to the present invention, it can be provided by eliminating excessive computation in the configuration space, the path generation equipment capable of reducing the operation plan steps in the robot arm. Therefore, since the present invention can be applied smoothly even to an articulated robot having a high degree of freedom, it is particularly suitable when it is desired to perform a complex operation in a narrow work space in the nuclear power field or the like.

It is a perspective view which shows 6 axis | shaft structure in a robot arm. It is a figure which shows the relationship between the attitude | position of a robot arm in a biaxial robot arm, and an obstruction, (a) shows the relationship in real space, (b) shows the relationship in configuration space. It is a block diagram which shows the structure of the route generation apparatus which concerns on embodiment of this invention. It is a figure for demonstrating the specific method of the path | route production | generation when the convex interference area in C space is assumed, (a) is a straight-ahead mode path | route, (b) is a boundary following mode path | route, (c) is a figure. Straight mode path. It is a figure for demonstrating the effect | action of the deadlock avoidance means at the time of assuming the concave-shaped obstacle in C space. It is a flowchart for demonstrating operation | movement of the route generation apparatus which concerns on embodiment of this invention. It is a perspective view which shows the image of operation | movement from the initial position of a robot arm to a target position, (a) is an initial position, (b) is a target position, (c) shows the image of operation | movement from an initial position to a target position. .

  A route generation device 1 (see FIG. 3) according to an embodiment of the present invention will be described in detail with reference to the drawings as appropriate. The path generation device 1 will be described by taking as an example a case where the operation path of the robot arm 100 (see FIG. 1) having six axes is generated using the configuration space (C space). In general, the robot arm 100 is not particularly limited as long as it is widely considered as a moving body in which the robot arm itself moves.

As shown in FIG. 1, the robot arm 100 includes six axes J1, J2, J3, J4, J5, and J6 including six joints (six degrees of freedom).
Assuming that a parameter representing the degree of freedom of the robot arm is a joint angle and the joint angles of the six axes J1, J2, J3, J4, J5, and J6 of the robot arm 100 are θ1, θ2, θ3, θ4, θ5, and θ6, The configuration (posture) of the robot arm 100 can be represented by θ1, θ2, θ3, θ4, θ5, and θ6.

  As described above, when the parameters representing the degree of freedom of the robot arm 100 (for example, the joint angle and the expansion / contraction length) are coordinate axis values, the configuration points of the robot arm 100 can be expressed as points on the C space. .

  For example, as shown in FIG. 2, for simplification, for example, a two-axis (θx, θy) robot arm 100 ′ (see FIG. 2A) in which joint angles can be expressed by θx and θy in real space. If the horizontal axis represents θx and the vertical axis represents θy, the configuration (posture) of the robot arm 100 ′ can be represented as configuration points (θx, θy) on the C space.

On the other hand, a configuration point where the robot arm 100 ′ interferes (collises) with an obstacle Ob (Obscale) in the work space is defined as an interference point. Interference points are rarely present alone, and can generally be represented on the C space as an interference area CA (Collision Area) which is a set of continuous points (see FIG. 2B).
In this way, as shown in FIG. 2B, the interference between the robot arm and the obstacle in the operation path is checked by representing the configuration of the robot arm 100 ′ and the interference area CA on the C space. be able to.

  In FIG. 2, the two axes (θx, θy) have been described. However, the six axes (θ1, θ2, θ3, θ4, θ5, θ6) like the robot arm 100 (FIG. 1) can be used in the same way even if the calculation amount is different. It is also possible to develop and process. For this reason, as shown in FIG. 4 and FIG. 5, it is easier to understand if only two axes are shown in a plan view, and therefore it is simplified for convenience.

As shown in FIGS. 3 and 4, the path generation device 1 sequentially sets new configuration points qn (for example, FIG. 5A) so as to avoid the interference area CA from the initial configuration point qi to the target configuration point qg. (Refer to FIG. 4 and FIG. 7).
Here, as shown in FIG. 4, the operation plan using the C space connects the initial configuration point qi (initial) and the target configuration point qg (goal) while avoiding the interference area CA on the C space. In this method, a route is generated and replaced with a route in real space to search for an operation route.

As shown in FIG. 3, the route generation device 1 includes an operation panel 2 on which an operator (not shown) inputs data, an arithmetic processing device 3 including a CPU (not shown), and a storage device that stores various data. 4 is provided.
The operation panel 2 is provided with a display 21 so that the robot arm 100 and the work space can be displayed in 3D (3D) graphics so that an operator (not shown) can visually recognize the generated operation path. Yes.

  As shown in FIGS. 3 and 4, the arithmetic processing unit 3 includes a current position detecting unit 31, a first candidate determining unit 32 for obtaining a current position adjacent configuration point qc adjacent to the current position qp, and an adjacent interference point. Second candidate determination means 33 for determining the interference point adjacent configuration point qca to be performed, interference check means 34 for determining whether the interference point is an interference point, and distance candidate configuration so as to approach the target configuration point qg from the current position qp Distance candidate determining means 35 for selecting points, and deadlock avoiding means 36 for excluding configuration points that have passed to the current position qp.

  The storage device 4 includes an initial position storage unit 41, a target position storage unit 42, a robot shape storage unit 43 that stores shape data of the robot arm 100 (FIG. 1), and a work space storage that stores shape data of the work space. Means 44, route storage means 45 for storing the route to the current position qp, and interference point storage means 46 for storing the found interference point.

Hereinafter, specific means constituting the route generation device 1 and its operation and effects will be described in the order of operations of the route generation device 1 with reference to FIGS. 3 and 4.
In FIG. 4 to be referred to, the vertical grid lines in the figure displayed as thin lines are indicated by a to l and the horizontal grid lines are indicated by signs from 1 to 11, and the intersection of these grid lines is defined as a grid point. Displayed as good (a, 1) to (l, 11). However, the display of the grid line spacing and the like is schematic, and the aspect ratio and the like are simplified.

The lattice point is a concept for finely dividing the C space and handling it as a set of discretized configuration points. In this way, the C space is not recognized as a continuous space, but is discretized (quantized) with reference to the lattice point, and the new configuration point is adopted so as to pass through the quantized lattice point. By generating the route, the arithmetic processing on the C space can be simplified.
For example, the joint angle θ1 to θ3 of the base portion of the robot arm 100 is set in increments of 1 and the joint angle θ4 to θ6 of the hand portion is set in increments of 5 degrees to set a lattice point in the C space, and this lattice point is used as a reference. Thus, a discretized C space can be constructed.

The initial position storage means 41 is a device that stores the initial posture of the robot arm 100 (see FIG. 7A in the real space), which is the initial configuration point qi (see FIG. 4), based on the joint angle of each joint, etc. An operator (not shown) can input from the operation panel 2, or the arithmetic processing unit 3 can detect the initial position with a detector such as an angle sensor and set it as the initial configuration point qi.
For example, the initial configuration point qi represents the joint angle as a parameter such that qi = (θ1, θ2, θ3, θ4, θ5, θ6) = (− 170, 90, 0, 25, −85, 75). Can do.

The target position storage means 42 is means for storing the target posture of the robot arm 100 that is the target configuration point qg (see FIG. 7B in the real space) based on the joint angle of each joint, and the like. A target position (not shown) can be input, or the target position can be detected by the arithmetic processing unit 3 with a detector such as an angle sensor and set as the target configuration point qg.
For example, the target configuration point qg represents the joint angle as a parameter such that qg = (θ1, θ2, θ3, θ4, θ5, θ6) = (− 71, −75, 38, 80, −10, 25). be able to.

The robot shape storage means 43 stores the shape data of the robot arm 100 created from a CAD file or the like, but in order to simplify the data processing, for example, the shape data of the robot arm 100 is a minimum configuration such as a polygon called a polygon. Expressed as a set of units (for example, triangles), this polygon set can be expressed and stored as a shape on the C space.
With this configuration, the shape data of the robot arm 100 in the C space is simplified (discrete) by representing the shape data of the robot arm 100 in the real space as a set of polygons such as triangles and recognizing the shape as the shape on the C space. Calculation processing can be shortened.

The work space storage means 44 stores the shape data of the work space in which the obstacle Ob (see FIG. 7) is arranged. In order to simplify the data processing, for example, the shape data of the obstacle Ob is converted into a polygon (for example, a triangle). The set of triangles and the like can be expressed and stored as a shape on the C space.
With this configuration, it is possible to simplify (discretize) the shape data of the work space on the C space and shorten the arithmetic processing.

The current position detecting means 31 measures, for example, the six-axis joint angles (θ1, θ2, θ3, θ4, θ5, θ6) of the robot arm 100 (see FIG. 1) using an angle sensor or the like, and searches on the C space. This is a device for detecting the current position qp in the route.
The current position qp is updated every time a new configuration point qn (for example, see FIG. 5A) newly adopted from the current position qp is generated during the route search, but the position data of the current position qp is It is stored in the route storage means 45.

The route storage unit 45 is a storage device that stores a route configuration configuration point that forms a route from the initial configuration point qi to the current position qp.
For example, the route storage means 45 uses (f, 3) as a route configuration configuration point that forms a route to the current position qp (g, 4) at the current position qp (g, 4) shown in FIG. , (G, 4) are stored.

As shown in FIGS. 3 and 4, the first candidate determination unit 32 is an arithmetic unit that obtains a current position adjacent configuration point qc adjacent to the current position qp in the operation path. For example, the C space when the joint angle (θ1, θ2, θ3, θ4, θ5, θ6) of the robot arm 100 in the real space changes by the discretized rotation angle with respect to the configuration point at the current position qp. The adjacent configuration points on the top can be calculated.
Specifically, the first candidate judging means 32, when the initial configuration point qi (f, 2) in FIG. 4A is the current position qp, the configuration point (e, 1), (e , 2), (e, 3), (f, 1), (f, 3), (g, 1), (g, 2), and (g, 3) are obtained as the current position adjacent configuration point qc. .

As shown in FIG. 4B, the second candidate determination unit 33 obtains an interference point adjacent configuration point qca adjacent to the interference point with the interference area CA among the current position adjacent configuration points qc with respect to the current position qp. Arithmetic unit.
For example, on the C space represented by a two-dimensional plane, at the current position qp (g, 4) in FIG. 4B, the configuration points (f, 3), (f, 4), (f, 5), Since (g, 3), (g, 5), (h, 3), (h, 4), and (h, 5) are the current position adjacent configuration points qc, the second candidate judging means 33 Among these current position adjacent configuration points qc, (f, 4) and (h, 4) are obtained as interference point adjacent configuration points qca adjacent to the interference point.

  In this way, the route generation device 1 according to the embodiment of the present invention is adjacent to the interference point with the obstacle Ob among the current position adjacent configuration points qc by the first candidate determination unit 32 and the second candidate determination unit 33. Rather than determining the presence or absence of interference for all configuration points corresponding to the degree of freedom of the robot arm 100 by obtaining the interference point adjacent configuration point qca to be constructed, the C space is constructed by constructing the current position qp of the robot arm 100. And because it is limited to the part around the interference point, excessive computation in the configuration space can be eliminated.

  Therefore, the path generation device 1 can effectively reduce the operation planning man-hours by limiting the construction of the C space to a partial one without using a probabilistic method, and has a high degree of freedom with six axes. Even the robot arm 100 can be suitably applied.

As shown in FIGS. 3 and 4, the interference check unit 34 targets the current position adjacent configuration point, and the target current position adjacent configuration point is an interference point with the obstacle Ob (interference area CA). It is an arithmetic unit that determines whether or not there is.
That is, the interference check unit 34 stores the robot shape storage unit 43 from the joint angles (θ1, θ2, θ3, θ4, θ5, θ6) of the robot arm 100 (see FIG. 1) at the current position adjacent configuration point. Based on the stored shape data of the robot arm 100, the shape (posture) of the robot arm 100 at the current position adjacent configuration point is obtained, and the obstacle Ob (interference area) stored in the work space storage unit 44 is obtained. Check whether it interferes with the shape data of CA).

  For example, when the CAD data of the robot arm 100 and the obstacle Ob is represented as a set of polygons (for example, polygons such as triangles), the interference check unit 34 is the robot arm 100 at the configuration point adjacent to the current position. It is determined by determining whether or not the polygon set of the obstacle and the polygon set of the obstacle Ob are in geometric contact.

  Specifically, the interference check unit 34 configures the shape of one obstacle (polygon) that forms the shape of the robot arm 100 at the current position adjacent configuration point qc and the shape of the obstacle Ob on the C space. To determine the contact with one polygon (polygon) to determine whether or not the set of all polygons (polygon), respectively, to determine whether or not the robot arm 100 and the obstacle Ob interfere, It can be displayed on the display 21 (FIG. 3).

In this embodiment, it is determined whether or not a set of polygons is in contact with each other on the C space. However, the present invention is not limited to this, and according to the degree of freedom (N-dimensional) of the robot arm 100, The interference between the robot arm 100 and the interference area CA may be checked assuming this N-dimensional shape of the interference area CA.
Further, instead of polygons (polygonal shapes), the robot arm 100 and the obstacle Ob may be represented as a set of points, lines, or surfaces, and such a point-to-surface interference check or a line-to-surface interference check may be used. .
Furthermore, the present invention is not limited to the interference check on the C space, and an interference check using points, lines, or polygons in the CAD space or the real space can also be adopted.

The interference point storage means 46 is a storage device for storing the interference points encountered and found during the search of the operation path by the interference check means 34.
For example, the interference point storage means 46 has configuration points (f, 5), (g, 5) as interference points discovered by the interference check means 34 at the current position qp (g, 4) in FIG. , And (h, 5) are stored.

As shown in FIGS. 3 and 4, the distance candidate determination unit 35 determines the target position from the current position qp in the direction along the virtual straight line path from the current position qp to the target configuration point qg among the current position adjacent configuration points qc. This is an arithmetic unit that selects a distance candidate configuration point so as to be closest to the configuration point qg .
For example, when the initial configuration point qi (f, 2) is the current position qp, as shown in FIG. 4A, the distance candidate determination means starts from the current position (f, 2) to the target configuration point. The distance candidate configuration point (f, 3) is selected so as to be closest to the target configuration point qg from the current position (f, 2) in the direction along the virtual straight line route (a straight line indicated by a broken line) up to qg .

  As shown in FIGS. 3 and 4, the deadlock avoiding unit 36 is configured to store path configuration configuration points stored in the path storage unit 45 that configures a path that has passed to the current position qp among the current position adjacent configuration points qc. Are excluded from being adopted as the new configuration point qn.

Next, the operation of the route generation device 1 according to the embodiment of the present invention will be described with reference to FIGS. 4 and 5 and the flowchart of FIG. 6 as appropriate.
4 and 5 are diagrams for explaining a state in which an operation path of the robot arm 100 is generated by adopting the new configuration point qn so as to avoid an obstacle from the initial configuration point qi to the target configuration point qg. FIG. 4 shows an example in which the interference area CA has a convex shape, and FIG. 5 is a schematic diagram for explaining the operation of the deadlock avoiding means 36 in which the interference area CA has a concave shape.

[Store data]
An operator (not shown) operates the initial position data, target position data, shape data of the robot arm 100, and shape data of the obstacle Ob (FIGS. 2A and 7) on the operation panel. 6, the path generation device 1 stores the initial position data of the robot arm 100 as the initial configuration point qi in the initial position storage means 41 on the C space, as shown in FIG. The target configuration point qg is stored in the target position storage means 42, the shape data of the robot arm 100 (see FIG. 1) is stored in the robot shape storage means 43, and the obstacle Ob (FIGS. 2A and 7) is stored. The shape data is stored in the work space storage means 44 (see step S1 in FIG. 6).

Then, the path generation device 1 detects the current position qp on the C space by the current position detection means (step S2 in FIG. 6), and the interference check means 34 targets the current position adjacent configuration point as a target. becomes current position adjacent configuration point shall determine whether the interference point between each obstacle Ob (interference region CA).
Further, at this time, the interference check means determines whether or not the distance candidate configuration point (f, 3) selected by the distance candidate determination means 35 among the current position adjacent configuration points is an interference point with the obstacle Ob. (Step S3 in FIG. 6).

  As shown in FIGS. 3 and 4, the route generation device 1 is a straight-ahead mode route that linearly adopts a route from the initial configuration point qi and the current position qp toward the target configuration point qg (FIG. ), (C)) and a boundary following mode route (FIG. 4B) that adopts a route along the boundary of the interference area CA (steps S4 and S5 in FIG. 6). ).

[Straight mode route]
As shown in FIGS. 4A and 4C, in the straight traveling mode route, the distance candidate configuration point (f, 3) selected by the distance candidate determination unit 35 at the current position qp in the operation route is the interference check unit 34. Is determined not to be an interference point (Yes in step S3 in FIG. 6), the distance candidate configuration point (f, 3) is adopted as a new configuration point qn (step S9 in FIG. 6). This route is stored in the route storage means 45 and proceeds to the new configuration point qn (steps S4 to S9 in FIG. 6).

  Specifically, as shown in FIG. 4A, the route generation device 1 uses the first candidate determination unit 32 to configure the initial configuration point qi (f, 2) when the current position qp. Point (e, 1), (e, 2), (e, 3), (f, 1), (f, 3), (g, 1), (g, 2), and (g, 3) Is determined as the current position adjacent configuration point qc.

  Then, the route generation device 1 selects the configuration point (f, 3) by the distance candidate determination unit 35 so that the current position qp is closest to the target configuration point qg among the current position adjacent configuration points qc. To do.

  Then, the path generation device 1 determines whether or not the configuration point (f, 3) is an interference point by the interference check unit 34, and is not an interference point. Therefore, the configuration point (f, 3) is a path. After confirming that it is not a configuration point (step S6 in FIG. 6) and not a target configuration point (step S7 in FIG. 6), this configuration point (f, 3) is set as a new configuration point pn. Adopt (step S9 in FIG. 6).

  Similarly, as shown in FIG. 4A, the route generation device 1 further adopts a straight-ahead mode route from the configuration point (f, 3) and generates a route that goes to the configuration point (g, 4). To do.

  With this configuration, the path generation device 1 can generate the straight traveling mode path in this way, so that it can quickly reach the target configuration point until it is adjacent to the interference point, thereby reducing unnecessary operations. Thus, an efficient operation path of the robot arm 100 can be generated.

[Boundary following mode path]
In the boundary tracking mode route, as shown in FIG. 4B, the distance candidate configuration point selected by the distance candidate determination unit 35 at the current position qp in the operation route is determined to be an interference point by the interference check unit 34. In the case (No in step S3 in FIG. 6), among the interference point adjacent configuration points qca obtained by the second candidate determining unit 33, the distance candidate determining unit 35 approaches the target configuration point qg from the current position qp. In this way, the new configuration point qn is adopted, stored in the path storage means 45, and proceeds to the new configuration point qn (steps S5 to S9 in FIG. 6).

  Specifically, as illustrated in FIG. 4B, the route generation device 1 uses the first candidate determination unit 32 to configure the configuration point when the current position qp is the configuration point (g, 4). (F, 3), (f, 4), (f, 5), (g, 3), (g, 5), (h, 3), (h, 4), and (h, 5) Obtained as a position adjacent configuration point qc.

  Then, the path generation device 1 uses, for example, the configuration point (g, 5) so that the distance candidate determination unit 35 is closest to the target configuration point qg from the current position qp among the current position adjacent configuration points qc. Is selected.

  Then, the path generation device 1 determines whether or not the configuration point (g, 5) is an interference point by the interference check unit 34. In this case, the configuration point (g, 5) is the interference point. Therefore, among the interference point adjacent configuration points qca (f, 4), (h, 4), the second candidate determination unit 33 approaches the target configuration point qg from the current position qp by the distance candidate determination unit 35. Thus, the new configuration point qn (h, 4) is adopted, stored in the route storage means 45, and a route that goes to the new configuration point (h, 4) is generated (steps S6, S7, FIG. 6). S9).

At this time, the route generation device 1 stores the configuration point (g, 5), which is the interference point discovered by the interference check unit 34 during the route search, in the interference point storage unit 46 and stores it.
By storing the discovered interference points in this way, it is possible to avoid repeated operations and speed up the processing.

  Similarly, as illustrated in FIG. 4B, the path generation device 1 generates a path by adopting a boundary following mode path from the configuration point (h, 4) to the configuration point (k, 8). .

  With this configuration, when the distance candidate configuration point is determined to be an interference point, the route generation device 1 generates a boundary tracking mode route so that a route that travels in the direction of following the boundary of the obstacle Ob is generated. Therefore, it is possible to generate an operation path while reliably avoiding the interference area CA with the obstacle Ob even in a narrow space.

As shown in FIG. 4 (c), the route generation device 1 performs the configuration from the configuration point (k, 8) to the target configuration point qg (k, 8) in the same manner as the straight-ahead mode route shown in FIG. 4 (a). Generate a straight mode path up to.
In this way, the path generation device 1 sequentially generates the straight-ahead mode path (FIGS. 4A and 4C) and the boundary tracking mode path (FIG. 4B) from the initial configuration point qi. An operation path to the action point qg is generated.

Next, the operation of the deadlock avoiding means 36 (FIG. 3) when the interference area CA is concave will be described with reference to FIG. Since the straight traveling mode route and the boundary following mode route are the same as those in FIG.
As shown in FIG. 5 (a), the path generation device 1 generates a straight-ahead mode path from the initial configuration point qi (b, 5) to the configuration point (g, 8) and proceeds to the configuration point. From (g, 8) to the configuration point (j, 8), a boundary following mode is generated and proceeds.

  Here, when the current position qp is the configuration point (j, 8), the configuration point (i, 8) that is the path that has passed to the current position qp (j, 8) by the deadlock avoiding means 36. ) Is not adopted (steps S6 to S8 in FIG. 6), it is possible to avoid a deadlock that reciprocates the route and reciprocates the same route.

  Therefore, since the path generation device 1 always adopts a new configuration point, the configuration point (j, 8) adopts (j, 7) as the new configuration point qn (step S9 in FIG. 6). In this way, the path generation device 1 generates and proceeds with the boundary following mode from the configuration point (j, 6) to the configuration point (i, 6) (step S5 in FIG. 6).

  When the current position qp is the configuration point (i, 6), as shown in FIG. 5B, the configuration points (j, 7) and (h, 6) are interference point adjacent configuration points qca. However, since the configuration point (j, 7) is a path configuration configuration point that has passed to the current position qp, it is excluded from being adopted as the new configuration point qn by the deadlock avoiding means 36 ( Steps S6 to S8 in FIG.

For this reason, the path generation device 1 excludes the configuration point (j, 7) from being adopted as the new configuration point, so that the configuration point (j, 7) to (j, 6), (i, It is possible to avoid a deadlock that goes around the same path as in 6).
In this way, the path generation device 1 adopts (h, 6) as the new configuration point qn from the configuration point (i, 6), and similarly, as shown in FIG. The target configuration point qg is reached while generating a boundary following mode route and a straight traveling mode route from the movement point (h, 6).

Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and can be implemented with appropriate modifications.
For example, in the present embodiment, the interference point storage unit 46 is provided. However, the present invention is not limited to this, and the interference check unit 34 does not provide the interference point storage unit 46 each time, and the interference point storage unit 46 provides an interference point. It may be determined whether or not.

  In the present embodiment, the path generation apparatus using the configuration space has been described. However, the present invention is not intended to exclude a method such as interference check in the real space, but the actual operation of the robot arm 100 after the path generation in the present invention. Can be verified and verified in real space.

DESCRIPTION OF SYMBOLS 1 Path | route generator 2 Operation panel 3 Arithmetic processor 4 Storage device 21 Display 31 Current position detection means 32 First candidate judgment means 33 Second candidate judgment means 34 Interference check means 35 Distance candidate judgment means 36 Deadlock avoidance means 41 Initial position Storage means 42 Target position storage means 43 Robot shape storage means 44 Work space storage means 45 Path storage means 46 Interference point storage means 100, 100 'Robot arm CA Interference area Ob Obstacle qi Initial configuration point qg Target configuration point qn New Configuration point qp Current position qc Current position adjacent configuration point qca Interference point adjacent configuration point

Claims (1)

Having a control unit comprising an arithmetic processing unit and a storage unit;
A path generation device using a configuration space that generates a movement path of a robot arm by adopting a new configuration point so as to avoid an obstacle from an initial configuration point to a target configuration point,
The controller is
Initial position storage means for storing the initial configuration points;
Target position storage means for storing the target configuration points;
A current position detecting means for detecting a current position of the robot arm;
Path storage means for storing each configuration point constituting a path from the initial configuration point to the current position;
First candidate determination means for obtaining a current position adjacent configuration point adjacent to the current position in the motion path;
Second candidate judgment means for obtaining an interference point adjacent configuration point adjacent to an interference point with the obstacle among the current position adjacent configuration points;
The targeting of the current position adjacent configuration point, the interference checking unit to which the current location adjacent configuration point to determine whether the interference point of the obstacle,
Distance candidate determination for selecting a configuration point closest to the target configuration point from the current position in a direction along a virtual straight line path from the current position to the target configuration point among the configuration points adjacent to the current position Means, and
The interference check means further determines whether or not the distance candidate configuration point selected by the distance candidate determination means at the current position in the operation path among the current position adjacent configuration points is an interference point with the obstacle. Judgment
If the previous Ki距 away candidate configuration point is determined not to be the point of interference by the interference checking means, the stores the distance candidate configuration point to the path memory means adopted as the new configuration point Generate a straight mode path to the new configuration point,
Before when Ki距 away candidate configuration point is determined to be the point of interference by the interference checking means, among the interference point adjacent configuration point obtained by the second candidate determination means, the distance candidate determination In the direction along the virtual straight line path from the current position to the target configuration point by the means, the new configuration point is adopted so as to be closest to the target configuration point from the current position and stored in the path storage means. Generate a boundary-following mode path that goes to the new configuration point,
In this way, an operation path from the initial configuration point to the target configuration point is generated while sequentially generating the straight traveling mode path and the boundary following mode path,
A deadlock that excludes the current configuration adjacent to the current position from being adopted as the new configuration point with reference to the path configuration point stored in the path storage means that configures the path that has passed to the current position. A route generation device further comprising an avoidance unit.

Images