JP2000020117A - Method and device for planning operation route of robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a route planning method, where a route is quickly obtained, even for an articulated manipulator which has a large degrees of freedom in a three-dimensional work space. SOLUTION: This device is provided with a start and goal arrangement input means 101, a robot environment model input means 102 which inputs geometrical information or the like of an environment, a storage means 104, an interference check means 105, an inter-subgoal distance evaluation means 107, a subgoal oriented graph generation means 103 which generates a subgoal oriented graph based on evaluation of the interference check means and the inter-subgoal distance evaluation means, a subgoal sequence generation and sort means 106, a subgoal sequence selection means 108, an adjacent subgoal selection means 109, an arrangement space localizing means 110 which sets a local discrete C space, a local arrangement space internal route search means 111 which searches routes of a minimum gap in the local arrangement space made discrete, a route existence discrimination means 112, a route storage means 113, and a route output means 114.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention utilizes geometric model means on a computer which describes the geometrical shapes of a robot and a work environment and their arrangement, and interference inspection means on a computer for inspecting interference between models. Also, the present invention relates to a method and an apparatus for planning an operation path of a robot in which, when a start and a goal arrangement of the robot are given, the robot does not interfere with an obstacle in a work environment.

[0002]

2. Description of the Related Art Conventionally, a path planning problem for avoiding an obstacle by a robot can be generalized into a form of a moving path planning problem for points in a configuration space (hereinafter, referred to as a "C space"). The C space is a parameter space that uniquely determines the arrangement of the robot, and in the case of an articulated manipulator, matches the joint space. In the case of a mobile robot in a two-dimensional work space, the position (X, Y) of the robot coordinate origin relative to the work space coordinate system
And a three-degree-of-freedom space (X, Y, θ) in the direction θ of the robot. In the C space, an arbitrary arrangement of all the robots is represented by dots. In order to find a path that does not interfere with an obstacle in the C space, a space description of the C space is usually required.
That is, an obstacle in the C space (hereinafter referred to as “C obstacle”) is described using the geometrical shapes of the robot and the obstacle in the work space and their positional relationship, and the freedom (ie, non-interference) in the C space is described. It is necessary to find a space (hereinafter referred to as "C free space"). Robot path planning methods based on C-space descriptions are broadly classified into roadmap methods, cell decomposition methods, C-space discretization methods, and potential field methods. The difference between the roadmap method, the cell decomposition method, and the C space discretization method lies in the description method of the C space and the search method based on the description. The roadmap method and the cell decomposition method usually require an "explicit description" of the C space. In the case of a two-degree-of-freedom mobile robot in a two-dimensional work space, the C free space can be explicitly described. That is, the boundary of the C obstacle can be explicitly obtained. In the case of an articulated manipulator, there is no general method for analytically obtaining a C obstacle, and an approximate method based on discretization of C space such as a Slice Projection (Needle) method, a Swept Volume method, and a Point Evaluation method is used. It is common to find a spatial description.
In this sense, the roadmap method and the cell decomposition method are suitable for a path planning problem of a mobile robot in a two-dimensional working space, and the C-space discretization method is suitable for a path planning problem of an articulated manipulator. These conventional methods are described in the literature `` Y.
K.Hwang and N.Ahuja: Gross Motion Planning-- A Su
rvey, ACM Computing Surveys, Vol.24, No.3, pp.219-
291, 1992 ". The path planning problem is based on Computational Complexity Tho
ery) according to NP (Nondeterministic Polymonial)
It belongs to a very hard problem called --hard. As the degree of freedom of the robot, the number of obstacles, the number of surfaces of obstacles, and the like increase, it becomes impossible to obtain an obstacle avoidance route within a realistic time. The above conventional method
The roadmap method and the cell decomposition method are based on a mobile robot in a two-dimensional work space, a C-space discretization method (local search), a potential field method, and the like. The range of application is limited due to the condition of a 6-DOF manipulator in a three-dimensional workspace.

A discretization description of the C space is represented by a connectivity graph in which each C space region (hereinafter, referred to as a "cell") discretized at minute intervals is a node, and the adjacency of the cells is an arc. You. Each node has information on whether it is a free space or an interference space. For example, in the Point Evaluation method, the interference between the robot and the work environment at a representative point (normally, the geometric center of gravity) of each discretized cell is inspected using a geometric modeler and an interference checker, and the interference must be detected at the representative point. For example, the cell is set as a free space, and if interference is detected, the cell is set as an obstacle space. The basic route of the C-space discretization method is to find the path from the start placement node to the goal placement node on this connectivity graph using a graph search method such as vertical, horizontal, hill climbing, and A * search. Approach.
In the C-space discretization method, (i) a method in which a discretization description of the entire C space is obtained in advance, and (ii) a method in which the discretization description is not obtained in advance and the interference in the successive discretization cell is inspected in the search process There is a method to do. Defines the artificial potential function consisting of the sum of the attractive force acting on the robot from the goal arrangement, the repulsive force acting on the robot from obstacles, and the repulsive force that keeps the robot within its motion range. This is a method of searching for a route in a steep gradient direction. In the case of an articulated manipulator, it is difficult to explicitly describe the C obstacle, so several representative points are provided on the link, and the repulsive force acting on each representative point from the obstacle within a certain distance from each representative point Is defined in the workspace.

[0004]

However, in the above-mentioned conventional example, in the case of the C-space discretization method, the amount of calculation is reduced with respect to the degree of freedom of the robot, the total number of obstacle surfaces, and the like.
The storage area increases exponentially. Therefore, 6
It is not realistic to apply the basic approach of the C-space discretization method to manipulators with more degrees of freedom as it is, and it is necessary to limit the search space (local search) and to devise ways to improve search efficiency. . Various schemes for reducing the search space and improving the search efficiency using various heuristics (heuristics) have been proposed. However, there is still a problem that there are few practical schemes for obtaining a route at high speed. Therefore, the present invention solves the above-mentioned technical problems, and provides a practical robot motion path planning method and a practical robot motion path planning method in which a path can be obtained at high speed even for an articulated manipulator having a large degree of freedom in a three-dimensional work space. It is intended to provide.

[0005]

SUMMARY OF THE INVENTION In order to solve the above-mentioned problems, the present invention examines a geometric model of a robot and a work environment and a geometric model means on a computer which describes the arrangement of the robot and an interference between the models. When the start and goal positions of the robot are given, the robot and the obstacles in the work environment do not interfere with each other, and the robot's motion path from the start position to the goal position is determined. In the planning method, a start arrangement is defined as a start subgoal, a goal arrangement is defined as an end subgoal, and a point that divides a straight line path in a work space or an arrangement space connecting the start subgoal and the end subgoal of the robot into two is defined as a first subgoal candidate. Are represented as subgoals a and b, and a work space or an arrangement space connecting the subgoals a and b. The point at which the straight line path is divided into two is represented as a candidate for subgoal 1 as a new candidate for subgoal, and the interference between the robot and the obstacle in the candidate for subgoal 1 is inspected using the geometric model unit and the interference inspection unit. If no interference is detected in subgoal candidate 1, subgoal candidate 1 is replaced with new subgoal c.
As a result, a directed graph describing the connection relation of the subgoals represented by a → c → b is generated, and when an interference is detected in the subgoal candidate 1, a plurality of predetermined robot avoiding operation directions are determined from the subgoal candidate 1. Interference inspection is performed at a plurality of points where the robot is moved at appropriate distance intervals, and from the points where no interference is detected in each of the avoiding operation directions or some of them, a new subgoal d1, d2, d3,... are selected one by one, and a → d1 → b, a → d2 → b, a → d3 →
b,... recursively repeat the procedure for generating the directed graph of the subgoals represented by b,... until the distance between the subgoals becomes equal to or less than a predetermined value, and from the starting subgoal to the ending subgoal using the directed graph of the subgoals. It is characterized in that a plurality of subgoal paths to be reached, that is, a plurality of subgoal sequences are planned. Further, the present invention is characterized in that candidates for the avoiding operation direction of the robot are set using heuristics according to the work content, the work environment, and the structure of the robot. In addition, a path that passes through each subgoal or in the vicinity of each subgoal in the subgoal sequence is obtained at smaller intervals by using a graph search method in a discretized arrangement space. Further, continuous subgoals in the subgoal sequence are defined as sbn-1 and sbn-1.
bn, sbn + 1, when searching for a minutely spaced path from the vicinity of the subgoal sbn-1 to the vicinity of the subgoal sbn, the discretized local arrangement space including sbn-1 and sbn, and the center of sbn Set the neighborhood area as
The discretized cell to be searched is cp, and the distance between points x and y is d.
(X, y), when arbitrary real positive numbers are represented as a, b, and c,
a × d (sbn-1, cp) + b × d (cp, sbn) + c
× d (cp, sbn + 1) as an evaluation function, the discretized cell having the smaller evaluation function value is preferentially searched in the discretized local allocation space, and the search path of the discretized cell is the subgoal s
sbn and sn
discretized local configuration space containing bn + 1, and sbn + 1
Is newly set, and a route to the vicinity of sbn + 1 is similarly searched based on the evaluation function. Further, a route passing through each subgoal in the subgoal series or near each subgoal is
A local potential field based on the artificial potential function expressed as the sum of the attractive force acting on the robot from the next subgoal, the repulsive force acting on the robot from obstacles near the current subgoal, and the repulsive force for keeping the robot within its motion range It is characterized in that it is obtained at smaller intervals using the method. Further, the sum of the distances between the subgoals included in a plurality of sets of subgoals from the start arrangement to the goal arrangement of the robot is used as an evaluation function. One subgoal sequence is selected in order from the highest subgoal sequence, and a small interval path passing through each subgoal or in the vicinity of each subgoal in the selected subgoal sequence is obtained. If it is determined that the route does not exist in the local configuration space including the two subgoals, or if the route is not found within a certain period of time, the next higher priority is given to the two adjacent subgoals. A subgoal sequence that does not include the subgoal sequence is selected. The procedure for obtaining the is characterized by repeating from the start arranged to find a very small distance path to the goal arrangement, or a certain time. Or, for an articulated robot manipulator with a wrist and a hand, search for each subgoal in the subgoal sequence or a minute interval path passing near each subgoal, and for the position of the wrist point, search for a graph in the discretized placement space The method is characterized in that a hand posture based on a coordinate system fixed to a wrist point using a method is obtained by using a local potential method. Further, the subgoal candidate is characterized in that an arbitrary curve connecting the two subgoals is divided into two points as appropriate. Also,
As for the subgoal candidates when the interference is detected, an appropriate number of subgoals are set in order of the avoidance direction in which the number of consecutive non-interference subgoal candidates in each robot avoidance direction is large. Means for inputting a geometric model of the robot and the work environment; means for storing the geometric model; means for inputting the start and goal arrangement of the robot; and interference between the robot and the environment using the geometric model storage means. An inspection unit, a subgoal directed graph generation unit using the geometric model storage unit and the interference inspection unit, an inter-subgoal distance evaluation unit that evaluates a distance between subgoals, and a subgoal directed graph generated by the subgoal directed graph generation unit. A plurality of subgoal sequence generation / sorting means for generating and sorting a plurality of subgoal sequences from a start arrangement to a goal arrangement using the inter-subgoal distance evaluation means, and a plurality of subgoal series obtained by the subgoal sequence generation / sorting means One subgoal system from the series Subgoal sequence selecting means for selecting a subgoal sequence, an adjacent subgoal selecting means for selecting two adjacent subgoals from the subgoal sequence obtained from the subgoal sequence selecting means, and a local area including two adjacent subgoals obtained from the adjacent subgoal selecting means. An arrangement space localizing means for generating an arrangement space; a path search means in the local arrangement space for searching for a minutely spaced path passing through the adjacent subgoal or the vicinity of the subgoal in the local arrangement space; Route existence determining means for determining whether there is a route passing through the subgoal or near the subgoal,
A route storage means for storing the searched minutely-spaced path and a path output means for outputting the stored minutely-spaced path are provided.

According to the above configuration, the subgoal candidate 1 that divides the straight path of the adjacent subgoals a and b into two is set as a new subgoal c if no interference is detected.
A subgoal directed graph of c → b is created, and when interference is detected, an interference test is performed at a plurality of points in a plurality of predetermined robot avoidance directions, and new subgoals d1, d2,. d3 ... one by one, a → d1 → b, a → d2 → b, a → d3 →
A subgoal directed graph b,... is created, and a plurality of sets of subgoal sequences from the start subgoal to the end subgoal are planned. From the plurality of subgoal sequences, the path that passes through the subgoal or the vicinity of each subgoal in the subgoal sequence selected from the smallest sum of distances is calculated by a graph search method in the discretized position space, by discretized cell unit, By obtaining the details in smaller intervals, the route to the terminal subgoal can be obtained at high speed and practically. Alternatively, a method of obtaining a minutely spaced path in units of discretized cells in the position space discretized in this manner is not simply a method of finding a minutely spaced path passing through each subgoal. The subgoals are sbn-1, sbn, sbn + 1
When searching for a minutely spaced route from the vicinity of the subgoal sbn-1 to the vicinity of the subgoal sbn, sbn-1
, Sbn and a nearby area centered on sbn are set, and the discretized cell to be searched is cp, the distance d between points xy, and any real numbers are a, b, and c. , A × d (sbn−1, cp) + b × d (cp, sbn) +
By using c × d (cp, sbn + 1) as an evaluation function, this evaluation function preferentially searches for a small discretized cell, whereby a sum of distances to the terminal subgoals is smaller and a smooth path can be obtained. . Alternatively, the minute interval path in such a subgoal sequence can be obtained by the local potential method instead of the conventional C-space discretization method. By limiting the search range to a local C space that includes two subgoals of short, a more precise route search can be performed. Alternatively, for determination when a route is not found and route change, one subgoal sequence is selected in order from the subgoal sequence having the highest priority, and a minute interval passing through each subgoal in the selected subgoal sequence or near each subgoal is selected. When finding a route, if it is determined that the route from the vicinity of one subgoal to the vicinity of the next subgoal does not exist in the local layout space including the two subgoals, or if the route is not found within a certain time Is
Since the procedure for selecting a subgoal sequence having the next highest priority and obtaining a minutely-interval route is repeated, an efficient and high-speed route search can be realized without useless search. Or 3
Even when setting the avoiding operation direction of the articulated manipulator in the three-dimensional work space, in the C space, the hand tip position or the wrist point position is along the four directions in the horizontal plane and the five directions such as the vertical direction. By applying the route search method, it is possible to effectively reduce the search space of the route and quickly obtain the avoidance operation route.

[0007]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS (First Embodiment) A first embodiment of the present invention will be described below with reference to the drawings.
FIG. 1 is a block diagram of a robot motion path planning device according to a first embodiment of the present invention. FIG. 2 is a flowchart of the subgoal directed graph generation procedure shown in FIG. FIG. 3 is an explanatory diagram of the subgoal directed graph generation process shown in FIG. FIG. 4 is a diagram showing a subgoal directed graph generated by the subgoal directed graph generation means shown in FIG. FIG. 5 is an explanatory diagram of the minute interval route obtained by the route search in the local arrangement space shown in FIG. FIG. 6 is a diagram showing C space when the robot shown in FIG. 1 is a two-robot robot. First, the basic configuration of the present embodiment will be described with reference to FIG.
This will be described with reference to FIG. FIG. 6 (a) shows two-dimensional work space.
FIG. 6B illustrates a two-degree-of-freedom mobile robot in a two-dimensional work space.

In FIGS. 6A and 6B, 701
Denotes a two-degree-of-freedom manipulator, 702 denotes its end effector, and 703 denotes a two-degree-of-freedom mobile robot. 7
04 is an obstacle in the work space. Position of 2-DOF robot end effector 702, mobile robot 703
Is represented as p (x, y). In addition, the manipulator 7
The first and second joint angles of 01 are represented by q1 and q2, respectively. Reference numeral 705 in FIG. 6C represents a joint space of the two-degree-of-freedom manipulator 701, that is, a C space. The C space of the mobile robot 703 is (x, y), but both are the same two-dimensional C space, and FIG. 6C shows a manipulator,
It represents the C space of an arbitrary two-degree-of-freedom robot without distinction between mobile robots. In FIG. 6C, 706, 7
Reference numeral 07 denotes a start arrangement S and a goal arrangement G of the robot. Hereinafter, this embodiment is generally described with reference to FIG.
(1) A method and apparatus for planning a non-interference path from S to G when given geometric information of a robot and a work environment, a start arrangement S, and a goal arrangement G as shown in (a) and (b). It is.

In FIG. 1, reference numeral 101 denotes a start / goal arrangement input means for inputting a start arrangement S, a goal arrangement G and the like as shown in FIG. Hereinafter, each means described here is a function set on the computer. 1
Numeral 02 denotes a robot / environment model input means for inputting geometric information of the robot and the working environment as shown in FIG. Numeral 104 denotes a storage unit for storing a geometric model describing the geometrical shapes of the robot and the work environment. Numeral 105 denotes an interference inspection unit, which refers to a geometric modeler of the storage unit 104 and determines whether or not there is interference between the models. The above determination means. 107 is an inter-subgoal distance evaluation means for determining the distance between the sub-goals, and 103 is an interference inspection means 1
Subgoal directed graph generation means for generating a subgoal directed graph from the start subgoal S to the end subgoal G based on the evaluation by the subgoal distance evaluation means 107 and the subgoal distance evaluation means 107. Reference numeral 106 denotes a subgoal sequence generating / sorting unit. The subgoal directed graph generated by the subgoal directed graph generating unit 103 realizes some subgoal sequences on a computer, and calculates the sum of distances from the starting subgoal S to the ending subgoal G. Sort in ascending order. 1
08 is a subgoal sequence selecting means, for example, selecting and extracting subgoal sequences in the sort order. 109 is an adjacent subgoal selecting means, which is a subgoal sequence selecting means 108.
The two adjacent subgoals in the subgoal sequence selected in are selected in order from the start arrangement. Reference numeral 110 denotes an arrangement space localizing means for setting a local discretized C space for searching a minutely spaced path by the C space discretization method. 111
Is a route search means in the local arrangement space, and performs a minute interval route search in the local discretized C space. Reference numeral 112 denotes a route existence determination unit that determines whether a minute interval route exists as a result of the minute interval route search. A route storage unit 113 stores the obtained search route. Reference numeral 114 denotes a route output unit which outputs a route held in the storage unit 113 in response to a request.

Next, the operation will be described. FIG. 3 is a view for explaining a process of generating a subgoal directed graph in the (obstruction avoidance) motion path planning method according to the present invention.
In FIG. 3, reference numeral 301 denotes a two-dimensional C space;
Indicates a start arrangement S and a goal arrangement G of the robot, respectively. The start arrangement S and the goal arrangement G are expressed as a start subgoal S and an end subgoal G, respectively.
Treated as a subgoal for convenience. First, in FIG. 3A, the middle point of a straight line connecting the start subgoal S and the end subgoal G is set as a subgoal candidate 304, and a geometric model storage unit 104 on a computer that describes the geometrical shapes of the robot and the work environment. The interference between the robot and the work environment in the subgoal candidate 304 is inspected by using the interference inspection means 105 on the computer that determines the interference between the models. The subgoal digraph generation means 103 generates a subgoal candidate 304
If no interference is detected in the subgoal candidate 304, as shown in FIG.
(Hereinafter referred to as a0) and the directed graph S of the subgoal
→ a0 → G is generated. When interference is detected in the subgoal candidate 304, a plurality of new subgoal candidates are set from that point along a plurality of robot avoiding operation directions. In FIG. 3C, reference numeral 306 denotes a candidate for a subgoal in which interference has been detected, and reference numerals 307 and 308 denote directions of the robot's avoidance operation. 307a, 307b, and 307c indicate subgoal candidates along the robot avoiding motion direction 307 as 3
08a, 308b, 308c are robot avoiding motion directions 3
Subgoal candidates along 08 are shown. Then, the subgoal candidates 307a, 307b, 307c, and 3
The interference at 08a, 308b, and 308c is inspected, and one new subgoal is set for each robot avoiding operation direction from the subgoal candidates for which no interference was detected. In FIG. 3D, reference numeral 309 denotes a new subgoal b0 along the robot operation avoiding direction 307.
310 indicates a new subgoal c0 along the robot operation avoiding direction 308. New subgoal b0,
For c0, a directed graph S → b0 → G and S → c0 → G of the subgoal are generated. The above procedure is recursively repeated for any two adjacent subgoals until the distance between the subgoals becomes equal to or smaller than a certain value. FIG. 4 shows a directed graph of subgoals obtained by recursive repetition of the above procedure. In FIG.
1, 402, and 403 are a two-dimensional C space, a starting subgoal S, and an end subgoal G, respectively. 404 is each subgoal. In FIG. 4, no particular reference is given,
All symbols similar to 04 represent subgoals. The subgoal directed graph is a subgoal directed graph generator 10.
According to 3, it can be realized on a computer by a data structure such as a linked list or an array. 3 and 4
Although the midpoint of the straight line in the C space connecting the two subgoals is set as the subgoal candidate, the midpoint of the straight line in the work space may be set as the subgoal candidate. Further, a point that arbitrarily divides an arbitrary curve connecting two subgoals into two may be used as a subgoal candidate. In FIG. 3C, the subgoal candidates along the robot avoiding operation directions 307 and 308 are set at equal intervals, but the subgoal candidates may be set at appropriate intervals. Also, it is not always necessary to set subgoals for all of the plurality of robot avoiding motion directions.
For example, when performing an interference test on subgoal candidates along each robot movement avoiding direction, for each avoiding direction, check how many consecutive subgoal candidates for which no interference was detected are found, and then select a continuous non-interfering subgoal candidate. The appropriate number,
A subgoal may be set. Also, in FIGS. 3 and 4, two robot avoiding motion directions are set in a direction perpendicular to a straight line in the C space connecting the subgoals, but any number of robot avoiding motion directions may be set based on other appropriate criteria or algorithms. Alternatively, the avoidance operation may be determined in any direction.

FIG. 2 shows a flowchart of the above-described subgoal directed graph generation procedure. In FIG. 2, when the above-described subgoal directed graph generation procedure is disregarded, the start arrangement S and the goal arrangement G are input (S200), and the creation of the subgoal directed graph by the subgoal directed graph generation means 103 is started (S201). The interval between each subgoal is confirmed based on the interval evaluation by the inter-subgoal distance evaluation means 107 (S202 to S204). If there is an interval equal to or more than a predetermined distance, a new subgoal candidate is created (S205), and interference check is performed by the interference inspection means 105 (S20).
6) If non-interference is confirmed, it is added as a new subgoal (S209), and a subgoal directed graph is created (S2).
01), and confirm the interval between the subgoals (S202-20)
4), when the distance becomes equal to or less than the certain distance, the process is terminated (S210).
On the other hand, if interference is confirmed in the interference check in S206,
A subgoal candidate in the avoiding direction is created (S207), and interference check and subgoal selection are performed again (S20).
8). Next, a subgoal path (subgoal sequence) from the starting subgoal S to the ending subgoal G is obtained using the subgoal directed graph obtained by the above procedure. In the example of FIG. 4, there are the following five subgoal sequences.
The subgoal sequence can be realized on a computer by a linked list having the joint angle of each subgoal as an element. (1) S → ・ ・ ・ → d1 → ・ ・ ・ → b0 → ・ ・ ・ → f1 → h1 → G (2) S → ・ ・ ・ → d1 → ・ ・ ・ → b0 → ・ ・ ・ → f1 → i1 → G (3) S → ・ ・ ・ → e1 → ・ ・ ・ → b0 → ・ ・ ・ → f1 → h1 → G (4) S → ・ ・ ・ → e1 → ・ ・ ・ → b0 → ・ ・ ・ → f1 → i1 → G (5) S → ... → c0 → j1 → g1 → ... → G When obtaining a subgoal sequence linked list as shown in the above equations (1) to (5), a subgoal sequence is generated. The sum of distances between subgoals is calculated by the sorting means 106. A header linked list having, as elements, a distance sum of each subgoal series and a pointer to the head element of each subgoal series linked list is created. By sorting the header linked list in ascending order of the distance sum, data can be taken out in order from the subgoal sequence having the smallest distance sum. Next, for the subgoal sequence having the smallest sum of the distances between subgoals, the arrangement space localization means 110 and the path search means 111 in the local arrangement space form a small interval path connecting the subgoals as shown in FIG. Determined based on the discretization method. In FIG. 5, reference numeral 602 denotes a start arrangement, that is, a starting subgoal. 603a, 603
b and 603c are continuous subgoals sb1 and sb, respectively.
b2 and sb3. Reference numeral 601 denotes a discretized local C space including sb1 and sb2. Each of the minute squares in 601 is a discretized cell. The route from one subgoal (sb1) to the next subgoal (sb2)
In each discretized cell in the local discretized C space 601, a search is performed sequentially using a graph search method such as A * while checking interference between the robot and the work environment. 604 indicates the searched route. When the route reaches the subgoal sb2, the subgoal sb2 and the next subgoal sb3
Is newly set again, and the minute interval path to the subgoal sb3 is searched in the same manner. Even if all the cells in the local discretized C space 601 are searched or a search is performed for a fixed time, if a path from the subgoal sb1 to the next subgoal sb2 in the local discretized C space cannot be found, The subgoal sequence that does not include the two subgoal sequences and has the next smaller sum of distances is selected from the subgoal sequence linked list, and the minute interval path is similarly obtained. This procedure is repeated until a very small interval path from the start arrangement S to the goal arrangement G is determined or for a predetermined time. In this way, even if the backtracking is repeated for a certain period of time, if the minute interval path from the start arrangement S to the goal arrangement G is not found, or if the minute interval path search fails for all subgoal sequences, The route existence determining means 113 determines that the route does not exist.

(Second Embodiment) Next, a second embodiment of the present invention will be described.
The embodiment will be described with reference to the drawings. FIG. 7 is a diagram showing a process of searching for a minute interval path by the C space discretization method according to the second embodiment of the present invention. The subgoal series as shown in the previous embodiment merely indicates that if the direction near each subgoal is traced, it is highly likely that the goal arrangement can be efficiently reached. There is not much need to find a micro-interval route through. In addition, there are many cases where the route is not smooth. Therefore, in the second embodiment, as shown in FIG. 7, neighboring areas 605a and 605b centered on each of the subgoals sb1 and sb2 in the locally discretized C space are set, and minute areas passing through the respective subgoal neighboring areas are set. It is configured to search for an interval route. Now, when searching for a micro-interval route from the vicinity of the subgoal sbn-1 to the vicinity of the subgoal sbn, assuming that the discretized cell to be opened is cp, f (cp) = d (sbn-1, cp) + 2 × d ( cp, sbn) + d (cp, sbn + 1)... (6) is used as an evaluation function, and a discretized cell having a small evaluation function value is preferentially searched. Here, d (x, y) represents the distance from point x to point y. When the search path of the discretized cell enters the area near the subgoal sbn, the local discretized C space including sbn and sbn + 1 is newly set, and the path to the vicinity of sbn + 1 is expressed by Equation (6). Is similarly searched based on the evaluation function of. However, no neighborhood area is set for the terminal G. In this way, the sum of distances to the terminal subgoal is improved, and a smoother path is obtained.

(Third Embodiment) Next, a third embodiment of the present invention will be described.
An embodiment will be described. In the third embodiment,
In contrast to the search performed in the previous embodiment using the C-space discretization method, the search is performed using the local potential method. In order to find the sub-goals in the subgoal sequence from the starting subgoal to the ending subgoal, or the minutely spaced paths passing through each subgoal neighboring area by the local potential method, two subgoals are used in the same manner as the above method based on the C space discretization method. Is set, and a path in the local C space is searched more finely. In the potential field method, a problem has been pointed out that the path drops to a potential minimum point other than the target arrangement, but in the method according to the present invention,
Sometimes this problem can be avoided. In other words, by limiting the search range to a local C space that includes two subgoals that are relatively short in distance, the possibility that a local minimum value of the local potential function occurs is reduced. In addition, by searching for a route passing near each subgoal, even if a potential minimum exists, if the minimum is in the area near the target subgoal, the minimum does not matter. This is because, when the target sub-goal is reached, the next subgoal is used as a new target, and the potential function is redefined to search for a route. In this way, the local potential method can be effectively introduced.

(Fourth Embodiment) Next, a fourth embodiment of the present invention will be described.
The embodiment will be described with reference to the drawings. FIG. 8 is a diagram showing a working environment of a 6-DOF manipulator according to the fourth embodiment of the present invention. In FIGS. 3 and 4 of the first embodiment, a two-degree-of-freedom robot has been described as an example for simplicity. However, for a multi-degree-of-freedom robot having two or more degrees of freedom, a subgoal directed graph can be similarly obtained. it can. FIG. 8A shows an articulated manipulator in a three-dimensional working space. In FIG. 8A, 501a, 501b, 501c, 501d, 501
e and 501f are the shoulders of the articulated manipulator,
The upper arm, elbow, forearm, wrist, and hand are shown. Also, 5
02 is a work table, and 503 is an obstacle on the work table 502. In FIG. 5B, reference numeral 510 denotes a base coordinate system of the articulated manipulator, and 511 denotes a point Wp at which the wrist point 501e is projected on the xy plane. The positive Z direction of the base coordinates 510 coincides with the vertically upward direction. That is, the xy plane is a horizontal plane. In many handling operations such as pick-and-place operations, as shown in FIG. 8A, the work table is installed horizontally, and the manipulator is installed vertically. In many cases, obstacles are also set vertically. In such a case, the avoiding operation of the manipulator is performed in the vertical upward direction indicated by an arrow 504, the inner rotation (within a horizontal plane) with respect to the manipulator base indicated by an arrow 505, 5.
Even if it is limited to the three outer circumferential patterns shown in FIG. 06 and clockwise and counterclockwise in the horizontal plane, there is often no practical problem. Therefore, the robot avoiding operation direction is set based on the pattern classification. In FIG. 8B, reference numeral 507 denotes a positive Z-axis direction, reference numeral 508a denotes a vector WpO direction, and reference numeral 508b denotes a vector OWp direction. 509a,
509b are two directions perpendicular to the straight line OWp in the horizontal plane. The robot avoiding operation direction is set in the space C such that the hand tip position or the wrist point position is along the four directions in the horizontal plane and the five directions vertically upward. As described above, the route search is performed by preliminarily determining the avoiding operation direction of the robot based on heuristics (that is, knowledge according to the situation obtained through experience) using the work content, the work environment, and the structure of the robot. The space can be greatly reduced, and a route can be obtained at high speed.

(Fifth Embodiment) Next, a fifth embodiment of the present invention will be described.
The embodiment will be described with reference to the drawings. In the case of the six-degree-of-freedom manipulator as shown in FIG. 8, as described above, first, a subgoal sequence is obtained, and a minute interval path passing through each subgoal or the vicinity of each subgoal is determined for the wrist point position path. In the C space discretization method, the hand posture path based on the wrist point coordinate system of the manipulator is obtained by the local potential method so that 2
A synergistic effect can be expected by combining the methods.

[0016]

As described above, according to the present invention,
The start arrangement is defined as the start subgoal, the goal arrangement is defined as the end subgoal, a point that divides a straight path in the work space or the arrangement space connecting the start and end subgoals of the robot into two is defined as the first subgoal candidate, and the adjacent subgoals are defined as subgoals a and b. A point that divides a straight path in the work space or the placement space connecting the two into two is represented as a subgoal candidate 1 and is set as a new subgoal candidate, and the interference between the robot and the obstacle in the subgoal candidate 1 interferes with the geometric model means. Inspection is performed using the inspection means, and if no interference is detected in subgoal candidate 1, subgoal candidate 1 is set as a new subgoal c and a
A directed graph describing the connection relation of the subgoals represented by → c → b is generated, and if interference is detected in the subgoal candidate 1, an appropriate distance from the subgoal candidate 1 in each of a plurality of predetermined robot avoiding motion directions is set. Interference inspection is performed at multiple points where the robot is moved at intervals,
For each of the avoiding motion directions or some of them, new subgoals d1, d2, d3,... Are selected one by one from points at which no interference is detected, and a → d1 →
b, a → d2 → b, a → d3 → b,... recursively repeat a procedure for generating a directed graph of subgoals until the distance between the subgoals becomes equal to or less than a predetermined value. By using a directed graph of subgoals, a subgoal path from a starting subgoal to an end subgoal, that is, a plurality of sets of subgoal sequences is planned, and a path connecting between subgoals or a path passing in the vicinity of each subgoal is defined in C space. The search space is greatly suppressed, the search efficiency is improved, and the articulated manipulator with a large degree of freedom in the three-dimensional work space is configured because the search space is calculated at a finer interval using the discretization method. There is an effect that a high-speed and practical route plan can be realized.

[Brief description of the drawings]

FIG. 1 is a block diagram of a motion path planning device for a robot according to a first embodiment of the present invention.

FIG. 2 is a flowchart of a subgoal directed graph generation procedure shown in FIG. 1;

FIG. 3 is an explanatory diagram of a subgoal directed graph generation process shown in FIG. 1;

FIG. 4 is a diagram showing a subgoal directed graph generated by the subgoal directed graph generation means shown in FIG. 1;

FIG. 5 is an explanatory diagram of a minute interval route obtained by searching for a route in the local arrangement space shown in FIG. 1;

FIG. 6 is a diagram showing a C space when the robot shown in FIG. 1 is a two-degree-of-freedom robot.

FIG. 7 is a diagram showing a process of searching for a minute interval path by a C-space discretization method according to the second embodiment of the present invention.

FIG. 8 is a diagram illustrating a working environment of a six-degree-of-freedom manipulator according to a fourth embodiment of the present invention.

[Explanation of symbols]

101 Start / Goal Arrangement Input Means 102 Robot / Work Environment Geometric Model Input Means 103 Subgoal Directed Graph Generation Means 104 Geometric Model Storage Means 105 Interference Inspection Means 106 Subgoal Sequence Generation / Sorting Means 107 Subgoal Distance Evaluation Means 108 Subgoal Sequence Selection Means 109 Adjacent subgoal selection means 110 Layout space localization means 111 Local layout space route search means 112 Route existence determination means 113 Route storage means 114 Route output means 301 C space 302 Start layout 303 Goal layout 304 A straight route connecting 302 and 303 is defined as 2 Subgoal candidate to be divided 305 Subgoal to divide the straight path connecting 302 and 303 into two 306 Subgoal candidate in which interference is detected 307 Robot avoiding operation direction 307a, b, c Robot Subgoal candidate along the avoiding motion direction 307 Robot avoiding motion direction 308 a, b, c Subgoal candidate along the robot avoiding motion direction 309 Subgoal selected from the robot avoiding motion direction 307 Selected from the robot avoiding motion direction 308 Subgoal 401 C space 402 Start arrangement 403 Goal arrangement 404 Subgoal 501a Manipulator shoulder 501b Manipulator upper arm 501c Manipulator elbow 501d Manipulator forearm 501e Manipulator wrist 501f Manipulator hand 502 Work table 503 Obstacle path 504 Obstacle obstacle 504 505 Obstacle avoidance route in horizontal plane inward for manipulator 506 Obstacle avoidance route in horizontal plane outward for manipulator 507 Manipulator avoiding operation direction in the vertical upward direction 508a Manipulator avoiding operation direction in the WpO direction 508b Manipulator avoiding operation direction in the OWp direction 509a, b Manipulator avoiding operation direction in a horizontal plane perpendicular to WpO 510 Base coordinate system of manipulator 511 manipulator Point Wp 601 at which the wrist point is projected on the horizontal plane (XY plane) Localized discretized C space 602 Start arrangement 603a, b, c Subgoals sb1, sb2, sb3 604 Searched discretized paths 605a, b Neighboring areas of subgoals 603a, b 701 Two-degree-of-freedom manipulator in two-dimensional workspace 702 End-effector of two-degree-of-freedom manipulator in two-dimensional workspace 703 Two-degree-of-freedom mobile robot in two-dimensional workspace 704 Obstacle 705 Space 706 start arrangement 707 goals arrangement

 ──────────────────────────────────────────────────続 き Continued on the front page F term (reference) 5H004 GA29 GB16 HA07 HB07 JA03 KA71 KD70 LA18 MA36 5H269 AB33 BB05 BB13 BB14 CC09 DD05 NN16 NN17

Claims (10)

[Claims] The present invention utilizes a geometric model on a computer which describes the geometrical shapes of a robot and a work environment and their arrangement, and an interference inspection unit on a computer for inspecting interference between the models. A motion path planning method for planning a motion path of a robot from a start position to a goal position without causing interference between a robot and an obstacle in a work environment when a start position and a goal position are given. The goal arrangement is defined as an end subgoal, a point that divides a straight line path in a work space or an arrangement space connecting a start end subgoal and an end subgoal of a robot into two is defined as an initial subgoal candidate, and adjacent subgoals are expressed as subgoals a and b. Points that divide the straight line path into two in the work space or layout space connecting A candidate for a new subgoal is represented as a goal candidate 1, and the interference between the robot and the obstacle is inspected in the subgoal candidate 1 by using the geometric model means and the interference inspection means. Is not detected, a directed graph describing the connection relation of the subgoals represented by a → c → b is generated using the subgoal candidate 1 as a new subgoal c, and (ii) interference is detected in the subgoal candidate 1 When the sub-goal candidate 1 is checked, an interference test is performed at a plurality of points at which the robot has been moved at appropriate distance intervals in each of the plurality of predetermined robot avoiding operation directions, and each of the avoiding operation directions or the For some of the cases, a new subgoal d
1, d2, d3, ... one by one, a → d1 →
b, a → d2 → b, a → d3 → b,... recursively repeat a procedure for generating a directed graph of subgoals until the distance between the subgoals becomes equal to or less than a predetermined value. A motion path planning method for a robot, comprising planning a plurality of subgoal paths from a starting subgoal to an end subgoal, that is, a plurality of subgoal sequences, using the directed graph of the subgoals. 2. The robot motion path planning method according to claim 1, wherein a candidate for the avoiding motion direction of the robot is set using heuristics according to the work content, the work environment, and the structure of the robot. Robot path planning method. 3. The robot motion path planning method according to claim 1, wherein a path passing through each subgoal or each subgoal in the subgoal sequence is graph-searched in a discretized arrangement space. A motion path planning method for a robot, wherein the motion path is determined at smaller intervals. 4. The robot motion path planning method according to claim 3, wherein successive subgoals in the subgoal sequence are represented by sb.
When expressed as n-1, sbn, sbn + 1, the subgoal sb
When searching for a minutely spaced route from the neighborhood of n-1 to the neighborhood of the subgoal sbn, a discretized local arrangement space including sbn-1 and sbn, and a neighborhood area centered on sbn are set. A × d (sbn−1, cp) + b × d, where cp is a transformed cell, d (x, y) is the distance between points x and y, and a, b, and c are any real positive numbers. (Cp, sbn) +
c × d (cp, sbn + 1)} is used as an evaluation function, and a discretized cell having a small evaluation function value is preferentially searched in the discretized local allocation space, and a search path of the discretized cell is a subgoal sbn At the time of entering the neighborhood, a discretized local arrangement space including sbn and sbn + 1, and a neighborhood near the sbn + 1 are newly set, and a path to the neighborhood of sbn + 1 is set to the evaluation function. A motion path planning method for a robot, wherein a similar search is performed based on the same. 5. The robot motion path planning method according to claim 1, wherein each of the subgoals in the subgoal sequence or a path passing near each subgoal is drawn from the next subgoal to an attractive force acting on the robot. Using the local potential field method based on the artificial potential function represented by the sum of the repulsive force acting on the robot from obstacles near the subgoal and the repulsive force to keep the robot within the motion range, it is necessary to find it at smaller intervals. Characteristic robot motion path planning method. 6. The robot motion path planning method according to claim 1, wherein a sum of distances between subgoals included in a plurality of sets of subgoal sequences from a start arrangement to a goal arrangement of the robot is calculated. As the evaluation function, a higher priority is set in order from the subgoal sequence having the smaller evaluation function value, one subgoal sequence is selected in order from the subgoal sequence having the higher priority, and each subgoal in the selected subgoal sequence or each subgoal is selected. A micro-interval route that passes through the neighborhood is obtained, and at this time, when it is determined that a route from the vicinity of one subgoal to the vicinity of the next subgoal does not exist in the local layout space including the two subgoals, or within a predetermined time, If no route is found, then the priority is higher and the two adjacent subgoals are A procedure for selecting a subgoal sequence not included and repeating the procedure for obtaining a minutely spaced route for the subgoal sequence in the same manner until a minutely spaced route from the start arrangement to the goal arrangement is found or for a certain period of time. Motion path planning method. 7. The robot motion path planning method according to claim 6, wherein the articulated robot manipulator having a wrist and a hand is provided with a wrist and a small interval path passing through each subgoal in the subgoal sequence or near each subgoal. The point position is obtained using a graph search method in a discretized arrangement space, and the hand posture based on a coordinate system fixed to a wrist point is obtained using a local potential method. Robot path planning method. 8. The robot motion path planning method according to claim 1, wherein the subgoal candidate is a point obtained by dividing an arbitrary curve connecting the two subgoals into two. 9. The subgoal candidate when the interference is detected, sets an appropriate number of subgoals in order from the avoidance direction in which the number of consecutive non-interference subgoal candidates in each robot avoidance direction is large. Item 3. The method for planning a motion path of a robot according to item 1 or 2. 10. A means for inputting a geometric model of a robot and a work environment; a means for storing the geometric model;
Means for inputting the start and goal arrangement of the robot, means for inspecting interference between the robot and the environment using the geometric model storage means, and means for generating a directed graph for subgoals using the geometric model storage means and the interference inspection means; A sub-goal distance estimating means for evaluating a distance between sub-goals; a plurality of sets of subgoal sequences from a start arrangement to a goal arrangement using a subgoal directed graph generated by the directed subgoal directed graph generation means and the subgoal distance evaluation means; Subgoal sequence generating / sorting means for sorting the subgoal sequence, subgoal sequence selecting means for selecting one subgoal sequence from a plurality of sets of subgoal sequences obtained by the subgoal sequence generating / sorting means, and subgoal obtained from the subgoal sequence selecting means series Subgoal selecting means for selecting two adjacent subgoals from the subgoal, layout space localizing means for generating a local layout space including two adjacent subgoals obtained from the adjacent subgoal selecting means, and A route search means in a local placement space for searching for a minutely spaced route passing through an adjacent subgoal or a vicinity of a subgoal; a route existence determining means for determining the presence or absence of a route passing in the local placement space in the vicinity of an adjacent subgoal or a subgoal; An operation path planning device for a robot, comprising: a path storage means for storing the stored micro-interval path; and a path output means for outputting the stored micro-interval path.