JP2000339012A - Robot global motion path planning method and its control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a practical robot route planning method capable of quickly finding out a non-interference route and reducing the missing ratio of a searching route. SOLUTION: The robot global operation route planning method for executing two- step route search by using a geometrical model means for describing the arrangement of geometric shapes on a computer and an interference detection means for detecting interference between models has a process (1) for forming discrete space C having grating points obtained by roughly discretizing the space C or working space (S303) and finding out a sub-goal sequence from start arrangement up to goal arrangement through the grating points by using a graph searching procedure (S304) and a process (2) for selecting adjacent sub-goals from the found sub-goal sequence (S306), forming a discrete space C having grating points obtained by discretizing space including the selected sub-goals at a micro-interval shorter than that of the process (1) and executing processing for finding out a local micro-space route between sub-goals by using the graph searching procedure (S307). These processes (1), (2) are repeated and back tracking for returning operation to the process (1) if the process (2) fails and re-search processing are repeated.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method of planning a motion path of a robot using a geometric model on a computer which describes a geometric shape of an object in a working environment of the robot and its arrangement. In particular, when a start arrangement and a goal arrangement of a robot are given, a global motion path of the robot that prevents interference between the robot and an obstacle in a work environment by using interference inspection means for inspecting interference between models. The present invention relates to a planning method and an apparatus for implementing the method.

[0002]

2. Description of the Related Art A path planning problem relating to obstacle avoidance of a robot can be generalized into a form of a point moving path planning problem in a configuration space (C space: Configuration space). Space C is the arrangement of robots (Configuration)
Is a parameter space that uniquely determines FIG. 12 is a diagram showing a coordinate system in which a conventional robot is placed and a robot placement space (C space). The C space will be described with reference to FIG. FIGS. 12A and 12B show a three-degree-of-freedom manipulator 101 and a three-degree-of-freedom mobile robot 102 in a two-dimensional work space 103, respectively. In the case of the three-degree-of-freedom manipulator 101, FIG.
The joint space (q1, q2, q3) shown in (c) is a C space. In the case of a mobile robot in a two-dimensional workspace, the position of the robot coordinate origin (X,
The three degrees of freedom space (X, Y, θ) of Y) and the direction θ of the robot are C spaces. 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, the obstacles 104a, 104b in the work space 103
Obstacles 105a and 105b (C-space obstacles) in the C space are described using the geometrical shapes of the robot 101 and their geometrical relationships, and a free (non-interfering) space (C free space) in the C space is obtained. There is a need. 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 space obstacle can be explicitly obtained. In the case of the articulated manipulator, there is no general method for analytically finding the C obstacle, and the Slice Projection (Needle) method, the SweptVolume method, and the P method
In general, the C space description is obtained by an approximation method based on the discretization of the C space, such as the oint evaluation method. The roadmap method and the cell decomposition method are suitable for a path planning problem of a mobile robot in a two-dimensional work space, and the C-space discretization method is suitable for a path planning problem of an articulated manipulator.
In the potential field method, the attractive force acting on the robot from the goal arrangement, the repulsive force acting on the robot from obstacles,
This is a method of defining an artificial potential function consisting of the sum of repulsive forces that keeps the robot within the motion range, and searching for a path in the steepest gradient direction in the C space of the potential function. In the case of the articulated manipulator, since it is difficult to explicitly describe the C obstacle, several representative points are provided on the link, and the obstacles within a certain distance from each representative point act on each representative point. The repulsion is defined in the workspace. The challenge is how to avoid potential minimum points other than the goal arrangement, and how to derive a realistically calculable potential function. These conventional methods are described in the literature "YKHwang and N. Ahuja
: Gross Motion Planning-A Survey, ACM Computing S
urveys, Vol.24, No.3, pp.219-291, 1992 ", so the details are omitted. 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. When the above conventional method is evaluated on the basis that a route can be determined within tens of minutes using a current computer of about 100 MIPS, the roadmap method and the cell decomposition method show that a mobile robot in a two-dimensional work space, a C-space discretization The potential range of the method and the potential field method is limited to a six-degree-of-freedom manipulator in a three-dimensional workspace.

A basic approach of the C space discretization method based on the Point Evaluation method will be described with reference to a conventional two-dimensional discretized C space and a progress diagram of a path search shown in FIG. Reference numeral 201 in FIG. 13A indicates a two-dimensional C space (q1, q2). 203 and 204 are start arrangements S,
The goal arrangement G is shown. 205a-b are C space obstacles. In the C space discretization method based on the Point Evaluation method, a space of lattice points obtained by discretizing the C space 201 at minute intervals as indicated by 202 in FIG.
Consider the space (d-q1, d-q2). The discretized C space 202 can be regarded as a connectivity graph in which each grid point is a node and their adjacent relation is an arc. The route (grid point sequence) from the start arrangement S on the connectivity graph to the goal arrangement G by tracing adjacent lattice points is obtained. Only at each lattice point, is it determined whether there is interference between the robot and the work environment. Inspect using a geometric model and an interference checker. A method in which the presence or absence of interference at all grid points is determined in advance,
There is a method of sequentially examining the presence or absence of interference at each grid point in the search process. When the working environment changes dynamically, the latter method is used.

[0004] A route (vertical, horizontal, hill-climbing, best priority, A ^* search, etc.) graph (state space) search technique is used for route search on the connectivity graph of grid points. FIG. 13C shows how the graph search proceeds in the discretized C space.
The graph search starts by generating a child node (adjacent node) of the start node S. 1-1, 1-2, 1-3, and 1-4 are child nodes of the generated S. When a child node is generated, an evaluation function value at the node is calculated based on a certain criterion and stored in the node. Each node also stores a link (pointer) to the parent node. Creating a child node of a node is called expanding the node. From the generated nodes 1-1, 1-2, 1-3, 1-4, which node should be expanded next is selected based on the evaluation function value. In the example of FIG. 13C, the node 1-2 is selected and expanded. FIG. 13D shows the progress of the graph search in FIG. 13C in a tree structure (called a search tree). The wavefronts (boundaries) 207a-e and 208a-b of the searched area in FIG.
It corresponds to the leaf of the search tree (node without direct descendants) of (d). Reference numerals 208a and 208b denote nodes in the leaves of the search tree that have interfered with each other. Start arrangement 20
3 and 206a-b indicate expanded nodes (nodes having children). The branch of the search tree corresponds to a pointer to the parent node. Graph search is expanded next from the leaves of the search tree based on the evaluation function value (no interference occurs)
The process of selecting a node and generating child nodes of the node without duplication is repeated until a goal arrangement is selected from the leaves or there are no more leaves to be selected. When the goal arrangement is selected from the leaves, a path is obtained by following the pointer to the parent node from the goal arrangement. As described above, in the C-space discretization method based on the Point Evaluation method, interference check is performed only at grid points, so that non-interference on an arc connecting non-interfering nodes is not guaranteed.
Therefore, it is necessary to make the discretization resolution sufficiently high.

[0005]

However, in the above-described conventional C-space discretization method, the amount of calculation and the storage area are reduced with respect to increases in discretization resolution, the degree of freedom of the robot, the total number of obstacle surfaces, and the like. Exponentially increases. Therefore, it is not realistic to apply the basic approach of the C-space discretization method as it is to manipulators having 6 degrees of freedom or more, and it is necessary to limit the search space (local search) and improve the search efficiency. Required. Various schemes have been proposed to reduce the search space and improve search efficiency using various heuristics (heuristic techniques), but there is a problem that there are few practical schemes that require a high-speed route. As a method for increasing the search efficiency, for example, Japanese Patent Application Laid-Open No. 10-9
7316, "Movement planning and control method for a system involving a large number of moving objects", a robot path planning method, and the like. FIG. 14 is a block diagram of the conventional method, in which a cell space database (free << no collision >>) in which the robot arrangement space is divided into cells on a first large scale.
Information, full "danger of collision" information, and mix "other" information. ), And a line planning module 912 for obtaining a line of segments connecting the intermediate waypoints linearly, and a list of the intermediate points of the line of segments obtained by the line planning module 912, with a finer time resolution. In consideration of deceleration, trajectory (trajectory) data at a second more minute time interval is obtained, and the trajectory data is directly given to the position control system of the robot manipulator system 916 to actually operate the robot, and at the same time, to reduce population potential The trajectory is modified so that collisions can be reliably avoided at the operation stage using the method (using feedback control), but from the viewpoint of route search, the trajectory between the first and second trajectories is modified. Neither backtracking nor re-searching is performed, and the time interval is not a spatial arrangement even for the fine second trajectory. Because not only is divided into a small gap is going the route completion, has not been exhausted is sufficiently route search. In addition, since the final collision avoidance is performed by the modification using the artificial potential method at the actual operation stage of the robot, formally, 2
Although the step route search method is used, the route search accuracy is not sufficient, and there is a problem that an existing route is easily overlooked. Also, Japanese Patent Application No. 10-356719.
Describes a two-step route planning method in which a subgoal sequence (intermediate waypoint) search and a local route search between adjacent subgoals are repeated. According to this method, 2 using a directed graph
Since the stage search is performed, the speed of the route calculation can be increased, but there is a problem that an existing route is easily overlooked. The present invention has been made in view of the above problems,
It is possible to obtain a path at high speed even for a multi-joint manipulator with a large degree of freedom in a three-dimensional workspace.
It is still another object of the present invention to provide a global motion path planning method for a robot that has a low possibility of overlooking a path and a control device therefor.

[0006]

According to a first aspect of the present invention, there is provided a method for planning a global motion path of a robot, wherein the robot and its working environment are provided when a start arrangement and a goal arrangement of the robot are given. Use geometrical model means on a computer to describe the geometric shapes of obstacles and their arrangement, and interference detection means on a computer to detect interference between models so that robots and obstacles do not cause interference. In the global motion path planning method for performing a two-stage path search on a robot, as a step 1, a space (discretized C space) having grid points (subgoal candidates) in which a layout space (C space) or a work space is roughly discretized ), And from the start arrangement or the goal arrangement to the goal arrangement or the start arrangement via the lattice points using a graph (state space) search method. A subgoal sequence is obtained, and in step 2, two adjacent subgoals are taken out of the subgoal sequence obtained in step 1 above, and grid points discretized at finer intervals than in step 1 in a space including the adjacent subgoals. (Discrete C space) having the following formula, and a process of obtaining a minute interval lattice point sequence between the adjacent subgoals, that is, a local minute interval route, using a graph (state space) search method is repeated. Adjacent subgoal Sg- included in the sequence
If the local route search between i and Sg-i + 1 fails, return to step 1 (backtracking) and start from subgoal Sg-i.
Another subgoal sequence that does not go through the arc to Sg-i + 1 is found again, and the process of step 2 is repeated again. When a continuous minute interval path from the start arrangement to the goal arrangement is obtained, the repetition processing is terminated. It is characterized by doing. According to a second aspect of the present invention, in the method for determining a local path between adjacent subgoals in the step 2, the search range is limited to a predetermined local region including two adjacent subgoals. Features. According to a third aspect of the present invention, in the method for determining a global motion path of the robot, when the local path connecting the adjacent subgoals Sg-i, Sg-i + 1 is obtained in the step 2, two points n, Assuming that the distance between m is d (n, m) and rPara is one or more parameters, the search area Srng is expressed as: Srng = ｛n | d (sg-i, n) + d (n, sg-i + 1) <= d (sg-i, sg-i
+1) × rPara｝. Claim 4
The global movement path planning method for a robot described above is characterized in that, in the step 2, when the local path between the adjacent subgoals is obtained, the search range including the adjacent subgoals is dynamically and multistagely changed. In the method of planning a global motion path of a robot according to a fifth aspect, when the subgoal sequence is obtained in the step 1, all the generated subgoals and their candidates are stored in a computer in a directed graph format without duplication. Then, the data of the subgoal directed graph is updated according to the execution result of the step 2, and the search is performed in both the steps 1 and 2 by reflecting the latest data of the subgoal directed graph.
Also, in the robot global motion path planning method according to claim 6, all the subgoal sequences obtained in the step 1 are stored in a list, and when the subgoal sequence search in the step 1 is successful, the subgoal sequence is added to the list. If the subgoal sequence search in step 1 fails in step 2 above, an appropriate one of the subgoal sequences is selected from the list of subgoal sequences. On the other hand, if the search range is made wider than the previous time and step 2 is executed, and the local route search is successful, the search range is restored for the subsequent unsearched local route search between adjacent subgoals. It is characterized by: Claim 7
In the global motion path planning method for a robot described above, when one subgoal sequence is selected from the subgoal sequence list, the local path search between adjacent subgoals in step 2 is succeeded to the closest to the goal arrangement, and the path length is the longest. Is preferentially selected from short subgoal sequences. In the global movement path planning method for a robot according to claim 8, when the local area search between the adjacent subgoals is performed in the step 2, the search range is set to a preset maximum range and the search fails. It is characterized in that a series is deleted or labeled from the list of subgoal series so that it is not selected thereafter. In addition, in the robot global motion path planning method according to the ninth aspect, when the subgoal sequence list becomes empty or the subgoal sequence selection fails due to labeling, the method is performed at roughly intervals for subgoal sequence search. The space between each grid point of the discretized C space is divided into integers to form a finely discretized discretized C space. Step 1 is executed in the new discretized C space, and so on. It is characterized in that the process is repeated recursively until a continuous minute interval path from the start arrangement to the goal arrangement is determined or within a predetermined time. A robot global motion path planning device according to claim 10 stores robot / environment model input means for inputting a geometric model of a robot and its working environment, and stores the geometric model output by the robot / environment model input means. Geometric model storage means, interference inspection means for inspecting interference between the robot and the environment using information of the geometric model storage means, and start / goal arrangement parameters for inputting start arrangement, goal arrangement and other parameters of the robot. Using input means, parameter setting / change means for setting a parameter output by the parameter input means, or changing according to a search situation, and parameters set by the parameter setting / change means,
A discretization arrangement space setting means for setting a discretization arrangement space for a subgoal sequence search, and in a discretization arrangement space determined by the discretization arrangement space setting means, utilizing an interference test result of the interference inspection means. A subgoal sequence search means for searching for a series of subgoals, a subgoal directed graph storage means for storing the subgoals and their candidates generated by the subgoal sequence search means in the form of a directed graph, and a subgoal sequence obtained by the subgoal sequence search means Subgoal sequence list storage means for storing in the form of a list, subgoal sequence selection means for selecting one subgoal sequence from information stored in the subgoal sequence list storage means, and subgoal sequence selected by the subgoal sequence selection means Temporarily store the Goal sequence temporary storage means, adjacent subgoal selection means for selecting two adjacent subgoals from the subgoal sequence selected by the subgoal sequence selection means and stored in the subgoal sequence temporary storage means, and the information obtained from the adjacent subgoal selection means. A route search means between adjacent subgoals for searching for a local route between two adjacent subgoals to be obtained, a search range limiting means for limiting a search range when the route search means between adjacent subgoals searches for the route, and the subgoal directed graph storage A path storage unit that receives information stored in the unit and information stored in the subgoal sequence temporary storage unit, and stores a continuous path from the start arrangement to the goal arrangement; and And a route output means for outputting a route. That.

In the robot global motion path planning method and its control device, a two-step path search is basically performed by the following steps 1 and 2. (1) First, as the processing of step 1, the arrangement space (C
Space) or a space (discrete C-space) having grid points (sub-gold candidates, for example, by the parameter disPar) roughly discretized from the work space, and using a graph (state space) search method, a start arrangement is performed. Alternatively, a subgoal sequence from the goal arrangement to the goal arrangement or the start arrangement via the grid points is obtained. (2) Next, as a process of step 2, two adjacent subgoals are taken out of the subgoal sequence obtained in the process of step 1 at a time, and the parameter disPar of step 1 is extracted.
Form a space (discrete C space) having grid points discretized at smaller intervals than a, and obtain a local minute interval path of the minute interval lattice point sequence between adjacent subgoals using a graph (state space) search method Repeat the process. (3) If the search in step 2 fails, the adjacent subgoals included in the subgoal sequence are set to Sgi and Sgi + 1 in step 2, and if the local route search between Sgi → Sgi + 1 fails, backtracking and re- The search is performed as follows. Returning to step 1, another subgoal sequence that does not pass through the arc from subgoal Sgi to Sgi + 1 is recalculated, and the process of step 2 is repeated again, and the sequence from the start arrangement to the goal arrangement is continued. When the determined minute interval path is obtained, the repetition processing is terminated. (4) If the search in step 1 is successful, all the obtained subgoal sequences are stored in a list, and step 2 is executed for all the subgoal sequences. In that case, the parameter uses rParaMin (minimum minute interval). If the search in step 1 fails, one is selected from the subgoal sequence list, and the search range is wider than the previous search between adjacent subgoals in which the local route search failed in step 2 (for example, the parameter is set to rParMax).
Step 2), and if the local route search is successful, the parameters are restored and the process is continued. (5) If the subgoal sequence cannot be selected from the previous subgoal sequence list, the interval (parameter disPar) of the rough step 1 is further divided into integers to form a discretized C space again. Try to run it again. With the above configuration, the search space when searching for a route is greatly suppressed, the search efficiency is improved, and the speed of the route can be increased even for an articulated manipulator having a large degree of freedom in a three-dimensional space. Can be obtained by In addition, the backtracking and re-searching of Steps 1 and 2 are repeatedly executed, and the search range of the local route search is dynamically changed by switching rPara, etc., so that the route search is sufficiently compared with the conventional method. And the search accuracy is increased, so that the possibility of overlooking the existing route is reduced.

[0008]

Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a flowchart of the processing of the global motion path planning method for a robot according to the first embodiment of the present invention. FIG. 2 is a diagram showing a discretized C space construction method shown in FIG. 1 and a subgoal sequence search result. The global motion path planning method for a robot according to the present invention is shown in FIG.
When the geometric information of the robot and the working environment, the start arrangement S, and the goal arrangement G as shown in FIG.
This is a method of planning a non-interference path from the start arrangement S to the goal arrangement G in the C space as shown in FIG. Hereinafter, for simplicity, the description will be given by taking a two-dimensional C space as an example, but the present invention is not limited to this.
It goes without saying that the present invention can be applied to a three-dimensional or more C space as it is. Start arrangement S, goal arrangement G,
It is assumed that the coordinates are given by coordinate values on a C space (d-q1, d-q2) discretized at minute intervals shown in FIG.
Further, the discretized C space of FIG.
Res (discrete C-space with High resolution). Hereinafter, the procedure of the method of the present invention will be described with reference to the flowchart shown in FIG. First, dC-Hre
After setting the discretized C space of s (S300), and inputting the start arrangement S and the goal arrangement G (S301). A rough discretized step size, disPar (403 in FIG. 2), is set (S302), and the discretized C space (d-q1 ', d-q2') is set as shown in FIGS. 2 (a) and 2 (b). It is formed (S303). FIG.
As shown in 403a and 403b of (a) and (b),
disPar is set to a value sufficiently larger than the step width of dC-Hres. The discretized C space (d-q1 ', d-q2') is a C space discretized at roughly intervals, and is dC-Lre
s (meaning discrete C-space with Low resolution). In FIG. 2A, the coordinate values of the start arrangement S401 and the goal arrangement G402 of dC-Lres are (0,
0), (5,0). Similarly, in FIG. 2B, (0,
0), (4,3). According to the method of the present invention, there is no need for the start arrangement to be the origin of the discretized C-space dC-Lres as in this example, and the coordinate axes need not be orthogonal. Furthermore, nonlinear conversion from (d-q1, d-q2) to (d-q1 ', d-q2') may be performed, and the definition of dC-Lres is arbitrary.

Next, a subgoal sequence search in STEP1 is performed (S304). FIG. 3 is an internal flowchart of the processing in STEP 1 shown in FIG. FIG. 3 shows that dC-Lr
This is equivalent to searching for a route from the start arrangement S to the goal arrangement G in es. A subgoal sequence is obtained by applying a graph search method such as the above-described best priority or A * search. In order to perform a graph search, it is necessary to have the data structure of the search tree shown in FIG. The leaves of the search tree and other nodes are stored in separate linked lists. FIG. 4 is a diagram showing an example of the data structure of a linked list used for the graph search shown in FIG. FIG. 4A shows a basic class of the linked list 601 and its cell 602. The class of the linked list 603 and its cell 604 used for graph search in FIG. 4B is defined as a derived class of FIG. are doing.
The linked list that stores the leaves of the search tree is called OPENLIST, and the list that stores the other nodes is called CLOSEDLIST. OPENLI
Another list of nodes in the ST where interference occurs (COLLISIONLIS
It may be stored in T). Since the graph search method is a known technique, the description of the algorithm is omitted. STEP arrangement including start arrangement S and goal arrangement G
All nodes (subgoals and their candidates) generated in 1 are stored on the computer in the form of a directed graph without duplication.

FIG. 5 is a diagram showing a data structure of the subgoal directed graph shown in FIG. In the figure, reference numeral 701 denotes a linked list for storing all the generated subgoal candidates, in which a data type (class) and an object (variable) name are CLListFor.
SgGraph SgGraph; Each cell 702 of the linked list 701 stores a list 703 that stores data on adjacent subgoal candidates in addition to data on the coordinate values of the subgoal candidates and the presence / absence of interference. CLListForAdjSG the data type (class) and object (variable) name
s AdjSGList; Reference numeral 704 denotes each cell of the AdjSGList. FIG. 2C shows an example of a subgoal sequence searched in STEP1. In the figure, S-Sg1-Sg2-Sg3-Sg4-
G is the subgoal sequence found. Returning to the flowchart of FIG. 1, if the subgoal sequence search in STEP1 succeeds (TRUE), the subgoal sequence SgSeq is stored in the subgoal sequence list ListOfSgSeq (S305).

FIG. 6 shows a data structure of the subgoal sequence list shown in FIG. In the figure, reference numeral 801 denotes a linked list for storing a subgoal sequence, and has a data type (class)
And object (variable) name CLListForSGSeq ListOfSg
Seq; Each cell 802 of the linked list 801 includes a subgoal sequence 803 (subgoal linked list).
And two or more BOOL-type flags related to the execution result of STEP2. Each cell 804 of the subgoal series 803 stores data such as coordinate values in dC-Lres of each subgoal. Next, one subgoal sequence is selected from the subgoal sequence list 801 according to some criterion (S306). If the subgoal sequence was not found due to failure in STEP1 (FALSE), S30
Step S306 is executed skipping step 5. A parameter rPara to be described later is set for the subgoal sequence selected in S306, and a local route search between adjacent subgoals in STEP2 is performed (S307). FIG. 7 is shown in FIG.
It is an internal flowchart of STEP2. Turning to FIG.
For the EP2 process, two adjacent subgoals are selected from the subgoal sequence selected in S306 of FIG. 1 (S901), and the parameter rPara is set (S901).
902), a local route search is performed on the dC-Hres (S903). Again, as in STEP 1, the highest priority,
A graph search technique such as A ^* search is applied. S903
If the local route search is successful, it is determined whether a local route up to the goal arrangement G has been obtained (S904). If it has not reached the goal arrangement G, the process returns to S901, and the next two adjacent subgoals are selected. If the goal arrangement G has been reached, TRUE is returned and STEP2 ends. If S903 fails in the middle, FALSE is returned and STEP2 ends. The parameter rPara set in S902 is
It is a parameter that limits the search range of the local route search between adjacent subgoals. FIG. 8 is a diagram showing a local area for limiting the route search range shown in FIG. In FIG. 8, 1001
Reference numerals a and 1001b denote ellipsoidal regions each including two adjacent subgoals. Reference numerals 1002 and 1003 denote subgoals Sg1, Sg2, and 1004, respectively, which are obstacles in the C space. When the search range is limited using the ellipsoidal region, the search range Srng is defined by the following equation. Here, d (n, m) represents the distance between two points n and m in the discretized C space dC-Hres. Srng = ｛n | d (Sg1, n)
+ D (n, Sg2) <= d (Sg-i, Sg-i + 1) × rPara, rPara> = 1｝ From the above expression, it can be seen that the smaller the parameter rPara, the more the search range Srng is limited. When rPara = 1, the nearest neighbor set is a line segment connecting the adjacent subgoals Sg1 and Sg2. FIG. 8A shows an ellipsoidal region 100.
FIG. 8B shows a case where a local route is not found in the ellipsoidal region 1001b when a local route is found in 1a. In FIG. 8, when searching for a local minute interval path from the subgoals Sg1 to Sg2, in the case of FIG. 8A, the shortest path 1005 is obtained by applying a modified A ^* algorithm or the like. In the case of FIG. 8B,
Since there is no route in the ellipsoidal region 1001b, after expanding all the grid points in the region 1006 surrounded by the ellipsoidal region 1001b and the C space obstacle 1004, it is determined that the route search has failed. Further, the search range may be limited by using a closed area other than the ellipsoidal area. The manner of setting the search range is not limited to an arbitrary one. In the processing of STEP2, S →
If the local route search between all the adjacent subgoals to G has succeeded (TRUE), the route is stored (S308), and the route planning method of the present invention ends the route search.

Next, a description will be given of a unique process of the present invention for eliminating oversight of a route by backtracking (re-searching) and improving the accuracy of route search. If the local route search between adjacent subgoals fails in the middle of the process in STEP2 (FALSE), the process returns to the subgoal sequence search in STEP1 (S304). For example, the subgoal sequence S in FIG.
In the local route search within Sg1, Sg2, Sg3, Sg4, and G, if the local route search between Sg1 and Sg2 fails as shown in FIG. 8B, the process returns to STEP1 and returns to Sg1 as shown in FIG. Subgoal sequence S → Sg1 ′ → Sg2 → Sg3 → Sg4 which does not pass through the arc from Sg2 to Sg2
→ Search for G again. For this purpose, as shown in the flowcharts of FIGS. 3 and 7, both STEP1 and STEP2 perform a search with reference to the data of the subgoal directed graph (FIG. 5).
Change the data according to the result of the search. Also, if STEP2 shown in FIG. 7 succeeds in the local route search S903 between the adjacent subgoals Sg1 and Sg2, the obtained route is 703 of the subgoal directed graph SgGraph shown in FIG.
Of SGgraph.Cell (Sg1) .AdjSGList.Cell (Arc (Sg2-
Sg1)). PathToAdjSG. Conversely, Sg1 → S
If the local route search S903 between g2 fails, SgGrap
h.Cell (Sg1) .AdjSGList.Cell (Arc (Sg2-Sg1)). Exis
Changed to tanceOfPathToAdjSG1 = FALSE. Also, STEP
1 always returns SgGraph.Cell (Sg1) .AdjSGList.Cell (Ar
Reference is made to the state of c (Sg2-Sg1) .ExistanceOfPathToAdjSG1, and if this flag is FALSE, the search does not proceed from Sg1 to Sg2. This is done so as not to hinder efficiency. In addition, since the information on the subgoals and their candidates generated up to the previous time is stored in the directed subgoal graph, by referring to the data, the interference between the robot and the work environment is again started from the beginning by using the geometric modeler. In order to improve the search efficiency, it is possible to eliminate the waste of inspecting. SgGraph is also referred to in the selection processing of S901 in STEP2, and SgGraph.Cell (Sg1) .AdjSG
List.Cell (Arc (Sg2-Sg1)). PathToAdjSG is not empty (that is, the adjacent subgoal Sg for which the local route has already been obtained)
1 and Sg2 are skipped, and the next adjacent subgoal Sg
2, Sg3 is selected. In the route planning method of the present invention, when the process of selecting the subgoal sequence list ListOfSgSeq as shown in FIG. 6 fails (FALSE) in the process of S306 in the flowchart of FIG. 1, it is determined whether a predetermined time has elapsed. (S309). If the time has expired, the process ends as a search failure. If the time has not expired, the discretized step width disPara is divided by N (integer) (S310), and d-
The C-Lres is reformed (S303), and a series of processing is continued.

Next, a process of selecting a subgoal sequence SgSeq from the subgoal sequence list ListOfSgSeq in the flowchart of FIG. 1 (S306). The parameter rPar of FIG.
a setting process (a) (S902). The relationship between the data of the subgoal directed graph (FIG. 5) and the data of the subgoal sequence list (FIG. 6) will be described in detail. The subgoal sequence SgSeq obtained in STEP1 is converted to the subgoal sequence list List shown in FIG.
Insert at the beginning of OfSgSeq (801). At the time of insertion, the flag BOOL SearchResul related to the execution result of STEP2
t1 and SearchResult2 are both TRUE. In the process of S306 in FIG. 1, the search of the first element of ListOfSgSeq
Result1 is checked first. If this is TRUE,
The subgoal sequence is selected. In other words, if a new subgoal sequence is found in STEP1, be sure to select it. In the process of S901 in FIG. 7, when selecting an adjacent subgoal, the flag ExistanceOfPathToAdjSg1 (FIG. 5: subgoal directed graph: 704) indicating the existence of a route to the adjacent subgoal is checked, and if this flag is TRUE, S902 In the processing of, the parameter rPar
a is set to a preset value rParaMin. Next, if the local route search fails in the route search process in STEP2 of S903, the flag ExistanceOfP is set as described above.
Rewrite athToAdjSG1 to FALSE, and searchResult1
Is also rewritten to FALSE. When the subgoal sequence search fails in STEP1 (processing of S304), all the subgoal sequence flags SearchResult1 (802 in FIG. 6) stored in the subgoal sequence list ListSgSeq of FIG. 6 are FALSE. In this case, skip to S306 and search
sult2 selects a subgoal sequence of TRUE. This subgoal sequence is the result of STEP2 executed in the past, and the route search process of STEP2 in S903 has failed in any of the adjacent subgoals. SearchResult1 = FALSE, Se
When selecting a subgoal sequence of archResult2 = TRUE, a local path search (S903) that is the closest to the goal arrangement G is succeeded and the path length is shortest. When selecting an adjacent subgoal in the subgoal sequence selected in this way in the process of S901 in FIG. 7, the one of ExistanceOfPathToAdjSG1 = FALSE is selected. At this time, in the process of S902 in FIG.
ara is set to a predetermined value, rParMax, and S903 is executed again. If the re-search in S903 succeeds, the corresponding Existanc of the subgoal directed graph
Rewrite eOfPathToAdjSG1 to TRUE. If the search process in S903 fails, the ExistingGoalToTodjjSG2 corresponding to the subgoal directed graph is rewritten to FALSE, and FIG.
Rewrite SearchResult2 of the subgoal sequence list to FALSE. In the process of S306, SearchResult1 = FALSE,
When selecting a subgoal sequence of SearchResult2 = TRUE, check the corresponding ExistanceOfPathToAdjSG2 of the directed subgoal graph between each adjacent subgoal, and this is FA
Do not choose LSE ones. And S306
Returns TRUE if the subgoal is selected from the subgoal sequence list, and returns FALSE if the selection fails because SearchResult2 is all FALSE. As described above, in the present embodiment, the flag ExistanceOfPathToAdjSG indicating the existence of the path between adjacent subgoals of the subgoal directed graph,
Flag Se indicating the execution result of STEP2 of the subgoal sequence list
Although two archResults are used, respectively, the present invention is not limited to this. By preparing a larger number of them, rPara may be changed to multiple stages and the local route search S903 between adjacent subgoals in STEP2 may be executed. .

FIG. 9 is a view showing a search result obtained by implementing the route planning method shown in FIG. 1 on a computer. In order to demonstrate the effect of the present embodiment, a route search is executed in a two-dimensional C space. FIG. In this example,
The resolution of dC-Hres is 100 (integer value of each axis 0 to 99). Also, dC-Lres is formed with disPar = 18.385. The set values of rPara are rParaMin = 1, rParaMax =
1.2. However, these ratios are not limited.
In FIG. 9 (a), since the free space is wide, STEP
RPar without failing to search the subgoal sequence of 1
STEP2 is executed with aMin, and in the thirteenth STEP2, a path from the start arrangement S to the call arrangement G is obtained. On the other hand, in the case of FIG. 9B, since the free space is locally narrow,
STEP1 (Sg1 → Sg2) failed, rPara = rP
araMax, and a local route search S903 between adjacent subgoals in STEP2 is executed. Sg1 to Sg2
After a successful local route search up to Sg2, the search proceeds from Sg2 to G without failing in STEP1. In addition,
In STEP1, an A ^* search is performed using a grid point on the diagonal line of dC-Lres as an adjacent grid point.
The modified A ^* search is applied without setting grid points on the diagonal line of C-Hres as adjacent grid points. According to such a route planning method of the present invention, since the parameter rPara for specifying the search range of the local route search is dynamically changed, the start arrangement and the goal arrangement are surrounded by obstacles, and a very narrow area is set. Even in places where the goal arrangement cannot be reached without passing through, a route can be found without oversight. FIG. 10 is a diagram of a conventional system for comparison. FIG. 10 shows a search result obtained by directly applying the conventional C-space discretization method (modified A ^* search) using the parameter dC-Hres. The obstacle shapes of FIGS. 9A and 9B are respectively shown in FIG.
This is the same as the cases (a) and (b). 9 and FIG. 10, it can be seen that the route planning method of the present invention greatly reduces the search area as compared with the conventional method. Therefore, according to the route planning method of the present invention, search efficiency is improved, search accuracy is improved, and a route can be obtained at high speed. Although the two-dimensional C space has been described as an example for the sake of convenience, the route planning method of the present invention can be applied to a three-dimensional or more C space.

Next, a second embodiment of the present invention will be described with reference to the drawings. FIG. 11 is a block diagram of a global motion path planning control device for a robot according to a second embodiment of the present invention. In FIG. 11, reference numeral 1301 denotes a robot / environment model input unit for inputting a geometric model of a robot and its working environment; and 1302, a geometric model storage unit for storing a geometric model output by the robot / environment model input unit 1301. . Reference numeral 1303 denotes an interference inspection unit that inspects interference between the robot and the environment by using information of the geometric model storage unit 1302. Reference numeral 1304 denotes a start / goal arrangement parameter input unit for inputting the start arrangement and the goal arrangement of the robot and other parameters. Reference numeral 1305 denotes a parameter setting / change means for setting a parameter output by the parameter input means 1304 or changing the parameter in accordance with a search situation. Reference numeral 1306 denotes a subgoal sequence search using the parameters set by the parameter setting / change means 1305. Is a discretized arrangement space setting means for setting a discretized arrangement space for the above. 130
Reference numeral 7 denotes a subgoal sequence search unit that searches for a subgoal sequence using the interference test result of the interference test unit 1303 in the discretized layout space determined by the discretized layout space setting unit 1306. Reference numeral 1308 denotes a subgoal directed graph storage unit that stores the subgoals and their candidates generated by the subgoal sequence search unit 1307 in the form of a directed graph. A subgoal sequence list storage unit 1309 stores the subgoal sequence obtained by the subgoal sequence search unit 1307 in the form of a list. Reference numeral 1310 denotes a subgoal sequence list storage unit 1309
Is a subgoal sequence selecting means for selecting one subgoal sequence from the information stored in the subgoal sequence. Reference numeral 1311 denotes a subgoal sequence temporary storage unit for temporarily storing the subgoal sequence selected by the subgoal sequence selection unit 1310. Reference numeral 1312 denotes an adjacent subgoal selecting unit that is selected by the subgoal sequence selecting unit 1310 and selects two adjacent subgoals from the subgoal sequence stored in the subgoal sequence temporary storage unit 1311. 1313
Is a route search unit between adjacent subgoals that searches for a local route between two adjacent subgoals obtained from the adjacent subgoal selection unit 1312, and 1314 is a search that limits the search range when the route search unit 1313 between adjacent subgoals searches for a route. This is a range limiting means. Reference numeral 1315 denotes a route storage unit that receives the information stored in the subgoal directed graph storage unit 1308 and the information of the subgoal sequence temporary storage unit 1311 and stores a continuous route from the start arrangement to the goal position. 131
Reference numeral 5 denotes a route output unit that outputs the stored route.

Next, the operation of the control device having the above configuration will be described. Regarding the operation of the entire control device, the STEP of the processing of the flowchart of FIG.
1. A route search is performed according to STEP2. The outline of the route search will be briefly described below. In the start / goal arrangement parameter input unit 1304, the parameter setting / change unit 1305, and the discretized arrangement space setting unit 1306, the search space is limited by the parameters disPara and STEP2 in the case of STEP1 in addition to the start arrangement S and the goal arrangement G. Parameters rParaMin, rPar
aMax etc. are set, and the subgoal sequence search means 1
At 307, a route search in STEP1 is executed. The subgoal series and the candidates searched for by the subgoal series search unit 1307 are stored in the subgoal directed graph storage unit in a data structure as shown in FIG. or,
The searched subgoal sequences are stored in the form of a list in the subgoal sequence list storage unit 1309 as a data structure as shown in FIG. Next, one subgoal sequence is selected from the information stored in the subgold sequence list storage unit by the subgoal sequence selection unit and stored in the subgoal sequence temporary storage unit 1311. Select an adjacent subgoal. The adjacent inter-goal route search means 1313 searches for a local route between the selected two adjacent sub-goals according to STEP2. In this case, the search range limiting unit 1314 sends the parameter rPara from the parameter setting unit 1305.
The search range Srng or the like is calculated using an ellipsoidal region as shown in FIG. 8 to limit the search range. The search result by the adjacent subgoal route search unit 1313 is stored in the subgoal directed graph storage unit 1308. Also, depending on the result during this time, repetitive processing such as back tracking and re-searching may be performed. Finally, based on the information in the subgoal directed graph storage unit 1308 and the information in the subgoal sequence temporary storage unit 1311, a route storage unit 131 storing a continuous path from the start arrangement S to the goal arrangement G is stored.
The route information is output to the route output means 1316 from 5 and the robot is operated so as not to interfere with the obstacle. As described above, according to the second embodiment, the operation has been described by taking a two-dimensional space route search as an example for convenience of explanation. However, it is needless to say that the operation can be applied to a three-dimensional or more C space without any change. Increased search efficiency, less oversight of routes,
In addition, it is possible to obtain a robot path at a high speed.

[0017]

As described above, according to the present invention,
Given the start and goal positions of the robot,
The discretized C space is formed by the search processing of STEP 1 that roughly divides the space between them, and a subgoal sequence is searched using the graph search method. Since a two-stage path search of forming a discretized C space having lattice points and finding a local path using a graph search method is performed, the search space is greatly suppressed, search efficiency is improved, and robot control is performed. This has the effect of suppressing resource consumption and improving overall performance. Furthermore, in the route search process, the search result of STEP1 is stored in the subgoal sequence list, and the local route search of STEP2 is executed using it, and the result is reflected in the subgoal directed graph, which is necessary for the local route search without waste. By storing a minimum amount of information, data processing is streamlined. Even if the search in STEP2 fails, backtracking (re-search) is repeatedly executed, improving search accuracy and improving the 3D workspace. Even for an articulated manipulator having a large degree of freedom, there is an effect that a high-speed and practical path planning can be realized. Furthermore, STEP1
If the search is successful, the obtained subgoal sequence is stored in the subgoal sequence list, and the parameter rParaMi
Step 2 is executed using n, and if the search for STEP 1 fails, the parameter rParaMa for expanding the search range is used.
Because the search range of the local route search is dynamically changed, such as executing STEP2 with x, the possibility of overlooking the route is reduced even if the free space is very narrow. effective.

[Brief description of the drawings]

FIG. 1 is a flowchart of a process of a global motion path planning method for a robot according to a first embodiment of the present invention.

FIG. 2 is a diagram showing a discretized C space construction method shown in FIG. 1 and a subgoal sequence search result.

FIG. 3 is an internal flowchart of processing in STEP1 shown in FIG. 1;

FIG. 4 is a diagram showing an example of a data structure of a linked list used for the graph search shown in FIG. 3;

FIG. 5 is a diagram showing a data structure of a subgoal directed graph shown in FIG. 3;

FIG. 6 is a diagram showing a data structure of a subgoal sequence list shown in FIG.

FIG. 7 is an internal flowchart of STEP2 shown in FIG. 1;

FIG. 8 is a diagram showing a local area for limiting the route search range shown in FIG. 7;

9 is a diagram showing a search result obtained by implementing the route planning method shown in FIG. 1 on a computer.

FIG. 10 is a comparison diagram of the route search result shown in FIG. 9;

FIG. 11 is a block diagram of a global motion path planning control device for a robot according to a second embodiment of the present invention.

FIG. 12 is a diagram showing a coordinate system in which a conventional robot is placed and an arrangement space for the robot.

FIG. 13 is a progress diagram of a conventional two-dimensional discretized C space and a path search.

FIG. 14 is a system configuration block diagram of a conventional movement planning and control method for a system involving many moving objects.

[Explanation of symbols]

101 Three-degree-of-freedom manipulator 102 Three-degree-of-freedom mobile robot 103 Two-dimensional work space 104 Obstacle in two-dimensional work space 105, 205, 404, 1004 Obstacle in C space 201 Two-dimensional C space 202 Two-dimensional discretized C space 203, 401 Start arrangement 204, 402 Goal arrangement 206 Expanded node 207 Unexpanded node 208 Unexpanded node where robot and obstacle interfered 403 Discretization interval 405 Subgoal 601 Linked list 602 Linked list cell 603 Linked list used for graph search 604 Cell 701 of linked list used for graph search 701 Linked list storing generated goal and candidate 702 Cell of linked list storing subgoal and its candidate 703 Cell of linked list storing information about adjacent subgoal 7 4 Cell of linked list storing information on adjacent subgoals 801 Implementation of linked list of subgoal sequence list 802 Cell of subgoal sequence list 803 Implementation of linked list of subgoal sequence 804 Cell of subgoal sequence 1001 Ellipsoidal area limiting search range 1002 Subgoal Sg1 1003 Subgoal Sg2 1005 Local path between adjacent subgoals 1006 Closed area surrounded by ellipsoidal area C space obstacle 1301 Robot / environment model input unit 1302 Geometric model storage unit 1303 Interference inspection unit 1304 Start / goal arrangement parameter input unit 1305 Parameter setting / change means 1306 Discretized arrangement space setting means 1307 Subgoal sequence search means 1308 Subgoal directed graph storage means 1309 Subgoal system List storage unit 1310 subgoal sequence selecting unit 1311 subgoal series temporary storage unit 1312 adjacent sub-goal selection means 1313 adjacent subgoal between the route search unit 1314 search range restriction unit 1315 route storage unit 1316 routes the output means

 ──────────────────────────────────────────────────続 き Continued on the front page F term (reference) 3F059 BA02 CA06 FA03 FA06 FA07 FB01 FC14 5H269 AB33 BB05 BB14 CC09 EE03 EE25 NN16 NN17

Claims (10)

[Claims] 1. When a start arrangement and a goal arrangement of a robot are given, a geometric model means on a computer for describing the geometric shapes of the robot and obstacles in the work environment and their arrangement, A global motion path planning method for a robot that uses a computer-based interference detection means for detecting the interference of the robot and performs a two-stage path search so that the robot and an obstacle do not interfere with each other. Space) or a space (discrete C space) having grid points (subgoal candidates) obtained by roughly discretizing the work space, and using a graph (state space) search method, the grid points are calculated from the start arrangement or the goal arrangement. The subgoal sequence leading to the goal placement or start placement via is obtained. Two adjacent subgoals are taken out at a time, and a space (discrete C space) having lattice points discretized at minute intervals from step 1 is formed in a space including the adjacent subgoals, and a graph (state space) search method is performed. Is repeated to obtain a sequence of minutely spaced grid points between adjacent subgoals, that is, local minutely spaced paths. In step 2, a local route search between adjacent subgoals Sg-i and Sg-i + 1 included in the subgoal sequence If it fails, the process returns to step 1 (backtracking), finds another subgoal sequence that does not go through the arc from subgoal Sg-i to Sg-i + 1, and repeats the process of step 2 again. Global motion path planning for a robot characterized by terminating repetitive processing when a continuous micro-interval path from the robot to the goal placement is determined Law. 2. The global robot according to claim 1, wherein in step 2, when a local route between adjacent subgoals is obtained, a search range is limited to a predetermined local area including two adjacent subgoals. Motion path planning method. 3. In the step 2, when obtaining a local path connecting the adjacent subgoals Sg-i, Sg-i + 1, the distance between two points n, m on the discretized C space is represented by d (n, m). , RPara as one or more parameters, the search area Srng is defined as Srng = ｛n | d (sg-i, n) + d (n, sg-i + 1) <= d (sg-i, sg-i
3. The global motion path planning method for a robot according to claim 2, wherein the restriction is performed by +1) × rPara｝. 4. The robot according to claim 2, wherein, in the step (2), when obtaining a local route between adjacent subgoals, the search range including the adjacent subgoals is dynamically and multistagely changed. Global motion path planning method. 5. When a subgoal sequence is obtained in step 1, all generated subgoals and their candidates are stored in a computer in a directed graph format without duplication, and the subgoal directed graph is stored in accordance with the execution result in step 2. 5. The global motion path planning method for a robot according to claim 1, wherein the search is performed by updating the data of the sub-goal directed graph in both the steps 1 and 2. 6. 6. The subgoal sequence obtained in step 1 is stored in a list, and if the subgoal sequence search in step 1 succeeds, step 2 is executed for the subgoal sequence. If the subgoal sequence search fails, a subgoal sequence is appropriately selected from the list of subgoal sequences, and if the local route search fails in the past step 2, the search range between adjacent subgoals is smaller than the previous one. And performing step 2 after widening the search range. If the local route search is successful, the search range is returned to the original range for subsequent unsearched local route searches between adjacent subgoals. 6. The global motion path planning method for a robot according to 4 or 5. 7. When one subgoal sequence is selected from the subgoal sequence list, the local route search between adjacent subgoals in step 2 is successfully performed to the closest to the goal arrangement, and the subgoal sequence having the shortest path length is prioritized. 7. The global motion path planning method for a robot according to claim 6, wherein the selection is performed selectively. 8. When performing a local route search between adjacent subgoals in step 2, if the search fails with the search range set to a preset maximum range, the subgoal sequence is deleted from the subgoal sequence list or 8. The global motion path planning method for a robot according to claim 6, wherein a label is attached so as not to be selected thereafter. 9. When the list of subgoal sequences becomes empty or when selection of a subgoal sequence fails due to labeling, the discretized C-space of the discretized C space formed by discretizing at rough intervals for subgoal sequence search. The space between each grid point is divided into an integer number to form a finely discretized discretized C space, and step 1 is executed in the new discretized C space. 9. The global motion path planning method for a robot according to claim 1, wherein recursive repetition is performed until a continuous minute interval path is obtained or within a predetermined time. 10. A robot / environment model input unit for inputting a geometric model of a robot and its working environment, a geometric model storage unit for storing the geometric model output by the robot / environment model input unit, and the geometric model storage. Interference inspection means for inspecting interference between the robot and the environment using information of the means, start / goal arrangement parameter input means for inputting start arrangement and goal arrangement and other parameters of the robot, and output from the parameter input means Parameter setting / changing means for setting a parameter or changing according to a search situation; and discretization for setting a discretized arrangement space for a subgoal sequence search using the parameters set by the parameter setting / change means. The arrangement space setting means, and the discretized arrangement space setting means In the discretized arrangement space, a subgoal sequence search unit that searches for a sequence of subgoals using the interference test result of the interference test unit, and a subgoal generated by the subgoal sequence search unit and a candidate for the subgoal in the form of a directed graph Subgoal directed graph storage means for storing; subgoal sequence list storage means for storing the subgoal sequence obtained by the subgoal sequence search means in the form of a list; one subgoal sequence from information stored in the subgoal sequence list storage means Subgoal sequence selecting means for selecting a subgoal sequence temporary storing means for temporarily storing a subgoal sequence selected by the subgoal sequence selecting means, and a subgoal sequence temporary storing means selected by the subgoal sequence selecting means Remembered in Subgoal selecting means for selecting two adjacent subgoals from a series of subgoals, an adjacent intergoal path searching means for searching for a local path between two adjacent subgoals obtained from the adjacent subgoal selecting means, and an adjacent subgoal path searching Means for limiting a search range when the means searches for the route; receiving information stored in the subgoal directed graph storage means and information stored in the subgoal sequence temporary storage means, A global motion path planning control device for a robot, comprising: path storage means for storing a continuous path from an arrangement to the goal arrangement; and path output means for outputting the path from the path storage means.