JP2010061293A - Route searching device, route searching method, and route searching program - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To easily search for a movement route on which a moving object can move while avoiding an obstacle, and to search for a movement route which enables the route length thereof and expected arrival time of the moving object to an optional spot on the route to be easily calculated. <P>SOLUTION: A route searching device includes: a searching space generation part 20 which sets a movement inhibition region with an obstacle present in a movement region as a center, and generates the shape of the outer periphery of the set movement inhibition region by using a graphic having prescribed curvature set based on the moving speed of an autonomous moving object 10; and a route searching part 21 which searches a route based on the moving speed of the autonomous moving object 10 from route candidates obtained by connecting the tangent of the movement inhibition area with the outer periphery of the movement inhibition region, and which, when the result of the route searching shows that the autonomous moving object passes on the outer periphery of the movement inhibition area, regards the movement route section excluding the outer periphery of the movement inhibition area as a tangent and regards the movement route section on the outer periphery of the movement inhibition area as the movement route following the graphic among the movement routes. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

  The present invention relates to a route search device, a route search method, and a route search program.

  In recent years, vehicles and walking robots as so-called autonomous moving bodies that autonomously move in a predetermined movement area such as a limited area indoors or outdoors have been developed. In order to move such an autonomous mobile body, it is necessary to recognize the self-position of the autonomous mobile body in the movement area and to create a movement route on which the autonomous mobile body is to move in advance or in real time.

As a method for creating a moving route of an autonomous mobile body, a method of performing a route search using a grid map holding an equally spaced grid is well known. That is, a movement start point and a movement end point are arranged on such a grid map, and a full route search is performed by a route search algorithm (A * search algorithm or the like). Here, since the searched route is a combination of line segments, the autonomous mobile body cannot follow the route as it is. For this reason, the process which correct | amends the searched path | route to the smooth curve path | route is performed (for example, patent document 1 etc.).
JP 2007-257094 A

  As described above, in the conventional technique, the route searched on the grid map is not a route that can be followed by the autonomous mobile body. For this reason, it is necessary to add a smoothing process so that it becomes a path | route which an autonomous mobile body can track. However, this process causes the following problems.

  First, it can be mentioned that the route after the smoothing process does not always guarantee the same safety as the route before the process. In this method, a smoothing process is performed on the grid path expressed by repeating the line segment, thereby generating a path that passes between the grids. This is because, when a new route is generated in this way, the collision with the obstacle is not determined, so that there is a risk of collision (contact) with the obstacle.

  In addition, although the shortest route before the smoothing process can be guaranteed from the nature of the search algorithm, the route after the processing is not always the shortest. This is because the route smoothing process does not include a process for guaranteeing the shortest path.

  Furthermore, since the route length cannot be accurately calculated for the route after the smoothing process, it is difficult to calculate the arrival time of the autonomous mobile body at each point. This is because the route after smoothing is often expressed non-linearly and it is difficult to calculate the route length.

  As described above, the conventional technique has the above-described various problems by performing a two-step process of searching for a route on a grid map and further applying a smoothing process to the route. Therefore, the moving body can easily search for a moving route that can move while avoiding an obstacle, and can also search for a moving route that can easily calculate the route length and the estimated arrival time to any point on the route. There is a strong demand for route search technology that can do this.

  Therefore, the present invention can easily search for a moving route in which the moving body can avoid the obstacle and can easily calculate the route length and the estimated arrival time to any point on the route. An object of the present invention is to provide a route search device, a route search method, and a route search program capable of searching for a moving route.

  A route search device according to the present invention is a route search device that starts a movement from a movement start point existing in a movement region and searches for a movement route of an autonomous mobile body that reaches a movement end point existing in the movement region. A figure having a predetermined curvature in which a movement prohibited area is set around an obstacle existing in the movement area, and the shape of the outer periphery of the set movement prohibited area is set based on the moving speed of the autonomous mobile body A search space generation unit that generates a path from a route candidate formed by connecting a tangent of the movement prohibited area and an outer periphery of the movement prohibited area, based on the moving speed of the autonomous mobile body, As a result of the route search, when the autonomous mobile body passes on the outer periphery of the movement prohibited area, the movement path portion excluding the outer periphery of the movement prohibited area is set as the tangent line, and the movement prohibited area A movement path portion on the outer periphery of those having a route searching unit for a moving path according to the figure.

  Thereby, it is possible to search for a movement route that avoids a collision with an obstacle. Further, since the searched movement path is composed of the tangent line of the movement prohibition area and the outer periphery of the movement prohibition area in accordance with a predetermined curvature, no further smoothing processing is required for the movement path. In other words, since a separate process for correcting the searched movement route to a smooth route is not required, the mobile body can move the searched movement route as it is. Further, since the travel route configured as described above can easily and accurately calculate the route length, it is possible to calculate the arrival time of the autonomous mobile body at each point on the route. Therefore, the moving body can easily search for a moving route that can move while avoiding an obstacle, and can also search for a moving route that can easily calculate the route length and the estimated arrival time to any point on the route. can do.

  Further, the figure is a circle having a predetermined curvature set based on the moving speed of the autonomous mobile body, or a plurality of circles having a predetermined curvature set based on the moving speed of the autonomous mobile body And a common tangent line between the plurality of circles. For example, when the figure has such a configuration, the length of the outer periphery of the movement prohibited area can be calculated easily and accurately.

  Furthermore, the route search unit may include a length of a common tangent line between the movement start point and the movement prohibited area, a length of an outer circumference between contact points of the tangent at an outer circumference of the movement prohibited area, and a common area between the movement prohibited areas. The movement time of the route candidate by the autonomous mobile body is calculated from the length of the tangent line, the length of the common tangent line between the movement prohibited area and the movement end point, and the movement speed of the autonomous mobile body, and the calculated movement It is preferable that the route search is performed using the route candidate having the shortest time as the travel route.

  Further, the search space generation unit generates the generation based on the arrangement relationship between the movement prohibited areas when the outer peripheral part of the generated movement prohibited area overlaps with the outer peripheral part of the other movement prohibited area. It is determined whether or not an outer peripheral part of the movement prohibited area constitutes the route candidate, and when it is determined that an outer peripheral part of the generated movement prohibited area does not constitute the route candidate, the generated movement prohibited area The outer peripheral portion of the movement prohibited area that is determined not to constitute the route candidate may be excluded from the outer peripheral portion. Accordingly, the route search can be performed more efficiently by excluding the outer peripheral portion of the movement prohibited area that cannot be a route candidate.

  Furthermore, the route search unit may exclude, from the route candidates, tangents that overlap with other movement prohibited regions among the tangents of the movement prohibited region. As a result, the route search can be performed more efficiently by excluding the tangent of the movement prohibited area that cannot be a route candidate.

  Further, the route search unit calculates a future position of the moving obstacle, sets a movement prohibited area based on the calculated future position of the moving obstacle, and the set movement prohibited area overlaps the route candidate Further using a route candidate formed by connecting a tangent line of the movement-inhibited area of the moving obstacle to the outer periphery of the movement-inhibited area of the moving obstacle, and passing on the outer circumference of the movement-inhibited area of the moving obstacle. It is preferable to perform a route search as described above. Thereby, even when a moving obstacle exists in the moving area, it is possible to search for a moving route that avoids the moving obstacle.

  Furthermore, when the route search unit passes the outer periphery of the movement prohibited area of the moving obstacle, the route search unit performs a route search so as to pass on the outer periphery behind the moving obstacle. May be. Thereby, since it avoids without moving ahead of the moving direction of a moving obstacle, a safer movement route can be searched.

  Further, the route search unit calculates a future position of the moving obstacle, sets a movement prohibited area based on the calculated future position of the moving obstacle, and the set movement prohibited area overlaps the route candidate Is configured to set a deceleration node for instructing to decelerate the moving speed of the autonomous moving body to a predetermined speed between the movement prohibited area of the moving obstacle and the autonomous moving body on the route candidate. It is preferable to perform a route search using the reduced deceleration node. Thereby, it is not necessary to search for a new movement route according to the movement of the moving obstacle, and it is possible to search for a movement route that can be avoided from the moving obstacle only by decelerating the autonomous mobile body.

  Furthermore, it is preferable that the tangent of the movement prohibited area and the movement prohibited area are connected by a clothoid curve set based on the moving speed of the autonomous mobile body. Thereby, the autonomous mobile body can move while maintaining the followability of the movement path even at the contact point between the tangent line of the movement prohibited area and the movement prohibited area.

  In addition, the search space generation unit adds a predetermined safe distance to the figure having a size set based on the moving speed of the autonomous mobile body and the size of the autonomous mobile body itself. It is preferable to select a figure having a larger value from the figure and use the selected figure as the figure having the predetermined curvature. As a result, it is possible to search for a movement route in consideration of the followability of the movement route based on the movement speed of the autonomous mobile body and the safety of the movement route that places importance on collision avoidance.

  Furthermore, the search space generation unit divides the movement area into four when the range of the obstacle in the movement area is equal to or greater than a predetermined range, and repeats the four-division process, thereby It is preferable to calculate a movement area in which the range of the obstacle occupying the movement area after processing is smaller than the predetermined range, and use the calculated movement area as a search space. Thereby, calculation time can be reduced by limiting the movement area | region used as the object of a route search according to a movement of an autonomous mobile body. Therefore, the route search can be realized even in a vast search space.

  In addition, the movement area is a three-dimensional area, and the search space generation unit converts a shape of the outer periphery of the movement prohibition area centered on a three-dimensional obstacle into a three-dimensional figure having a predetermined curvature. It is preferable to generate by using. Thus, the present invention is not limited to the two-dimensional space, and can be applied to trajectory calculation of a manipulator, for example, by performing a route search in the three-dimensional space.

  Furthermore, it is preferable that the size of the figure is set according to the arrangement position of the obstacle in the moving area. Thereby, it is possible to search for a movement route reflecting the user's intention.

  In addition, the present invention is also embodied as a method, and the route search method according to the present invention starts moving from a movement start point existing in the movement area and reaches the movement end point existing in the movement area. A path search method for searching for a movement path of a vehicle, wherein a movement prohibited area is set around an obstacle existing in the movement area, and the shape of the outer periphery of the set movement prohibited area is set to move the autonomous mobile body. Based on the moving speed of the autonomous mobile body from a route candidate generated using a figure having a predetermined curvature set based on the speed and connecting a tangent line of the movement prohibited area and the outer periphery of the movement prohibited area When the autonomous mobile body passes on the outer periphery of the movement prohibited area as a result of the route search, the movement route portion excluding the outer periphery of the movement prohibited area is selected from the movement route. Tangent And, a movement path portion on the outer periphery of the movement inhibition region in which the movement path according to the figure.

  Thereby, it is possible to search for a movement route that avoids a collision with an obstacle. Further, since the searched movement path is composed of the tangent line of the movement prohibition area and the outer periphery of the movement prohibition area in accordance with a predetermined curvature, no further smoothing processing is required for the movement path. In other words, since a separate process for correcting the searched movement route to a smooth route is not required, the mobile body can move the searched movement route as it is. Further, since the travel route configured as described above can easily and accurately calculate the route length, it is possible to calculate the arrival time of the autonomous mobile body at each point on the route. Therefore, the moving body can easily search for a moving route that can move while avoiding an obstacle, and can also search for a moving route that can easily calculate the route length and the estimated arrival time to any point on the route. can do.

  The present invention is also embodied as a program, and the route search program according to the present invention starts moving from a movement start point existing in the movement area and reaches an end point of movement existing in the movement area. A path search program for searching for a movement path of a computer, wherein a movement prohibition area is set around an obstacle existing in the movement area for a computer, and an outer periphery shape of the set movement prohibition area is The step of generating using a figure having a predetermined curvature set based on the moving speed of the autonomous moving body, and the route candidate formed by connecting the tangent of the movement prohibited area and the outer periphery of the movement prohibited area, When a route search is performed based on the moving speed of the moving body, and the autonomous moving body passes on the outer periphery of the movement prohibited area as a result of the route search, It said moving and said tangential movement path portion excluding the outer periphery of the prohibited area is a moving path portion on the outer periphery of the movement inhibition area which and a step of the movement path according to the figure.

  As a result, the moving body can easily search for a moving route that can move while avoiding an obstacle, and can easily calculate the route length and the expected arrival time to any point on the route. Can be explored.

  According to the present invention, it is possible to easily search for a moving route in which a moving body can avoid an obstacle, and to easily determine the route length and the estimated arrival time to a point on the route. It is possible to provide a route search device, a route search method, and a route search program that can search for a computable travel route.

Embodiment 1 of the Invention
The route search apparatus according to the first embodiment first sets a movement prohibited area centering on an obstacle present in the movement area. Here, the outer peripheral shape of the set movement prohibited area is generated using a figure having a predetermined curvature set based on the moving speed of the autonomous mobile body. Then, the route search device searches for a route based on the moving speed of the autonomous mobile body from a route candidate formed by connecting the tangent of the movement prohibited region and the outer periphery of the movement prohibited region. As a result of the route search, when the autonomous mobile body passes on the outer periphery of the movement prohibited region, the route search device uses the movement route portion of the movement route excluding the outer periphery of the movement prohibited region as a tangent, Let the movement path part on the outer periphery be a movement path according to the figure. The autonomous mobile body refers to the searched movement route, starts moving from the movement start point existing in the movement area, and reaches the movement end point existing in the movement area.

  Since the travel path searched in this way is composed of the tangent line of the travel prohibition area and the outer periphery of the travel prohibition area according to a predetermined curvature, no further smoothing processing is required for the travel path. In other words, since a separate process for correcting the searched movement route to a smooth route is not required, the mobile body can move the searched movement route as it is. Further, since the travel route configured as described above can easily and accurately calculate the route length, it is possible to calculate the arrival time of the autonomous mobile body at each point on the route. Therefore, the moving body can easily search for a moving route that can move while avoiding an obstacle, and can also search for a moving route that can easily calculate the route length and the estimated arrival time to any point on the route. can do.

  Hereinafter, the route search apparatus according to the first embodiment will be described with reference to the drawings. FIG. 1 shows an autonomous mobile body control system in which a vehicle 10 as an autonomous mobile body moves in a limited area P (area surrounded by a broken line) on a floor 1 as a mobile area by a signal from a control unit 15. 1 schematically illustrates one embodiment of 100. In FIG. 1, objects are not shown on the area P on the floor portion 1, but there are known fixed obstacles and fixed obstacles and moving obstacles detected by an external sensor, and the vehicle 10 detects these obstacles. There is a need to avoid things.

  As shown in FIG. 2, the vehicle 10 is an opposed two-wheel vehicle including a box-shaped vehicle main body 10 a, a pair of opposed wheels 11, and casters 12. The vehicle body 10a is supported horizontally. Further, a drive unit (motor) 13 for driving the wheels 11, a counter 14 for detecting the number of rotations of the wheels, and a control signal for driving the wheels are generated and driven inside the vehicle body 10 a. The control part 15 which transmits the control signal to the part 13 is provided. A control program for controlling the moving speed, moving direction, moving distance, etc. of the vehicle 10 based on the control signal is recorded in a storage area 15a such as a memory as a storage unit provided in the control unit 15. ing. The moving speed, moving distance, etc. described above are obtained based on the number of rotations of the wheel 11 detected by the counter 14.

  In addition, an external sensor 16 for recognizing an obstacle or the like appearing in the moving direction is fixed on the front surface of the vehicle main body 10a, and information such as an image and a video recognized by the external sensor 16 is controlled by the control unit 15. As a result, the moving direction and speed of the vehicle are determined according to the control program. The external sensor 16 can be configured by a sensor that detects a laser reflected by an obstacle or the like, or a CCD camera.

  Furthermore, the upper surface of the vehicle body 10a is provided with an antenna 17 for recognizing its own position. For example, by receiving position information from a GPS or the like (not shown) and analyzing the position information in the control unit 15, It is possible to accurately recognize its own position.

  The vehicle 10 configured in this way independently controls the drive amount of the pair of wheels 11 so that the vehicle travels straight, moves in a curve (turns), moves backward, and rotates on the spot (centering on the midpoint of both wheels). It is possible to perform a movement operation such as turning. Then, the vehicle 10 creates a movement route to the designated destination in the area P in accordance with a command from the control unit 15 that designates a movement location from the outside, and moves so as to follow the movement route. In order to reach the destination.

  In the storage area 15a provided inside the control unit 15, grid lines that connect lattice points arranged at substantially constant intervals d (for example, 10 cm) are virtually depicted in the shape of the entire area P on the floor portion 1. The grid map obtained by this is stored. Obstacle information indicating the presence or absence of an obstacle is set for each grid in advance or in real time. The control unit 15 creates a movement route from the self-position specified on the grid map to the movement start point to the movement end point that is the destination, and moves according to the created movement route.

  FIG. 3 is a block diagram showing the configuration of the route search function that the control unit 15 has. The control unit 15 includes a search space generation unit 20, a route search unit 21, route data 22, a self-position calculation unit 23, and obstacle position data 24.

  The obstacle position data 24 is position information of obstacles existing in the area. The position information of the obstacle may be registered in advance by the user, or may be acquired by the external sensor 16 as described above. For example, the user may register obstacle position information in advance from a drawing of a moving area of the vehicle 10, or the grid map after detecting an obstacle by a distance measuring sensor such as a laser range sensor provided in the vehicle 10. Obstacle position information may be registered in.

  As described above, the self-position calculating unit 23 replaces the position information of the vehicle 10 obtained from the GPS or the like with its own position on the grid map and recognizes it as the self-position on the grid map. On the grid map, the location corresponding to the vehicle's own position, the movement end point that is the destination, and the movement direction of the vehicle 10 at the movement end point are specified.

  The search space generation unit 20 sets a movement prohibited area centering on an obstacle present in the grid map. Here, the search space generation unit 20 generates the shape of the outer periphery of the set movement prohibited area using a figure having a predetermined curvature set based on the moving speed of the vehicle 10.

  The route search unit 21 performs a route search based on the moving speed of the vehicle 10 from a route candidate formed by connecting the tangent line of the movement prohibited region and the outer periphery of the movement prohibited region. Here, when the vehicle 10 passes on the outer periphery of the movement prohibited area as a result of the route search, the route search unit 21 uses the movement route portion excluding the outer periphery of the movement prohibited area as a tangent to move. The moving path portion on the outer periphery of the prohibited area is set as a moving path according to the figure. The route search unit 21 stores the searched travel route as route data 22. The control unit 15 controls the movement of the vehicle 10 according to the searched route data 22.

  Next, the route search function of the control unit 15 will be described with reference to the flowchart shown in FIG. 4 and FIGS. FIG. 4 is a flowchart showing route search processing by the control unit 15.

  First, the control unit 15 creates a grid map as a search space in which the vehicle 10 searches for a route (S101). FIG. 5 shows an example of a grid map on which the vehicle 10 searches for a route. As shown in FIG. 5, in the grid map, an obstacle grid B is arranged on the way from the current position as the movement start point to the goal G as the movement end point. The vehicle 10 searches for a movement route that reaches the goal G from the current position so as to avoid the obstacle grid B existing in the grid map. Note that the search space is not limited to a grid map, but contacts (hereinafter referred to as nodes) on the outer periphery of a movement prohibited area described later, and curves and straight lines (hereinafter referred to as arcs) connecting the contacts. Any search space that can be expressed using

  Next, the control unit 15 sets a movement prohibited area that is an area where the movement of the vehicle 10 is prohibited. Here, a circle is adopted as a figure having a predetermined curvature set based on the moving speed of the vehicle 10, and the control unit 15 generates the shape of the outer periphery of the movement prohibited area using the circle. That is, the control unit 15 creates a route circle centered on each obstacle grid B (S102). In FIG. 6, the example of the path | route circle produced centering on each obstacle grid B is shown.

  The figure that defines the outer periphery of the movement prohibited area is not limited to a circle, and may be a figure that maintains a safe distance so that the vehicle 10 does not collide with the obstacle grid B and has a curvature that the vehicle 10 can follow. That's fine. As the figure to be adopted, for example, as shown in FIG. 7, a circle having a predetermined curvature set based on the moving speed of the vehicle 10 (FIG. 7A), or a predetermined set based on the moving speed of the vehicle 10. It is good also as a figure as shown in the figure (FIG. 7B, FIG. 7C) which consists of a several circle | round | yen which has a curvature, and the common tangent line between these several circles. According to such a figure, the length of the outer periphery of the movement prohibited area can be calculated easily and accurately. Moreover, even if it is a figure other than the figure shown in FIG. 7, it is preferable to employ a figure that can calculate the path length of the finally generated movement route. In the following description, for simplification of description, the figure that defines the outer periphery of the movement prohibited area is assumed to be a circle.

  Further, the control unit 15 excludes a circle that cannot be a movement route from the created route circle, and calculates a range of an arc that does not collide with another obstacle grid B for the remaining circle. That is, when a part of the arc of the path circle overlaps with another path circle, based on the arrangement relationship of these path circles, it is unnecessary among circles that cannot be travel paths and circles that can be travel paths. Excludes the arc part. By excluding route circles that cannot be route candidates and unnecessary arc portions of the route circle, route search can be performed more efficiently. Note that there are clockwise and counterclockwise movement paths that follow the circle. In the following description, even when the centers are the same, the case of turning clockwise and the case of turning counterclockwise are treated as cases of turning different circles.

図８Ａに示す例では、θ_１２は数１に示す条件を満足しているため、円Ｃ_０は経路となりうるものと判定される。一方で、図８Ｂに示す例では、どのθ_ｘｙも数１に示す条件を満足しておらず、円Ｃ_０は経路となりえないものと判定される。従って、制御部１５は、図８Ｂに示す円Ｃ_０を除外する。 With reference to FIG. 8, a method of determining a circle that cannot be a movement route will be described. The determination here is to determine whether or not the circle is buried by surrounding circles. In the example shown in FIG. 8, it is determined whether or not the route circle C ₀ is a circle that can be a moving route. Note that the radius of the circle C _x is r _x and the center of the circle C _x is c _x . A line segment connecting the center _{c x} center _{c 0} and the circle _{C x} of the circle _{C 0} and _c 0 _{c x, c 0} a line segment connecting the center _{c y} of the center _{c 0} and the circle _{C y} of the circle _{C 0} Let it be _cy . An angle formed by the line segment c ₀ c _x and the line segment c ₀ c _y is defined as θ _xy . First, all the circles C _x satisfying the relationship of the inequality | c ₀ c _x | <r ₀ + r _x are listed, and the set is defined as C _U. Then, the _{C x} ∈ C _U a is any of a circle _{C x,} sorted in order of the angle between the line segment _c 0 _{c x} and x-axis, to calculate the angle theta _xy adjacent segment. At this time, if there is at least one θ _xy that satisfies the following formula 1, the circle C ₀ can be a path. If all the radii are equal, Equation 1 is simply replaced by θ _xy > π. When Equation 1 becomes an equation, the circle C ₀ , the circle C _x, and the circle C _y have a common tangent (for example, the tangents L ₀ and L ₁₂ shown in FIG. Overlapping state.)

In the example shown in FIG. 8A, θ ₁₂ satisfies the condition shown in Equation 1, and therefore it is determined that the circle C ₀ can be a route. On the other hand, in the example shown in FIG. 8B, none of the θ _xy satisfies the condition shown in _Equation 1, and it is determined that the circle C ₀ cannot be a route. Accordingly, the control unit 15 excludes the circle _{C 0} shown in FIG. 8B.

  Next, the control unit 15 creates a virtual circle that the vehicle 10 first follows from the current position, speed, and angular velocity of the vehicle 10, and performs the obstacle grid for the created virtual circle in the same manner as described above. The range of the arc that does not collide with B is calculated. The virtual circle creates two on the left and right sides of the course. FIG. 9 shows the path circle and the virtual circles ICR and ICL created as described above. In the example shown in FIG. 9, the range of arcs that do not collide with the obstacle grid B is indicated by bold lines in the path circle.

  The position and size of the virtual circle are determined from the position information and speed information of the vehicle 10. When the vehicle 10 is stopped, for example, as shown in FIG. 10A, virtual circles ICR and ICL are arranged so as to contact the vehicle 10. When the vehicle 10 is moving, for example, as shown in FIG. 10B, the radii of the virtual circles ICR and ICL are increased so that the vehicle 10 can easily follow in accordance with the moving speed of the vehicle 10. To place. The virtual circle may be a curve that can be differentiated along the traveling direction vector of the vehicle 10 and may be created using a clothoid curve, a spline curve, or the like other than the circle.

  Next, the control unit 15 calculates the time until the vehicle 10 reaches the two created virtual circles from the moving speed of the vehicle 10, and sets the arrival point as a node. The control unit 15 sets the two nodes of the virtual circle as the first Open node and adds them to the Open node list (S103). It is determined from the calculation result in S103 whether an Open node exists (S104). If there is no Open node as a result of the determination, the process is terminated as a search failure. On the other hand, if there are one or more Open nodes, the process proceeds to S105.

  Hereinafter, the control unit 15 searches for the node N having the smallest evaluation value of the full search among the Open nodes. The search target is a circle indicating the goal G and the movement prohibited area. Here, as the evaluation value used for the full search, the shorter the arrival time to the node, the smaller the value. As the full search method, known means such as an A * search algorithm or Dijkstra method can be used. In the following description, the circle to which the node N belongs is expressed as a circle C.

  The control unit 15 selects the node N having the lowest evaluation value from the Open node list (S105). For example, as illustrated in FIG. 10A, the control unit 15 selects the node N. Then, the control unit 15 determines whether or not the selected node N is a goal (S106). As a result of the determination, if the node N is a goal, the search is successful and the route search process is terminated. On the other hand, if it is determined that the node N is not a goal, the process proceeds to S107.

  Next, the control unit 15 determines whether or not the circle C to which the node N belongs is the first visited circle (S107). As a result of the determination, if the circle C is visited for the second time or later, the tangent information created at the first visit is used as it is, and the process proceeds to S109.

  On the other hand, when it is determined that the circle C to which the node N belongs is visited for the first time, the control unit 15 draws a tangent line that does not intersect the movement prohibition area for each circle other than the circle C (S108). More specifically, the control unit 15 draws a common tangent line between the circle C and another circle and a tangent line between the circle C and the goal G from the circle C. And the control part 15 excludes the tangent which cross | intersects another movement prohibition area | region among these tangents. That is, the control unit 15 obtains all tangents that do not contact the other obstacle grid B. By removing unnecessary tangents that cannot be route candidates from the processing target, route search can be performed more efficiently.

Here, as a common tangent, we deal with two common tangent inscribed line _{L in} the circumscribed line _{L out} shown in Figure 11. When calculating the arrival time of the vehicle 10, left and right around the way of the connected circle circumscribed line L _out it is assumed to be identical to the circle C, as shown in FIG. 11A, which are connected by internal line L _in The direction of the circle is calculated assuming that it is opposite to the circle C as shown in FIG. 11B. Even if the centers are the same, the two cases of clockwise rotation and counterclockwise rotation are treated as different circles.

  Next, the control unit 15 sets both ends of each tangent drawn in S108 as the departure node / arrival node (S109). Here, a contact point between a tangent drawn from a circle other than the circle C to the circle C and the arc of the circle C is defined as an arrival node, and the circle C on the circle C when the tangent is drawn from the circle C to another circle is illustrated. Define a contact as a departure node.

  Next, the control unit 15 puts an arc between each departure node of the node N and the circle C and between each departure node and each arrival node, and calculates a movement cost necessary for the vehicle 10 to move the arc (S110). . For example, as shown in FIG. 12, the control unit 15 extends the arc 1 from the reaching node of the circle C to the starting node of the circle C, and further extends the arc 2 from the starting node of the circle C to the reaching node of another circle. . The arc cost is a travel time calculated from the arc length (distance) and the travel speed of the vehicle 10.

  Next, the control unit 15 adds the reaching node on the arc of another circle to the Open node list (S111). For example, the control unit 15 newly adds the reaching node illustrated in FIG. 13 to the Open node list. After adding the Open node, the control unit 15 returns to S104 and repeats the above-described processing until the node N with the lowest evaluation value becomes a goal or the search fails. .

  When the route search is successful, the control unit 15 outputs, for example, a movement route R as shown in FIG. 14 as route data. In each part of the movement path R, the movement path part on the outer periphery of the movement prohibited area is configured as an arc, and the movement path part excluding the outer periphery of the movement prohibited area is configured by a tangent line between the movement prohibited areas.

  Thereby, it is possible to search for a movement route that avoids a collision with an obstacle. Further, since the searched movement path is composed of the tangent line of the movement prohibition area and the outer periphery of the movement prohibition area in accordance with a predetermined curvature, no further smoothing processing is required for the movement path. In other words, since there is no need for a separate process for correcting the searched travel route to a smooth route, the vehicle 10 can move the searched travel route as it is. Further, since the travel route configured as described above can easily and accurately calculate the route length, it is possible to calculate the arrival time of the autonomous mobile body at each point on the route. Therefore, the vehicle 10 easily searches for a moving route that can move while avoiding an obstacle, and searches for a moving route that can easily calculate the route length and the estimated arrival time to any point on the route. can do.

Embodiment 2. FIG.
The route search apparatus according to the second embodiment can search for a moving route that moves behind the moving obstacle in the traveling direction even when a moving obstacle exists in the moving region. In the route search device according to the first embodiment described above, the movement prediction of the moving obstacle is not taken into consideration. For this reason, when a moving obstacle exists in the moving area, a route that covers the front of the moving obstacle in the traveling direction may be searched. When the moving obstacle is a human being, for example, when a large autonomous moving body (robot) crosses the front of the human at high speed, the human being crossed feels fear. For this reason, it is preferable that the movement path | route which avoids a movement obstacle is a movement path | route which turns around to the advancing direction of a movement obstacle.

  Hereinafter, the route search apparatus according to the second embodiment will be described with reference to the drawings. The configuration and functions of the vehicle 10 according to the second embodiment are the same as those of the vehicle 10 shown in the first embodiment except for the points specifically described below, and the same configurations and functions are described in detail. The detailed explanation is omitted.

  FIG. 15 is a block diagram illustrating a configuration of a route search function included in the control unit 15 of the vehicle 10 according to the second embodiment. The control unit 15 includes a search space generation unit 30, a route search unit 31, route data 32, a self-position calculation unit 33, and a distance data acquisition unit 35. The self-position calculation unit 33 has the same configuration and function as the self-position calculation unit 23 in the above-described first embodiment, and thus the description thereof is omitted here.

  The distance data acquisition unit 35 detects obstacle distance data from a distance measuring sensor such as a laser range sensor provided in the vehicle 10 and detects an obstacle in the moving region. The distance data acquisition unit 35 registers the detected obstacle position information in the grid map. Note that the moving obstacle moving in the moving region is approximated by, for example, a cylindrical shape, and information is held separately from the grid space. Further, it is assumed that the movement of the moving obstacle is a constant velocity linear movement. Further, the change in the moving direction of the moving obstacle is dealt with by repeating the route search in a short cycle.

  The search space generation unit 30 sets a movement prohibition area centering on an obstacle present in the grid map. Here, the search space generation unit 30 generates the shape of the outer periphery of the set movement prohibited area using a figure having a predetermined curvature set based on the moving speed of the vehicle 10. In addition, the search space generation unit 30 sets a movement prohibited area for a moving obstacle moving in the movement area. The details of the movement prohibited area set for the moving obstacle will be described later.

  The route search unit 31 performs a route search based on the moving speed of the vehicle 10 from the route candidates formed by connecting the tangent line of the movement prohibited region and the outer periphery of the movement prohibited region. Here, the route search unit 31 calculates a future position of the moving obstacle, and searches for a moving route that avoids the moving obstacle based on the calculated future position of the moving obstacle. The route search unit 31 stores the searched travel route as route data 32. The control unit 15 controls the movement of the vehicle 10 according to the searched route data 32.

  Next, the route search function of the control unit 15 according to the second embodiment will be described with reference to the flowchart shown in FIG. 16 and FIGS. FIG. 6 is a flowchart showing route search processing by the control unit 15 according to the second embodiment.

  First, the control unit 15 creates a grid map as a search space in which the vehicle 10 searches for a route (S201).

  Next, the control unit 15 estimates the current position and moving speed of the moving obstacle (S202). The current position of the moving obstacle may be estimated based on the detection value of the distance measuring sensor 34, and is not limited to the sensor mounted on the vehicle 10, but is set in an environment in which the vehicle 10 moves. You may estimate using a camera etc. In addition, the moving direction and moving speed of the moving obstacle can be estimated using a statistical method such as a particle filter.

  Next, the control unit 15 creates a route circle centered on each obstacle grid B (S203). Further, the control unit 15 excludes a circle that cannot be a movement route from the created route circle, and calculates a range of an arc that does not collide with another obstacle grid B for the remaining circle.

  Next, the control unit 15 creates a virtual circle that the vehicle 10 first follows from the current position, speed, and angular velocity of the vehicle 10, and performs the obstacle grid for the created virtual circle in the same manner as described above. The range of the arc that does not collide with B is calculated. Moreover, the control part 15 calculates the time until the vehicle 10 reaches | attains two created virtual circles from the moving speed of the vehicle 10, and sets the arrival point as a node. Then, the control unit 15 sets the two nodes of the virtual circle as the first Open node and adds them to the Open node list (S204). The control unit 15 determines whether or not an Open node exists (S205). If there is no Open node as a result of the determination, the process is terminated as a search failure. On the other hand, if there are one or more Open nodes, the process proceeds to S206.

  Hereinafter, the control unit 15 searches for the node N having the smallest evaluation value of the full search among the Open nodes. The search target is a circle indicating the goal G and the movement prohibited area. Here, as the evaluation value of the full search, the shorter the arrival time to the node, the smaller the value. As the full search method, known means such as an A * search algorithm or Dijkstra method can be used. In the following description, the circle to which the node N belongs is expressed as a circle C.

  The control unit 15 selects the node N having the lowest evaluation value from the Open node list (S206). Then, the control unit 15 determines whether or not the selected node N is a goal (S207). As a result of the determination, if the node N is a goal, the search is successful and the route search process is terminated. On the other hand, if it is determined that the node N is not a goal, the process proceeds to S208.

  Next, the control unit 15 determines whether or not the circle C to which the node N belongs is the first visited circle (S208). As a result of the determination, if the circle C is visited for the second time or later, the tangent information created at the first visit is used as it is, and the process proceeds to S210.

  On the other hand, if the result of determination is that the circle C to which the node N belongs is visited for the first time, the control unit 15 draws tangents that do not collide with stationary obstacles from the circle C and creates these tangent sets L (S209). .

  Next, the control unit 15 selects a tangent s included in the tangent set L (S210). Hereinafter, the processing of S210 to S218 is repeated for all tangents s included in the tangent set L.

  Next, the control unit 15 calculates the estimated arrival time ts of the selected tangent s to the departure node (S211). That is, the estimated arrival time ts until the vehicle 10 reaches the departure node on the tangent line s is calculated. In other words, the estimated arrival time ts at which the vehicle 10 reaches the departure point (xr, yr) of the tangent s at the current time is calculated.

Next, the control unit 15 creates a relative space for each tangent s and each moving obstacle centered on the predicted position of the vehicle at time ts (S212). More specifically, the control unit 15 calculates the position of the moving obstacle at the time ts, and creates a relative space indicating the relative positional relationship between the vehicle and the moving obstacle at the time ts. For example, as shown in FIG. 17A, a case is assumed in which a moving obstacle is moving in the direction of a white arrow with respect to a tangent L ₁ that is a part of the moving path of the vehicle 10. At this time, the control unit 15, as shown in FIG. 17B, and calculates the time t1 when the vehicle 10 reaches the starting point of the tangential line L _1, to calculate the position of the moving obstacle at the time t1. In FIG. 17B, the vehicle 10 and the moving obstacle at time t1 are indicated by broken lines.

  Next, the control unit 15 refers to the created relative space, and performs a collision determination process regarding whether or not the vehicle 10 and each moving obstacle collide with respect to the tangent line s (S213).

  FIG. 18 is a diagram illustrating an example of the relative space created by the control unit 15. The control unit 15 sets a movement prohibited area around the moving obstacle. Here, a circle is set as the movement prohibited area, and a circle having a radius rm from the center is set for the moving obstacle. Let (vxm, vym) be the moving speed vector of the moving obstacle. Further, the radius of the vehicle 10 is rr, and the moving speed vector of the vehicle 10 is (vxr, vyr). The control unit 15 determines that the distance between the extension line of the relative velocity vector (vxm-vxr, vym-byr) between the vehicle 10 and the moving obstacle and the relative center position (xm-xr, ym-yr) of the moving obstacle. It is determined whether or not rm + rr or less. When the distance between the extension line and the relative center position of the moving obstacle is equal to or less than rm + rr, the control unit 15 determines that the vehicle 10 and the moving obstacle may collide. In the example illustrated in FIG. 18, the control unit 15 determines that the vehicle 10 and the moving obstacle may collide.

  From the collision determination processing result in S213, it is determined whether or not there is a possibility that the vehicle 10 and each moving obstacle collide with respect to the tangent line s (S213). As a result of the determination, if there is no possibility of colliding with a moving obstacle, the process proceeds to S216.

  On the other hand, when there is a possibility of colliding with the moving obstacle, the control unit 15 determines whether or not a tangent to the avoiding circle can be drawn to the moving obstacle that may collide (S215).

As a result of the determination, if the tangent to the avoidance circle can be drawn, the control unit 15 draws the tangent to the avoidance circle to the moving obstacle, registers both ends of the tangent in the departure / arrival nodes, and s is removed from the tangent set L (S216). That is, the control unit 15 draws a tangent line to the avoidance circle including the moving obstacle at time ts. At this time, the tangent is drawn so that the inner product of the moving direction vector of the moving obstacle and the drawn tangent vector becomes negative (that is, the moving path passes behind the moving obstacle). For example, as shown in FIG. 17B, the control unit 15 draws a tangent L ₂ with respect to avoidance circle moving obstacle. Then, the control unit 15 sets both ends of each drawn tangent as a departure node / arrival node.

  On the other hand, if the result of determination is that a tangent to the avoidance circle cannot be drawn, the control unit 15 removes the tangent s from the tangent set L (S217). Then, the control unit 15 determines whether or not the set L is empty (S218). If the tangent set L is not empty, the process returns to S210 and the processes up to S218 are repeated. On the other hand, if the tangent set is empty, the process proceeds to S219.

  Next, the control unit 15 establishes an arc between each departure node of the node N and the circle C, between each departure node and each arrival node, and calculates a movement cost necessary for the vehicle 10 to move the arc (S219). . Furthermore, the control unit 15 adds a reaching node on the arc of each arrival other circle to the Open node list (S220). After adding the Open node, the control unit 15 returns to S205, and repeats the above-described processing until the node N with the lowest evaluation value becomes a goal or the search fails. .

  Hereinafter, the processing of S209 to S218 described above will be specifically described with reference to FIGS. First, in the example illustrated in FIG. 19, it is assumed that the circle C is determined to be the first visited circle in S208. Next, in S209, the control unit 15 changes from the circle C to a stationary obstacle (in FIG. 19, a circle other than the circle C, and a stationary obstacle is indicated by another circle excluding the moving obstacles M1 and M2). A non-collision tangent set L is created. For example, as shown in FIG. 20, as a result of the process in S209, the control unit 15 creates tangents s1 to s5, and creates a tangent set L including these tangents.

  Next, the control unit 15 performs the processes shown in S210 to S218 for each tangent. That is, the control unit 15 performs the processing shown in S211 to S213 described above for the selected tangent s. For example, when the tangent set L includes tangents s1 to s5 illustrated in FIG. 21, the control unit 15 performs a collision determination with the moving obstacle on each of the tangents s1 to s5. As a result of the collision determination for each tangent line, in the example shown in FIG. 21, the control unit 15 determines that the tangent line s1 collides with the moving obstacle M1 and the tangent line s4 collides with the moving obstacle M2 (that is, tangent line). For s1 and tangent s4, the result of determination in S214 is Yes). On the other hand, in the example illustrated in FIG. 21, the control unit 15 determines that the tangent lines s1, s2, and s5 do not collide with any moving obstacle (that is, for the tangent lines s1, s2, and s5, the determination in S214). Result is No). Then, for the tangent lines s1, s2, and s5, the control unit 15 registers both ends of the tangent line as the departure / arrival nodes, and then removes these tangent lines s1, s2, and s5 from the tangent set L.

  On the other hand, the control unit 15 tries to generate an avoidance trajectory for avoiding the collision with the moving obstacle because the tangent lines s3 and s4 may collide with the moving obstacle. First, the control unit 15 generates an avoidance trajectory for the moving obstacle M1 because the tangent s3 may collide with the moving obstacle M1. That is, it is determined whether or not a tangent to the avoiding circle C_M1 can be drawn to the colliding moving obstacle M1. In the example shown in FIG. 22, since the tangent s up to the avoidance circle C_M1 collides with another stationary obstacle, the control unit 15 abandons the generation of the avoidance trajectory using the tangent s (that is, the determination in S215). Result is No). Then, the control unit 15 removes the tangent s3 from the tangent set L.

  Further, the control unit 15 generates an avoidance trajectory for the moving obstacle M2 because the tangent s4 may collide with the moving obstacle M2. That is, it is determined whether or not a tangent to the avoiding circle C_M2 can be drawn to the colliding moving obstacle M2. In the example shown in FIG. 23, since the tangent s6 up to the avoidance circle C_M1 does not collide with other stationary obstacles, the control unit 15 generates an avoidance trajectory using the tangent s6 (that is, the result of determination in S215). Becomes Yes). Accordingly, after registering both ends of the tangent s6 as the departure / arrival nodes, the tangent s6 is removed from the tangent set L.

  As a result of repeating the processing for each tangent included in the tangent set L in this way, the tangents in which the departure / arrival nodes are finally registered are, for example, four tangents s1, s2, s5 and s6.

  When the route search is performed as described above and the route search is successful, the control unit 15 outputs, for example, an avoidance route R as indicated by a solid line in FIG. 17B as route data. In each part of the avoidance path R, the movement path part on the circle of the moving obstacle is configured as an arc, and the movement path part excluding the circle of the moving obstacle is configured by a tangent line of the moving obstacle circle. Furthermore, the avoidance route R passes rearward with respect to the moving direction of the moving obstacle. For this reason, the vehicle 10 that follows the avoidance route R moves more safely in order to avoid moving ahead of the moving obstacle in the moving direction.

Note that the control unit 15 sets a movement route so as to avoid the obstacle around the moving obstacle. There are four types of avoidance routes as shown in FIG. A route is assumed. FIG 25A, after the vehicle 10 has left-handed front of the circle C, indicating the avoidance path in the case of avoidance of moving obstacle and right (. Including tangent L _3). FIG 25B, after the vehicle 10 has left-handed front of the circle C, indicating the avoidance path when the avoidance of moving obstacle was left (. Including tangent L _4). FIG 25C, after the vehicle 10 has clockwise in front of the circle C, indicating the avoidance path in the case of avoidance of moving obstacle and right (. Including tangent L _5). FIG 25D, after the vehicle 10 has clockwise in front of the circle C, indicating the avoidance path when the avoidance of moving obstacle was left (. Including tangent L _6).

Embodiment 3 FIG.
The route search device according to the third embodiment searches for a movement route that avoids a collision with a movement obstacle without changing the movement route itself even when a movement obstacle exists in the movement region. Can do. In the route search device according to the above-described second embodiment, the vehicle 10 newly searches for a moving route along which the vehicle 10 moves behind the moving obstacle. The configuration and functions of the vehicle 10 according to the third embodiment are the same as those of the vehicle 10 shown in the second embodiment except for points specifically described below, and the same configurations and functions are described in detail. The detailed explanation is omitted.

The control unit 15 according to the third embodiment calculates the estimated arrival time t1 at which the vehicle 10 will reach the starting point of the route circle C at the current time in the same manner as described in the second embodiment. To do. Next, the control unit 15 calculates the position of the moving obstacle at time t1. For example, as shown in FIG. 26A, a case is assumed in which a moving obstacle is moving in the direction of a white arrow with respect to a tangent line L ₁ that is a part of the moving path of the vehicle 10. At this time, as shown in FIG. 26B, the control unit 15 calculates time t1 when the vehicle 10 reaches the starting point of the circle C, and calculates the position of the moving obstacle at time t1. In FIG. 26B, the vehicle 10 and the moving obstacle at time t1 are indicated by broken lines.

Next, the control unit 15 determines whether or not the vehicle 10 and the moving obstacle may collide at the time t1. As a result of the determination, if there is a possibility of a collision, the control unit 15 sets a deceleration node between the circle of the moving obstacle at the time t1 and the position of the vehicle 10 on the tangent that is the route candidate. . The deceleration node is a node that instructs to decelerate the moving speed of the vehicle 10 to a predetermined speed. The predetermined speed of the deceleration node is obtained as a speed at which the vehicle 10 can move the path candidate after the moving obstacle circle passes the tangent line of the current path candidate. The control unit 15 performs a subsequent route search using the set deceleration node. For example as shown in FIG. 26B, the control unit 15 sets the deceleration node _{N 8} on the tangent _{L 8.} Vehicle 10 to decelerate when passing through the deceleration node N ₈ is set, it is possible to avoid a collision with the moving obstacle. Thereby, it is not necessary to search for a new movement route according to the movement of the moving obstacle, and it is possible to avoid the moving obstacle only by decelerating the vehicle 10.

  In addition, it is good also as what avoids the collision with a moving obstacle by combining Embodiment 2 mentioned above and this Embodiment 3. FIG. That is, as an action to shorten the time to reach the goal G, either an action to newly search for an avoidance route that avoids the rear of the moving obstacle, or an action to decelerate in front of the moving obstacle The action may be selected according to the situation.

Embodiment 4 FIG.
The route search device according to the fourth embodiment can search for a moving route that the vehicle 10 can follow at the contact point between the tangent to the route circle and another route circle. In the route search device according to each of the above-described embodiments, when the followability of the angular velocity of the vehicle 10 is low, it may be difficult for the vehicle 10 to follow the searched travel route. This is because the movement path is composed of only a circle and a tangent, and a discontinuity occurs in the radius of curvature at the contact point of the tangent. The configuration and function of the vehicle 10 according to the fourth embodiment are the same as those of the vehicle 10 described in each of the above-described embodiments except for the points specifically described below. Detailed description thereof is omitted.

  The control unit 15 according to the fourth embodiment connects the connection between the tangent line of the route circle and the contact point of the other route circle using a clothoid curve set based on the moving speed of the vehicle 10. As a result, the vehicle 10 can move while maintaining the followability of the movement route even at the contact point of the route circle and the contact point of the other route circle.

  FIG. 27 is a flowchart showing route search processing by the control unit 15 according to the fourth embodiment. The example shown in FIG. 27 is different from the route search process by the control unit 15 according to the first embodiment shown in FIG. 4 in that the processing target in S108 and S109 is a clothoid curve. Other processes are the same as those of the above-described embodiments, and detailed description thereof is omitted.

  For example, as shown in FIGS. 28A and 28B, the control unit node 15 connects the tangent line of the route circle and another route circle with a clothoid curve. In the clothoid curve, the control unit 15 performs a full search using a node at which the curve and the straight line are switched as a node. By using a clothoid curve instead of a tangent line, it is possible to search for a movement path that can be followed even by a moving body having a small angular velocity or a moving body having a low followability, such as an automobile. Moreover, since the length can be calculated from the equation representing the clothoid curve, the control unit 15 can calculate the length of the searched movement route. Furthermore, as a method of setting the curvature of the clothoid curve, for example, the curvature of the clothoid curve is increased when the moving speed of the vehicle 10 is fast, and the curvature of the clothoid curve is decreased when the moving speed of the vehicle 10 is slow. May be set.

Embodiment 5 FIG.
The route search device according to the fifth embodiment can achieve both a search for a travel route that places importance on the followability of the vehicle 10 and a search for a travel route that places importance on safe avoidance with an obstacle. In the route search device according to each of the above-described embodiments, two types of properties of the vehicle 10 such as followability and safety are expressed using one kind of circle. For this reason, when the obstacle circle is created with emphasis on the path following property of the vehicle 10 (minimum turning radius of the vehicle 10), the radius of the circle created according to the turning radius of the vehicle 10 becomes small. May collide with obstacles. On the other hand, when an obstacle circle is created only for the purpose of not colliding with an obstacle, the radius of the circle created according to the size of the obstacle is small, and the vehicle 10 may not be able to follow. is there. The configuration and function of the vehicle 10 according to the fifth embodiment are the same as those of the vehicle 10 described in each of the above-described embodiments except for the points specifically described below. Detailed description thereof is omitted.

  The control unit 15 according to the fifth embodiment has a circle having a size set based on the moving speed of the vehicle 10 and a circle having a size set by adding a predetermined safety distance to the size of the vehicle 10. Then, a circle with a larger value is selected, and the selected circle is created as a route circle. As a result, it is possible to search for a movement route while achieving both the followability of the movement route based on the moving speed of the vehicle 10 and the safety of the movement route that places importance on collision avoidance.

  For example, as shown in FIG. 29, the control unit 15 selects a larger circle from the two circles. In the figure, R1 is the limit radius of tracking of the vehicle 10 (that is, a circle that places importance on followability), and R2 is (the radius of the vehicle 10 + a predetermined safety distance) (that is, a circle that places importance on safety). Is shown.) The control unit 15 calculates the values of R1 and R2, selects a circle with a larger value, and performs a route search. For example, in the route search process by the control unit 15 according to the first embodiment shown in FIG. 4, prior to the route circle creation process in S102, a selection is made from these two circles. In FIG. 29A, a circle with a radius of R1 (a circle that emphasizes followability) may contact an obstacle even if followability is possible. Therefore, the control unit 15 selects a circle having a radius R2 (a safety-oriented circle). In FIG. 29B, a circle with a radius of R2 (a safety-oriented circle) does not collide with an obstacle, but the vehicle 10 may not be able to follow. Therefore, the control unit 15 selects a circle having a radius R1 (a circle that emphasizes followability).

Embodiment 6 FIG.
The route search apparatus according to the sixth embodiment can realize a search for a moving route even in a vast search space. In the route search device according to each of the embodiments described above, the amount of calculation increases in proportion to the square of the number of obstacle grids due to the nature of the search algorithm. For this reason, in the case of a wide search space, it may be difficult to realize the travel route search process. The configuration and function of the vehicle 10 according to the sixth embodiment are the same as those of the vehicle 10 described in each of the above-described embodiments except for the points specifically described below. Detailed description thereof is omitted.

  The control unit 15 according to the sixth embodiment uses a data structure representation represented by a quad-tree to decompose the search space to such an extent that the amount of calculation does not increase. And the control part 15 performs a route search for the area | region of each division | segmentation decomposed | disassembled. Thereby, calculation time can be reduced by limiting the movement area | region used as the object of a route search according to the movement of the vehicle 10. FIG. Therefore, the route search can be realized even in a vast search space. The quadtree data structure means that the entire plane is divided into four halves vertically and horizontally, and if the divided areas are all occupied by figures, or if no figures are included at all, the division is stopped. In other cases, each area is further divided into four to store data. This is repeated and the two-dimensional figure is segmented as shown in FIG. 30B, for example. Thereby, the search space can be expressed using the tree structure of the quadtree.

  More specifically, first, the control unit 15 calculates the number of obstacle grids in the area where the vehicle 10 is currently located, and determines whether or not the calculated number of obstacle grids is equal to or greater than a predetermined number. To do. As a result of the determination, when the number of obstacle grids is equal to or greater than the predetermined number, the control unit 15 divides the moving region where the current vehicle 10 is located into four. The control unit 15 repeats this division process until the number of obstacle grids is less than a predetermined number. Then, the control unit 15 sets a subgoal in the area where the division is completed (area where the number of obstacles is smaller than a predetermined number), and performs a route search from the current position to the subgoal as described above. When the subgoal is reached in the divided area, the control unit 15 calculates the number of obstacle grids in the adjacent area, and determines whether or not the calculated number of obstacle grids is a predetermined number or more. . Thereafter, the movement route to the goal is searched by repeating the above-described processing.

  For example, as shown in FIG. 30A, it is assumed that the vehicle 10 moves in the region P. As illustrated in FIG. 24B, the control unit 15 represents the grid map MG corresponding to the region P with a quadtree structure. Here, an enlarged view of a section where the vehicle 10 is currently located is shown as a grid map ML. The number of obstacle grids included in the grid map ML is less than a predetermined number. The control unit 15 searches for a local movement route R from the current position to the subgoal in the grid map ML.

Other embodiments.
In each of the above-described embodiments, the route search process in the two-dimensional plane has been described, but the present invention is not limited to this. That is, the moving area in which the moving body moves may be a three-dimensional area. In this case, the route search device generates a shape of the outer periphery of the movement prohibited area centered on the three-dimensional shape obstacle using a three-dimensional shape figure having a predetermined curvature. For example, as illustrated in FIG. 31, the route search device generates spheres S1, S2, and S3 with an obstacle as a center. Then, the route search device searches for a moving route along the spheres S1 and S3 centered on the obstacle. Thus, the present invention is not limited to the two-dimensional space, and can be applied to trajectory calculation of a manipulator, for example, by performing a route search in the three-dimensional space.

  In addition, the route search device may set the size of the figure according to the arrangement position of the obstacle in the moving area. Thereby, it is possible to search for a movement route reflecting the user's intention. For example, as shown in FIG. 32, for the obstacle arranged at the center of the room, the center of the room is not moved by the vehicle 10 by setting the size of the route circle to be larger than that of other obstacles. Such a movement route can be intentionally searched for by the route search device. In addition, as shown in FIG. 32, by setting the size of the path circle for the obstacle indicating the wall to be smaller than the path circle of the other obstacles, a movement path that causes the vehicle 10 to move along the wall is provided. You can search intentionally.

  In the above-described embodiment, the control unit of the autonomous mobile body has been described as having a route search function, but the present invention is not limited to this. The route search function is realized by, for example, a computer mounted on the autonomous mobile body or a computer provided separately from the autonomous mobile body. The computer includes, for example, a central processing unit (CPU), an auxiliary storage device such as a ROM, a RAM, and a hard disk, a storage medium driving device into which a portable storage medium such as a CD-ROM is inserted, an input unit, and an output unit. Yes. In a storage medium such as a ROM, an auxiliary storage device, or a portable storage medium, an application program can be recorded by giving instructions to the CPU in cooperation with the operating system, and executed by being loaded into the RAM. . This application program includes a unique route search program for realizing the route search function according to the present invention. The route search by the route search program is executed by the central processing unit developing the application program on the RAM, reading the data stored in the auxiliary storage device and executing the processing according to the application program, and storing it. The

  It should be noted that the present invention is not limited to the above-described embodiment, and various modifications can be made without departing from the gist of the present invention already described.

1 is an overall view schematically showing an autonomous mobile control system according to Embodiment 1 of the present invention. It is the schematic which shows the vehicle as an autonomous mobile body which concerns on Embodiment 1 of this invention. It is a figure which shows the structure of the control part which concerns on Embodiment 1 of this invention. It is a flowchart which shows the route search process which concerns on Embodiment 1 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 1 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 1 of this invention. It is a figure which illustrates the figure of the movement prohibition area | region which concerns on Embodiment 1 of this invention. It is a figure explaining a mode that an unnecessary course circle concerning Embodiment 1 of the present invention is judged. It is a figure explaining the mode of the route search process which concerns on Embodiment 1 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 1 of this invention. It is a figure which illustrates the kind of tangent of the path | route circle which concerns on Embodiment 1 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 1 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 1 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 1 of this invention. It is a figure which shows the structure of the control part which concerns on Embodiment 2 of this invention. It is a flowchart which shows the route search process which concerns on Embodiment 2 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 2 of this invention. It is a conceptual diagram which illustrates the relative space which concerns on Embodiment 2 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 2 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 2 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 2 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 2 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 2 of this invention. It is a figure explaining the mode of the route search process which concerns on Embodiment 2 of this invention. It is a figure which illustrates the kind of moving obstacle avoidance method concerning Embodiment 2 of the present invention. It is a flowchart which shows the route search process which concerns on Embodiment 3 of this invention. It is a flowchart which shows the route search process which concerns on Embodiment 4 of this invention. It is a figure which illustrates the clothoid curve which concerns on Embodiment 4 of this invention. It is a figure explaining the selection process of the path | pass circle which concerns on Embodiment 5 of this invention. It is a conceptual diagram explaining the quadtree data structure which concerns on Embodiment 6 of this invention. It is a figure explaining the mode of the route search in the three-dimensional space which concerns on other embodiment of this invention. It is a figure explaining the mode of the setting of the magnitude | size of the path | route circle which concerns on other embodiment of this invention.

Explanation of symbols

1 floor, 10 vehicle, 10a vehicle body, 11 wheels, 12 casters,
13 drive unit (motor), 14 counter, 15 control unit, 15a storage area,
16 external sensor, 17 antenna, 100 autonomous mobile control system,
21, 31 Route search unit, 22, 32 Route data, 23, 33 Self-position calculation unit,
24 obstacle position data, 34 distance measuring sensor, 35 distance data acquisition unit,
P area, G goal, B obstacle grid,
C path circle, ICR, ICL virtual circle, C_M1, C_M2 avoidance circle, N node,
L, s tangent, R path,
MG, ML Grid map

Claims (27)

A path search device that starts a movement from a movement start point existing in a movement area and searches for a movement path of an autonomous mobile body that reaches a movement end point existing in the movement area,
A figure having a predetermined curvature in which a movement prohibition area is set around an obstacle existing in the movement area, and the shape of the outer periphery of the set movement prohibition area is set based on the movement speed of the autonomous mobile body. A search space generation unit to be generated using,
From a route candidate formed by connecting the tangent of the movement prohibited area and the outer periphery of the movement prohibited area, a route search is performed based on the movement speed of the autonomous mobile body, and as a result of the route search, the autonomous mobile body is When passing on the outer periphery of the movement prohibited area, the movement path portion of the movement path excluding the outer periphery of the movement prohibited area is defined as the tangent line, and the movement path portion on the outer periphery of the movement prohibited area follows the figure. A route search device having a route search unit as a movement route. The figure is
A circle having a predetermined curvature set based on the moving speed of the autonomous mobile body, or a plurality of circles having a predetermined curvature set based on the moving speed of the autonomous mobile body and the plurality of circles The route search device according to claim 1, wherein the route search device is a figure composed of a common tangent line. The route search unit
The length of the common tangent line between the movement start point and the movement prohibited area, the length of the outer circumference between the contact points of the tangent at the outer circumference of the movement prohibited area, the length of the common tangent line between the movement prohibited areas, and the movement prohibited area And a moving time of the route candidate by the autonomous moving body from the length of a common tangent line between the moving end point and the moving speed of the autonomous moving body, and the route candidate that minimizes the calculated moving time is calculated. The route search apparatus according to claim 1 or 2, wherein route search is performed as a movement route. The search space generation unit
When the outer peripheral part of the generated movement prohibited area overlaps with the outer peripheral part of the other movement prohibited area, the outer peripheral part of the generated movement prohibited area is based on the arrangement relationship between the movement prohibited areas. It is determined whether or not a candidate is configured, and if it is determined that the outer peripheral portion of the generated movement prohibited area does not constitute the route candidate, the route candidate is determined from the outer peripheral portion of the generated movement prohibited area. The route search device according to any one of claims 1 to 3, wherein an outer peripheral portion of a movement prohibited area determined not to be configured is excluded. The route search unit
5. The route search device according to claim 1, wherein, among the tangent lines of the movement prohibited area, a tangent line overlapping with another movement prohibited area is excluded from the route candidates. The route search unit
A future position of the moving obstacle is calculated, a movement prohibited area is set based on the calculated future position of the moving obstacle, and when the set movement prohibited area overlaps the route candidate, The route search is further performed using the route candidate formed by connecting the tangent of the movement prohibited area and the outer periphery of the movement prohibited area of the moving obstacle so as to pass on the outer periphery of the movement prohibited area of the moving obstacle. The route search device according to claim 1. The route search unit
The route search is performed so as to pass on a rear outer periphery with respect to a moving direction of the moving obstacle when passing on the outer periphery of the movement prohibited area of the moving obstacle. Route search device. The route search unit
When a future position of the moving obstacle is calculated, a movement prohibited area is set based on the calculated future position of the moving obstacle, and when the set movement prohibited area overlaps with the route candidate, A deceleration node for instructing to decelerate the moving speed of the autonomous moving body to a predetermined speed is set between the movement prohibited area of the moving obstacle and the autonomous moving body, and a route search is performed using the set deceleration node. The route search device according to claim 1, wherein: The route search device according to claim 1, wherein a connection between the tangent of the movement prohibited area and the movement prohibited area is connected by a clothoid curve set based on a moving speed of the autonomous mobile body. The search space generation unit
From a figure of a size set based on the moving speed of the autonomous mobile body and a figure of a size set by adding a predetermined safety distance to the size of the autonomous mobile body itself, The route search device according to claim 1, wherein a figure is selected, and the selected figure is used as the figure having the predetermined curvature. The search space generation unit
When the range of the obstacle occupying the moving area is equal to or greater than a predetermined range, the moving area is divided into four parts, and the obstacle occupying the moving area after the quartering process is performed by repeating the four-dividing process. The route search device according to claim 1, wherein a moving area in which an object range is smaller than the predetermined range is calculated, and the calculated moving area is used as a search space. The moving area is a three-dimensional area;
The search space generation unit
The route search device according to claim 1, wherein the shape of the outer periphery of the movement prohibited area centered on the three-dimensional obstacle is generated using a three-dimensional figure having a predetermined curvature. The route search device according to claim 1, wherein the size of the figure is set in accordance with an arrangement position of the obstacle in the movement area. A path search method for starting a movement from a movement start point existing in a movement area and searching for a movement path of an autonomous mobile body reaching a movement end point existing in the movement area,
A figure having a predetermined curvature in which a movement prohibition area is set around an obstacle existing in the movement area, and the shape of the outer periphery of the set movement prohibition area is set based on the movement speed of the autonomous mobile body. Generated using
From a route candidate formed by connecting the tangent of the movement prohibited area and the outer periphery of the movement prohibited area, a route search is performed based on the movement speed of the autonomous mobile body, and as a result of the route search, the autonomous mobile body is When passing on the outer periphery of the movement prohibited area, the movement path portion of the movement path excluding the outer periphery of the movement prohibited area is defined as the tangent line, and the movement path portion on the outer periphery of the movement prohibited area follows the figure. A route search method for a moving route. The figure is
A circle having a predetermined curvature set based on the moving speed of the autonomous mobile body, or a plurality of circles having a predetermined curvature set based on the moving speed of the autonomous mobile body and the plurality of circles The route search method according to claim 14, wherein the route search method is a figure including a common tangent line. When performing the route search, the length of the common tangent line between the movement start point and the movement prohibited area, the length of the outer circumference between the contact points of the tangent at the outer circumference of the movement prohibited area, and the common tangent line between the movement prohibited areas , The movement time of the route candidate by the autonomous mobile body is calculated from the length of the common tangent line between the movement prohibited area and the movement end point, and the movement speed of the autonomous mobile body, and the calculated travel time The route search method according to claim 14 or 15, wherein a route search is performed by using a route candidate having a minimum value as the travel route. When generating the outer periphery of the movement prohibited area when the outer peripheral part of the generated movement prohibited area overlaps with the outer peripheral part of the other movement prohibited area, based on the arrangement relationship between these movement prohibited areas, It is determined whether or not an outer peripheral part of the generated movement prohibited area constitutes the route candidate. When it is determined that an outer peripheral part of the generated movement prohibited area does not constitute the route candidate, the generated The route search method according to any one of claims 14 to 16, wherein an outer peripheral portion of the movement prohibited region that is determined not to constitute the route candidate is excluded from an outer peripheral portion of the movement prohibited region. When performing the route search,
18. The route search method according to claim 14, wherein, among the tangent lines of the movement prohibited area, a tangent line overlapping with another movement prohibited area is excluded from the route candidates. When performing the route search,
A future position of the moving obstacle is calculated, a movement prohibited area is set based on the calculated future position of the moving obstacle, and when the set movement prohibited area overlaps the route candidate, The route search is further performed using the route candidate formed by connecting the tangent of the movement prohibited area and the outer periphery of the movement prohibited area of the moving obstacle so as to pass on the outer periphery of the movement prohibited area of the moving obstacle. The route search method according to claim 14. When performing the route search,
The route search is performed so as to pass on a rear outer periphery with respect to a moving direction of the moving obstacle when passing on the outer periphery of the movement prohibited area of the moving obstacle. Route search method. When performing the route search,
When a future position of the moving obstacle is calculated, a movement prohibited area is set based on the calculated future position of the moving obstacle, and when the set movement prohibited area overlaps with the route candidate, A deceleration node for instructing to decelerate the moving speed of the autonomous moving body to a predetermined speed is set between the movement prohibited area of the moving obstacle and the autonomous moving body, and a route search is performed using the set deceleration node. The route search method according to claim 14, wherein: The route search method according to claim 14, wherein the tangent of the movement prohibited area and the movement prohibited area are connected by a clothoid curve set based on a moving speed of the autonomous mobile body. When generating the outer periphery of the movement prohibited area,
From a figure of a size set based on the moving speed of the autonomous mobile body and a figure of a size set by adding a predetermined safety distance to the size of the autonomous mobile body itself, The route search method according to claim 14, wherein a figure is selected, and the selected figure is used as the figure having the predetermined curvature. When generating the outer periphery of the movement prohibited area,
When the range of the obstacle occupying the moving area is equal to or greater than a predetermined range, the moving area is divided into four parts, and the obstacle occupying the moving area after the quartering process is performed by repeating the four-dividing process. The route search method according to claim 14, wherein a movement area whose object range is smaller than the predetermined range is calculated, and the calculated movement area is used as a search space. The moving area is a three-dimensional area;
When generating the outer periphery of the movement prohibited area,
The route search method according to claim 14, wherein the shape of the outer periphery of the movement prohibited area centered on the three-dimensional obstacle is generated using a three-dimensional figure having a predetermined curvature. The route search method according to claim 14, wherein the size of the figure is set in accordance with an arrangement position of the obstacle in the movement area. A route search program for starting a movement from a movement start point existing in a movement region and searching for a movement route of an autonomous mobile body reaching a movement end point existing in the movement region,
Against the computer,
A figure having a predetermined curvature in which a movement prohibition area is set around an obstacle existing in the movement area, and the shape of the outer periphery of the set movement prohibition area is set based on the movement speed of the autonomous mobile body. Generating using, and
From a route candidate formed by connecting the tangent of the movement prohibited area and the outer periphery of the movement prohibited area, a route search is performed based on the movement speed of the autonomous mobile body, and as a result of the route search, the autonomous mobile body is When passing on the outer periphery of the movement prohibited area, the movement path portion of the movement path excluding the outer periphery of the movement prohibited area is defined as the tangent line, and the movement path portion on the outer periphery of the movement prohibited area follows the figure. A route search program for executing a step of making a moving route.

Images