JP2011227807A - Route search system, route search method, and mobile body - Google Patents

Abstract

PROBLEM TO BE SOLVED: To generate a route closer to an ideal shortest route.SOLUTION: A route search system 110 includes: a map information storing section 111 that stores a grid map with no-entry grid set thereto: a distance potential generating section 112 that generates a distance potential value of each grid on the basis of a potential value corresponding to the distance from a start point and an obstacle: a search grid deciding section 113 that decides a next search grid on the basis of the previous grid search vector and selects a grid for calculating a local potential field: a local potential field calculating section 114 that calculates a local potential field: and a grid search direction deciding section 115 that extends a search branch in a direction where the local potential fields drops most steeply, on the basis of a point on an edge when the previous grid search vector enters the next search grid, and decides the direction as a grid search vector.

Description

  The present invention relates to a route search system, a route search method, and a moving object.

  A technique for searching for the shortest path in a grid space has been proposed. Such route search techniques are disclosed in Patent Documents 1 to 5. For example, in the moving body route generation device disclosed in Patent Document 1, a grid map in a moving area is generated using information on a moving body and an obstacle, and an attractive potential and a repulsive potential are calculated from the relative positional relationships. . Then, a route search in the map is performed based on the combined potential of these potentials, and a route is generated by determining the local minimum.

JP 2006-350776 A JP-A-5-274037 JP 2003-029833 A JP 2005-032196 A JP 2009-025974 A

  However, the technique for searching for a route in the grid space has a problem that a route that is significantly different from the ideal shortest route may be generated as the shortest route.

  In order to explain this problem, a general route search method in the grid space will be described with reference to FIG. In the figure, a route is searched from the grid S at the movement start point toward the grid G at the movement end point. In the figure, a grid indicated by a diagonal line in the lower left direction indicates a grid in which movement is prohibited due to an obstacle or the like.

  In this route search, the front / rear / right / left grid and the surrounding grid located in the diagonal direction are set as grids that can be selected next. Based on the distance between the grids from the current grid to the surrounding grids, the grids constituting the route are sequentially determined so that the route length from the movement start point to the movement end point is the shortest. . For this reason, in the example of FIG. 10A, when the distance between the front and rear, left and right grids and the distance between the diagonally located grids are set to 5 and 7, for example, the search is started from the grid S, The grid in the direction indicated by the arrow is sequentially determined as a grid constituting the route. At this time, the route length of the searched route is expressed as the sum of the distances between the grids.

  On the other hand, an example of the optimum route from the grid S to the grid G is shown in FIG. That is, the ideal route from the grid S to the grid G is a straight line as shown in FIG. 10C, but the route shown in FIG. It will be different.

  This is because the route length is expressed as the sum of the distances between the grids in the calculation process of the route search in the front, rear, left, right, and diagonal positions, so that the distance resolution of the route length becomes rough to the distance between the grids. As a result, a route significantly different from the ideal route is generated. In other words, because it is discretized in units of grids, the calculated numerical value of the path length is rounded for each grid, and accumulation of discrete errors in the grid space is optimal locally, but overall optimization This is because a route that should not be generated is generated.

  Therefore, an object of the present invention is to provide a route search system, a route search method, and a mobile body that can solve the above-described problems and can generate a route closer to an ideal shortest route.

  The route search system according to the first aspect of the present invention divides a moving region into a plurality of grid-like grids, with the surrounding frame separating each grid as an edge of the grid, and between the movement start point and the movement end point. A route search system for searching a grid sequence to be connected, a map information storage unit storing a grid map in which an entry prohibition grid in which an obstacle occupies and a moving body is prohibited is set, and from the movement start point A distance potential generation unit that generates a potential value according to a distance and a potential value according to a distance from the obstacle to generate a distance potential value of each grid of the grid map, and a previous search target grid The next search grid to which the determined grid search vector enters next is determined as the next search grid to be searched next, and the previous grid A search grid determining unit that selects the first and second grids adjacent to the next search grid as local potential field calculation grids for the next search grid according to the direction of the grid search vector; and the next search grid A local potential field having vertices based on a relative change in the distance potential value of the local potential field calculation grid with respect to a distance potential value of the local potential field, and a local potential field calculation unit for calculating a local potential field for the next search grid; The point on the edge when the grid search vector determined in the previous search target grid enters the next search grid is the base of the search branch, and the direction in which the potential value of the local potential field has the steepest gradient, Extend the search branch in the direction of descending the gradient, The direction of the search branches extended, in which and a grid search direction determination section for determining a grid search vector in the next search grid.

  Thus, the local potential field is calculated for the next search grid, the search branch between the edges of the next search grid is obtained according to the gradient in the calculated local potential field, and the direction of the search branch is determined as the grid search vector. By searching the next search target grid from the grid search vector, the next search grid can be determined in more detail compared to the conventional method. A route closer to the route can be generated. For example, in the example shown in FIG. 10, the search result by the prior art is shown in FIG. 10A, whereas the search result by the route search system according to the present invention is shown in FIG.

  Further, the local potential field calculation unit, as the local potential field, a vertex determined according to the difference between the square of the distance potential value of the next search grid and the square of the distance potential value of the grid for local potential field calculation It is also possible to calculate a conical potential field having

  Furthermore, the grid search direction determination unit is a point on a straight line from the base point of the search branch to the vertex of the local potential field, and is different from the edge where the base point is located. A point located on another edge may be obtained as an end point of the search branch, and a direction from the base point of the search branch toward the end point may be determined as a grid search vector in the next search grid.

  Further, the distance potential generation unit determines a predetermined value as a movement cost between adjacent grids, searches for a grid series from the movement start point to a target grid, and sums the movement costs for the searched grid series. The total potential value and the potential value corresponding to the distance from the obstacle may be added to calculate the distance potential value of the target grid.

  A route search method according to a second aspect of the present invention is a route search method for a moving body that moves from a movement start point to a movement end point in a movement region, and the movement region is divided into surrounding frames that divide each grid. A grid map that is represented by a grid map divided into a plurality of grid-like grids as an edge of the grid and in which an entry prohibition grid that is occupied by an obstacle and prohibited from entering the moving body is set is stored. Adding a potential value according to a distance from the moving start point and a potential value according to a distance from the obstacle, and generating a distance potential value of each grid of the grid map; The next search grid to be searched next is determined as the next grid to which the grid search vector determined in the previous search target grid enters next. And selecting the first and second grids adjacent to the next search grid as local potential field calculation grids for the next search grid according to the direction of the grid search vector, and the determined next For the search grid and the selected local potential field calculation grid, a local potential field having vertices based on a relative change in the distance potential value of the local potential field calculation grid with respect to the distance potential value of the next search grid, Calculating the local potential field for the next search grid, and calculating the point on the edge when the grid search vector determined in the previous search target grid enters the next search grid as a base point of the search branch Potency of local potential field Extending the search branch in a direction in which the gradient value has the steepest gradient and descending the gradient, and determining the direction of the extended search branch as a grid search vector in the next search grid; and Outputting a grid sequence for which a search vector is determined as a path connecting the movement start point and the movement end point. As a result, a route closer to the ideal shortest route can be generated.

  The moving body according to the third aspect of the present invention divides the moving region into a plurality of grid-like grids having the surrounding frames dividing each grid as the edges of the grid, and connects between the moving start point and the moving end point. A map information storage unit that stores a grid map that searches for a grid series as a route and moves according to the route, and is configured with an entry prohibition grid in which an obstacle occupies and entry of the mobile object is prohibited Adding a potential value according to the distance from the moving start point and a potential value according to the distance from the obstacle, and generating a distance potential generating unit as a distance potential value of each grid of the grid map; The next adjacent search grid that the grid search vector determined in the previous search target grid enters next is determined as the next search target grid. And a search grid determining unit that selects the first and second grids adjacent to the next search grid as local potential field calculation grids for the next search grid according to the direction of the grid search vector. A local potential field having a vertex based on a relative change in the distance potential value of the local potential field calculation grid with respect to the distance potential value of the next search grid, the local potential field calculating a local potential field for the next search grid A direction in which the potential value of the local potential field has the steepest gradient with a point on the edge when the grid search vector determined in the previous search grid enters the next search grid as a search branch base point And the search in the direction of descending the gradient The extended, the direction of the search branches extended the one in which and a grid search direction determination section for determining a grid search vector in the next search grid. As a result, a route closer to the ideal shortest route can be generated.

  According to the present invention, it is possible to provide a route search system, a route search method, and a moving body that can generate a route closer to an ideal shortest route.

3 is a diagram illustrating a configuration of a moving object according to Embodiment 1. FIG. 3 is a configuration diagram of a control unit of a moving body according to Embodiment 1. FIG. It is a figure for demonstrating terms, such as a grid which concerns on Embodiment 1. FIG. It is a figure for demonstrating the determination method of the search grid which concerns on Embodiment 1, and the selection method of the grid for local potential field calculation. 6 is a diagram for explaining a method of calculating a local potential field according to Embodiment 1. FIG. 6 is a diagram for explaining derivation of a conical potential field according to Embodiment 1. FIG. 6 is a diagram for explaining a method of determining a grid search vector according to Embodiment 1. FIG. 6 is a flowchart of route search processing according to Embodiment 1. FIG. FIG. 10 is a diagram for explaining an effect of route search according to the first embodiment. It is a figure for demonstrating the technique relevant to this invention, and the route search result by this invention.

Embodiment 1 FIG.
Embodiments of the present invention will be described below with reference to the drawings.
With reference to FIG. 1, the structure of the robot which is an example of the mobile body which concerns on this Embodiment is demonstrated. FIG. 1 is an external view schematically showing the configuration of the robot 100. In the present 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, a motor connected to the wheel 2, a battery for driving the motor, and the like are provided. 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. . The controller 110 is not limited to one physical configuration.

  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. The robot 100 is not limited to a wheel type mobile robot, but 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 searches for 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.

  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 distance potential generation unit 112, a search grid determination unit 113, a local potential field calculation unit 114, and a grid search vector determination unit 115. I have.

  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.

  In addition, 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. The robot 100 cannot move to the position occupied by the 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, a grid series (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.

  As shown in FIG. 3A, the grid indicates a region of each lattice that is partitioned vertically and horizontally. In the example shown in the figure, four grids A, B, C, and D are shown. The edge of the grid indicates a frame around the grid. In the example shown in the figure, for example, the edges of the grid A are four edges a1, a2, a3, and a4. One arbitrary grid is adjacent to the four grids on the top, bottom, left, and right. That is, in the grids other than the end of the moving region, the four neighboring grids in the upward direction, the downward direction, the left direction, and the right direction are adjacent to each other.

Further, as shown in FIG. 3B, for a certain grid, a straight line connecting one point on the edge and one point on another different edge is set as a search branch between the edges. A vector indicating the direction in which the search branch extends is defined as a grid search vector. That is, the grid search vector indicates the direction (slope) of the search branch. In the example shown in the figure, a straight line connecting the point Q _j and the point Q _{j + 1} is a search branch between edges, and the direction of this search branch is a grid search vector. When the grid search vector enters the edge of the adjacent grid, this entry point (intersection of the grid search vector and the edge) becomes the start point of the search branch in the grid adjacent via the edge. In the route search process, the coordinates of the end points of the search branch on the edge and the angle of the search branch (grid search vector) are stored as information. The angle of the search branch can be obtained from the coordinate values of these end points.

  The distance potential generation unit 112 generates a distance potential for the grid in the grid map based on the distance from the movement start point and the distance from the obstacle. First, the distance potential generation unit 112 sets a potential value corresponding to the distance from the movement start point for each grid in the grid map. Here, a value of 0 is set for the grid at the movement start point, a potential value that increases with distance from the movement start point is calculated, and these potential values are set in each grid. For example, an appropriate value such as a distance between grids is determined as a cost required for moving to an adjacent grid. Then, the shortest path from the movement start point to the target grid is searched using a known search algorithm such as Dijkstra method or A * (star) algorithm. Then, the total cost for the grid on the searched route is obtained (that is, a value corresponding to the path length from the movement start point to the target grid), and the total value is set as the distance potential value of the target grid.

  In addition, when there is an obstacle, the distance potential generation unit 112 calculates a potential value corresponding to the distance from the obstacle, and adds it to the potential value set according to the distance from the movement start point. For example, consider a potential function that decreases monotonically according to the distance from the obstacle grid, and generate a potential field that uses the sum of the values calculated by the potential function for each of the grids that can be entered. To do. However, the obstacle grid itself is set to a certain potential value (a positive value sufficiently larger than the potential value set for a grid that is not an obstacle and is treated as infinite). . The distance potential value calculation method set according to the distance from the obstacle by the distance potential generation unit 112 may be performed using a conventionally known potential method, and detailed description thereof is omitted here.

  The search grid determination unit 113 determines a grid to be searched next from the grid search vector determined for the grid that was previously searched, and selects a grid to be used for calculating the local potential field for the determined grid. To do. More specifically, the adjacent grid that the grid search vector determined last time enters by crossing the edge is determined as a grid to be searched next (hereinafter may be referred to as a next search grid). Then, among the determined adjacent grids of the next search grid, a plurality of grids on the extension of the grid search vector are selected as local potential field calculation grids (hereinafter referred to as local potential field calculation grids). May be.) Note that the local potential field calculation grid is selected from grids adjacent to the next search grid in the front, rear, left, and right directions.

  FIG. 4 is a diagram for explaining how the search grid determination unit 113 determines a search grid and selects a local potential field calculation grid. In the example shown in the figure, a 3 × 3 grid is shown. Here, for simplification of description, each grid is identified by coordinates as follows. For example, the upper left corner node is (1, 1) grid, and the lower right corner node is (3, 3). Further, the node immediately below the grid (1, 1) is set as the grid (2, 1), and the grid immediately below the grid (1, 1) is set as (1, 2). In this way, each grid is identified using the coordinates. Note that the example shown in FIG. 4 shows processing in a global coordinate system in which the right direction on the paper is the positive x-axis direction and the upward direction is the positive y-axis direction. For this reason, here, the coordinates of the end points of the search branch and the angles of the search branch in the global coordinate system are held in a temporary storage unit such as a RAM.

  In the example shown in FIG. 4A, first, the grid (2, 2) is determined as the next search grid from the search direction (grid search vector) indicated by the arrow in the grid (2, 1). Then, paying attention to the x and y components of the grid search vector and x> 0 and y> 0, the grid (1, 2) and the grid (2, 3) are selected as the local potential field calculation grid. In the example shown in FIG. 4B, since the grid (2, 2) is determined as the next search grid and x> 0 and y <0, the grid (2, 3) and the grid (3, 2) are determined. ) Is selected as the grid for calculating the local potential field. Thus, the next search grid and the local potential field calculation grid are selected according to the component of the grid search vector (the incident angle of the arrow).

  When x = 0 and y = 0 for the x and y components of the grid search vector, two local potential field calculation grids are selected in the same manner as shown in FIG. Alternatively, two local potential field calculation grids may be selected in the same manner as in the case shown in FIG. In the example illustrated in FIG. 4, for example, the grid (2, 2) and any one of the grid (2, 3) or the grid (3, 2) may be selected. In addition, when there exists any one obstacle grid of a grid (2, 3) or a grid (3, 2), you may make it select the other grid which is not an obstacle grid.

  Although details will be described later, a grid search vector in the grid (2, 1) is determined using a local potential field calculated for the grid (2, 1). The next search grid is determined from the determined grid search vector.

  The local potential field calculation unit 114 calculates a local potential field for the next search grid based on a relative change in the distance potential value of the local potential field calculation grid with respect to the distance potential value of the next search grid. Here, a conical potential field is calculated as the local potential field. The apex of the conical potential field is determined according to the difference between the square of the potential value of the next search grid and the square of the potential value of the local potential field calculation grid.

  FIG. 5 is a diagram for explaining a local potential field calculation method. In the example shown in the figure, three grids A, B, and C for calculating a conical potential field as a local potential field are shown. In FIG. 5, the next search grid is grid A, the local potential field calculation grids are grid B and grid C, and the point where the edges of these three grids intersect with each other is determined as the origin of the coordinate system. That is, in the calculation of the local potential field, processing is performed in a local coordinate system determined from the determined next search grid and the selected local potential field calculation grid.

In the example shown in FIG. 5, it is assumed that a search branch (indicated by a solid line arrow) having a negative slope enters a point Q1 (x _q , y _q ) on the edge a1 of the grid A (shown by a solid arrow). That is, for the components of the grid search vector, dy / dx <0 for x and y in the global coordinate system. At this time, as described above, the grid A becomes the next search grid, and the grids B and C are selected as the local potential field calculation grid. The local potential field calculation unit 114 calculates a local potential field P _N for the grid A using the potential value P _A of the grid _A and the potential values P _B and P _C of the grids B and C.

The local potential field _PN is expressed using the following number (1).

The local potential field represented by the number (1) is a conical potential field whose vertex Oc is represented by the following number (2). In the number (1), Δd represents the grid size (edge size).

If any one of the grids B and C selected as the local potential field calculation grid is an obstacle grid, a planar potential field is calculated instead of calculating the conical potential field. It shall be. For example, when the grid B selected as the local potential field calculation grid is an obstacle grid, the potential value P _A of the grid _A and the grid are replaced with the local potential field represented by the number (1) above. The potential value PC of _C is linearly interpolated, and a planar potential field formed by the linearly interpolated value is defined as a local potential field. If both grids B and C are obstacle grids, it may be determined that there is no solution by route search and the search may be aborted.

Here, with reference to FIG. 6, the derivation of the conical potential field will be supplemented.
In the example shown in the figure, three grids A, B and C for calculating the conical potential field are shown. It is assumed that a grid search vector (not shown) enters on the edge of the grid A, and the entry point is Q1. The origin of the local coordinate system is O.

First, each position A (x, y), B (x, y), and C (x, y) of the grids A, B, and C in the local coordinate system with O as the origin is represented by the following number (3): , (4), (5).

Next, the following numbers (6), (7), and (8) are defined with respect to the potential value of each grid with the vertex Oc of the conical potential as the center. In the following description, the x and y coordinate values of the vertex Oc are simply expressed as O _cx and O _cy , and the x and y coordinate values of the position of the grid A are simply expressed as Ax and Ay.

また、数（６）−数（８）を計算して、以下の数（１０）を得る。

従って、数（９）及び数（１０）から、以下の数（１１）に示すように、円錐ポテンシャルの頂点Ｏｃを求める。

従って、数（１１）の頂点Ｏｃから、以下の数（１２）で示す円錐ポテンシャルＰが定まる。

And the number (6) -number (7) is calculated and the following number (9) is obtained.

Also, the number (6) −number (8) is calculated to obtain the following number (10).

Therefore, the vertex Oc of the conical potential is obtained from the numbers (9) and (10) as shown in the following number (11).

Therefore, the cone potential P shown by the following number (12) is determined from the vertex Oc of the number (11).

Returning to FIG. 2, the description will be continued.
The grid search vector determination unit 115 sets the point on the edge where the previously determined grid search vector enters as the base point of the search branch, and the direction in which the calculated potential value of the local potential field has the steepest gradient (the change in potential value is the largest). The search branch is extended in a direction in which the gradient decreases (the direction in which the potential value decreases). The grid search vector determination unit 115 determines the direction of the extended search branch as a grid search vector. The determined grid search vector is held in a temporary storage unit such as a RAM, and is used as information for determining a grid to be searched next.

  FIG. 7 is a diagram for explaining a grid search vector determination method. In the figure, the current search target grid is grid A, and a conical potential as a local potential field is calculated. Note that the conical potential field is calculated based on the potential values of the grids A, B, and C, and Oc indicates the apex of the conical potential field.

  In FIG. 7, the previously determined grid search vector enters the edge a1 of the grid A, and the entry point is defined as a point Q1. In the grid A, with the point Q1 as the base point of the search branch, the search branch is extended in the direction of descending in the steepest gradient direction in the local potential field. Although the details will be described later, when the search branch extending from the point Q1 reaches the edge a3 of the grid A, the direction of the search branch from the point Q1 to the point Q2 with the intersection point on the edge a3 as the point Q2, It is the steepest gradient direction in the local potential field and the falling direction.

  In the cone potential field, the direction descending in the steepest gradient direction corresponds to the direction of a straight line from the base point on the edge to the apex of the cone (this line intersects at right angles to the circumference of the cone). That is, the search branch extends in a direction from the base point on the edge toward the apex of the conical potential field. The end point Q2 of the search branch is a point on the straight line from the base point Q1 to the apex of the conical potential field and on the edge a3. The coordinates of the point Q2 can be obtained by the following method, for example.

First, the vertex Oc of the conical potential field is given by the following number (13).

The following number (14) is a linear equation indicating the relationship between the coordinates of the vertex of the conical potential field, the coordinates of the point Q1, and the coordinates of the point Q2. A straight line connecting the vertex of the conical potential field and the point Q1 and a straight line connecting the point Q1 and the point Q2 are parallel to each other. Therefore, the coordinates (Q _2x , Q _2y ) of the point Q2 can be obtained using this linear equation.

Further, since the point Q1 and the point Q2 are respectively points on the edge, for example, using the relationship of Q _2x −Q _1x = Δd, the point Q2 is expressed as shown in the following number (15). Can be obtained.

  Therefore, in the example shown in FIG. 7, the coordinates of the point Q2 on the edge a2 are obtained from the coordinates of the base point Q1 on the edge a1 and the coordinates Oc of the vertex of the conical potential field. Then, a straight line connecting the base point Q1 on the edge a1 and the point Q2 on the edge a2 can be determined as a search branch between edges, and the direction of the search branch can be determined as a grid search vector.

In the example shown in FIG. 7, the grid A is a grid closer to the movement end point than the other grids, the grid B is a grid closer to the wall than the other grids, and the grid C is This is a grid closer to the movement start point than other grids, and shows a situation where P _C <P _B <P _A holds for the potential value of each grid. A conical potential field reflecting such a situation is illustrated in FIG. That is, the vertex Oc of the conical potential field is calculated as being located in a direction closer to the movement start point and away from the obstacle as viewed from the base point Q1 of the search branch. Accordingly, the grid search vector that has entered the grid A is bent in a direction away from the obstacle by the calculated local potential field.

Next, the flow of the route search process will be described with reference to FIG.
First, the control unit 110 sets a movement start point grid and a movement end point grid in the grid space (S101).

  Next, the distance potential generation unit 112 sets a potential value corresponding to the distance from the movement end point and the obstacle for each grid in the grid map, and generates a distance potential (S102).

  Next, the search grid determination unit 113 determines a grid to be searched next from the previously determined grid search vector, and selects a grid to be used for calculation of the local potential field for the determined grid (S103). .

  Next, the local potential field calculation unit 114 calculates a local potential field for the determined next search grid based on the potential value of the next search grid and each potential value of the selected local potential field calculation grid. (S104).

  Next, in the next search grid in which the local potential field is calculated, the grid search vector determination unit 115 sets the potential value to fall in the steepest gradient direction with the point where the previously determined grid search vector enters the edge as the base point of the search branch. The search branch is extended in the direction, and the direction of the extended search branch is determined as a new grid search vector for determining the search target grid next to the next search grid (S105).

  The control unit 110 determines whether or not the next search grid indicated by the grid search vector is the movement start point grid, and determines whether or not the movement start point grid has been reached (S106). As a result of the determination, if it has not reached, the process of steps S103 to S105 is repeated to search for the grid sequentially. On the other hand, if the result of the determination is that the control unit 110 has reached, the control unit 110 outputs the grid sequence searched until the movement start point is reached. That is, this grid series is generated as a route between the movement end point and the movement start point.

  As described above, according to the present embodiment, unlike the grid search by the conventional potential method, the grid having the smallest potential value among the adjacent grids is not simply selected. In this grid, search branches between edges are obtained, the direction of the search branch is set as a grid search vector, and the next search target grid is sequentially determined from the grid search vector. Here, when determining the grid search vector, a local potential field is calculated in consideration of the potential value of the adjacent grid, and a more detailed potential field is calculated locally from the distance potential value. In the local potential field, the search branch is extended in the direction in which the potential value falls in the steepest gradient direction.

FIG. 9 is a diagram for more specifically explaining the effect of the present embodiment.
FIG. 9A shows a route search result according to a conventional method in which a route having the shortest route length is obtained using the distance between grids. The obtained grid sequence is significantly different from the optimum path indicated by the straight line.
On the other hand, FIG. 9B shows a route search result according to the present embodiment. The obtained grid series is closer to the optimum path indicated by the straight line.

In the route search method using the conventional potential method, the selection of the grid having the smallest potential among the adjacent grids is simply repeated, and the search is terminated when the grid of the movement end point is reached. .
In contrast, in the present embodiment, a local potential can be avoided because a distance potential is generated using a route search method (such as Dijkstra method) that basically performs a full search, and is ideal. A route closer to the shortest route can be generated.

Further, in the path generation technique using the conventional potential method as disclosed in Patent Document 1, a preset potential field itself is corrected with a synthetic potential for the purpose of avoiding a local minimum.
On the other hand, in the present embodiment, the potential field itself generated in advance is not corrected, but a detailed local potential field is obtained for each search grid in the course of the route search. In other words, a route search is performed while obtaining a detailed solution locally using a potential value generated in advance. Thus, by finding a detailed potential value locally, a route closer to the optimum solution can be searched.

  In the above-described embodiment, the example of the route search in the case where there is an obstacle in the movement region from the movement start point to the movement end point has been described, but the present invention is not limited to this and depends on the presence or absence of the obstacle. Therefore, a more ideal route can be generated.

  In the above-described embodiment, the case where the search is performed from the movement end point grid toward the movement start point grid has been described as an example. However, the search is performed from the movement start point grid toward the movement end point grid. In other words, the present invention is applicable to a route search technique that connects two arbitrary points.

  In addition, when the potential value according to the distance from the movement start point and the obstacle is set as a negative value, the search branch is extended in the direction in which the potential value climbs in the steepest gradient direction in the local potential field, and the extension is extended. The search direction may be determined as a grid search vector.

  Note that interpolation processing may be further performed on the path obtained by the above-described processing. Since the route generated by the present invention is closer to the shortest route, it is possible to generate a more optimal and smooth route by performing interpolation processing based on such a route.

  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.

2 wheels,
3 housing,
5 sensors,
100 robots,
110 control unit,
111 Map information storage unit,
112 distance potential generator,
113 Search grid determination unit,
114 Local potential field calculator,
115 grid search vector determination unit,

Claims (6)

A route search system that divides a moving region into a plurality of grid-like grids, with a surrounding frame that divides each grid as an edge of the grid, and searches for a grid sequence that connects between a moving start point and a moving end point,
A map information storage unit for storing a grid map in which an entry prohibition grid in which an obstacle occupies and entry of a moving object is prohibited is stored;
A distance potential generator that adds a potential value according to a distance from the moving start point and a potential value according to a distance from the obstacle, and generates a distance potential value of each grid of the grid map;
The next search grid that the grid search vector determined in the previous search target grid enters next is determined as the next search grid to be searched next, and is adjacent to the next search grid according to the direction of the grid search vector. A search grid determining unit that selects the first and second grids to be used as a local potential field calculation grid for the next search grid;
A local potential field having a vertex based on a relative change in the distance potential value of the local potential field calculation grid with respect to the distance potential value of the next search grid, the local potential field calculating a local potential field for the next search grid A calculation unit;
The point on the edge when the grid search vector determined in the previous search target grid enters the next search grid is the base of the search branch, and the direction in which the potential value of the local potential field has the steepest gradient, A path search system comprising: a grid search direction determination unit that extends the search branch in a direction of descending the gradient, and determines a direction of the extended search branch as a grid search vector in the next search grid. The local potential field calculator is
Calculating a conical potential field having a vertex determined according to a difference between the square of the distance potential value of the next search grid and the square of the distance potential value of the local potential field calculation grid as the local potential field. The route search system according to claim 1. The grid search direction determination unit
A point on a straight line from the base point of the search branch to the vertex of the local potential field, the point located on the other edge of the next search grid different from the edge on which the base point is located, 3. The route search system according to claim 1, wherein the route search system is obtained as an end point of a search branch, and a direction from the base point of the search branch toward the end point is determined as a grid search vector in the next search grid. The distance potential generator is
A predetermined value is defined as a movement cost between adjacent grids, a grid series from the movement start point to the target grid is searched, a total of the movement costs for the searched grid series is obtained, the total value, The route search system according to any one of claims 1 to 3, wherein a distance potential value of the target grid is calculated by adding a potential value corresponding to a distance from an obstacle. A route search method for a moving body that moves from a movement start point to a movement end point in a movement area,
The moving area is represented by using a grid map divided into a plurality of grid-like grids with the surrounding frame separating each grid as an edge of the grid, occupied by obstacles, and entry of the moving object is prohibited. Storing a grid map in which an entry prohibition grid is set;
Adding a potential value according to a distance from the moving start point and a potential value according to a distance from the obstacle, and generating a distance potential value of each grid of the grid map;
The next search grid that the grid search vector determined in the previous search target grid enters next is determined as the next search grid to be searched next, and is adjacent to the next search grid according to the direction of the grid search vector. Selecting the first and second grids as local potential field calculation grids for the next search grid;
For the determined next search grid and the selected local potential field calculation grid, a local potential field having a vertex based on a relative change in the distance potential value of the local potential field calculation grid with respect to the distance potential value of the next search grid. Calculating a local potential field for the next search grid;
The point on the edge when the grid search vector determined in the previous search target grid enters the next search grid is used as the base point of the search branch, and the calculated potential value of the local potential field is in the direction in which the steepest gradient is obtained. Extending the search branch in the direction of descending the gradient, and determining the direction of the extended search branch as a grid search vector in the next search grid;
Outputting a grid sequence for which the grid search vector is determined as a path connecting the movement start point and the movement end point. The moving area is divided into a plurality of grid-like grids with the surrounding frame separating each grid as the edge of the grid, and a grid series connecting the moving start point and the moving end point is searched as a route and moved according to the route. A moving object,
A map information storage unit for storing a grid map in which an entry prohibition grid in which an obstacle occupies and entry of a moving object is prohibited is stored;
A distance potential generator that adds a potential value according to a distance from the moving start point and a potential value according to a distance from the obstacle, and generates a distance potential value of each grid of the grid map;
The next search grid that the grid search vector determined in the previous search target grid enters next is determined as the next search grid to be searched next, and is adjacent to the next search grid according to the direction of the grid search vector. A search grid determining unit that selects the first and second grids to be used as a local potential field calculation grid for the next search grid;
A local potential field having a vertex based on a relative change in the distance potential value of the local potential field calculation grid with respect to the distance potential value of the next search grid, the local potential field calculating a local potential field for the next search grid A calculation unit;
The point on the edge when the grid search vector determined in the previous search target grid enters the next search grid is the base of the search branch, and the direction in which the potential value of the local potential field has the steepest gradient, A grid search direction determination unit that extends the search branch in a direction of descending the gradient and determines a direction of the extended search branch as a grid search vector in the next search grid.

Images