JP2017037468A - Route search system, route search method and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To search for a route by eliminating an extraneous route candidate.SOLUTION: A route search system includes: a node spread part for repeating processing of calculating a cost for each of plural grids corresponding to plural directions of spreading from a minimum-cost node, moving by selecting a minimum-cost node, and setting and spreading the post-move node as a minimum-cost node; and a setup part for setting the number of directions of spreading of each node of spreading destination from the minimum-cost node. In a case where a node of a spreading destination is determined to be on an extension line from an adjacent node of a movement origin in a movement direction toward a minimum-cost node for which the number of spreading directions is set at "3", the setup part sets the number of spreading directions at "3" for the node of the spreading destination, whereas in a case where the node of the spreading destination is determined to be not on the extension line from the adjacent node of the movement origin in a movement direction toward the minimum-cost node for which the number of spreading directions is set at "3", the setup part sets the number of spreading directions at "2" for the node of the spreading destination.SELECTED DRAWING: Figure 3

Description

  The present invention relates to a route search system, a route search method, and a program for selecting a route having the lowest cost among routes from a start point to an end point included in an area represented by a grid.

  For example, there is known a route search system that expands a minimum cost node and sets the number of expansion directions from the minimum cost node in accordance with the movement direction with respect to the expanded minimum cost node (see Patent Document 1). . In this route search system, the number of development directions is set to 5 when the movement direction is oblique, and the number of development directions is set to 3 when the movement direction is vertical or horizontal. .

JP 2011-128758 A

In the route search system, the search branch is always searched for routes in all traveling directions with five branches (the number of development directions is five) or three branches (the number of development directions is three). In this case, since the route search is performed including a useless route candidate that cannot clearly become the shortest route candidate, there is room for improvement in shortening the route search time.
The present invention has been made in view of the above problems, and a route search system, a route search method, and a route search method that can shorten the route search time by performing route search by removing useless route candidates. The purpose is to provide a program.

One aspect of the present invention for achieving the above object is a route search system that selects a route from the start point to the end point included in an area represented by a grid and that has the lowest cost, and the minimum cost. Calculate the cost for multiple grids according to multiple deployment directions from the selected node, select and move the node with the least cost, and set the node after the move as the least cost node for deployment A node expansion unit that repeats the processing to be performed, and a setting unit that sets the number of expansion directions of each node of the expansion destination from the node of the minimum cost, the setting unit, the node of the expansion destination, When it is determined that the node of the minimum cost with the number of expansion directions set to 3 is on the extension line of the movement direction from the adjacent node of the movement source, the number of expansion directions of the node of the expansion destination is determined. And when it is determined that the node of the deployment destination is not on the extension line in the movement direction from the adjacent node of the movement source with respect to the node of the minimum cost in which the number of deployment directions is set to 3, Set the number of deployment directions of the deployment destination node to 2,
This is a route search system characterized by that.
In this aspect, the number of the deployment directions may be increased at a node adjacent to the grid where the obstacle exists.
In this aspect, the setting unit determines that the minimum cost node adjacent to the obstacle grid has moved from the adjacent node of the movement source adjacent in the vertical or horizontal direction of the obstacle grid. In this case, the number of deployment directions of the minimum cost node is set to 5, and the movement source adjacent in the diagonal direction of the obstacle grid to the minimum cost node adjacent to the obstacle grid. The number of deployment directions of the minimum cost node is set to 4 and the number of deployment directions is 4 when it is determined that the node has moved from the adjacent node of the movement source that is not adjacent to the grid of obstacles. Alternatively, the number in the deployment direction of the deployment destination node from the minimum cost node set to 5 may be set to 3.
In this aspect, the node expansion unit includes a minimum cost node whose number of expansion directions is set to 2 by the setting unit, a movement direction from an adjacent node to the minimum cost node, and the extension line. You may expand | deploy in two directions, a parallel direction.
One aspect of the present invention for achieving the above object is a route search method for selecting a route having the lowest cost among routes from a start point to an end point included in an area represented by a grid. Calculate the cost for multiple grids according to multiple deployment directions from the selected node, select and move the node with the least cost, and set the node after the move as the least cost node for deployment And a step of setting the number of expansion directions of each node of the expansion destination from the node with the lowest cost, wherein the number of expansion directions is 3 in the expansion destination node. When it is determined that the node of the minimum cost set is on the extension line of the movement direction from the movement source adjacent node, the number of expansion directions of the expansion destination node is set to 3, and the expansion When it is determined that the previous node is not on the extension line in the movement direction from the adjacent node of the movement source with respect to the node of the minimum cost in which the number of expansion directions is set to 3, the expansion of the expansion destination node The route search method may be characterized in that the number of directions is set to two.
One aspect of the present invention for achieving the above object is a program that selects a route having the lowest cost from a start point to an end point included in an area represented by a grid, the node having the lowest cost. Calculate the cost for multiple grids according to multiple deployment directions, select and move the node with the lowest cost, and set the expanded node as the node with the lowest cost and deploy When it is determined that the expansion destination node is on the extension line of the movement direction from the adjacent node of the movement source with respect to the node of the minimum cost in which the number of expansion directions is set to 3 In addition, the process of setting the number of deployment directions of the deployment destination node to 3 and the deployment destination node of the migration source with respect to the node of the minimum cost for which the number of deployment directions is set to 3 A program for causing a computer to execute a process of setting the number of deployment directions of a deployment destination node to 2 when it is determined that it is not on an extension line of a movement direction from an adjacent node. Also good.

  According to the present invention, it is possible to provide a route search system, a route search method, and a program that can shorten the route search time by performing route search by removing useless route candidates.

It is an external view which shows the structure of a robot typically. It is a block diagram which shows the control part of a robot. It is a figure which shows an example of the minimum cost node expand | deployed by the (a) thru | or (d) node expansion | deployment part. 3 is a flowchart illustrating an example of a flow of a route search method according to the first embodiment. (A) thru | or (k) It is a figure which shows the state which increased the number of the unfolding directions in the node adjacent to the entry prohibition grid where an obstacle exists. 10 is a flowchart illustrating an example of a flow of a route search method according to the second embodiment. It is the figure which branched the search branch to the XYZ-axis direction in the three-dimensional coordinate.

Embodiment 1
Hereinafter, embodiments of the present invention will be described with reference to the drawings. However, the present invention is not limited to the following embodiments. In addition, for clarity of explanation, the following description and drawings are simplified as appropriate.

  First, a configuration of a robot which is an example of a moving body according to the present invention will be described with reference to FIG. FIG. 1 is an external view schematically showing the configuration of the robot 100. In the first embodiment, the robot 100 will be described as a mobile robot that moves autonomously. The robot 100 includes a wheel 2, a housing 3, and a sensor 5. Inside the housing 3 are provided a motor connected to the wheels 2, a battery for driving the motor, and the like. This motor serves as a drive mechanism for driving the robot 100. By driving the motor, the wheel 2 rotates and the robot 100 moves. For example, the robot 100 moves at a speed similar to a human walking speed. Further, the head 1 is provided with a sensor 5 having a CCD camera, a laser sensor, or the like. The sensor 5 detects obstacles or humans existing around the robot 100.

  The robot 100 is provided with a control unit 110. The control unit 110 is an arithmetic processing unit having a CPU (Central Processing Unit), a ROM (Read Only Memory), a RAM (Random Access Memory), a communication interface, and the like. The control unit 110 includes a detachable HDD, an optical disk, a magneto-optical disk, and the like, stores various programs and control parameters, and supplies the programs and data to a memory (not shown) or the like as necessary. . Of course, the controller 110 is not limited to one physical configuration physically. The control unit 110 generates a movement path along which the robot 100 moves. Then, a motor or the like for driving the wheels 2 is controlled so that the robot 100 moves along the movement path. Of course, the robot 100 is not limited to a wheel-type mobile robot, and may be a walking type or other mobile robot. The robot 100 may be a moving body that moves by performing self-position estimation.

  The control unit 110 determines a movement route from the movement start point to the movement end point in the movement environment. The robot 100 starts moving from the movement start point. And it moves along a movement route and stops when it moves to the movement end point. That is, the control unit 110 functions as a route search system that searches for a movement route from the movement start point to the movement end point. In the first embodiment, the control unit 110 performs a route search using the Dijkstra method, but is not limited thereto. The control unit 110 can use any shortest path search algorithm such as A * and Theta *, for example.

  Next, the control system of the robot 100 will be described with reference to FIG. FIG. 2 is a block diagram showing the control unit 110 of the robot 100. The control unit 110 executes a calculation process for determining a route along which the robot 100 moves. As shown in FIG. 2, the control unit 110 includes a map information storage unit 111, a node expansion unit 112, and a setting unit 113.

  The map information storage unit 111 stores map information in a moving area where the robot 100 moves. For example, the coordinates of obstacles such as walls and desks existing in the moving environment are stored. The map information storage unit 111 expresses the moving area by a two-dimensional grid. That is, the map information storage unit 111 divides the moving area by vertical and horizontal grid lines and represents the grid by a grid-like grid. Further, an entry prohibition grid is set in the map information based on information such as obstacles. That is, the robot 100 cannot move to a position where there is an obstacle. For this reason, the grid corresponding to the position of the obstacle becomes an entry prohibition grid.

  Therefore, the map information storage unit 111 stores a grid map including the entry prohibition grid and the entry enable grid. The grid size is determined according to the size of the moving area and the processing speed of the computer. In the route search, the optimum route from the grid corresponding to the movement start point of the robot 100 to the grid corresponding to the movement end point is searched. That is, the route with the minimum cost is selected from the routes from the start point to the end point included in the area represented by the grid.

  As shown in FIGS. 3A to 3D, the grid is divided vertically and horizontally. For this reason, one arbitrary grid (for example, the center grid of nine grids) is adjacent to four grids in the vertical (vertical direction), left and right (horizontal direction), and four diagonal grids therebetween. To do. That is, in the grids other than the edge of the moving region, there are adjacent eight grids in the upward direction, the downward direction, the left direction, the right direction, the right diagonally upward direction, the right diagonally downward direction, the left diagonally downward direction and the left diagonally upward direction. Will be.

  The node expansion unit 112 performs cost calculation and expands a node with the lowest cost (hereinafter referred to as a minimum cost node). Here, the node corresponds to the grid. Therefore, one grid becomes one node. The node expansion unit 112 expands the minimum cost nodes in order as a result of the cost calculation. The node expansion unit 112 calculates a cost for a plurality of grids corresponding to a plurality of expansion directions from the node with the lowest cost, selects and moves the node with the lowest cost, and further minimizes the node after the movement. The process of setting and expanding as a cost node is repeated.

In this way, the node expansion unit 112 sequentially expands the node for which the path with the lowest cost is determined as the minimum cost node. Among the nodes for which the path with the lowest cost has not been determined, the node with the lowest cost as a result of the cost calculation is sequentially added to the database or the like as the lowest cost node. Then, by adding the minimum cost node, the minimum cost node is expanded.
The node expansion unit 112 performs cost calculation using, for example, a pruning search with three expansion directions from the minimum cost node.

  3A to 3D are diagrams illustrating an example of the minimum cost node expanded by the node expansion unit. 3A to 3D, the numerical value described in each node is the number of expansion directions (the number of branches of the search branch) expanded by the node expansion unit 112 at each node. For example, as shown in FIG. 3 (a), the robot moves from D1 to D2, and branches into three at D2, and moves from D2 to C3, D2 to D3, and D2 to E3.

  By the way, in the conventional route search system, the search branch of the minimum cost node is always searched for routes in all traveling directions by always branching 5 branches (the number of expansion directions is 5) or 3 branches (the number of expansion directions is 3). . In this case, since the route search is performed including a useless route candidate that cannot clearly become the shortest route candidate, there is room for improvement in shortening the route search time.

For example, in FIG. 3A, in the route from D1 to B3, the route D1 → C2 → B3 is shorter than the route D1 → D2 → C3 → B3. Therefore, the route D1 → D2 → C3 → B3, that is, the route C3 → B3 is a useless route candidate that cannot be a candidate for the shortest route.
Similarly, in the route from D1 to A4, the route D1->C2->B3-> A4 is shorter than the route D1->D2->C3->B4-> A4. Therefore, the route D1 → D2 → C3 → B4 → A4, that is, the route B4 → A4 is a useless route candidate that cannot be a candidate for the shortest route.

  As described above, for example, as shown in FIGS. 3A to 3D, on the extension line (central search branch) in the movement direction from the adjacent node with respect to the minimum cost node in which the number of development directions is set to 3. Then, the number of expansion directions of the expansion destination node is set to 3. In this case, the minimum cost node is developed in three directions (three branches) diagonally left forward, forward (in the direction of the extension line), and diagonally forward right.

  On the other hand, with respect to the minimum cost node whose number of expansion directions is set to 3, the expansion destination node is not on the extension line (central search branch) of the movement direction from the adjacent node (on both side branches of the central search branch). Is set to 2 in the direction of expansion. In this case, only the two directions (two branches) parallel to the front (the movement direction from the adjacent node with respect to the minimum cost node) and the central search branch (extension line) are expanded from the minimum cost node. Once branching from the central search branch, branching in the direction to return to the original central search branch is a branch that cannot be the shortest path, so two directions parallel to the forward and central search branches It is said. That is, the expansion direction number 2 is set to a node branched from the node where the expansion direction number is set to 2. Thus, by reducing the number of search branches from 3 branches to 2 branches, the amount of calculation for the route search can be reduced to approximately 2/3.

In the route search system according to the first exemplary embodiment, the expansion destination node is an extension line (middle of the center) from the movement source adjacent node to the minimum cost node in which the number of expansion directions is set to 3. If it is determined that the node is on the search branch), the number of expansion directions of the expansion destination node is set to 3. Further, the deployment destination node is not on the extension line in the movement direction from the adjacent node of the movement source with respect to the node of the minimum cost in which the number of deployment directions is set to 3 (moving on the search branches on both sides). Is determined, the number of expansion directions of the expansion destination node is set to 2.
As a result, the route search time can be shortened by performing route search excluding useless route candidates.

  The setting unit 113 determines whether or not the deployment destination node is on the extension line in the movement direction from the adjacent node of the movement source with respect to the node with the minimum cost in which the number of deployment directions is set to 3. The number of deployment directions of the deployment destination node is set.

  When the setting unit 113 determines that the expansion destination node is on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node in which the number of expansion directions is set to 3, the expansion destination node The number of nodes in the expansion direction is set to 3.

  For example, as illustrated in FIG. 3B, the setting unit 113 determines that the deployment destination nodes C2, C3, and C4 are the movement source adjacent nodes C1 with respect to the minimum cost node C2 in which the number of deployment directions is set to 3. When it is determined that the position is on the extension line in the moving direction from, the number of development directions of the deployment destination nodes C2, C3, C4 is set to 3. In this way, the number 3 in the expansion direction is set to the node on the central search branch (the extension line in the movement direction from the adjacent node with respect to the minimum cost node in which the number in the expansion direction is set to 3).

  On the other hand, when the setting unit 113 determines that the expansion destination node is not on the extension line in the movement direction from the movement source adjacent node with respect to the node of the minimum cost in which the number of expansion directions is set to 3, the setting unit 113 The number of expansion directions of the expansion destination node is set to 2.

  For example, as illustrated in FIG. 3B, the setting unit 113 determines that the deployment destination nodes A4 to A5, B3 to B5, and D3 to D4 correspond to the node C2 with the minimum cost in which the number of deployment directions is set to 3. When it is determined that it is not on the extension line in the movement direction from the movement source adjacent node C1, the number of expansion directions of the expansion destination nodes A4 to A5, B3 to B5, and D3 to D4 is set to 2. Thus, the number of expansion directions is set to 2 for nodes that are not on the search branches on both sides (the extension line in the movement direction from the adjacent node with respect to the node of the minimum cost where the number of expansion directions is set to 3). The

  As described above, in the first embodiment, cost calculation is performed using a pruning search in which the expansion direction from the minimum cost node is limited from eight directions to three directions. Further, in the first embodiment, in consideration of the characteristics of the shortest path, the expansion direction from the minimum cost node is decreased from three directions to two directions. The number of expansion nodes for newly calculating costs is determined by the expansion direction. As a result, the number of nodes for cost calculation can be reduced, and the calculation time can be further shortened.

  The node expansion unit 112 calculates the cost for the three nodes corresponding to the three expansion directions set as described above or the two nodes corresponding to the two expansion directions. Then, the cost map of the map information is updated according to the calculated cost. That is, when a path with a low cost is found in three or two grids, the cost of the grid is updated. In this way, the cost map is updated. With reference to the cost map, the node with the lowest cost is selected from the nodes for which the path with the lowest cost has not been determined. This node is newly added as a minimum cost node. In this way, the minimum cost node is expanded.

Next, the route search method according to the first embodiment will be described in detail. FIG. 4 is a flowchart illustrating an example of a flow of the route search method according to the first embodiment.
When the route search is started, the eight directions (upward, downward, leftward, rightward, diagonally upward right, diagonally downward right, diagonally downward left, diagonally upward left) from the node at the movement start point The search branch is branched to the node (step S101).

It is determined whether or not the node ahead of the search branch branched from the node is the movement end point (step S102). If it is determined that the node ahead of the search branch branched from the node is the movement end point (YES in step S102), the route search process ends.
On the other hand, if it is determined that the node ahead of the search branch branched from the node is not the movement end point (NO in step S102), the minimum cost node is selected from the nodes ahead of the search branch (step S103). . The minimum cost node selected for the first time is set to the number 3 in the deployment direction.

When expanding from the selected minimum cost node, whether or not the expansion destination node is on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node whose number of expansion directions is set to 3. Is determined (step S104).
When it is determined that the minimum cost node with the number of expansion directions set to 3 is on the extension line of the moving direction from the adjacent node (YES in step S104), the number of expansion directions of the expansion destination node is 3. It is set (step S105), and the process returns to the above (step S102).

  On the other hand, when it is determined that the minimum cost node for which the number of expansion directions is set to 3 is not on the extension line of the moving direction from the adjacent node (NO in step S104), the number of expansion directions of the expansion destination node is 2 is set (step S106), and the process returns to the above (step S102).

As described above, in this embodiment, when it is determined that the deployment destination node is on the extension line in the movement direction from the movement source adjacent node with respect to the minimum cost node in which the number of deployment directions is set to 3, The number of deployment directions of the deployment destination node is set to 3. Further, when it is determined that the deployment destination node is not on the extension line in the movement direction from the adjacent node of the movement source with respect to the node of the minimum cost with the number of deployment directions set to 3, the deployment destination node Set the number of deployment directions to 2.
As a result, the route search time can be shortened by performing route search excluding useless route candidates.

Embodiment 2
In Embodiment 2 of the present invention, the number of deployment directions is increased at a node adjacent to the entry prohibition grid where an obstacle exists. As described above, since the shortest path in the area where no obstacle exists basically moves linearly, it is only necessary to reduce the tip of the search branch to two branches. However, if an obstacle is present in the vicinity, the number of branches in the search branch (the number in the development direction) is also increased (the number of branches in the search branch is increased, for example, light is scattered and diffracted to avoid the obstacle). ).

  On the other hand, in the second embodiment, for example, when the setting unit 113 is deployed from the minimum cost node by the node deployment unit 112, the setting unit 113 detects the obstacle with respect to the minimum cost node adjacent to the obstacle entry prohibition grid. When it is determined that the node has moved from the adjacent node of the movement source adjacent in the vertical or horizontal direction of the entry prohibition grid, the number of development directions of the minimum cost note is set to 5. On the other hand, when the setting unit 113 expands from the minimum cost node by the node expansion unit 112, the setting unit 113 moves adjacent to the obstacle entry prohibition grid in an oblique direction with respect to the minimum cost node adjacent to the obstacle entry prohibition grid. When it is determined that the node has moved from the original adjacent node or an adjacent node not adjacent to the obstacle entry prohibition grid, the number of deployment directions of the minimum cost node is set to 4. Further, the node expansion unit 112 is located in front of the moving direction from the adjacent node to the minimum cost node from the minimum cost node in which the number of expansion directions is set to 4 or 5, and is adjacent to other than the entry prohibition grid. Expand to the node you want. In the second embodiment, the same parts as those in the first embodiment are denoted by the same reference numerals, and detailed description thereof is omitted.

  FIGS. 5A to 5K are diagrams illustrating a state in which the number of deployment directions is increased at a node adjacent to the entry prohibition grid where an obstacle exists. 5A to 5D and 5G to 5J are examples in which the number of deployment directions of the deployment destination node is set to 4. FIG. FIGS. 5E, 5F, and 5K are an example in which the number of deployment directions of the deployment destination node is set to 5. FIG.

  As shown in FIG. 5E, for example, when the setting unit 113 is expanded from the minimum cost node by the node expansion unit 112, the setting unit 113 performs an obstacle against the minimum cost node B2 adjacent to the obstacle entry prohibition grid C3. When it is determined that the node has moved from the adjacent node B3 of the movement source adjacent to the entry prohibition grid C3 in the vertical or horizontal direction, the number in the deployment direction of the minimum cost node B2 is set to five. In this case, the node expansion unit 112 is positioned forward from the minimum cost node B2 in which the number of expansion directions is set to 5 with respect to the movement direction from the adjacent node B3 with respect to the minimum cost node B2, and other than the entry prohibition grid. To adjacent nodes A1, A2, B1, C1, and C2.

  On the other hand, as illustrated in FIG. 5A, for example, when the setting unit 113 is expanded from the minimum cost node by the node expansion unit 112, the setting unit 113 performs the following operation on the minimum cost node B2 adjacent to the obstacle entry prohibition grid C3. When it is determined that the node has moved from the adjacent node B1 that is not adjacent to the obstacle entry prohibition grid, the number of deployment directions of the minimum cost node B2 is set to four. Similarly, as shown in FIG. 5 (j), the setting unit 113, when deployed from the minimum cost node by the node deployment unit 112, provides an obstacle to the minimum cost node C2 adjacent to the obstacle entry prohibition grid C3. When it is determined that the object has moved from the adjacent node B2 of the movement source adjacent in the diagonal direction of the object entry prohibition grid C3, the number of development directions of the minimum cost node C2 is set to four. In this case, the node expansion unit 112 is located forward from the minimum cost node B2 in which the number of expansion directions is set to 4 with respect to the movement direction from the adjacent node B1 to the minimum cost node B2, and other than the entry prohibition grid. To adjacent nodes A2, A3, B3, and C2.

  As described above, the number of expansion directions of the previous node expanded from the minimum cost node in which the number of expansion directions is set to 4 or 5 is set to 3. For example, in FIG. 5A, the number of deployment directions of the deployment destination nodes C2, B3, A3, and A2 is set to 3.

FIG. 6 is a flowchart illustrating an example of a flow of the route search method according to the second embodiment.
When the route search is disclosed, the search branch is branched in 8 directions from the grid of the movement start point (step S201).

It is determined whether or not the node ahead of the search branch branched from the node is the movement end point (step S202). If it is determined that the node ahead of the search branch branched from the node is the movement end point (YES in step S202), the route search process ends.
On the other hand, if it is determined that the node ahead of the search branch branched from the node is not the movement end point (NO in step S202), the minimum cost node is selected from the nodes ahead of the search branch (step S203). .

  Whether or not the minimum cost node adjacent to the obstacle entry prohibition grid has moved from the adjacent node in the vertical or horizontal direction of the obstacle entry prohibition grid when deploying from the selected minimum cost node Is determined (step S204).

  If it is determined that the minimum cost node adjacent to the obstacle entry prohibition grid moves from the adjacent node adjacent to the obstacle entry prohibition grid in the vertical or horizontal direction (YES in step S204), the minimum cost node is determined. The number of expansion directions of the node is set to 5 (step S205), and the process proceeds to the following (step S211).

  When it is determined that the minimum cost node adjacent to the obstacle entry prohibition grid does not move from the adjacent node adjacent to the obstacle entry prohibition grid in the vertical or horizontal direction (NO in step S204), the selected node is selected. When deploying from the minimum cost node, adjacent to the adjacent node in the diagonal direction of the obstacle entry prohibition grid or adjacent to the obstacle entry prohibition grid with respect to the minimum cost node adjacent to the obstacle entry prohibition grid It is determined whether or not it has moved from an adjacent node that does not (step S106).

  It is determined that the minimum cost node adjacent to the obstacle entry prohibition grid has moved from an adjacent node diagonally adjacent to the obstacle entry prohibition grid or an adjacent node not adjacent to the obstacle entry prohibition grid. Then (YES in step S206), the number of minimum cost nodes in the expansion direction is set to 4 (step S207), and the process proceeds to the following (step S211).

  If the minimum cost node adjacent to the obstacle entry prohibition grid is not moved from an adjacent node obliquely adjacent to the obstacle entry prohibition grid or an adjacent node not adjacent to the obstacle entry prohibition grid If determined (NO in step S206), when expanding from the selected minimum cost node, on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node whose number of expansion directions is set to 3. It is determined whether or not (step S208).

  When it is determined that the minimum cost node whose number of expansion directions is set to 3 is on the extension line of the movement direction from the adjacent node of the movement source (YES in step S208), the number of expansion directions of the expansion destination node Is set to 3 (step S209), and the process returns to the above (step S202).

  On the other hand, when it is determined that the minimum cost node with the number of expansion directions set to 3 is not on the extension line of the movement direction from the movement source adjacent node (NO in step S208), the expansion direction of the expansion destination node Is set to 2 (step 210), and the process returns to the above (step S202).

The number of expansion directions of the previous node expanded from the minimum cost node where the number of expansion directions is set to 4 or 5 is set to 3 (step S211), and the processing returns to the above (step S202).
As described above, according to the second embodiment, it is possible to perform a route search so as to avoid an obstacle even when an obstacle is present while shortening the route search time.

Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.
In the above embodiment, the route search system is suitable for route search in a complicated environment with many obstacles, such as indoor autonomous movement. Of course, the mobile body equipped with the route search system is not particularly limited. For example, it can be mounted on a robot wheelchair having two inverted wheels, a walking robot, or an autonomously moving assistant robot. Furthermore, the solvability of the solution at the lowest cost is protected. Moreover, since it becomes possible to avoid an obstacle and a person, safe autonomous movement is attained.

  In the said embodiment, it is applicable also to a three-dimensional route search. When performing a three-dimensional route search, for example, the moving area can be represented by voxels. That is, the moving area is partitioned by a grid in each of the three-dimensional directions. As a result, the moving area is represented by a three-dimensional grid. Then, the voxel is decomposed into a plane. In a 3x3x3 voxel, each plane contains 9 squares of voxels.

When the three-dimensional coordinates are moved only in the X-axis direction and / or the Y-axis direction (on the XY plane), the path search is performed on each plane of the decomposed voxel in the same manner as in the above-described two-dimensional coordinates. Good.
Further, in the three-dimensional coordinates, for example, as shown in FIG. 7, a case is assumed where the search branch from the node (3, 0, 2) to the node (3, 1, 2) further branches (moves in the XYZ axis direction). To do. In this case, from the node (3, 1, 2) to the nodes (2, 2, 4), (3, 2, 4), (4, 2, 4), (2, 2, 3), (3, 2 3), (4, 2, 3), (2, 2, 2), (3, 2, 2), (4, 2, 2). Here, as in the case of the two-dimensional coordinate system, useless route candidates that cannot be candidates for the shortest route can be excluded. For example, when further branching from the search branch from the node (3, 1, 2) to the node (4, 2, 3), the nodes (5, 2, 4), (5, 2, 3), (5, 2 The three branches to 2) are useless route candidates that cannot be candidates for the shortest route, and can be excluded. Therefore, from the node (4, 2, 3), the following six nodes (4, 3, 4), (5, 3, 4), (4, 3, 3), (5, 3, 3), ( 4, 3, 2) and (5, 3, 2). This can be applied in the same manner as the above-described two-dimensional coordinates when a branch branch of three-dimensional coordinates is projected onto the XY plane.

Further, the present invention can be realized, for example, by causing the CPU to execute a computer program for the processing shown in FIG. 4 or FIG.
The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included.

  The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

  1 head, 2 wheels, 3 housing, 5 sensor, 100 robot, 110 control unit, 111 map information storage unit, 112 node development unit, 113 setting unit

Claims (6)

A route search system that selects a route from the start point to the end point included in the area represented by the grid and that has the lowest cost,
Calculate the cost for multiple grids according to multiple deployment directions from the lowest cost node, select and move the lowest cost node, and set the moved node as the lowest cost node Node expansion unit that repeats the process of expanding
A setting unit configured to set the number of deployment directions of each node of the deployment destination from the node of the minimum cost,
The setting unit
When it is determined that the expansion destination node is on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node in which the number of expansion directions is set to 3, the expansion destination node Set the number of deployment directions to 3
When it is determined that the expansion destination node is not on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node in which the number of expansion directions is set to 3, the expansion destination node Set the number of deployment directions to 2
A route search system characterized by that. The route search system according to claim 1,
A route search system, characterized in that the number of expansion directions is increased at a node adjacent to a grid where an obstacle exists. The route search system according to claim 2,
The setting unit
When it is determined that the node of the minimum cost adjacent to the obstacle grid moves from the adjacent node of the movement source adjacent in the vertical or horizontal direction of the obstacle grid, the node of the minimum cost of Set the number of deployment directions to 5,
Move from the adjacent node of the moving source adjacent to the obstacle grid diagonally or from the moving adjacent node adjacent to the obstacle grid to the least cost node adjacent to the obstacle grid If it is determined that the minimum cost node deployment direction is set to 4,
Setting the number of deployment directions of the deployment destination node from the node of the lowest cost set to 4 or 5 as the number of deployment directions;
A route search system characterized by that. A route search system according to any one of claims 1 to 3,
The node expansion unit sets a minimum cost node, the number of expansion directions of which is set to 2 by the setting unit, as a moving direction from an adjacent node to the minimum cost node, and a direction parallel to the extension line. A route search system that develops in a direction. A route search method that selects a route with the lowest cost from a start point to an end point included in an area represented by a grid,
Calculate the cost for multiple grids according to multiple deployment directions from the lowest cost node, select and move the lowest cost node, and set the moved node as the lowest cost node Steps to repeat the process of developing and
Setting the number of deployment directions of each node to be deployed from the node with the lowest cost,
When it is determined that the expansion destination node is on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node in which the number of expansion directions is set to 3, the expansion destination node Set the number of deployment directions to 3
When it is determined that the expansion destination node is not on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node in which the number of expansion directions is set to 3, the expansion destination node Set the number of deployment directions to 2
A route search method characterized by that. A program that selects a route from the start point to the end point included in the area represented by the grid with the lowest cost,
Calculate the cost for multiple grids according to multiple deployment directions from the lowest cost node, select and move the lowest cost node, and set the moved node as the lowest cost node Process to repeat the process of expanding and
When it is determined that the expansion destination node is on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node in which the number of expansion directions is set to 3, the expansion destination node Processing to set the number of deployment directions of 3 to,
When it is determined that the expansion destination node is not on the extension line in the movement direction from the adjacent node of the movement source with respect to the minimum cost node in which the number of expansion directions is set to 3, the expansion destination node Processing to set the number of deployment directions of 2 to
A program characterized by causing a computer to execute.

Images