JP7004475B1 - Route generation system, route generation method, route generation program, and autonomous mobile - Google Patents

Abstract

Obtain a route generation system that can set the movement route of the autonomous moving body more appropriately. The route generation system according to the present disclosure has an environmental information acquisition unit that acquires environmental information indicating the position of an object existing around an autonomous moving body and the speed of the object, and shows the flow of movement of the object based on the environmental information. Whether the streamline and the first movement path intersect with the streamline generation unit that generates the streamline and the first movement route acquisition unit that acquires the first movement route that is the movement route to the destination of the autonomous moving body. When the intersection determination unit and the intersection determination unit determine that the streamline and the first movement route intersect, a second movement route modified from the first movement route is generated, and the second movement route is used as the movement of the autonomous moving object. It is equipped with a second movement route acquisition unit that is set as a route.

Description

The present disclosure relates to a route generation system, a route generation method, a route generation program, and an autonomous mobile body.

When humans move through crowded situations, to avoid collisions with other pedestrians and to reach their destinations faster, in line with the flow formed by multiple other pedestrians in the same direction of travel. It is desirable that autonomous moving objects such as vehicles and robots, which often move in an environment where they coexist with humans, also move along the flow of humans.
For example, the technique described in Patent Document 1 regards a plurality of moving objects existing around an autonomous moving object as a group, and moves the autonomous moving object in the same direction as the velocity vector of the group. It controls autonomous moving objects along the flow.

Japanese Unexamined Patent Publication No. 2019-144612

However, in Patent Document 1, since the travel selection is determined only from the current state of the flow of a mass, the current traveling direction is not obstructed, but in the future, the traveling direction of the autonomous moving body will be determined by the region of the mass. In the case of obstruction, the autonomous moving body and the group of the autonomous moving body approach each other, and the action against the flow of the moving object is not taken until the group of the mass of the autonomous moving body blocks the traveling direction. There is a possibility of making a sudden change of direction.
The present disclosure has been made to solve the above-mentioned problems, and an object of the present disclosure is to obtain a route generation system capable of more appropriately setting a movement route of an autonomous moving body.

The route generation system according to the present disclosure has an environmental information acquisition unit that acquires environmental information indicating the position of an object existing around an autonomous moving body and the speed of the object, and shows the flow of movement of the object based on the environmental information. Whether the streamline and the first movement path intersect with the streamline generation unit that generates the streamline and the first movement route acquisition unit that acquires the first movement route that is the movement route to the destination of the autonomous moving body. When the intersection determination unit and the intersection determination unit determine that the streamline and the first movement route intersect, a second movement route modified from the first movement route is generated, and the second movement route is used as the movement of the autonomous moving body. It is equipped with a second movement route acquisition unit that is set as a route.

The route generation system according to the present disclosure has an intersection determination unit that determines whether a streamline and a first movement route intersect, and a first movement route when the intersection determination unit determines that the streamline and the first movement route intersect. Since it is equipped with a second movement path acquisition unit that generates a second movement path modified from the above and sets the second movement path as the movement path of the autonomous moving body, the moving path of the autonomous moving body is the streamline of the object. In the case of intersection, the movement route of the autonomous moving body can be set more appropriately by modifying the movement route in advance.

It is a block diagram which shows the structure of the autonomous mobile body 1000 which concerns on Embodiment 1. FIG. It is a block diagram which shows the structure of the map generation system 200 which concerns on Embodiment 1. FIG. It is a block diagram which shows the structure of the route generation system 300 which concerns on Embodiment 1. FIG. It is a block diagram which shows the hardware composition of the autonomous mobile body 1000 which concerns on Embodiment 1. FIG. It is a flowchart which shows the operation of the autonomous mobile body 1000 which concerns on Embodiment 1. It is a conceptual diagram which shows the specific example of the operation which the map generation system 200 which concerns on Embodiment 1 generate map information. It is a flowchart which shows the detail of the operation which the route generation system 300 which concerns on Embodiment 1 generate a streamline. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 generate a vector field. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 generate a vector field. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 generate a vector field. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 generate a vector field. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 generate a streamline. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 generate a vector field. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 generate a vector field. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 generate a vector field. It is a conceptual diagram which shows the specific example of the operation which the streamline generation part 320 which concerns on Embodiment 1 calculates the density of the object on the streamline. It is a conceptual diagram which shows the detail of the operation which the route generation system 300 which concerns on Embodiment 1 generate a movement route. It is a flowchart which shows the detail of the operation which the 2nd movement path acquisition part 332 which concerns on Embodiment 1 generate the 2nd movement path. It is a conceptual diagram which shows the specific example of the operation which the 2nd movement path acquisition part 332 which concerns on Embodiment 1 generate the 2nd movement path. FIG. 5 is a conceptual diagram showing a specific example of a cost function used by the second movement route acquisition unit 332 according to the first embodiment when specifying a confluence point. It is a conceptual diagram which shows the specific example of the operation which the 2nd movement path acquisition part 332 which concerns on Embodiment 1 generate the 2nd movement path. It is a conceptual diagram which shows the specific example of the operation which the 2nd movement path acquisition part 332 which concerns on Embodiment 1 generate the 2nd movement path. It is a block diagram which shows the structure of the autonomous mobile body 2000 which concerns on Embodiment 2. It is a block diagram which shows the structure of the route generation system 2300 which concerns on Embodiment 2. FIG. It is a flowchart which shows the detail of the operation which the route generation system 300 which concerns on Embodiment 2 generate a streamline. It is a conceptual diagram which shows the specific example of the operation of the streamline generation part 2320 which concerns on Embodiment 2. FIG. It is a conceptual diagram which shows the specific example of the operation of the streamline generation part 2320 which concerns on Embodiment 2. FIG. It is a conceptual diagram which shows the specific example of the operation of the streamline generation part 2320 which concerns on Embodiment 2. FIG. It is a conceptual diagram which shows the specific example of the operation of the streamline generation part 2320 which concerns on Embodiment 2. FIG.

Embodiment 1.
FIG. 1 is a configuration diagram showing a configuration of an autonomous mobile body 1000 according to the first embodiment. The autonomous mobile body 1000 includes an interface system 100, a map generation system 200, a route generation system 300, a travel control system 400, and a drive system 500. Here, the autonomous mobile body 1000 may be a mobile body such as an electric wheelchair or personal mobility that carries a person and moves, or a mobile body that moves unmanned such as a working robot. In the following, personal mobility will be described as an example.

The interface system 100 inputs various information from the outside and presents various information to the passenger. More specifically, the interface system 100 acquires various information by accepting wireless settings and state management of a control system or the like, or by directly accepting settings from a human by various operations.

In the first embodiment, the interface system 100 receives environmental information including an object existing in the area where the autonomous moving body 1000 moves, the position of an entrance / exit, and the like, and transmits self-position information to another autonomous moving body. It includes a communication unit 110, a display unit 120 that displays an environmental map generated by the map generation system 200, which will be described later, and a movement route generated by the route generation system, and a reception unit 130 that receives various information by user operation.

The map generation system 200 generates an environmental map around the autonomous mobile body 1000 and estimates the self-position of the autonomous mobile body 1000. Here, the environmental map is a kind of environmental information, and the environmental information shows the position of an object existing around the autonomous moving body and the speed of the object. In the following, among objects, moving objects such as people and bicycles will be referred to as moving objects, and non-moving objects such as buildings will be referred to as fixed objects.
The details of the map generation system 200 will be described later.

The route generation system 300 generates a movement route for the autonomous mobile body 1000 to reach the destination, and sets the generated movement route as the movement route of the autonomous mobile body 1000, and the details will be described later.

The travel control system 400 calculates the control amount of the autonomous moving body 1000 based on the movement path generated by the route generation system 300, and outputs a control signal indicating the control amount.

In the first embodiment, the travel control system 400 includes a route tracking calculation unit 410, a collision avoidance determination unit 420, and a movement control unit 430.

The route tracking calculation unit 410 is within a range in which it can move to the next control step with the route tracking information based on the deviation between the movement route received from the route generation system 300 and the self-position received from the map generation system 200. It determines the ideal target position. Here, the path tracking calculation unit 410 determines the path tracking information and the ideal target position based on the constraints of the autonomous mobile body 1000 in addition to the deviation between the received movement path and the self-position.

The collision avoidance determination unit 420 calculates the collision risk from the positions of moving and fixed objects included in the environmental map received from the map generation system 200 and the ideal target position calculated by the path tracking calculation unit 410, and determines the magnitude of the risk. The control target position is determined by modifying the ideal target position according to the above.

More specifically, the collision avoidance determination unit 420 compares the ideal target position calculated by the route tracking calculation unit 410 with the collision risk map created from the environment map, and makes a collision avoidance determination. For the collision risk map, for example, a well-known technique such as a potential map may be used. Here, the collision avoidance determination unit 420 sets the control target position as the ideal target position when the collision risk is equal to or less than a predetermined threshold value, and considers the constraint of the autonomous moving body 1000 from the collision risk map when the collision risk exceeds the threshold value. The position where the risk is the smallest is set as the control target position.

The movement control unit 430 calculates the control amount of the autonomous moving body 1000 based on the control target position calculated by the collision avoidance determination unit 420 and the feedback from the drive system 500, and transmits the control signal to the drive system 500. .. Further, the movement control unit 430 stores parameters such as the gear ratio of the drive system 500 in order to calculate the control amount of the autonomous moving body 1000.

The drive system 500 receives a control signal from the travel control system 400 and drives the autonomous mobile body 1000 based on the control amount indicated by the control signal.

More specifically, the drive system 500 has a drive unit 510 composed of a motor, a motor driver, gears, wheels, and the like, and a feedback unit 520 that observes the state of the drive unit 510 and feeds back to the travel control system 400.

The details of the map generation system 200 will be described with reference to FIG. FIG. 2 is a configuration diagram showing the configuration of the map generation system 200 according to the first embodiment.

The map generation system 200 includes an outside world detection unit 210, a satellite signal reception unit 220, an environment map generation unit 230, and a self-position estimation unit 240.

The outside world detection unit 210 detects an object around the autonomous moving body 1000, and for example, a three-dimensional LiDAR (Light Detection Ranking) for acquiring point cloud data, a camera for acquiring image data, or the like is used.

The satellite signal receiving unit 220 receives a positioning signal from a GNSS (Global Positioning System System) satellite such as a GPS (Global Positioning System) satellite or a quasi-zenith satellite, and receives 3 positioning signals on the earth such as the latitude, longitude, and altitude of the autonomous mobile body 1000. It acquires the dimensional position.

The environment map generation unit 230 generates an environment map around the autonomous moving body 1000, and includes a surrounding object position calculation unit 231, a fixed object map generation unit 232, and a moving object map generation unit 233.

The surrounding object position calculation unit 231 identifies the position of the object around the autonomous moving body 1000 by using the sensor data acquired from the outside world detection unit 210, and the object position information indicating the position of the object is used as the fixed object map generation unit 232. It is output to the object position matching unit 241.

The fixed object map generation unit 232 generates a fixed object map based on the object position information acquired from the surrounding object position calculation unit 231 and the self-position estimated by the self-position estimation unit 240 described later. Here, the fixed object map is map information indicating the position of a fixed object among the objects. Further, the fixed object map generation unit 232 may use the environmental information acquired from the interface system 100 in addition to the above information when generating the fixed object map information.

Further, when the fixed object map generation unit 232 stores the past fixed object map MAP (fix, old), the fixed object map generation unit 232 updates the fixed object map using the newly acquired environmental information. A specific example of updating the fixed object map will be described with reference to FIG.

The fixed object map generation unit 232 uses the object position calculated by the surrounding object position calculation unit 231, the self-position calculated by the self-position estimation unit 240, the past fixed object map MAP (fix, old), and the interface system 100. The set fixed object position data is input, and the past fixed object map MAP (fix, old) shown in FIG. 3a, the newly acquired object position, and the fixed object position set via the interface system 100 are self-determined. Based on the position, it is arranged on one map as shown in FIG. 3b.

Next, in the area where the past fixed object map MAP (fix, old) is already included in the map as shown in FIG. 3c, although it exists in the past fixed object map MAP (fix, old), the surrounding object position calculation The object (point (a + 2, b + 3) in FIG. 3c) that does not exist at the object position calculated by unit 231 is deleted, and the object does not exist in the past fixed object map MAP (fix, old), but the surrounding object position calculation unit 231. The object existing at the object position calculated by (the (a + 3, b + 2) point in FIG. 3c) is determined to be the moving body OB1.

Further, the object existing in both the past fixed object map MAP (fix, old) and the newly acquired object position, and the past fixed object map MAP (fix, old) exist in the area not included in the map yet. The object is determined to be a fixed object. From the above determination results, the past fixed object map is updated and the latest fixed object map MAP (fix, new) and the data of the moving object position are output.

The moving object map generation unit 233 generates a moving object map. More specifically, the moving object map generation unit 233 tracks the moving object and generates a moving object map by calculating the velocity vector of the moving object.

The moving object map generation unit 233 describes the current and past moving objects based on the current moving object position input from the fixed object map generation unit 232 and the stored past moving object map MAP (float, old). , Identification is performed based on information such as position and grid shape, and the current velocity vector (velocity magnitude, direction) of the identified moving object is calculated from the position change amount and control cycle. The moving object map generation unit 233 generates a moving object map MAP (float, new) including the current position, velocity vector, and grid shape of each moving object obtained from the above calculation results, and outputs the moving object map MAP (float, new) to the route generation system 3000. ..

The self-position estimation unit 240 calculates the self-position of the autonomous moving body 1000 by using three-dimensional position information by the satellite signal receiving unit 220 and SLAM (Simultaneus Localization and Mapping) technology, and the object position matching unit 241 and the position fusion unit. A calculation unit 242 is provided.

The object position matching unit 241 sets the object position using the position and shape as clues based on the object position information acquired from the surrounding object position calculation unit 231 and the past fixed object map stored in the fixed object map generation unit 232. Matching is performed to minimize the placement error of the past fixed object map. As a method for performing this matching, for example, an ICP (Iterative Closest Point) algorithm and an NDT (Normal Distributions Transfer) algorithm are known for point clouds. By aligning the object position with the coordinate system of the past fixed object map MAP (fix, old), it is possible to output the data of the self-position of the autonomous moving body 1000 in the past fixed object map MAP (fix, old).

The position fusion calculation unit 242 outputs the final result of the self-position based on the self-position calculated by the object position matching unit 241 derived from the outside world detection unit 210 and the self-position calculated by the satellite signal receiving unit 220. It is something to do. The self-position derived from the outside world detection unit 210 tends to have a large error in an environment where surrounding objects are sparse, while the self-position by the satellite signal receiving unit 220 has a large error in an indoor environment, or the self-position is in the first place. It may not be possible to obtain it. Therefore, the position fusion calculation unit 242 outputs the other self-position as the final result of the self-position when one self-position cannot be acquired, and filters processing such as a Kalman filter when both self-positions can be acquired. Is used to output the final calculation result of the self-position based on both self-positions.

As described above, the map generation system 200 is configured, and then the details of the route generation system 300 will be described with reference to FIG. FIG. 4 is a configuration diagram showing the configuration of the route generation system 300 according to the first embodiment.

The route generation system 300 includes an environmental information acquisition unit 310, a streamline generation unit 320, a movement route acquisition unit 330, and an intersection determination unit 340.

The environmental information acquisition unit 310 acquires environmental information indicating the position of an object existing around the autonomous moving body 1000 and the speed of the object.

The environmental information acquisition unit 310 acquires the latest fixed object map MAP (fix, new) and the latest moving object map MAP (float, new) as environmental information from the map generation system 200, and superimposes them to superimpose the map. Generate. In the following, the superimposed map is also treated as a kind of environmental information. Further, the environmental information is not limited to the grid-shaped map information as described above, and may be any information indicating the position and speed of the object.

In addition, the velocity of the object indicated by the environmental information is not limited to the case where the velocity is clearly indicated as in the moving object map, but the information that the object is a fixed object is given as in the fixed object map. Includes those that indirectly indicate that the velocity is zero.

Further, in the above, the map generation system 200 outputs the fixed object map and the moving object map separately, and the environmental information acquisition unit 310 superimposes them to generate a superposed map. However, the map generation system 200 Generates a superimposed map, and the environmental information acquisition unit 310 may acquire the superimposed map generated by the map generation system 200 as environmental information.

The streamline generation unit 320 generates a streamline indicating the flow of movement of an object based on environmental information. Here, the streamline is not limited to the streamline in the strict sense in fluid mechanics, but includes a curve or a straight line indicating the flow of movement of other objects such as a streamline and a streamline. That is, the streamline here means a movement path when a plurality of objects are viewed macroscopically.

The movement route acquisition unit 330 acquires a movement route to which the autonomous mobile body 1000 moves, and includes a first movement route acquisition unit 331 and a second movement route acquisition unit 332.

The first movement route acquisition unit 331 acquires the first movement route, which is the movement route to the destination of the autonomous moving body. Here, the first movement route is a movement route as an initial setting in the process of setting the movement route of the autonomous mobile body 1000.

When the intersection determination unit 340, which will be described later, determines that the streamline and the first movement route intersect, the second movement route acquisition unit 332 generates a second movement route modified from the first movement route, and sets the second movement route. It is set as a movement path of the autonomous mobile body 1000. Further, when the intersection determination unit 340 determines that the streamline and the first movement route do not intersect, the second movement route acquisition unit 332 sets the first movement route as the movement route of the autonomous mobile body 1000 as it is.

Further, in the first embodiment, when the intersection determination unit 340 determines that the streamline and the first movement route intersect, the second movement route acquisition unit 332 merges at least a part of the second movement route with the streamline. As described above, the second movement route is generated by modifying the first movement route.

The intersection determination unit 340 determines whether the streamline generated by the streamline generation unit 320 intersects with the first movement path acquired by the first movement route acquisition unit 331. The intersection determination unit 340 outputs a determination result as to whether or not the streamline and the first movement route intersect to the second movement route acquisition unit 332.

Further, in the first embodiment, when the streamline and the first movement path intersect, the intersection determination unit 340 specifies the position of the intersection of the streamline and the movement path, and the second movement path acquisition unit 332 determines the position of the intersection. Based on the position, the position of the confluence where the second movement path and the streamline meet is determined.

Further, in the first embodiment, in addition to the merging point, the second movement route acquisition unit 332 determines the position of the departure point at which the autonomous moving body 1000 leaves the streamline and the position of the return point at which the autonomous moving body 1000 returns to the first movement path. decide.

Further, in the first embodiment, the streamline generation unit 320 calculates the density of objects on the streamline based on the environmental information, and the second movement path acquisition unit 332 merges based on the position of the intersection and the density of the objects. Determine the position of the point.

Further, in the first embodiment, the streamline generation unit 320 generates a vector field indicating the flow velocity of the moving flow of the object based on the position of the object and the velocity of the object indicated by the environmental information, and flows based on the vector field. Generate a line.

As described above, the route generation system 300 is configured.

Next, the hardware configuration of the autonomous mobile body 1000 according to the first embodiment will be described.
FIG. 5 is a hardware configuration diagram of the autonomous mobile body 1000 according to the first embodiment.

The hardware shown in FIG. 5 includes a processing device 10001 such as a CPU (Central Processing Unit), a storage device 10002 such as a ROM (Read OnlyMemory) and a hard disk, a sensor interface 1003, a display interface 1004, a setting interface 10005, and an actuator interface 10006. , GNSS receiver 10087, rotary encoder 10009, liquid crystal display 10010, touch panel 10011, and drive device 10012.

Each function of the map generation system 200, the route generation system 300, and the travel control system 400 shown in FIG. 1 is realized by executing the program stored in the storage device 10002 on the processing device 10001. Further, the method of realizing each function is not limited to the combination of the hardware and the program described above, and may be realized by the hardware alone such as an LSI (Large Scale Integrated Circuit) in which the program is implemented in the processing device. However, some functions may be realized by dedicated hardware, and some may be realized by a combination of a processing device and a program.

Further, the outside world detection unit 210 of the map generation system 200 is realized by the outside world sensor 10007, and the satellite signal reception unit 220 is realized by the GNSS receiver 10048. Further, the display unit 120 of the interface system 100 is realized by the liquid crystal display 10010 and the touch panel 10011, the reception unit 130 is realized by the touch panel 10011, the drive unit 510 of the drive system 500 is realized by the drive device 10012, and the feedback unit 520. Is realized by the rotary encoder 10009.
As described above, the autonomous mobile body 1000 according to the first embodiment is configured.

Next, the operation of the autonomous mobile body 1000 according to the first embodiment will be described.
FIG. 6 is a flowchart showing the operation of the autonomous mobile body 1000 according to the first embodiment.
Here, the operation of the route generation system 300 corresponds to the route generation method, and the program that causes the computer to execute the operation of the route generation system 300 corresponds to the route generation program. Further, "part" may be appropriately read as "process".

First, in step S1, the map generation system 200 generates environmental information indicating the positions of objects around the autonomous moving body 1000.

Next, in step S2, the environmental information acquisition unit 310 acquires the environmental information generated in step S1, and the streamline generation unit 320 generates a streamline indicating the flow of the object based on the acquired environmental information. do.

Next, in step S3, the movement route acquisition unit 330 acquires the movement route of the autonomous mobile body 1000 based on the streamline generated in step S2, and sets the acquired movement route as the movement route of the autonomous mobile body 1000. ..

Next, in step S4, the travel control system 400 calculates the control amount of the autonomous mobile body 1000 based on the movement route set in step S3.

Finally, in step S5, the drive system 500 drives the autonomous mobile body 1000 based on the control amount calculated in step S4.

Here, the details of the streamline generation process in step S2 will be described with reference to FIG. 7. FIG. 7 is a flowchart for explaining the details of the streamline generation process.

First, in step S201, the environmental information acquisition unit 310 acquires the fixed object map MAP (fix, new) and the moving object map MAP (float, new) from the map generation system 200, and generates environmental information by generating a superimposed map. To get.

Next, in step S202, the streamline generation unit 320 generates a vector field indicating the flow velocity of the flow of the object from the environmental information. More specifically, the streamline generation unit 320 generates a vector in the grid point shape of the grid for map information of different attributes such as a moving object, a fixed object, and an entrance / exit by a method according to each attribute, and a grid map. The vector generated for all the grid points of is output as a vector field.

A specific example of the process in which the streamline generation unit 320 generates a vector field will be described with reference to FIG.

FIG. 8 shows a method of creating a vector on a grid point from a moving object OB2 and a fixed object OB3 expressed on a grid, and the streamline generation unit 320 uses a velocity vector held by the moving object OB2 as a moving object OB2. Is generated at the four corners of the grid that contains. Further, the streamline generation unit 320 generates a vector radially from the four corners of the grid in which the fixed object OB3 is inserted as an expression of the repulsive force of the fixed object OB3.

FIG. 9 shows a method of generating a vector when the moving object OB4 and the moving object OB5, and the fixed object OB6 and the fixed object OB7 are present in adjacent grids, and the streamline generating unit 320 shows the moving object or the moving object or the fixed object OB7. , Vectors are synthesized at grid points sandwiched between fixed objects, and the size of the composite vector is divided by the number of composite vectors to generate the vector of the grid points.

FIG. 10 shows a method of creating a vector when a moving object OB8 and a moving object OB9, and a fixed object OB10 and a fixed object OB11 exist in adjacent grids, and a moving object OB8 and a fixed object OB11 exist in an adjacent grid. At the grid points between moving objects, between fixed objects, or between moving objects OB8 and fixed objects OB11, vectors are combined, and the size of the combined vector is divided by the number of combined vectors. Is created as a vector of the grid points.

FIG. 11 shows a method of creating a vector when the doorway GA1 is set. The entrance / exit GA1 is given information on whether it is an exit or an entrance to the points at both ends, and based on that information, the potential f (p) of the two-dimensional position p is calculated by the following mathematical formula 1.

In Equation 1, the potential is expressed by a two-dimensional normal distribution. When the entrance / exit is the entrance, k> 0, when the entrance / exit is the exit, k <0, and the average μ is the center of the entrance / exit based on the positions of the points at both ends. The position and covariance Σ indicate the direction of the entrance / exit. The ellipse formed by the alternate long and short dash line in FIG. 6d indicates an equipotential line, and the magnitudes of the gradients are equal on the equipotential line. The gradient ∇f (p) is calculated by the following formula 2.

Here, Δp is a minute displacement of the two-dimensional position p, and the grid width is set as a guide. The vector field by the entrance / exit is generated as a gradient vector calculated based on Equation 2 with the coordinates of the lattice points as p. The vector field by the entrance / exit extrapolates the streamlines created in the range of the fixed object map MAP (fix, new) and the moving object map MAP (float, new) to the outside of each map range (outside the detection range of the outside world sensor 22). Used when doing.

Returning to FIG. 7, the continuation of the streamline generation process will be described.
In step S203, the streamline generation unit 320 generates a streamline from the vector field. A specific example of the process in which the streamline generation unit 320 generates a streamline from the vector field will be described with reference to FIGS. 12 to 15. The streamline generation unit 320 calculates the movement locus drawn as a result of placing the fluid particles on the grid, receiving a force from the vector field and evolving with time, as a streamline.

FIG. 12 schematically illustrates the calculation of the trajectory of the fluid particle VP1 in a grid with a vector field.

The grid is surrounded by four grid points (x i + 1, y j + ₁ ), (x _{i + 1} , y _{j + 1} ), (x _{i +} 1, y _j +) clockwise from the lower left point. , Each grid point is (vi _{, j} , wi _{, j} ), (vi _{, J + 1} , wi _{, j + 1} ), (vi _{+ 1, j + 1} , wi _{+ 1, j + 1} ), (vi _{+ 1, j} , wi + ₁ ). _{, J} ) has a vector. It should be noted that there is a relationship of x _{i + 1} = x _i + Δx and y _{i + 1} = y _i + Δ y.

The streamline generation unit 320 places the fluid particle VP1 at the position p (t) = (x (t), y (t)) at the time t, and the position p (t + 1) and the velocity of the fluid particle VP1 at the next time t + 1. Calculate v (t).

The force received by the fluid particle VP1 placed at the position p (t) is the sum of the vectors of the grid points multiplied by the coefficients, and the coefficients ki _{, j} = multiplied by the vectors of the grid points (x _i , y _j ). (A _{i, j} , bi _{, j} ) is calculated by the formula 3.

In FIG. 12, since the position p (t) is closest to the grid point (x _{i + 1} , y _j ) and farthest from the grid point (x i + 1, y _{j + 1} ), the vector (vi _{+ 1, j} ) is shown in Equation 3. , Wi _{+ 1, j} ) has the largest coefficient ki _{+ 1, j} so that the action is the largest, and the vector (vi _{, j + 1} , wi _{, j + 1} ) has the smallest coefficient ki _{1, j + 1} so that the action is the smallest. It turns out that it is a small value.

Based on the above, the streamline generation unit 320 sets the position p (t + 1) = (x (t + 1), y (t + 1)) and the velocity vector v (t) = (v _x (t), v _y (t)). Calculation is performed using formulas 4 to 7.

In FIG. 12, the fluid particle VP1 moves in the same grid from the time t to the time t + 2, but since the fluid particle VP1 moves to the adjacent grid from the time t + 3, it becomes a grid point surrounding the grid. It is necessary to replace the equations 3 to 5 with the corresponding numerical values. This calculation is repeated until the fluid particle VP1 goes out of the region of the vector field or the amount of movement in the vector field becomes a magnitude equal to or less than the threshold value. As described above, the streamline SL1 (movement locus of fluid particles) is expressed as a waypoint composed of a plurality of waypoints.

13 to 15 schematically illustrate an example of a method of arranging fluid particles for the streamline generation unit 320 to create a plurality of streamlines with an appropriate distribution.

FIG. 13 is surrounded by four grid points (x ₁ , y ₁ ), (x ₁ , y ₆ ), (x ₆ , y ₆ ), and (x ₆ , y ₁ ) clockwise from the lower left point. It shows the one in which a plurality of fluid particles VP2 and VP3 are placed at the initial points in the regions of the x-axis 5 grid and the y-axis 5 grid having regions. As in FIG. 12, there is a relationship of x _{i + 1} = x _i + Δx, y _{j + 1} = y _j + Δy (i = 1, ..., 6, j = 1, ..., 6).

This initial point is arranged when it is a grid point on the boundary line of the set area and the vector faces the inside of the set area. That is, at the boundary line x = x1, the grid point where the vector has _a positive component of x, at the boundary line y = y1, the grid _point where the vector has the positive component of y, and at the boundary line x = _x6 , the vector. Is a grid point having a negative component of x, and at the boundary line y = y ₆ , the vector is a grid point having a negative component of y.

FIG. 14 shows the movement locus SL2 due to the action of the vector field of the arranged fluid particle VP2 and the deleted fluid particle VP3. The calculation of the movement locus of the fluid particles is as shown in FIG. The arranged fluid particle VP3 is deleted when the movement locus SL2 by another arranged fluid particle VP2 passes in the vicinity. This neighborhood means (x _i − Δx / 2) ≦ x ≦ (x _i + Δx / 2) at the grid point (x _i , y _j ) and (y _j − Δ y / 2) ≦ y ≦ (y _j ). It is in the range of + Δy / 2).

FIG. 15 shows the results of performing the same processing as in FIGS. 13 and 14 with the arrangement of the initial points of the fluid particles on a straight line narrowed by one grid from the boundary line to the grid point near the center of the vector field. That is, in this example, the straight lines on which the fluid particles VP4 are arranged are x = x ₂ , y = y ₂ , x = x ₅ , and y = y ₅ . At this time, since the movement locus SL2 has already been drawn, as a condition for arranging the fluid particles in advance, it is possible to omit the trouble of deleting the arranged fluid particles later by adding that the movement locus does not pass in the vicinity. I can. When the straight line in which the fluid particles are arranged is gradually narrowed in this way and the processing is completed when the center of the vector field is reached, it becomes possible to draw a streamline in the entire region of the vector field. The obtained streamlines are labeled with Li _{and j} . i and j are indexes of the x-axis and y-axis of the initial point of the fluid particle.

In this example of the vector field, a square area is shown, but a rectangular or polygonal area may be used. For example, in the case of a rectangular grid, the quadrangle surrounded by the straight line on which the fluid particles are arranged may be narrowed on the long side first, and when the quadrangle becomes a square, the same processing as described above may be performed.

Further, in the above, a two-dimensional grid has been described as an example as a grid, but when generating a route of an autonomous moving body that moves in the vertical direction such as a drone, the same processing as above is performed using the three-dimensional grid. Just do it and generate a streamline.

Returning to FIG. 7, the continuation of the streamline generation process will be described.
Finally, in step S204, the streamline generation unit 320 calculates the object density on the streamline based on the environmental information. A specific example of the process in which the streamline generation unit 320 calculates the object density on the streamline will be described with reference to FIG.

FIG. 16 shows a specific example of a process in which the streamline generation unit 320 generates a linear density distribution graph showing the density of an object on a certain streamline obtained as a movement locus of a fluid particle in step S204.

FIG. 16a shows the distribution of moving objects shown by the moving locus SL2 (streamline Li _{, j} ) of the fluid particles before and after time t = T and the moving object map MAP (float, new). FIG. 16a shows a procedure for calculating the density ρ (T) at the position p (T) = (x (T), y (T)) of the fluid particle at time t.

It can be seen that the position p (T) is in the grid G _{k, l} having (x _k , y _l ) as the lower left grid point. There are four grids including moving objects in the vicinity of this grid and its surroundings: G _{k, l-1} , G _{k + 1, l-1} , G _{k + 1, l} , and G _{k-1, l + 1} . Assuming that the vector from the position p (T) to the center position of the grid G _{k, l-1} is rk _{, l-1} , the magnitude | rk _{, l-1} | is calculated by the following formula 8.

This is the distance from the position p (T) to the center position of the grids _{Gk and l-1} . By calculating this distance in the same way for a grid containing other moving objects, the density ρ (T) at the position p (T) is calculated by the following formula.

Here, the distance function f (| r |) is a function that is positive and monotonically increases at | r | ≧ 0, such as f (| r |) = | r |. That is, the density ρ (T) at the position p (T) takes a larger value as there are more moving objects nearby. The reason why the range is separated from the surrounding neighborhood is to reduce the calculation cost. There is no problem because the effect is small.

FIG. 16b shows a linear density graph in which the distance l (t) moved from ( _xi , y _j ) along the streamlines Li _{, j} is taken on the vertical axis and the density ρ (t) is taken on the horizontal axis. The distance l (t) is the distance traveled from (xi _i , y _j ) along the streamlines Li _{i, j} at time t, and is calculated by the following equation 10.

The initial points of the streamlines L _{i and j} are set to (x _i , y _j ) = (x (0), y (0)). In the above procedure, a linear density graph expressing the density of moving objects on streamlines Lii _{and j by} plotting (l (t), ρ (t)) at each time t on streamlines Lii and j. Is sought. As described above, the information regarding the flow of the object is represented by the combination of the streamlines Li _{and j} and the linear density graph.
With the above, the streamline generation unit 320 ends the streamline generation process.

Next, the details of the movement route acquisition process in step S3 will be described with reference to FIG. FIG. 17 is a flowchart for explaining the details of the movement route acquisition process.

First, in step S301, the first movement route acquisition unit 331 determines whether or not the movement route has already been set. Here, if the movement route is already set, the process proceeds to step S302, and if it is not set, the process proceeds to step S303.

When the process proceeds to step S302, the first movement route acquisition unit 331 acquires the fixed object map MAP (fix, new) from the map generation system 200, and is there any change in the acquired fixed object map MAP (fix, new)? Is determined. Here, if it is determined that there is no change, the first movement route acquisition unit 331 acquires the already set movement route as the first movement route, and proceeds to step S304. On the other hand, if it is determined that there is a change in the fixed object map MAP (fix, new), the process proceeds to step S303.

When proceeding to step S303, the first movement route acquisition unit 331 calculates the movement route from the current position to the destination based on the fixed object map MAP (fix, new), and uses the calculated movement route as the first movement route. Get as. That is, the first movement path here is a movement path generated based on the position of the fixed object.

Further, the first movement route acquisition unit 331 stores the first movement route generated in step S303.

Next, in step S304, the intersection determination unit 340 determines whether or not the autonomous moving body 1000 has already joined the flow of the object. For example, it is determined whether or not the merging has been completed by holding a flag such that "1" is set when the merging is performed and "0" is set when the merging is not performed, and the value of the flag is determined. ..

Here, the intersection determination unit 340 proceeds to step S305 when it is determined that the autonomous moving body 1000 has already merged with the flow of the object, and proceeds to step S306 when it is determined that the autonomous moving body 1000 has not merged.

When the process proceeds to step S305, the intersection determination unit 340 determines whether or not the first movement path and the streamline intersect. If the intersection determination unit 340 determines that the first movement route and the streamline intersect, the process proceeds to step S307, and if it is determined that the intersection does not occur, the second movement route acquisition unit 332 moves with the first movement route as the final output. Set as a route and end the operation.

The m waypoints of the first movement path are p ₁ (i) = (x ₁ (i), y ₁ (i)) (i = 1, ..., M), and the n waypoints of the streamline are p. When ₂ (j) = (x ₂ (j), y ₂ (j)) (j = 1, ..., N), the waypoints p ₁ (i) and p ₁ (i + 1) adjacent to each other in the first movement route. The intersection determination of the line segment connected by) and the line segment connected by the streamline adjacent transit points p ₂ (j) and p ₂ (j + 1) is calculated using the following equations 11 to 15.

When formulas 11 to 15 are calculated with all combinations of i = 1, ..., m-1 and j = 1, ..., N-1 i and j, and even one formula 15 as a judgment formula is True. , It is determined that the first movement path and the streamline intersect. When the intersection is _{pc = (x c ,} y _c ), α ₁ , α ₂ , x _c , y _c are calculated by equations 16 to 19.

When the process proceeds to step S307, the second movement route acquisition unit 332 generates a second movement route obtained by modifying the first movement route, sets the generated second movement route as the movement route as the final output, and sets the generated second movement route. End the operation. More specifically, the second movement path acquisition unit 332 separates from the first movement path once, joins the merging point for merging with the streamline, and the merging path which is a path to the merging point, and for leaving the streamline. The second movement path is generated by calculating the return point between the departure point and the return point and the return point for returning to the first movement path again.

On the other hand, when the process proceeds from step S304 to step S306, the intersection determination unit 340 determines whether the first movement path intersects the merged streamline. This is because moving objects are often humans and the flow can change over time. Here, the method of determining the intersection is the same as that of S305. If the intersection determination unit 340 determines that the first movement path intersects the streamline that has already merged, the process proceeds to step S308, and if it is determined that the intersection does not intersect, the step Proceed to S309.

When the process proceeds to step S308, the second movement route acquisition unit 332 corrects the first movement route based on the merged streamline and generates the second movement route. This process is calculated by applying steps S404 to S406 in the flowchart shown in FIG. 18 only to the merged streamlines. Then, the second movement route acquisition unit 350 sets the generated second movement route as the movement route of the autonomous mobile body 1000, and ends the operation.

When the process proceeds to step S309, the second movement route acquisition unit 350 generates a return route returning to the first movement route as a part of the second movement route. This process is also the same as in step S308 in that steps S404 to S406 in the flowchart shown in FIG. 18 are applied only to the merged streamlines to calculate, but the candidate for the departure point is first selected for the autonomous mobile body 1000. The difference is that the current position is Pego.

The route generation system 300 ends the route acquisition process, and further details of the process in which the second movement route acquisition unit 350 generates the second movement route in step S307 will be described with reference to FIG. FIG. 18 is a flowchart for explaining the details of the process in which the second movement route acquisition unit 332 generates the second movement route.

First, in step S401, the second movement path acquisition unit 350 sets a confluence path in consideration of the constraint condition of the autonomous moving body 1000 and the position of the fixed object for a plurality of waypoints of one streamline. Generate as part.

FIG. 19 shows the streamline SL3 from the waypoint p1 (i) (i = τ, τ + ₁ ) of the self-positioning Pego of the autonomous mobile body 1000 or the first movement path TR1 in step S401 by the second movement route acquisition unit 332. An example of a method of calculating a merging route for each of the waypoints p ₂ (j) (j = T, ..., T + 5) is schematically described.

The autonomous mobile body 1000 has physical restrictions due to the output upper limit of the drive unit 510, and control restrictions that allow the impact on the load to be within the allowable value, such as being offended when a sharp turn is made when a person is on board. have. Further, when there is a fixed object OB12 between the autonomous moving body 1000 and the streamline SL3, it is necessary to join the streamline SL3 while avoiding the fixed object OB12.

Here, in the merging paths CR1, CR2, and CR3 in FIG. 19, when the minimum value of the turning radius of the autonomous moving body 1000 is r _min , only a straight line and a right / left turn with a turning radius r _min up to the waypoint of the streamline SL3. By configuring with, the above physical restrictions are satisfied.

At the waypoints p ₂ (T), p ₂ (T + 1), and p ₂ (T + 2), the confluence path CR1 is drawn from the current position Pego of the autonomous mobile body 1000. On the other hand, at the waypoint p2 ₍ T + 3), it can be seen that when the merging path CR2 is drawn from the current position Pego of the autonomous mobile body 1000, it comes into contact with the fixed object OB12. Therefore, the waypoint p ₁ (i) of the first movement path TR1 is brought closer to the intersection IP1 one by one with p ₁ (τ) and p ₁ (τ + 1) to avoid the fixed object OB12 and the autonomous moving body 1000. Search for a route that satisfies the constraint conditions of. The confluence route CR3 to the waypoints p2 (T + ₄ ) and p2 ₍ T + 5) closer to the intersection IP1 _than the waypoint p2 (T + 3) is calculated from the waypoint p1 (τ + ₁ ).

The above calculation of the merging route may be performed for all the waypoints of the streamline SL3, but if there are many waypoints, it is limited to the waypoints within a certain radius around the autonomous mobile body 1000. You may.

Returning to FIG. 18, the continuation of the second movement route generation process will be described.
Next, in step S402, the second movement route acquisition unit 332 selects the optimum waypoint for merging as the merging point based on the merging route to each waying point generated in step S401.

FIG. 20 schematically illustrates an example of a method in which the second movement route acquisition unit 332 selects the optimum confluence point CP1 from the waypoints of the streamline SL3 corresponding to FIG.
The optimum confluence point CP1 is the movement cost C _m = _fm ( _dm ) with respect to the distance of the confluence point CP1 indicated by the function of the distance dm ( _t ) from the current position of the autonomous moving body 1000 to the waypoint of the streamline SL3. (T)), The distance from the intersection IP1 to the waypoint of the streamline SL3 _de (t) The departure cost C _e = f _{e (} t) for the small margin for leaving the intersection IP1. )), Two variables of the density ρ ( _t ) on the streamline shown in FIG. 16 and the distance dm (t) to the waypoint for considering the movement of the density ρ (t) until reaching the waypoint. The density cost C _d = f _d (ρ ( _t ), dm (t)) for the difficulty of joining the dense part of the flow of the object shown by the function is calculated for each waypoint p ₂ (t), and these are calculated. It is calculated as the point where the total cost of _Call is minimized.

The graph of FIG. 20 shows the case where the function of each cost increases monotonically with respect to the argument, and the movement cost increases left and right from the nearest point to the current position of the autonomous moving body 1000 to the lowest cost point, and the withdrawal cost. Is lower as the distance from the intersection IP1 increases, and it can be seen that the density cost varies depending on the streamline. The distance l (t) on the horizontal axis is calculated by the formula 10.

Then, the total cost _Call is calculated by the formula 20.

Here, _{km, ke, and k d are weights} for each cost. In the graph of the total cost in FIG. 20, the waypoint p2 (T + ₂ ) has the lowest total cost, so this point is designated as the confluence point CP1.

Returning to FIG. 18, the continuation of the second movement route generation process will be described.
Next, in step S403, the second movement path acquisition unit 332 moves the autonomous moving body 1000 to the opposite bank in the flow from the magnitude | v (t) | of the flow velocity on the streamline and the object density ρ (t). Calculate the distance it takes to calculate the departure point. The distance _le (| v (t) |, ρ (t)) from the confluence to the departure point is generally monotonous with respect to the magnitude of velocity | v (t) | and the density ρ (t). Represented as a function that increases to. The departure point is arranged at the waypoint of the streamline at the place where the distance _le (| v (t) |, ρ (t)) has moved from the confluence on the streamline, or at the place where the distance has moved further.

Next, in step S404, the second movement path acquisition unit 332 determines the return point and the return path in consideration of the constraint condition of the autonomous moving body 1000 and the position of the fixed object with respect to the plurality of waypoints of the first movement path. Calculate. Here, the return route is a part of the second movement route.

Next, in step S405, the second movement path acquisition unit 332 performs an operation on the next loop if the return path calculated in step S404 can move without being blocked by a fixed object or the like, and step S406 if the return path cannot be moved. Proceed to.

Next, in step S406, the second movement route acquisition unit 332 moves the departure point calculated in step S403 in the direction of the intersection on the streamline. At this time, a waypoint on the streamline may be selected as a new departure point, or a point between the waypoints may be selected as a new departure point by setting a certain moving distance.

The reason why the new departure point is brought closer to the intersection here is that the intersection exists on the first movement path that can avoid the fixed object, and there is a possibility that a path that can avoid the fixed object can be created. Is high.

21 and 22 show that in step S406 from S404, the second movement route acquisition unit 332 is located at each of the waypoints p ₁ (i) (i = τ', ... τ'+ 2) from the departure point to the first movement route. On the other hand, an example of a method of calculating a candidate for the return path RR1 and adjusting the departure point until there is a candidate for the return path RR1 that can be moved is schematically described. This assumes, for example, a case where there is a flow of people near the entrance and if the person exits the flow early, the person collides with the structure (fixed object OB13) at the entrance.

In the situation of FIG. 21, the departure point is WP1, and in step S404, the waypoint p1 (τ') of the _first movement path TR2 closest to the intersection IP2 is selected as a candidate for the return _point , and then p1 (. τ'+ 1) is selected and the return path RR1 is calculated in the same manner as the calculation of the merging path in step S401, but since all the paths are obstructed by the fixed object OB13, the step is determined by step S405. Transition to S406.

In FIG. 22, as a result of the transition to step S406, a waypoint on the streamline SL3 near the intersection IP2 is set as a new departure point WP2. Then, in step S404, the return path RR3 with the waypoint p ₁ (τ') of the first movement path TR2 closest to the intersection IP2 as the return point is calculated, and since this is movable, the movement path for this streamline is calculated. Finish the correction process of and move to the process for the next streamline.

The second movement route acquisition unit 332 ends the calculation of the second movement route after rotating the loop from step S401 to step S406 by the number of intersecting streamlines, and if the number has not been reached yet, the next intersection occurs. The operation is performed from step S401 on the streamline. In this way, by repeating the calculation of the merging point, the merging route, the departure point, the return route, and the return point, which exist as many as the intersecting streamlines, the second movement route obtained by modifying the first movement route is generated. It is set as the movement path of the autonomous moving body 1000.
With the above, the second movement route acquisition unit 332 ends the second movement route generation process.

By the above operation, the route generation system 300 according to the first embodiment corrects the movement route in advance when the movement route of the autonomous moving body intersects the streamline of the object, so that the autonomous movement is more appropriate. You can set the movement route of your body. For example, it is possible to allow time to pass through the flow by modifying the path according to the flow in advance, and when the flow is strong, that is, the speed or density of the moving body forming the flow is high. Even in such a case, it is possible to go to the destination without disturbing the flow of the moving body and without getting stuck.

Further, in the first embodiment, when the intersection determination unit 340 determines that the streamline and the first movement path intersect, the second movement route acquisition unit 332 merges at least a part of the second movement path with the streamline. As described above, since the second movement path is generated by modifying the first movement path, it is possible to generate a movement path capable of smooth movement that does not go against the flow of the object.

Further, in the first embodiment, when the streamline and the first movement path intersect, the intersection determination unit 340 specifies the position of the intersection of the streamline and the movement path, and the second movement path acquisition unit 332 determines the position of the intersection. Since the position of the confluence where the second movement path and the streamline meet is determined based on the position, a more appropriate second movement path should be generated according to the position of the streamline and the first movement path. Can be done.

Further, in the first embodiment, in addition to the confluence point, the second movement route acquisition unit 332 determines the position of the departure point where the autonomous moving body 1000 leaves the streamline and the position of the return point where the autonomous moving body 1000 returns to the first movement path. Since the determination is made, the autonomous moving body 1000 does not remain moving along the flow of the object, and it is possible to generate a moving path capable of quickly reaching the destination.

Further, in the first embodiment, the streamline generation unit 320 calculates the density of objects on the streamline based on the environmental information, and the second movement route acquisition unit 332 merges based on the position of the intersection and the density of the objects. Since the position of the point is determined, the movement path can be generated more appropriately according to the bias of the position where the object exists.

Further, in the first embodiment, the streamline generation unit 320 generates a vector field indicating the flow velocity of the moving flow of the object based on the position of the object and the velocity of the object indicated by the environmental information, and flows based on the vector field. Since the line is generated, the autonomous moving body 1000 corresponds to the velocity of the actual object, and the moving object naturally takes a path avoiding the fixed object.

Further, in the first embodiment, the autonomous moving body 1000 calculates the control amount of the autonomous moving body 1000 based on the above-mentioned route generation system and the movement path set by the second movement route acquisition unit 332, and determines the control amount. Since the traveling control system 400 that outputs the indicated control signal and the driving system 500 that drives the autonomous moving body 1000 based on the control signal received from the traveling control system 400 are provided, the autonomous moving body 1000 has a plurality of autonomous moving bodies 1000. Even if an object obstructs the movement path of the autonomous moving body 1000 in the future, it is possible to perform appropriate movement by modifying the movement path in advance and moving based on the corrected movement path.

Here, a modified example will be described.
In the above, the streamline generation process in step S2 is performed before the route acquisition process in step S3, but the streamline generation is performed until it is determined whether the first movement path and the streamline intersect. Therefore, it may be performed in parallel with the acquisition of the first movement route.

In the above, the map generation system 200 and the route generation system 300 are configured to be included inside the autonomous mobile body 1000, but they do not necessarily have to be provided inside the autonomous mobile body 1000, but are outside the autonomous mobile body 1000 and are autonomous. Information may be transmitted and received by means such as communication with the mobile body 1000.

In the above, the method of extrapolating the streamline by creating a vector field outside the detection range by providing a potential at the position of the entrance / exit has been described, but as the extrapolation method, the vector field outside the detection range is stored. The vector field under the same conditions in the past (time, day, etc.) may be used, or a straight line may be simply extended in the direction of the tip of the streamline.

Embodiment 2.
The streamline generation unit 320 according to the first embodiment creates a streamline from a vector field indicating a flow velocity, but in the second embodiment, a plurality of moving objects constituting the flow are classified in the direction of the velocity vector. Then, a method of generating a streamline will be described using a regression model such as a polynomial or a regression tree generated by performing regression from the position distribution of moving objects having the same direction.

FIG. 23 is a configuration diagram showing the configuration of the autonomous mobile body 2000 according to the second embodiment, and FIG. 24 is a configuration diagram showing the configuration of the route generation system 2300 according to the second embodiment.

The differences from the first embodiment will be mainly described, and the description of the same configuration as that of the first embodiment will be omitted as appropriate.

The operation of the streamline generation unit 2320 according to the second embodiment to generate streamlines will be described with reference to FIG. 25.

First, in step S501, the streamline generation unit 2320 classifies moving objects according to the direction of the velocity vector of the moving objects to create a plurality of clusters C (i). At this time, the cluster C (i) is assumed to include at least two or more moving objects.

Next, the streamline generation unit 2320 starts a loop process in which the processes of steps S502 and S503 are performed by the number N of the clusters C (i) in which the moving objects are classified by the angle range.

Next, in step S502, the streamline generation unit 2320 performs a regression operation on the specified model using the position distribution of the moving object, and obtains a streamline determined in one direction from the angle range of the cluster C (i). There are multiple regression methods such as general least squares method, RANSAC (RANdom Sample Consensus), and regression tree, and one method may be selected, or multiple methods may be selected and the position of the most moving object. A method with less variance to the distribution may be selected.

Next, in step S503, the streamline generation unit 2320 moves so that the distance from the streamline generated in step S502 is within a predetermined threshold value and the angle formed with the streamline is less than the predetermined threshold value. It is determined that the object belongs to the streamline, and the other moving objects do not belong to the streamline.

Further, the streamline generation unit 2320 determines whether the number of moving objects determined not to belong to the line is equal to or greater than a predetermined threshold value, and the number of moving objects determined not to belong to the streamline is the threshold value. If it is the above, it transitions to the next loop, and if it is less than the threshold value, it transitions to step S504.

The streamline generation unit 2320 completes the streamline generation process after rotating the loop from step S502 to step S503 by the number N of the clusters C (i), and if the number has not been reached yet, the next cluster C ( The calculation is performed from step S502 for i + 1).

A specific example of the operation of the streamline generation unit 2320 according to the second embodiment will be described with reference to FIGS. 26 to 29.

In FIG. 26, in step S501, the streamline generation unit 2320 uses a moving object map MAP (float, new) to 0 ° ≤ θ <90 °, 90 ° ≤ θ <180 °, 180 ° ≤ θ <270. A method of classifying by a total of four angle ranges of °, 270 ° ≤ θ <360 ° is shown. Here, the clusters C (1), C (2), and C (3) having an angle range of 0 ° ≤ θ <90 °, 90 ° ≤ θ <180 °, and 180 ° ≤ θ <270 ° are created. , The cluster with the angle range 270 ° ≤ θ <360 ° is not created because the number of moving objects to which it belongs is one. In the second embodiment, the angles are not overlapped, but the angle range allows the overlap, and the same moving object may be included in a plurality of clusters. Further, in the second embodiment, the angle range is specified, but clustering by an unsupervised learning method may be used.

27 and 28 show a method of creating two streamlines by regression operation for a moving object of cluster C (2) in steps S502 and S503.

In FIG. 27, the streamline generation unit 2320 performs the first regression operation by selecting three points by the RANSAC method and fitting them into a quadratic function. Here, since the angle range of the cluster C (2) is 90 ° ≦ θ <180 °, the streamline generator 2320 is negative in the x direction and positive in the y direction in the range fitted by the quadratic function. Is determined as the direction of the streamline SL4.

In FIG. 28, the streamline generation unit 2320 uses the moving object belonging to the streamline SL4 obtained in the first regression calculation as the distance between the streamline SL4 and the moving object, the direction of the streamline SL4, and the speed of the moving object. Based on the angle formed by the direction of the vector, it is determined whether the moving object belongs to the streamline. Here, the moving object determined to belong to the first streamline SL4 is shown in white. Then, the streamline generation unit 2320 performs the second regression calculation on the remaining moving object in the same manner as the first time, and generates the second streamline SL5.

FIG. 29 shows a case where streamlines are not generated for a certain cluster in steps S502 and S503.

Since there are only two moving objects included in the cluster C (3), the moving object OB14 and the moving object OB15, only linear regression can be applied. At this time, in step S502, the streamline generation unit 2320 is the streamline SL6 in the direction positive in the x direction and negative in the y direction close to the direction of 180 ° ≦ θ <270 °, which is the angle range of the cluster C (3). Determine the direction of. However, in step S503, it is obvious that the distance between the two moving objects is 0 with respect to the streamline SL6, but as shown in the figure, the direction of the streamline SL6 and the direction of the velocity vectors of the moving objects OB14 and OB15. The angle between them is open by nearly 90 °, and the two moving objects OB14 and OB15 do not belong to the streamline SL5. Then, the streamline generation unit 2320 erases the streamline SL5 to which the moving object to which the streamline belongs does not exist.

Other configurations are the same as the configurations of the route generation system 300 and the autonomous mobile body 1000 according to the first embodiment, and thus the description thereof will be omitted. Further, the modification described in the first embodiment can also be applied to the second embodiment.

The streamline generation unit 2320 according to the second embodiment generates a cluster containing at least two or more objects based on the position of the object and the speed of the object indicated by the environmental information, and is based on the position of the object included in the cluster. Since the streamline is generated by the regression calculation, when the autonomous moving body 1000 joins the streamline, it is possible to take a route that is not easily affected by individual moving objects and has few corrections.

The route generation system according to the present disclosure is suitable for mounting on an autonomously moving vehicle or robot.

1000, 2000 autonomous mobile body, 100, 2100 interface system, 200, 2200 map generation system, 300, 2300 route generation system, 400, 2400 driving control system, 500, 2500 drive system, 110 communication unit, 120 display unit, 130 reception Unit, 210 External world detection unit, 220 satellite signal reception unit, 230 environment map generation unit, 240 self-position estimation unit, 310, 2310 environment information acquisition unit, 320, 2320 streamline generation unit, 330, 2330 movement route acquisition unit, 331 , 2331 1st movement route acquisition unit, 332, 2332 Second movement route acquisition unit, 340, 2340 intersection determination unit, 410 route tracking calculation unit, 420 collision avoidance determination unit, 430 movement control unit, 510 drive unit, 520 feedback unit , 10001 processing device, 10002 storage device, 10003 sensor interface, 10004 display interface, 10005 setting interface, 10006 actuator interface, 10007 external sensor, 10008 GNSS receiver, 10009 rotary encoder, 10010 liquid crystal display, 10011 touch panel, 10012 drive device.

Claims (9)

An environmental information acquisition unit that acquires environmental information indicating the position of an object existing around an autonomous moving body and the speed of the object, and
Based on the environmental information, a streamline generator that generates a streamline indicating the flow of movement of the object, and a streamline generator.
The first movement route acquisition unit that acquires the first movement route, which is the movement route to the destination of the autonomous moving body, and the first movement route acquisition unit.
An intersection determination unit that determines whether the streamline and the first movement path intersect,
When the intersection determination unit determines that the streamline and the first movement path intersect, a second movement path modified from the first movement path is generated, and the second movement path is used as the movement path of the autonomous mobile body. The second movement route acquisition unit set as
Equipped with
When the intersection determination unit determines that the streamline intersects with the first movement path, the second movement route acquisition unit determines that at least a part of the second movement path merges with the streamline. By modifying the first movement route, the second movement route is generated .
When the streamline and the first movement path intersect, the intersection determination unit identifies the position of the intersection of the streamline and the movement path.
The second movement route acquisition unit is a route generation system that determines the position of a confluence where the second movement route and the streamline meet, based on the position of the intersection . The second movement route acquisition unit determines, in addition to the merging point, the position of the departure point at which the autonomous moving body leaves the streamline and the position of the return point at which the autonomous moving body returns to the first movement path. The route generation system described in. The streamline generator calculates the density of the object on the streamline based on the environmental information.
The route generation system according to claim 1 or 2 , wherein the second movement route acquisition unit determines the position of the confluence based on the position of the intersection and the density of the object. The streamline generation unit generates a vector field indicating the flow velocity of the movement of the object based on the position of the object and the velocity of the object indicated by the environmental information, and creates the streamline based on the vector field. The route generation system according to any one of claims 1 to 3 to be generated. The streamline generation unit generates a cluster containing at least two or more of the objects based on the position of the object and the velocity of the object indicated by the environmental information, and is based on the position of the object included in the cluster. The route generation system according to any one of claims 1 to 3 , wherein the streamline is generated by a regression operation. The route generation system according to any one of claims 1 to 5 .
A travel control system that calculates the control amount of the autonomous moving body based on the movement path set by the second movement route acquisition unit and outputs a control signal indicating the control amount.
A drive system that drives the autonomous mobile body based on the control signal received from the travel control system, and
Autonomous mobile with. An environmental information acquisition process for acquiring environmental information indicating the position of an object existing around an autonomous moving body and the speed of the object, and
A streamline generation step of generating a streamline indicating the flow of movement of the object based on the environmental information, and a streamline generation step.
The first movement route acquisition step of acquiring the first movement route which is the movement route to the destination of the autonomous moving body, and
An intersection determination step of determining whether the streamline and the first movement path intersect, and
When it is determined in the intersection determination step that the streamline intersects with the first movement path, a second movement path modified from the first movement path is generated, and the second movement path is used as the movement path of the autonomous moving body. In the second movement route acquisition step set as, when it is determined by the intersection determination step that the streamline and the first movement path intersect, at least a part of the second movement path joins the streamline. As described above, the second movement route acquisition step of generating the second movement route by modifying the first movement route, and
It is a route generation method including
In the intersection determination step, when the streamline and the first movement path intersect, the position of the intersection of the streamline and the movement path is specified.
The second movement route acquisition step is a route generation method for determining the position of a confluence where the second movement path and the streamline meet, based on the position of the intersection . A route generation program for causing a computer to execute all the steps in the route generation method according to claim 7 . An environmental information acquisition unit that acquires environmental information indicating the position of an object existing around an autonomous moving body and the speed of the object, and
Based on the environmental information, a streamline generator that generates a streamline indicating the flow of movement of the object, and a streamline generator.
The first movement route acquisition unit that acquires the first movement route, which is the movement route to the destination of the autonomous moving body, and the first movement route acquisition unit.
An intersection determination unit that determines whether the streamline and the first movement path intersect,
When the intersection determination unit determines that the streamline and the first movement path intersect, a second movement path modified from the first movement path is generated, and the second movement path is used as the movement path of the autonomous mobile body. The second movement route acquisition unit set as
Equipped with
The streamline generation unit calculates the potential at the entrance / exit based on the environmental information including the entrance / exit, and uses the potential to generate a vector field indicating the flow velocity of the movement of the object at the entrance / exit, and the vector field. A route generation system that generates the streamline based on the above.

Images