WO2022102078A1 - 経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体 - Google Patents

経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体 Download PDF

Info

Publication number
WO2022102078A1
WO2022102078A1 PCT/JP2020/042433 JP2020042433W WO2022102078A1 WO 2022102078 A1 WO2022102078 A1 WO 2022102078A1 JP 2020042433 W JP2020042433 W JP 2020042433W WO 2022102078 A1 WO2022102078 A1 WO 2022102078A1
Authority
WO
WIPO (PCT)
Prior art keywords
streamline
movement
route
movement route
moving body
Prior art date
Application number
PCT/JP2020/042433
Other languages
English (en)
French (fr)
Inventor
航平 廣松
Original Assignee
三菱電機株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 三菱電機株式会社 filed Critical 三菱電機株式会社
Priority to PCT/JP2020/042433 priority Critical patent/WO2022102078A1/ja
Priority to JP2021532872A priority patent/JP7004475B1/ja
Publication of WO2022102078A1 publication Critical patent/WO2022102078A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions

Definitions

  • the present disclosure relates to a route generation system, a route generation method, a route generation program, and an autonomous mobile body.
  • 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 to move a human. It controls autonomous moving objects along the flow.
  • 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 object 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 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 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.
  • FIG. It is a block diagram which shows the structure of the autonomous moving 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 moving body 1000 which concerns on Embodiment 1.
  • FIG. 5 is a conceptual diagram showing a specific example of an operation in which the streamline generation unit 320 according to the first embodiment calculates the density of an object on the streamline.
  • FIG. 3 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.
  • 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. 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. 1 is a configuration diagram showing a configuration of an autonomous moving body 1000 according to the first embodiment.
  • the autonomous moving 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.
  • the autonomous moving body 1000 may be a moving body such as an electric wheelchair or personal mobility that carries a person and moves, or a moving 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.
  • 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 moving body 1000 and estimates the self-position of the autonomous moving body 1000.
  • 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.
  • moving objects such as people and bicycles
  • non-moving objects such as buildings
  • 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 moving body 1000 to reach the destination, and sets the generated movement route as the moving route of the autonomous moving 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 route generated by the route generation system 300, and outputs a control signal indicating the control amount.
  • 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 uses the route tracking information and the range within the range that can be moved to the next control step. It determines the ideal target position.
  • the path tracking calculation unit 410 determines the path tracking information and the ideal target position based on the constraints of the autonomous moving 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.
  • 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.
  • a collision risk map for example, a well-known technique such as a potential map may be used.
  • 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 moving body 1000 based on the control amount indicated by the control signal.
  • the drive system 500 has a drive unit 510 composed of a motor, a motor driver, gears, wheels, etc., and a feedback unit 520 that observes the state of the drive unit 510 and feeds back to the travel control system 400.
  • 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.
  • a three-dimensional LiDAR Light Detection Ranking
  • the satellite signal receiving unit 220 receives a positioning signal from a GNSS (Global Navigation Satellite 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.
  • GNSS Global Navigation Satellite System
  • GPS Global Positioning System
  • 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.
  • the fixed object map is map information indicating the position of a fixed object among the objects.
  • 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.
  • 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.
  • 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.
  • 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). , The identification is performed based on the information such as the position and the grid shape, and the current velocity vector (velocity magnitude, direction) of the identified moving object is calculated from the position change amount and the 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.
  • SLAM Simultaneus Localization and Mapping
  • 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.
  • an ICP Intelligent Closest Point
  • NDT Normal Distributions Transfer
  • 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.
  • 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.
  • 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.
  • the superimposed map is also treated as a kind of environmental information.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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 moving 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.
  • the first movement route is a movement route as an initial setting in the process of setting the movement route of the autonomous moving body 1000.
  • the intersection determination unit 340 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 moving 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 moving body 1000 as it is.
  • the second movement route acquisition unit 332 merges at least a part of the second movement route with the streamline.
  • 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 route 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.
  • 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.
  • 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. decide.
  • 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. Determine the position of the point.
  • 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.
  • the route generation system 300 is configured.
  • 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.
  • 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
  • GNSS receiver 10087 GNSS receiver 10087
  • rotary encoder 10009 liquid crystal display 10010
  • touch panel 10011 touch panel 10011
  • 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.
  • 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.
  • 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.
  • the autonomous moving body 1000 according to the first embodiment is configured.
  • FIG. 6 is a flowchart showing the operation of the autonomous moving body 1000 according to the first embodiment.
  • the operation of the route generation system 300 corresponds to the route generation method
  • the program that causes the computer to execute the operation of the route generation system 300 corresponds to the route generation program.
  • “part” may be appropriately read as "process”.
  • step S1 the map generation system 200 generates environmental information indicating the positions of objects around the autonomous moving body 1000.
  • 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.
  • step S3 the movement route acquisition unit 330 acquires the movement route of the autonomous moving body 1000 based on the streamline generated in step S2, and sets the acquired movement route as the movement route of the autonomous moving body 1000. ..
  • step S4 the travel control system 400 calculates the control amount of the autonomous moving body 1000 based on the movement route set in step S3.
  • step S5 the drive system 500 drives the autonomous moving body 1000 based on the control amount calculated in step S4.
  • FIG. 7 is a flowchart for explaining the details of the streamline generation process.
  • 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.
  • 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.
  • 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
  • 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 the grid points sandwiched between fixed objects, and the size of the composite vector is divided by the number of composited 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.
  • 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 entrance / exit 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.
  • Equation 1 the potential is expressed by a two-dimensional normal distribution.
  • 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.
  • ⁇ p is a minute displacement of the two-dimensional position p
  • 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.
  • 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 the grid with the vector field.
  • the grid is surrounded by four grid points (x i + 1, y j + 1 ), (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 + 1 ).
  • (A i, j , bi , j ) is calculated by the formula 3.
  • Equation 3 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 ), in Equation 3, the vector (vi + 1, j) , Wi + 1, j ) has the largest coefficient ki + 1, j so that the action is the largest, and the coefficient k i, j + 1 has the largest value so that the action of the vector (vi , j + 1 , wi , j + 1 ) is the smallest. It turns out that it is a small value.
  • 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.
  • the streamline SL1 (movement locus of fluid particles) is expressed as a waypoint composed of a plurality of waypoints.
  • FIGS. 13 to 15 schematically explain 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. 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).
  • the movement locus SL2 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. You can.
  • the example of the vector field this time shows a square area
  • a rectangular or polygonal area may be used.
  • 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.
  • 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.
  • step S204 the streamline generation unit 320 calculates the object density on the streamline based on the environmental information.
  • the streamline generation unit 320 calculates the object density on the streamline based on the environmental information.
  • 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 fluid particles in step S204.
  • the position p (T) is in the grid G k, l having (x k , y l ) as the lower left grid point.
  • the vector from the position p (T) to the center position of the grid G k, l-1 is rk , l-1
  • is calculated by the following formula 8.
  • ) is a function that is positive and monotonically increases at
  • )
  • 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.
  • 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.
  • the information regarding the flow of the object is represented by the combination of the streamlines Li and j and the linear density graph.
  • the streamline generation unit 320 ends the streamline generation process.
  • FIG. 17 is a flowchart for explaining the details of the movement route acquisition process.
  • step S301 the first movement route acquisition unit 331 determines whether 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.
  • 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.
  • the first movement route acquisition unit 331 acquires the already set movement route as the first movement route, and proceeds to step S304.
  • the process proceeds 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.
  • the first movement route acquisition unit 331 stores the first movement route generated in step S303.
  • 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. ..
  • 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.
  • 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 waypoints p 1 (i) and p 1 (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 2 (j) and p 2 (j + 1) is calculated using the following formulas 11 to 15.
  • the second movement route acquisition unit 332 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.
  • step S304 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.
  • 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.
  • 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 moving body 1000, and ends the operation.
  • 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.
  • 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 moving body 1000. The difference is that the current position is Pego.
  • 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.
  • 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.
  • the autonomous moving 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 if a person makes a sharp turn 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.
  • the confluence path CR1 is drawn from the current position Pego of the autonomous moving body 1000.
  • 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 moving body 1000, it comes into contact with the fixed object OB12. Therefore, the waypoint p 1 (i) of the first movement path TR1 is brought closer to the intersection IP1 one by one with p 1 ( ⁇ ) and p 1 ( ⁇ + 1) to avoid the fixed object OB12 and the autonomous moving body 1000. Search for a route that satisfies the constraint conditions of.
  • 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 moving body 1000. You may.
  • 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. 19.
  • 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 2 (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.
  • km, ke, and k d are weights for each cost.
  • the waypoint p2 (T + 2 ) has the lowest total cost, so this point is designated as the confluence point CP1.
  • 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
  • , ⁇ (t)) from the confluence to the departure point is generally monotonous with respect to the magnitude of velocity
  • the departure point is arranged at the waypoint of the streamline at the place where the distance le (
  • 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.
  • the return route is a part of the second movement route.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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 p1 ( ⁇ ') 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. The correction process of is finished, and the process moves to 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.
  • 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.
  • the second movement route acquisition unit 332 ends the second movement route generation process.
  • the route generation system 300 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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. 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.
  • 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.
  • 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.
  • step S2 is performed before the route acquisition process in step S3, but the streamline generation is performed by the time 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.
  • the map generation system 200 and the route generation system 300 are configured to be included inside the autonomous moving body 1000, but they do not necessarily have to be provided inside the autonomous moving body 1000, but are outside the autonomous moving body 1000 and are autonomous. Information may be transmitted and received by means such as communication with the mobile body 1000.
  • 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 the vector field outside the detection range is stored as the extrapolation method.
  • the vector field under the same conditions in the past 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.
  • 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
  • FIG. 24 is a configuration diagram showing the configuration of the route generation system 2300 according to the second embodiment.
  • step S501 the streamline generation unit 2320 classifies moving objects according to the direction of the velocity vector of the moving objects and creates a plurality of clusters C (i).
  • the cluster C (i) is assumed to include at least two or more moving objects.
  • 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.
  • 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).
  • 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.
  • 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.
  • 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).
  • step S501 the streamline generation unit 2320 uses a moving object map MAP (float, new) to 0 ° ⁇ ⁇ ⁇ 90 °, 90 ° ⁇ ⁇ ⁇ 180 °, 180 ° ⁇ ⁇ ⁇ 270.
  • MAP moving object map
  • a method of classifying by a total of four angle ranges of °, 270 ° ⁇ ⁇ ⁇ 360 ° is shown.
  • 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 an angle range of 270 ° ⁇ ⁇ ⁇ 360 ° is not created because the number of moving objects to which it belongs is one.
  • 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.
  • 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 calculation for a moving object of cluster C (2) in steps S502 and S503.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • the streamline generation unit 2320 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.

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

より適切に自律移動体の移動経路を設定することができる経路生成システムを得る。　本開示に係る経路生成システムは、自律移動体の周囲に存在する物体の位置と物体の速度とを示す環境情報を取得する環境情報取得部と、環境情報に基づき、物体の移動の流れを示す流線を生成する流線生成部と、自律移動体の目的地までの移動経路である第一移動経路を取得する第一移動経路取得部と、流線と第一移動経路が交差するかを判定する交差判定部と、交差判定部が流線と第一移動経路が交差すると判定した場合、第一移動経路を修正した第二移動経路を生成し、第二移動経路を自律移動体の移動経路として設定する第二移動経路取得部と、を備えた。

Description

経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体
　本開示は、経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体に関する。
　人間が混雑状況を移動する場合、他の歩行者との衝突を避けるため、また、目的地まで早く到達するために、進行方向が同じである他の複数の歩行者が形成する流れに合わせて移動することが多く、人間と共存する環境で移動する車両やロボット等の自律移動体もまた、人間の流れに沿って移動することが望ましい。
　例えば、特許文献１に記載の技術は、自律移動体の周辺に存在する複数の移動物体を一塊の集団とみなし、その集団の速度ベクトルと同じ方向に自律移動体を移動させることにより、人の流れに沿った自律移動体の制御を行っている。
特開２０１９－１４４６１２号公報
　しかしながら、特許文献１では一塊の流れの現在の状態だけから走行選択の判断をしているため、現在の進行方向は遮られていないが、将来的に自律移動体の進行方向を一塊の領域が遮る場合には、自律移動体と一塊の集団が接近し、自律移動体の進行方向を一塊の集団が遮るまで移動物体の流れに対する行動をとらず、経路変更の判断が遅れて、走行停止や急な方向転換をしてしまう可能性がある。 
　本開示は、上記のような課題を解決するためになされたものであり、より適切に自律移動体の移動経路を設定することができる経路生成システムを得ることを目的とする。
　本開示に係る経路生成システムは、自律移動体の周囲に存在する物体の位置と物体の速度とを示す環境情報を取得する環境情報取得部と、環境情報に基づき、物体の移動の流れを示す流線を生成する流線生成部と、自律移動体の目的地までの移動経路である第一移動経路を取得する第一移動経路取得部と、流線と第一移動経路が交差するかを判定する交差判定部と、交差判定部が流線と第一移動経路が交差すると判定した場合、第一移動経路を修正した第二移動経路を生成し、第二移動経路を自律移動体の移動経路として設定する第二移動経路取得部と、を備えた。
　本開示に係る経路生成システムは、流線と第一移動経路が交差するかを判定する交差判定部と、交差判定部が流線と第一移動経路が交差すると判定した場合、第一移動経路を修正した第二移動経路を生成し、第二移動経路を自律移動体の移動経路として設定する第二移動経路取得部と、を備えたので、自律移動体の移動経路が物体の流線と交差する場合に、事前に移動経路を修正することにより、より適切に自律移動体の移動経路を設定することができる。
実施の形態１に係る自律移動体１０００の構成を示す構成図である。 実施の形態１に係る地図生成システム２００の構成を示す構成図である。 実施の形態１に係る経路生成システム３００の構成を示す構成図である。 実施の形態１に係る自律移動体１０００のハードウェア構成を示す構成図である。 実施の形態１に係る自律移動体１０００の動作を示すフローチャートである。 実施の形態１に係る地図生成システム２００が地図情報を生成する動作の具体例を示す概念図である。 実施の形態１に係る経路生成システム３００が流線を生成する動作の詳細を示すフローチャートである。 実施の形態１に係る流線生成部３２０がベクトル場を生成する動作の具体例を示す概念図である。 実施の形態１に係る流線生成部３２０がベクトル場を生成する動作の具体例を示す概念図である。 実施の形態１に係る流線生成部３２０がベクトル場を生成する動作の具体例を示す概念図である。 実施の形態１に係る流線生成部３２０がベクトル場を生成する動作の具体例を示す概念図である。 実施の形態１に係る流線生成部３２０が流線を生成する動作の具体例を示す概念図である。 実施の形態１に係る流線生成部３２０がベクトル場を生成する動作の具体例を示す概念図である。 実施の形態１に係る流線生成部３２０がベクトル場を生成する動作の具体例を示す概念図である。 実施の形態１に係る流線生成部３２０がベクトル場を生成する動作の具体例を示す概念図である。 実施の形態１に係る流線生成部３２０が流線上の物体の密度を算出する動作の具体例を示す概念図である。 実施の形態１に係る経路生成システム３００が移動経路を生成する動作の詳細を示す概念図である。 実施の形態１に係る第二移動経路取得部３３２が第二移動経路を生成する動作の詳細を示すフローチャートである。 実施の形態１に係る第二移動経路取得部３３２が第二移動経路を生成する動作の具体例を示す概念図である。 実施の形態１に係る第二移動経路取得部３３２が合流点を特定する際に用いるコスト関数の具体例を示す概念図である。 実施の形態１に係る第二移動経路取得部３３２が第二移動経路を生成する動作の具体例を示す概念図である。 実施の形態１に係る第二移動経路取得部３３２が第二移動経路を生成する動作の具体例を示す概念図である。 実施の形態２に係る自律移動体２０００の構成を示す構成図である。 実施の形態２に係る経路生成システム２３００の構成を示す構成図である。 実施の形態２に係る経路生成システム３００が流線を生成する動作の詳細を示すフローチャートである。 実施の形態２に係る流線生成部２３２０の動作の具体例を示す概念図である。 実施の形態２に係る流線生成部２３２０の動作の具体例を示す概念図である。 実施の形態２に係る流線生成部２３２０の動作の具体例を示す概念図である。 実施の形態２に係る流線生成部２３２０の動作の具体例を示す概念図である。
　実施の形態１．
　図１は、実施の形態１に係る自律移動体１０００の構成を示す構成図である。自律移動体１０００は、インターフェースシステム１００、地図生成システム２００、経路生成システム３００、走行制御システム４００、及び駆動システム５００を備える。ここで、自律移動体１０００は、電動車椅子やパーソナルモビリティのような人を載せて移動する移動体であっても、作業用ロボットのような無人で移動する移動体であってもよい。以下では、パーソナルモビリティを例として説明する。
　インターフェースシステム１００は、外部から各種情報を入力したり、搭乗者に各種情報を提示したりするものである。より具体的には、インターフェースシステム１００は、管制システム等の無線による設定や状態管理を受け付けたり、各種操作により直接人間から設定を受け付けたりすることにより各種情報を取得する。
　実施の形態１において、インターフェースシステム１００は、自律移動体１０００が移動する領域内に存在する物体や出入口の位置等を含む環境情報を受信したり、自己位置情報を他の自律移動体に送信したりする通信部１１０、後述する地図生成システム２００が生成した環境地図や経路生成システムが生成した移動経路を表示する表示部１２０、及びユーザの操作により各種情報を受け付ける受付部１３０を備える。
　地図生成システム２００は、自律移動体１０００周囲の環境地図を生成するとともに、自律移動体１０００の自己位置の推定を行うものである。ここで、環境地図とは、環境情報の一種であり、環境情報とは、自律移動体の周囲に存在する物体の位置及び物体の速度を示すものである。また、以下では、物体のうち、人や自転車等の移動する物体を移動物体、建築物等の移動しない物体を固定物体と呼ぶこととする。
　地図生成システム２００の詳細については後述する。
　経路生成システム３００は、自律移動体１０００が目的地に到達するための移動経路を生成し、生成した移動経路を自律移動体１０００の移動経路として設定するものであり、詳細については後述する。
　走行制御システム４００は、経路生成システム３００が生成した移動経路に基づき、自律移動体１０００の制御量を算出し、当該制御量を示す制御信号を出力するものである。
　実施の形態１において、走行制御システム４００は、経路追従演算部４１０、衝突回避判断部４２０、及び移動制御部４３０を備える。
　経路追従演算部４１０は、経路生成システム３００から受信した移動経路と地図生成システム２００から受信した自己位置の偏差をもとに、経路追従情報と、次の制御ステップまでに移動可能な範囲内の理想目標位置を決定するものである。ここで、経路追従演算部４１０は、受信した移動経路と自己位置の偏差に加え、自律移動体１０００が有する制約に基づいて、経路追従情報と理想目標位置を決定する。
　衝突回避判断部４２０は、地図生成システム２００から受信した環境地図が含む移動物体と固定物体の位置と、経路追従演算部４１０が算出した理想目標位置から衝突のリスクを演算し、リスクの大きさに応じて理想目標位置を修正した制御目標位置を決定するものである。
　より具体的には、衝突回避判断部４２０は、経路追従演算部４１０が算出した理想目標位置と、環境地図から作成される衝突リスク地図を比較し、衝突回避判断を行う。衝突リスク地図は、例えば、ポテンシャルマップ等の周知技術を用いればよい。ここで、衝突回避判断部４２０は、衝突リスクが所定の閾値以下である場合は、制御目標位置を理想目標位置とし、閾値を超える場合は、衝突リスク地図から自律移動体１０００が有する制約を考慮した中で最もリスクが小さくなる位置を制御目標位置にする。
　移動制御部４３０は、衝突回避判断部４２０が算出した制御目標位置と駆動システム５００からのフィードバックに基づき、自律移動体１０００の制御量を算出し、制御信号を駆動システム５００に送信するものである。また、移動制御部４３０は、自律移動体１０００の制御量を算出するために、駆動システム５００のギヤ比等のパラメータを記憶する。
　駆動システム５００は、走行制御システム４００から制御信号を受信し、制御信号が示す制御量に基づき自律移動体１０００を駆動するものである。
　より具体的には、駆動システム５００は、モータやモータドライバ、歯車、車輪などで構成される駆動部５１０と、駆動部５１０の状態を観測し、走行制御システム４００にフィードバックするフィードバック部５２０を有する
　地図生成システム２００の詳細について、図２を用いて説明する。図２は、実施の形態１に係る地図生成システム２００の構成を示す構成図である。
　地図生成システム２００は、外界検知部２１０、衛星信号受信部２２０、環境地図生成部２３０及び自己位置推定部２４０を備える。
　外界検知部２１０は、自律移動体１０００周囲の物体を検知するものであり、例えば、点群データを取得する３次元ＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　Ｒａｎｇｉｎｇ）や画像データを取得するカメラ等が用いられる。
　衛星信号受信部２２０は、ＧＰＳ（Ｇｌｏｂａｌ　Ｐｏｓｉｔｉｏｎｉｎｇ　Ｓｙｓｔｅｍ）衛星や準天頂衛星等のＧＮＳＳ（Ｇｌｏｂａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓａｔｅｌｌｉｔｅ　Ｓｙｓｔｅｍ）衛星から測位信号を受信し、自律移動体１０００の緯度、経度、標高といった地球上の３次元位置を取得するものである。
　環境地図生成部２３０は、自律移動体１０００周囲の環境地図を生成するものであり、周囲物体位置演算部２３１、固定物体地図生成部２３２、及び移動物体地図生成部２３３を備える。
　周囲物体位置演算部２３１は、外界検知部２１０から取得したセンサーデータを用いて、自律移動体１０００周囲の物体の位置を特定し、物体の位置を示す物体位置情報を固定物体地図生成部２３２と物体位置マッチング部２４１に出力するものである。
　固定物体地図生成部２３２は、周囲物体位置演算部２３１から取得した物体位置情報と、後述する自己位置推定部２４０により推定された自己位置に基づいて、固定物体地図を生成するものである。ここで、固定物体地図とは、物体のうち、固定物体の位置を示す地図情報である。また、固定物体地図生成部２３２は、固定物体地図情報を生成する際、上記の情報に加え、インターフェースシステム１００から取得した環境情報を用いてもよい。
　また、固定物体地図生成部２３２は、過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）を記憶している場合、新規に取得した環境情報を用いて固定物体地図を更新する。図３を用いて固定物体地図の更新の具体例について説明する。
　固定物体地図生成部２３２は、周囲物体位置演算部２３１が算出した物体位置、自己位置推定部２４０が算出した自己位置、過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）、及びインターフェースシステム１００を介して設定された固定物体位置のデータを入力し、図３ａに示す過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）、新規に取得した物体位置とインターフェースシステム１００を介して設定された固定物体位置を、自己位置に基づいて、図３ｂのように１つの地図上に配置する。
　次に、図３ｃのように過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）が既に地図に含んでいる領域において、過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）には存在するが、周囲物体位置演算部２３１が算出した物体位置には存在しない物体（図３ｃの（ａ＋２，ｂ＋３）地点）を削除し、過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）には存在しないが、周囲物体位置演算部２３１が算出した物体位置には存在する物体（図３ｃの（ａ＋３，ｂ＋２）地点）を移動体ＯＢ１と判定する。
　また、過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）と新たに取得した物体位置の両方に存在する物体と、過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）が未だ地図に含んでいない領域に存在する物体は固定物体と判定する。以上の判定結果から過去の固定物体地図を更新して最新の固定物体地図ＭＡＰ（ｆｉｘ，ｎｅｗ）と移動物体位置のデータを出力する。
　移動物体地図生成部２３３は、移動物体地図を生成するものである。より具体的には、移動物体地図生成部２３３は、移動物体の追跡を行い、移動物体の速度ベクトルを算出することにより、移動物体地図を生成する。
　移動物体地図生成部２３３は、固定物体地図生成部２３２から入力した現在の移動物体位置と、記憶している過去の移動物体地図ＭＡＰ（ｆｌｏａｔ，ｏｌｄ）とに基づき、現在と過去の移動物体について、位置やグリッド形状などの情報をもとに同定を行い、その位置変化量と制御周期から、同定した移動物体の現在の速度ベクトル（速度の大きさ、方向）を演算する。移動物体地図生成部２３３は、以上の演算結果から求まる個々の移動物体に関する現在の位置、速度ベクトルやグリッド形状を含む移動物体地図ＭＡＰ（ｆｌｏａｔ，ｎｅｗ）を生成し、経路生成システム３０００に出力する。
　自己位置推定部２４０は、衛星信号受信部２２０による３次元位置情報やＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　Ｍａｐｐｉｎｇ）技術により自律移動体１０００の自己位置の演算を行うものであり、物体位置マッチング部２４１及び位置融合演算部２４２を備える。
　物体位置マッチング部２４１は、周囲物体位置演算部２３１から取得した物体位置情報と、固定物体地図生成部２３２が記憶している過去の固定物体地図に基づいて、位置や形状を手掛かりとして物体位置と過去の固定物体地図の配置の誤差を最小化するマッチングを行うものである。このマッチングを行う手法として、例えば点群ではＩＣＰ（Ｉｔｅｒａｔｉｖｅ　Ｃｌｏｓｅｓｔ　Ｐｏｉｎｔ）アルゴリズムやＮＤＴ（Ｎｏｒｍａｌ　Ｄｉｓｔｒｉｂｕｔｉｏｎｓ　Ｔｒａｎｓｆｏｒｍ）アルゴリズムが知られている。過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）の座標系に物体位置を合わせることで過去の固定物体地図ＭＡＰ（ｆｉｘ，ｏｌｄ）における自律移動体１０００の自己位置のデータを出力することができる。
　位置融合演算部２４２は、外界検知部２１０に由来する物体位置マッチング部２４１により算出された自己位置と、衛星信号受信部２２０により算出された自己位置とに基づいて、自己位置の最終結果を出力するものである。外界検知部２１０に由来する自己位置は周囲の物体が疎な環境では誤差が大きくなる傾向にあり、一方、衛星信号受信部２２０による自己位置は屋内環境では誤差が大きくなる、またはそもそも自己位置が取得できない可能性がある。そこで、位置融合演算部２４２は、一方の自己位置が取得出来ない場合は、もう一方の自己位置を自己位置の最終結果として出力し、両方の自己位置が取得出来る場合は、カルマンフィルタ等のフィルタ処理を用いて両方の自己位置に基づき、自己位置の最終演算結果を出力する。
　以上のように、地図生成システム２００は構成され、次に、経路生成システム３００の詳細について、図４を用いて説明する。図４は、実施の形態１に係る経路生成システム３００の構成を示す構成図である。
　経路生成システム３００は、環境情報取得部３１０、流線生成部３２０、移動経路取得部３３０、及び交差判定部３４０を備える。
　環境情報取得部３１０は、自律移動体１０００の周囲に存在する物体の位置及び物体の速度を示す環境情報を取得するものである。
　環境情報取得部３１０は、地図生成システム２００から最新の固定物体地図ＭＡＰ（ｆｉｘ，ｎｅｗ）及び最新の移動物体地図ＭＡＰ（ｆｌｏａｔ，ｎｅｗ）を環境情報として取得し、それらを重畳して重畳地図を生成する。以下では、重畳地図も環境情報の一種として扱う。また、環境情報とは、上記したようなグリッド状の地図情報に限らず、物体の位置と速度を示すものであれば良い。
　また、環境情報が示す物体の速度については、移動物体地図のようにあらわに速度を示す場合に限らず、固定物体地図のように物体が固定物体であるという情報が付与されていることにより、間接的に速度がゼロであることを示すものを含む。
　また、上記において、地図生成システム２００は、固定物体地図と移動物体地図とをばらばらに出力し、環境情報取得部３１０がそれらを重畳し、重畳地図を生成するようにしたが、地図生成システム２００が重畳地図を生成し、環境情報取得部３１０は、地図生成システム２００が生成した重畳地図を環境情報として取得するようにしてもよい。
　流線生成部３２０は、環境情報に基づき、物体の移動の流れを示す流線を生成するものである。ここで、流線とは流体力学における厳密な意味での流線に限らず、流跡線や流脈線等、その他物体の移動の流れを示す曲線または直線を含むものとする。すなわち、ここでの流線とは、複数の物体を巨視的に見たときの移動経路を意味する。
　移動経路取得部３３０は、自律移動体１０００が移動する移動経路を取得するものであり、第一移動経路取得部３３１及び第二移動経路取得部３３２を備える。
　第一移動経路取得部３３１は、自律移動体の目的地までの移動経路である第一移動経路を取得するものである。ここで、第一移動経路とは、自律移動体１０００の移動経路を設定する処理における初期設定としての移動経路である。
　第二移動経路取得部３３２は、後述する交差判定部３４０が流線と第一移動経路が交差すると判定した場合、第一移動経路を修正した第二移動経路を生成し、第二移動経路を自律移動体１０００の移動経路として設定するものである。また、第二移動経路取得部３３２は、交差判定部３４０が流線と第一移動経路が交差しないと判定した場合、第一移動経路をそのまま自律移動体１０００の移動経路として設定する。
　また、実施の形態１において、第二移動経路取得部３３２は、交差判定部３４０が流線と第一移動経路が交差すると判定した場合、第二移動経路の少なくとも一部が流線に合流するように、第一移動経路を修正することにより第二移動経路を生成する。
　交差判定部３４０は、流線生成部３２０が生成した流線と第一移動経路取得部３３１が取得した第一移動経路が交差するかを判定するものである。交差判定部３４０は、流線と第一移動経路が交差するか否かの判定結果を第二移動経路取得部３３２に出力する。
　また、実施の形態１において、交差判定部３４０は、流線と第一移動経路が交差する場合、流線と移動経路の交差点の位置を特定し、第二移動経路取得部３３２は、交差点の位置に基づき、第二移動経路と流線が合流する合流点の位置を決定する。
　また、実施の形態１において、第二移動経路取得部３３２は、合流点に加え、自律移動体１０００が流線から離脱する離脱点の位置と、第一移動経路に復帰する復帰点の位置を決定する。
　また、実施の形態１において、流線生成部３２０は、環境情報に基づき、流線上の物体の密度を算出し、第二移動経路取得部３３２は、交差点の位置と物体の密度に基づき、合流点の位置を決定する。
　また、実施の形態１において、流線生成部３２０は、環境情報が示す物体の位置と物体の速度とに基づき、物体の移動の流れの流速を示すベクトル場を生成し、ベクトル場に基づき流線を生成する。
　以上のように、経路生成システム３００は構成される。
　次に、実施の形態１に係る自律移動体１０００のハードウェア構成について説明する。
　図５は、実施の形態１に係る自律移動体１０００のハードウェア構成図である。
　図５に示したハードウェアには、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）等の処理装置１０００１、ＲＯＭ（Ｒｅａｄ　ＯｎｌｙＭｅｍｏｒｙ）やハードディスク等の記憶装置１０００２、センサーインターフェース１０００３、表示インターフェース１０００４、設定インターフェース１０００５、アクチュエータインターフェース１０００６、外界センサー１０００７、ＧＮＳＳ受信機１０００８、ロータリエンコーダ１０００９、液晶ディスプレイ１００１０、タッチパネル１００１１、駆動装置１００１２を備える。
　図１に示した地図生成システム２００、経路生成システム３００、及び走行制御システム４００の各機能は、記憶装置１０００２に記憶されたプログラムが処理装置１０００１で実行されることにより実現される。また、各機能を実現する方法は、上記したハードウェアとプログラムの組み合わせに限らず、処理装置にプログラムをインプリメントしたＬＳＩ（Ｌａｒｇｅ Ｓｃａｌｅ ＩｎｔｅｇｒａｔｅｄＣｉｒｃｕｉｔ）のような、ハードウェア単体で実現するようにしてもよいし、一部の機能を専用のハードウェアで実現し、一部を処理装置とプログラムの組み合わせで実現するようにしてもよい。
　また、地図生成システム２００の外界検知部２１０は、外界センサー１０００７により実現され、衛星信号受信部２２０は、ＧＮＳＳ受信機１０００８により実現される。また、インターフェースシステム１００の表示部１２０は液晶ディスプレイ１００１０及びタッチパネル１００１１により実現され、受付部１３０は、タッチパネル１００１１により実現され、駆動システム５００の駆動部５１０は、駆動装置１００１２により実現され、フィードバック部５２０はロータリエンコーダ１０００９により実現される。
　以上のように、実施の形態１に係る自律移動体１０００は構成される。
　次に、実施の形態１に係る自律移動体１０００の動作について説明する。
　図６は、実施の形態１に係る自律移動体１０００の動作を示すフローチャートである。
　ここで、経路生成システム３００の動作が経路生成方法に対応し、経路生成システム３００の動作をコンピュータに実行させるプログラムが経路生成プログラムに対応する。また、「部」は「工程」に適宜読み替えてもよい。
　まず、ステップＳ１において、地図生成システム２００は、自律移動体１０００周囲の物体の位置を示す環境情報を生成する。
　次に、ステップＳ２において、環境情報取得部３１０は、ステップＳ１で生成された環境情報を取得し、取得した環境情報に基づいて、流線生成部３２０が、物体の流れを示す流線を生成する。
　次に、ステップＳ３において、移動経路取得部３３０は、ステップＳ２で生成した流線に基づき、自律移動体１０００の移動経路を取得し、取得した移動経路を自律移動体１０００の移動経路として設定する。
　次に、ステップＳ４において、走行制御システム４００は、ステップＳ３で設定された移動経路に基づき自律移動体１０００の制御量を算出する。
　最後に、ステップＳ５において、駆動システム５００は、ステップＳ４で算出した制御量に基づき、自律移動体１０００を駆動する。
　ここで、ステップＳ２の流線生成処理の詳細について図７を用いて説明する。図７は、流線生成処理の詳細を説明するためのフローチャートである。
　まず、ステップＳ２０１において、環境情報取得部３１０は、地図生成システム２００から固定物体地図ＭＡＰ（ｆｉｘ，ｎｅｗ）及び移動物体地図ＭＡＰ（ｆｌｏａｔ，ｎｅｗ）を取得し、重畳地図を生成することにより環境情報を取得する。
　次に、ステップＳ２０２において、流線生成部３２０は、環境情報から物体の流れの流速を示すベクトル場を生成する。より具体的には、流線生成部３２０は、移動物体、固定物体、出入口とそれぞれ異なる属性の地図情報について、それぞれの属性に応じた方法でグリッドの格子点状にベクトルを生成し、グリッドマップの全格子点についてベクトルを生成したものをベクトル場として出力する。
　流線生成部３２０がベクトル場を生成する処理の具体例について、図８を用いて説明する。
　図８はグリッド上に表現された移動物体ＯＢ２、固定物体ＯＢ３から格子点上にベクトルを作成する方法を示しており、流線生成部３２０は、移動物体ＯＢ２は保有する速度ベクトルを移動物体ＯＢ２が入るグリッドの４隅に生成する。また、流線生成部３２０は、固定物体ＯＢ３について、斥力の表現として、固定物体ＯＢ３が入るグリッドの４隅から放射状にベクトルを生成する。
　図９は移動物体ＯＢ４と移動物体ＯＢ５、及び固定物体ＯＢ６と固定物体ＯＢ７がそれぞれ隣接するグリッドに存在する場合のベクトルを生成する方法を示しており、流線生成部３２０は、移動物体、または、固定物体同士に挟まれる格子点ではベクトルを合成し、合成したベクトルの本数で合成ベクトルの大きさを割ったものをその格子点のベクトルとして生成する。
　図１０は移動物体ＯＢ８と移動物体ＯＢ９、及び固定物体ＯＢ１０と固定物体ＯＢ１１がそれぞれ隣接するグリッドに存在し、かつ移動物体ＯＢ８と固定物体ＯＢ１１が隣接するグリッドが存在する場合のベクトルを作成する方法を示しており、移動物体同士、または、固定物体同士、または、移動物体ＯＢ８と固定物体ＯＢ１１の両方に挟まれる格子点ではベクトルを合成し、合成したベクトルの本数で合成ベクトルの大きさを割ったものをその格子点のベクトルとして作成する。
　図１１は出入口ＧＡ１を設定した場合のベクトルを作成する方法を示している。出入口ＧＡ１は両端の点に対して出口か入口かの情報を付与されており、その情報をもとに下記の数式１により２次元位置ｐのポテンシャルｆ（ｐ）を計算する。
Figure JPOXMLDOC01-appb-M000001
　数式１では、ポテンシャルを２次元正規分布で表現しており、出入口が入口のときにｋ＞０、出口のときにｋ＜０とし、両端の点の位置をもとに平均μで出入口の中心位置、共分散Σで出入口の方向を示している。図６ｄの一点鎖線による楕円は等ポテンシャル線を示しており、等ポテンシャル線上では勾配の大きさが等しくなる。勾配∇ｆ（ｐ）は下記の数式２により計算する。
Figure JPOXMLDOC01-appb-M000002
　ここでΔｐは２次元位置ｐの微小変位であり、グリッド幅を目安に設定する。出入口によるベクトル場は、格子点の座標をｐとして、数式２に基づいて算出される勾配ベクトルとして生成される。出入口によるベクトル場は固定物体地図ＭＡＰ（ｆｉｘ，ｎｅｗ）と移動物体地図ＭＡＰ（ｆｌｏａｔ，ｎｅｗ）の範囲で作成された流線を各地図の範囲外（外界センサ２２の検知範囲外）まで外挿する際に使用される。
　図７に戻り、流線生成処理の続きについて説明する。
　ステップＳ２０３において、流線生成部３２０は、ベクトル場から流線を生成する。流線生成部３２０がベクトル場から流線を生成する処理の具体例について、図１２から図１５を用いて説明する。流線生成部３２０は、流体粒子をグリッド上に置き、ベクトル場から力を受け時間発展した結果描かれる移動軌跡を流線として算出する。
　図１２はベクトル場のあるグリッド内での流体粒子ＶＰ１の軌跡の演算を模式的に説明している。
　グリッドは左下点から時計回りに、（ｘｉ，ｙｊ）、（ｘｉ，ｙｊ＋１）、（ｘｉ＋１，ｙｊ＋１）、（ｘｉ＋１，ｙｊ）の４つの格子点で囲まれており、各格子点はそれぞれ（ｖｉ，ｊ，ｗｉ，ｊ）、（ｖｉ，Ｊ＋１，ｗｉ，ｊ＋１）、（ｖｉ＋１，ｊ＋１，ｗｉ＋１，ｊ＋１）、（ｖｉ＋１，ｊ，ｗｉ＋１，ｊ）のベクトルを持つ。なおｘｉ＋１＝ｘｉ＋Δｘ、ｙｉ＋１＝ｙｉ＋Δｙの関係がある。
　流線生成部３２０は、時刻ｔに流体粒子ＶＰ１を位置ｐ（ｔ）＝（ｘ（ｔ），ｙ（ｔ））に置き、次の時刻ｔ＋１の流体粒子ＶＰ１の位置ｐ（ｔ＋１）と速度ｖ（ｔ）を算出する。
　位置ｐ（ｔ）に置かれた流体粒子ＶＰ１が受ける力は各格子点のベクトルに係数を掛けたものの総和であり、格子点（ｘｉ，ｙｊ）のベクトルに掛ける係数ｋｉ，ｊ＝（ａｉ，ｊ，ｂｉ，ｊ）は数式３により計算する。
Figure JPOXMLDOC01-appb-M000003
　図１２において、位置ｐ（ｔ）は格子点（ｘｉ＋１，ｙｊ）に最も近く、格子点（ｘｉ，ｙｊ＋１）から最も離れていることから、数式３では、ベクトル（ｖｉ＋１，ｊ，ｗｉ＋１，ｊ）の作用が最も大きくなるよう係数ｋｉ＋１，ｊは最も大きな値となり、ベクトル（ｖｉ，ｊ＋１，ｗｉ，ｊ＋１）の作用が最も小さくなるよう係数ｋｉ，ｊ＋１は最も小さな値となることが分かる。
　以上を踏まえ、流線生成部３２０は、位置ｐ（ｔ＋１）＝（ｘ（ｔ＋１），ｙ（ｔ＋１））、速度ベクトルｖ（ｔ）＝（ｖｘ（ｔ），ｖｙ（ｔ））を数式４から数式７を用いて計算する。
Figure JPOXMLDOC01-appb-M000004
Figure JPOXMLDOC01-appb-M000005
Figure JPOXMLDOC01-appb-M000006
Figure JPOXMLDOC01-appb-M000007
　図１２においては時刻ｔから時刻ｔ＋２までは流体粒子ＶＰ１が同一グリッド内を移動しているが、時刻ｔ＋３からは流体粒子ＶＰ１が隣接するグリッドに移動しているため、そのグリッドを囲む格子点に対応した数値に数式３から数式５を置き換える必要がある。この計算は流体粒子ＶＰ１がベクトル場の領域外に出る、または、ベクトル場の中での移動量が閾値以下の大きさになるまで繰り返し行う。以上により、流線ＳＬ１（流体粒子の移動軌跡）は複数の経由点からなるウェイポイントとして表現される。
　図１３から図１５は、流線生成部３２０が適切な分布で複数の流線を作成するための流体粒子の配置方法の一例を模式的に説明している。
　図１３は左下点から時計回りに、（ｘ１，ｙ１）、（ｘ１，ｙ６）、（ｘ６，ｙ６）、（ｘ６，ｙ１）の４つの格子点に囲まれた領域を持つｘ軸５グリッド、ｙ軸５グリッドの領域に複数の流体粒子ＶＰ２，ＶＰ３を初期地点に置いたものを示している。なお、図１２と同様にｘｉ＋１＝ｘｉ＋Δｘ、ｙｊ＋１＝ｙｊ＋Δｙ　（ｉ＝１，…，６，ｊ＝１，…，６）の関係がある。
　この初期地点は設定された領域の境界線上の格子点で、かつ、ベクトルが設定された領域の内側を向いている場合に配置される。つまり、境界線ｘ＝ｘ１ではベクトルがｘの正方向の成分を持つ格子点、境界線ｙ＝ｙ１ではベクトルがｙの正方向の成分を持つ格子点、境界線ｘ＝ｘ６ではベクトルがｘの負方向の成分を持つ格子点、境界線ｙ＝ｙ６ではベクトルがｙの負方向の成分を持つ格子点である。
　図１４は配置した流体粒子ＶＰ２のベクトル場の作用による移動軌跡ＳＬ２と、削除された流体粒子ＶＰ３を示している。流体粒子の移動軌跡の計算は図１２で示した通りである。配置した流体粒子ＶＰ３は別の配置した流体粒子ＶＰ２による移動軌跡ＳＬ２が近傍を通過する場合に削除する。この近傍とは、格子点（ｘｉ，ｙｊ）における（ｘｉ－Δｘ／２）≦ｘ≦（ｘｉ＋Δｘ／２）、かつ、（ｙｊ－Δｙ／２）≦ｙ≦（ｙｊ＋Δｙ／２）の範囲である。
　図１５は流体粒子の初期地点の配置を境界線からベクトル場の中心に近い格子点に１グリッド狭めた直線上とし、図１３及び図１４と同様の処理を行った結果を示している。つまりこの例では、流体粒子ＶＰ４を配置する直線はｘ＝ｘ２、ｙ＝ｙ２、ｘ＝ｘ５、ｙ＝ｙ５としている。このとき、既に移動軌跡SL２が引かれているため、あらかじめ流体粒子を配置する条件として、近傍を移動軌跡が通過しないことを加えることで配置した流体粒子を後で削除する手間を省略することが出来る。このように流体粒子を配置する直線を徐々に狭めていき、ベクトル場の中心に到達したところで処理を終えると、ベクトル場の全領域に流線を引くことが可能となる。求めた流線はＬｉ，ｊと符号を付す。ｉ，ｊは流体粒子の初期地点のｘ軸、ｙ軸それぞれのインデックスとしている。
　なお、今回のベクトル場の例では正方形の領域を示しているが、長方形や多角形の領域を用いてもよい。例えば、長方形のグリッドの場合、流体粒子を配置する直線で囲まれる四角形は長辺の方を先に狭め、その四角形が正方形になったときに上記と同様の処理を行えばよい。
　また、上記では、グリッドとして２次元のグリッドを例にとって説明したが、ドローン等上下方向にも移動する自律移動体の経路生成を行う場合には、３次元グリッドを用いて上記と同様の処理を行い、流線を生成すればよい。
　図７に戻り、流線生成処理の続きについて説明する。
　流線生成部３２０は、最後に、ステップＳ２０４において、環境情報に基づき、流線上の物体密度を算出する。図１６を用いて、流線生成部３２０が流線上の物体密度を算出する処理の具体例について説明する。
　図１６は、流線生成部３２０が、ステップＳ２０４において流体粒子の移動軌跡として求まるある１つの流線上の物体の密度を示す線密度分布グラフを生成する処理の具体例を示している。
　図１６ａは時刻ｔ＝Ｔ前後の流体粒子の移動軌跡ＳＬ２（流線Ｌｉ，ｊ）と移動物体地図ＭＡＰ（ｆｌｏａｔ，ｎｅｗ）が示す移動物体の分布を示している。この図１６ａを用いて時刻ｔにおける流体粒子の位置ｐ（Ｔ）＝（ｘ（Ｔ），ｙ（Ｔ））における密度ρ（Ｔ）の計算手順を示す。
　位置ｐ（Ｔ）は（ｘｋ，ｙｌ）を左下の格子点とするグリッドＧｋ，ｌの中にあることがわかる。このグリッド及びその周囲近傍に移動物体が含まれるグリッドは、Ｇｋ，ｌ－１、Ｇｋ＋１，ｌ－１、Ｇｋ＋１，ｌ、Ｇｋ－１，ｌ＋１の４つである。位置ｐ（Ｔ）からグリッドＧｋ，ｌ－１の中心位置へのベクトルをｒｋ，ｌ－１とすると、その大きさ｜ｒｋ，ｌ－１｜は以下の数式８で計算される。
Figure JPOXMLDOC01-appb-M000008
　これは位置ｐ（Ｔ）からグリッドＧｋ，ｌ－１の中心位置までの距離である。この距離を他の移動物体が含まれるグリッドでも同様に計算することで、位置ｐ（Ｔ）における密度ρ（Ｔ）を以下の式で計算する。
Figure JPOXMLDOC01-appb-M000009
　ここで距離の関数ｆ（｜ｒ｜）は、ｆ（｜ｒ｜）＝｜ｒ｜等、｜ｒ｜≧０において、正でありかつ単調に増加する関数とする。つまり位置ｐ（Ｔ）における密度ρ（Ｔ）は近くに移動物体が多くあるほど大きな値を取るようになっている。なお範囲を周囲近傍と区切っているのは計算コストを減らすためであり、処理負荷に余裕がある場合は拡大したり区切りをなくしたりしても距離が遠い移動物体ほど密度ρ（Ｔ）に与える影響は小さくなるため問題はない。
　図１６ｂは流線Ｌｉ，ｊに沿って（ｘｉ，ｙｊ）から移動した距離ｌ（ｔ）を縦軸、密度ρ（ｔ）を横軸に取った線密度グラフを示している。距離ｌ（ｔ）は時刻ｔにおける流線Ｌｉ，ｊに沿って（ｘｉ，ｙｊ）から移動した距離であり、以下の数式１０で計算する。
Figure JPOXMLDOC01-appb-M000010
　なお流線Ｌｉ，ｊの初期地点を（ｘｉ，ｙｊ）＝（ｘ（０），ｙ（０））としている。以上の手順で、流線Ｌｉ，ｊにおいて各時刻ｔにおける（ｌ（ｔ），ρ（ｔ））をプロットすることで流線Ｌｉ，ｊ上の移動物体の密度を表現する線密度グラフが求まる。以上、物体の流れに関する情報は、流線Ｌｉ，ｊと線密度グラフの2つの組合せで表現される。
　以上で、流線生成部３２０は流線生成処理を終了する。
　次に、ステップＳ３の移動経路取得処理の詳細について図１７を用いて説明する。図１７は、移動経路取得処理の詳細を説明するためのフローチャートである。
　まず、ステップＳ３０１において、第一移動経路取得部３３１は、移動経路が既に設定されているかを判定する。ここで、移動経路が既に設定されている場合は、ステップＳ３０２に進み、設定されていない場合は、ステップＳ３０３に進む。
　ステップＳ３０２に進んだ場合、第一移動経路取得部３３１は、地図生成システム２００から固定物体地図ＭＡＰ（ｆｉｘ，ｎｅｗ）を取得し、取得した固定物体地図ＭＡＰ（ｆｉｘ，ｎｅｗ）に変更が無いかを判定する。ここで、変更が無いと判定した場合、第一移動経路取得部３３１は、既に設定されている移動経路を第一移動経路として取得し、ステップＳ３０４に進む。一方、固定物体地図ＭＡＰ（ｆｉｘ，ｎｅｗ）に変更があると判定した場合、ステップＳ３０３に進む。
　ステップＳ３０３に進んだ場合、第一移動経路取得部３３１は、固定物体地図ＭＡＰ（ｆｉｘ，ｎｅｗ）に基づき、現在位置から目的地までの移動経路を算出し、算出した移動経路を第一移動経路として取得する。すなわち、ここでの第一移動経路は、固定物体の位置に基づいて生成された移動経路である。
　また、第一移動経路取得部３３１は、ステップＳ３０３で生成した第一移動経路を記憶する。
　次に、ステップＳ３０４において、交差判定部３４０は、自律移動体１０００が物体の流れに合流済みか否かを判定する。例えば、合流済みか否かの判定は、合流している場合「１」、合流していない場合「０」とするようなフラグを保持しておき、そのフラグの値を判定することによりなされる。
　ここで、交差判定部３４０は、自律移動体１０００が物体の流れに合流済みであると判定した場合、ステップＳ３０５に進み、合流済みでないと判定した場合、ステップＳ３０６に進む。
　ステップＳ３０５に進んだ場合、交差判定部３４０は、第一移動経路と流線が交差するか否かを判定する。交差判定部３４０が、第一移動経路と流線が交差すると判定した場合、ステップＳ３０７に進み、交差しないと判定した場合、第二移動経路取得部３３２は第一移動経路を最終出力としての移動経路として設定し、動作を終了する。
　第一移動経路のｍ個の経由点をｐ１（ｉ）＝（ｘ１（ｉ），ｙ１（ｉ））　（ｉ＝１，…，ｍ）、流線のｎ個の経由点をｐ２（ｊ）＝（ｘ２（ｊ），ｙ２（ｊ））　（ｊ＝１，…，ｎ）としたとき、第一移動経路で隣接する経由点ｐ１（ｉ）とｐ１（ｉ＋１）が結ぶ線分と、流線で隣接する経由点ｐ２（ｊ）とｐ２（ｊ＋１）が結ぶ線分の交差判定は以下の数式１１から数式１５を用いて計算される。
Figure JPOXMLDOC01-appb-M000011
Figure JPOXMLDOC01-appb-M000012
Figure JPOXMLDOC01-appb-M000013
Figure JPOXMLDOC01-appb-M000014
Figure JPOXMLDOC01-appb-M000015
　数式１１から数式１５について、ｉ＝１，…，ｍ－１及びｊ＝１，…，ｎ－１のｉとｊすべての組合せで演算し、判定式である数式１５が１つでもＴｒｕｅのとき、第一移動経路と流線は交差すると判定される。交差点をｐｃ＝（ｘｃ，ｙｃ）としたとき、α１、α２、ｘｃ、ｙｃは数式１６から数式１９により計算される。
Figure JPOXMLDOC01-appb-M000016
Figure JPOXMLDOC01-appb-M000017
Figure JPOXMLDOC01-appb-M000018
Figure JPOXMLDOC01-appb-M000019
　ステップＳ３０７に進んだ場合、第二移動経路取得部３３２は、第一移動経路を修正した第二移動経路を生成し、生成した第二移動経路を最終的な出力としての移動経路として設定し、動作を終了する。より具体的には、第二移動経路取得部３３２は、第一移動経路から一度離れ、流線に合流するための合流点と合流点までの経路である合流経路と、流線から離れるための離脱点と第一移動経路に再び復帰するための復帰点と離脱点から復帰点の間の復帰経路を演算することで、第二移動経路を生成する。
　一方、ステップＳ３０４からステップＳ３０６に進んだ場合、交差判定部３４０は、第一移動経路が合流済みの流線と交差するか判定する。これは、移動物体は人間である場合が多く、流れは時間の経過により変化する可能性があるためである。ここで、交差の判定方法はＳ３０５と同様であり、交差判定部３４０が、第一移動経路が合流済みの流線と交差すると判定した場合、ステップＳ３０８に進み、交差しないと判定した場合、ステップＳ３０９に進む。
　ステップＳ３０８に進んだ場合、第二移動経路取得部３３２は、合流済みの流線に基づき第一移動経路を修正し、第二移動経路を生成する。この処理は図１８に示したフローチャートにおけるステップＳ４０４からステップＳ４０６を合流済みの流線にのみ適用して計算する。そして、第二移動経路取得部３５０は、生成した第二移動経路を自律移動体１０００の移動経路として設定し、動作を終了する。
　ステップＳ３０９に進んだ場合、第二移動経路取得部３５０は、第一移動経路に復帰する復帰経路を第二移動経路の一部として生成する。この処理も図１８に示したフローチャートにおけるステップＳ４０４からステップＳ４０６を合流済みの流線にのみ適用して計算する点はステップＳ３０８と同様であるが、最初に離脱点の候補を自律移動体１０００の現在位置Ｐｅｇｏにする点が異なる。
　以上で経路生成システム３００は経路取得処理を終了するが、ステップＳ３０７で第二移動経路取得部３５０が第二移動経路を生成する処理の更なる詳細について、図１８を用いて説明する。図１８は、第二移動経路取得部３３２が第二移動経路を生成する処理の詳細を説明するためのフローチャートである。
　まず、ステップＳ４０１で第二移動経路取得部３５０は、１つの流線が持つ複数の経由点に対して自律移動体１０００の拘束条件および固定物体の位置を考慮した合流経路を第二移動経路の一部として生成する。
　図１９は、第二移動経路取得部３３２がステップＳ４０１において、自律移動体１０００の自己位置Ｐｅｇｏまたは第一移動経路ＴＲ１の経由点ｐ１（ｉ）　（ｉ＝τ，τ＋１）から、流線ＳＬ３の経由点ｐ２（ｊ）　（ｊ＝Ｔ，…，Ｔ＋５）それぞれに対して合流経路を演算する方法の一例を模式的に説明している。
　自律移動体１０００は駆動部５１０の出力上限などによる物理的な制約や、人が乗っている場合には、急旋回すると気分を害するといった積載物への衝撃を許容値に収める制御上の制約を持っている。また、自律移動体１０００と流線ＳＬ３との間に固定物体ＯＢ１２がある場合には、固定物体ＯＢ１２を回避しながら、流線ＳＬ３に対する合流する必要がある。
　ここで、図１９における合流経路ＣＲ１、ＣＲ２、ＣＲ３は、自律移動体１０００の旋回半径の最小値をｒｍｉｎとしたとき、流線ＳＬ３の経由点までを旋回半径ｒｍｉｎによる右左折と直線のみで構成することによって上記の物理的な制約を満たしている。
　経由点ｐ２（Ｔ）、ｐ２（Ｔ＋１）、ｐ２（Ｔ＋２）には、自律移動体１０００の現在位置Ｐｅｇｏから合流経路ＣＲ１を引いている。一方、経由点ｐ２（Ｔ＋３）においては自律移動体１０００の現在位置Ｐｅｇｏから合流経路ＣＲ２を引くと、固定物体ＯＢ１２に接触することがわかる。そこで第一移動経路ＴＲ１の経由点ｐ１（ｉ）を、ｐ１（τ）、ｐ１（τ＋１）と１つずつ交差点ＩＰ１に近づけていき、固定物体ＯＢ１２を回避し、かつ自律移動体１０００の制約条件を満たす経路を探索する。経由点ｐ２（Ｔ＋３）より交差点ＩＰ１に近い経由点ｐ２（Ｔ＋４）、ｐ２（Ｔ＋５）への合流経路ＣＲ３は、経由点ｐ１（τ＋１）から演算する。
　なお上記の合流経路の演算は流線ＳＬ３の全ての経由点に対して行っても良いが、経由点が多い場合には自律移動体１０００を中心とした一定半径内にある経由点などに限定しても良い。
　図１８に戻り、第二移動経路生成処理の続きについて説明する。
　次に、ステップＳ４０２で、第二移動経路取得部３３２は、ステップＳ４０１で生成した各経由点への合流経路をもとに合流に最適な経由点を合流点として選択する
　図２０は、第二移動経路取得部３３２が、図１９に対応する流線ＳＬ３の経由点から最適な合流点ＣＰ１を選択する方法の一例を模式的に説明している。
　最適な合流点ＣＰ１は、自律移動体１０００の現在位置から流線ＳＬ３の経由点までの距離ｄｍ（ｔ）の関数で示す合流点ＣＰ１の遠さに対する移動コストＣｍ＝ｆｍ（ｄｍ（ｔ））、交差点ＩＰ１から流線ＳＬ３の経由点までの距離ｄｅ（ｔ）の関数で示す交差点ＩＰ１までの離脱にかけて良い余裕の少なさに対する離脱コストＣｅ＝ｆｅ（ｄｅ（ｔ））、図１６に示した流線上の密度ρ（ｔ）と、経由点に到着するまでの密度ρ（ｔ）の移動を考慮するための経由点までの距離ｄｍ（ｔ）の２変数関数で示す物体の流れの密な部分への合流しにくさに対する密度コストＣｄ＝ｆｄ（ρ（ｔ），ｄｍ（ｔ））を各経由点ｐ２（ｔ）について計算し、これらの総コストＣａｌｌが最小となる点として計算される。
　図２０のグラフは各コストの関数が引数に対して単調に増加する場合を示しており、移動コストは自律移動体１０００の現在位置への最近傍点を最低コスト点に左右に増加し、離脱コストは交差点ＩＰ１から遠ざかるほど低くなり、密度コストは流線によって異なることがわかる。なお、横軸である距離ｌ（ｔ）は数式１０で計算されるものを使用している。
　そして、上記のコストの総和Ｃａｌｌを数式２０により計算する。
Figure JPOXMLDOC01-appb-M000020
　ここでｋｍ、ｋｅ、ｋｄは各コストに対する重みである。図２０の総コストのグラフでは経由点ｐ２（Ｔ＋２）が最も総コストが低いことからこの点を合流点ＣＰ１とする。
　図１８に戻り、第二移動経路生成処理の続きについて説明する。
　次に、ステップＳ４０３において、第二移動経路取得部３３２は、流線上の流速の大きさ｜ｖ（ｔ）｜や物体密度ρ（ｔ）から自律移動体１０００が流れの中で対岸に移動するためにかかる距離を計算して離脱点を計算する。合流点から離脱点までの距離ｌｅ（｜ｖ（ｔ）｜，ρ（ｔ））は、一般的に速度の大きさ｜ｖ（ｔ）｜及び密度ρ（ｔ）のそれぞれに対して単調に増加する関数として表される。離脱点は、合流点から流線上を距離ｌｅ（｜ｖ（ｔ）｜，ρ（ｔ））だけ移動した場所、またはそれ以上移動した場所にある流線の経由点に配置する。
　次に、ステップＳ４０４において、第二移動経路取得部３３２は、第一移動経路が持つ複数の経由点に対して自律移動体１０００の拘束条件および固定物体の位置を考慮した復帰点と復帰経路を演算する。ここで、復帰経路は第二移動経路の一部である。
　次に、ステップＳ４０５において、第二移動経路取得部３３２は、ステップＳ４０４で演算した復帰経路が固定物体などに阻まれずに移動できる場合は、次のループについて演算を行い、移動できない場合はステップＳ４０６に進む。
　次に、ステップＳ４０６において、第二移動経路取得部３３２は、ステップＳ４０３で演算した離脱点を流線上で交差点の方向に移動させる。この際、流線上の経由点を新たな離脱点として選択してもよいし、一定の移動距離を設定して経由点の間の点を新たな離脱点として選択してもよい。
　ここで新たな離脱点を交差点に近づける理由としては、交差点は固定物体を避けて移動することが可能である第一移動経路上に存在しており、固定物体を避けられる経路を作成できる可能性が高くなるためである。
　図２１及び図２２は、Ｓ４０４からステップＳ４０６において、第二移動経路取得部３３２が、離脱点から第一移動経路の経由点ｐ１（ｉ）　（ｉ＝τ′，…τ′＋２）それぞれに対して復帰経路ＲＲ１の候補を演算し、移動できる復帰経路ＲＲ１の候補があるまで離脱点を調整する方法の一例を模式的に説明している。これは例えば入口付近に人の流れがあり、流れから早く出ると入口の構造物（固定物体ＯＢ１３）にぶつかってしまう場合などを想定している。
　図２１の状況は、離脱点をＷＰ１とし、ステップＳ４０４にて復帰点の候補としては交差点ＩＰ２から最も近い第一移動経路ＴＲ２の経由点ｐ１（τ′）を選択し、続いてｐ１（τ′＋１）を選択して、ステップＳ４０１の合流経路の計算と同様に復帰経路ＲＲ１の計算をしているが、全ての経路が固定物体ＯＢ１３に妨げられているため、ステップＳ４０５の判定によりステップＳ４０６に遷移する。
　図２２では、ステップＳ４０６に遷移した結果として、交差点ＩＰ２に近い流線ＳＬ３上の経由点を新たな離脱点ＷＰ２として設定している。そしてステップＳ４０４にて、交差点ＩＰ２から最も近い第一移動経路ＴＲ２の経由点ｐ１（τ′）を復帰点とした復帰経路ＲＲ３を演算し、これが移動可能であるため、この流線に対する移動経路の修正処理を終了し、次の流線に対する処理に移る。
　第二移動経路取得部３３２は、交差する流線の数だけステップＳ４０１からステップＳ４０６までのループを回したら第二移動経路の演算を終了し、まだその数に達していない場合は次の交差する流線に対してステップＳ４０１から演算を行う。このように、交差する流線の分だけ存在する、合流点、合流経路、離脱点、復帰経路、復帰点の算出を繰り返すことにより、第一移動経路を修正した第二移動経路を生成し、自律移動体１０００の移動経路として設定する。
　以上で、第二移動経路取得部３３２は、第二移動経路生成処理を終了する。
　以上のような動作により、実施の形態１に係る経路生成システム３００は、自律移動体の移動経路が物体の流線と交差する場合に、事前に移動経路を修正するので、より適切に自律移動体の移動経路を設定することができる。例えば、事前に流れに応じて経路を修正することで流れを通過する時間に余裕を持たせることが可能であり、流れが強い場合、つまり流れを形成する移動体の速度が速かったり密度が大きかったりする場合においても、移動体の流れを乱しにくく、かつ、立ち往生することなく目的地に向かうことが可能となる。
　また、実施の形態１において、第二移動経路取得部３３２は、交差判定部３４０が流線と第一移動経路が交差すると判定した場合、第二移動経路の少なくとも一部が流線に合流するように、第一移動経路を修正することにより第二移動経路を生成するようにしたので、物体の流れに逆らわないスムーズな移動が可能な移動経路を生成することができる。
　また、実施の形態１において、交差判定部３４０は、流線と第一移動経路が交差する場合、流線と移動経路の交差点の位置を特定し、第二移動経路取得部３３２は、交差点の位置に基づき、第二移動経路と流線が合流する合流点の位置を決定するようにしたので、流線と第一移動経路の位置に応じて、より適切な第二移動経路を生成することができる。
　また、実施の形態１において、第二移動経路取得部３３２は、合流点に加え、自律移動体１０００が流線から離脱する離脱点の位置と、第一移動経路に復帰する復帰点の位置を決定するようにしたので、自律移動体１０００が物体の流れに沿って移動するままにならず、目的地に迅速に到達することができる移動経路を生成することができる。
　また、実施の形態１において、流線生成部３２０は、環境情報に基づき、流線上の物体の密度を算出し、第二移動経路取得部３３２は、交差点の位置と物体の密度に基づき、合流点の位置を決定するようにしたので、物体が存在する位置の偏りに応じて、より適切に移動経路を生成することができる。
　また、実施の形態１において、流線生成部３２０は、環境情報が示す物体の位置と物体の速度とに基づき、物体の移動の流れの流速を示すベクトル場を生成し、ベクトル場に基づき流線を生成するようにしたので、自律移動体１０００が実際の物体の速度に対応し、かつ移動物体が自然と固定物体を回避した経路を取ることを可能にしている。
　また、実施の形態１において、自律移動体１０００は、上記の経路生成システムと、第二移動経路取得部３３２が設定した移動経路に基づき、自律移動体１０００の制御量を算出し、制御量を示す制御信号を出力する走行制御システム４００と、走行制御システム４００から受信した制御信号に基づき自律移動体１０００を駆動する駆動システム５００と、を備えるようにしたので、自律移動体１０００は、複数の物体が自律移動体１０００の移動経路を将来的に遮る場合でも、事前に移動経路を修正し、修正した移動経路に基づき移動することにより、適切な移動を行うことができる。
　ここで、変形例について説明する。
　上記において、ステップＳ２の流線生成処理は、ステップＳ３の経路取得処理の前に行うようにしたが、流線の生成は、第一移動経路と流線が交差するかの判定までに行えばよいので、第一移動経路の取得と並行して行うようにしてもよい。
　上記において、地図生成システム２００や経路生成システム３００は自律移動体１０００の内部に含まれるように構成したが、必ずしも自律移動体１０００内部に備える必要は無く、自律移動体１０００の外部にあり、自律移動体１０００と通信などによる手段で情報の送受信を行っても良い。
　上記において、出入口の位置にポテンシャルを設けて検知範囲外のベクトル場を作成することで流線を外挿する方法について説明したが、外挿方法としては検知範囲外のベクトル場を記憶してある過去の同条件（時刻、曜日など）のベクトル場を用いたり、単純に流線の先端の方向に直線を伸ばしたりしても良い。
　実施の形態２．
　実施の形態１に係る流線生成部３２０は、流速を示すベクトル場から流線を作成するようにしたが、実施の形態２では、流れを構成する複数の移動物体を速度ベクトルの方向で分類し、方向が一致する移動物体の位置分布から回帰を行って生成した多項式や回帰木などの回帰モデルを用いて、流線を生成する方法について説明する。
　図２３は、実施の形態２に係る自律移動体２０００の構成を示す構成図であり、図２４は、実施の形態２に係る経路生成システム２３００の構成を示す構成図である。
　実施の形態１との相違点を中心に説明し、実施の形態１と同様の構成については、適宜説明を省略する。
　図２５を用いて、実施の形態２に係る流線生成部２３２０が、流線を生成する動作について説明する。
　まず、ステップＳ５０１で、流線生成部２３２０は、移動物体を、移動物体の速度ベクトルの方向により分類して複数のクラスタＣ（ｉ）を作成する。このときクラスタＣ（ｉ）は少なくとも２つ以上の移動物体を含むこととする。
　次に、流線生成部２３２０は、移動物体を角度範囲で分類したクラスタＣ（ｉ）の個数ＮだけステップＳ５０２及びステップＳ５０３の処理を行うループ処理を開始する。
　次に、ステップＳ５０２で、流線生成部２３２０は、移動物体の位置分布を用いて指定したモデルに対する回帰演算を行い、クラスタＣ（ｉ）の角度範囲から一方向に決定した流線を求める。回帰の手法としては一般的な最小二乗法、ＲＡＮＳＡＣ（ＲＡＮｄｏｍ　ＳＡｍｐｌｅ　Ｃｏｎｓｅｎｓｕｓ）、回帰木など複数の方法があり、１つの方法を選択してもよいし、複数の方法を選択し最も移動物体の位置分布に対する分散が少ない方法を選択してもよい。
　次に、ステップＳ５０３で、流線生成部２３２０は、ステップＳ５０２で生成した流線からの距離が所定の閾値以内の範囲に存在し、かつ流線とのなす角が所定の閾値未満である移動物体を流線に属すると判定し、それ以外の移動物体は流線に属さないと判定する。
　また、流線生成部２３２０は、線に属さないと判定されている移動物体の個数が所定の閾値以上であるかを判定し、流線に属さないと判定されている移動物体の個数が閾値以上である場合は次のループに遷移し、閾値未満である場合はステップＳ５０４へ遷移する。
　流線生成部２３２０は、クラスタＣ（ｉ）の個数ＮだけステップＳ５０２からステップＳ５０３までのループを回したら、流線生成処理を終了し、まだその数に達していない場合は次のクラスタＣ（ｉ＋１）に対してステップＳ５０２から演算を行う。
　図２６から図２９を用いて、実施の形態２に係る流線生成部２３２０の動作の具体例について説明する。
　図２６は、ステップＳ５０１において、流線生成部２３２０が、移動物体地図ＭＡＰ（ｆｌｏａｔ，ｎｅｗ）を用いて、０°≦θ＜９０°、９０°≦θ＜１８０°、１８０°≦θ＜２７０°、２７０°≦θ＜３６０°の計４つの角度範囲による分類を行う方法を示している。ここで、角度範囲０°≦θ＜９０°、９０°≦θ＜１８０°、１８０°≦θ＜２７０°によるクラスタＣ（１）、Ｃ（２）、Ｃ（３）を作成しているが、角度範囲２７０°≦θ＜３６０°によるクラスタは属する移動物体の個数が１個のため作成していない。実施の形態２では、角度を重複させていないが、角度範囲は重複を許し、同じ移動物体が複数のクラスタに含まれていても良い。また実施の形態２では角度範囲を指定する形式になっているが、教師なし学習手法によるクラスタリングを用いてもよい。
　図２７及び図２８は、ステップＳ５０２とステップＳ５０３において、クラスタＣ（２）の移動物体に対して２つの流線を回帰演算により作成する方法を示している。
　図２７において、流線生成部２３２０は、ＲＡＮＳＡＣ手法により３点を選択し、２次関数にフィッティングさせることで１回目の回帰演算を行っている。ここで、クラスタＣ（２）の角度範囲が９０°≦θ＜１８０°であることから、流線生成部２３２０は、２次関数によりフィッティングされている範囲でｘ方向に負、ｙ方向に正である方向を流線ＳＬ４の方向として決定している。
　図２８において、流線生成部２３２０は、１回目の回帰演算で得られた流線ＳＬ４に属する移動物体を、流線ＳＬ４と移動物体との距離、及び流線ＳＬ４の方向と移動物体の速度ベクトルの方向とのなす角に基づき、移動物体が流線に属するか判定している。ここで、１回目の流線ＳＬ４に属すると判定された移動物体は白塗りで示している。そして、流線生成部２３２０は、さらに残る移動物体に対して２回目の回帰演算を１回目と同様に行い、２つ目の流線ＳＬ５を生成する。
　図２９は、ステップＳ５０２とステップＳ５０３において、あるクラスタに対して、流線を生成しない場合を示している。
　クラスタＣ（３）に含まれる移動物体は移動物体ＯＢ１４と移動物体ＯＢ１５の２個のみであるため線形回帰しか適用できない。このときステップＳ５０２において、流線生成部２３２０は、クラスタＣ（３）の角度範囲である１８０°≦θ＜２７０°の方向に近いｘ方向に正、ｙ方向に負である方向を流線ＳＬ６の方向と決定する。しかし、ステップＳ５０３において、流線ＳＬ６に対して２つの移動物体の距離が０であることは自明であるが、図示する通り流線ＳＬ６の方向と移動物体ＯＢ１４，ＯＢ１５の速度ベクトルの方向とのなす角は９０°近く開いており、２つの移動物体ＯＢ１４，ＯＢ１５は、流線ＳＬ５に属さない。そして、流線生成部２３２０は、属する移動物体が存在しない流線ＳＬ５を消去する。
　その他の構成については、実施の形態１に係る経路生成システム３００及び自律移動体１０００の構成と同様であるので、説明を省略する。また、実施の形態１で説明した変形例は、実施の形態２にも適用可能である。
　実施の形態２に係る流線生成部２３２０は、環境情報が示す物体の位置と物体の速度とに基づき、物体を少なくとも２つ以上含むクラスタを生成し、クラスタに含まれる物体の位置に基づいた回帰演算により流線を生成するようにしたので、自律移動体１０００が流線に合流する際に個別の移動物体の影響を受けにくく修正が少ない経路を取ることが可能となる。
　本開示に係る経路生成システムは、自律的に移動する車両やロボットに搭載するのに適している。
　１０００，２０００　自律移動体、１００，２１００　インターフェースシステム、２００，２２００　地図生成システム、３００，２３００　経路生成システム、４００，２４００　走行制御システム、５００，２５００　駆動システム、１１０　通信部、１２０　表示部、１３０　受付部、２１０　外界検知部、２２０　衛星信号受信部、２３０　環境地図生成部、２４０　自己位置推定部、３１０，２３１０　環境情報取得部、３２０，２３２０　流線生成部、３３０，２３３０　移動経路取得部、３３１，２３３１　第一移動経路取得部、３３２，２３３２　第二移動経路取得部、３４０，２３４０　交差判定部、４１０　経路追従演算部、４２０　衝突回避判断部、４３０　移動制御部、５１０　駆動部、５２０　フィードバック部、１０００１　処理装置、１０００２　記憶装置、１０００３　センサーインターフェース、１０００４　表示インターフェース、１０００５　設定インターフェース、１０００６　アクチュエータインターフェース、１０００７　外界センサー、１０００８　ＧＮＳＳ受信機、１０００９　ロータリエンコーダ、１００１０　液晶ディスプレイ、１００１１　タッチパネル、１００１２　駆動装置。

Claims (10)

  1. 　自律移動体の周囲に存在する物体の位置と前記物体の速度とを示す環境情報を取得する環境情報取得部と、
    　前記環境情報に基づき、前記物体の移動の流れを示す流線を生成する流線生成部と、
    　前記自律移動体の目的地までの移動経路である第一移動経路を取得する第一移動経路取得部と、
    　前記流線と前記第一移動経路が交差するかを判定する交差判定部と、
    　前記交差判定部が前記流線と前記第一移動経路が交差すると判定した場合、前記第一移動経路を修正した第二移動経路を生成し、前記第二移動経路を前記自律移動体の移動経路として設定する第二移動経路取得部と、
    　を備える経路生成システム。
  2. 　前記第二移動経路取得部は、前記交差判定部が前記流線と前記第一移動経路が交差すると判定した場合、前記第二移動経路の少なくとも一部が前記流線に合流するように、前記第一移動経路を修正することにより前記第二移動経路を生成する
    　ことを特徴とする請求項１に記載の経路生成システム。
  3. 　前記交差判定部は、前記流線と前記第一移動経路が交差する場合、前記流線と前記移動経路の交差点の位置を特定し、
    　前記第二移動経路取得部は、前記交差点の位置に基づき、前記第二移動経路と前記流線が合流する合流点の位置を決定する
    　ことを特徴とする請求項２に記載の経路生成システム。
  4. 　前記第二移動経路取得部は、前記合流点に加え、前記自律移動体が前記流線から離脱する離脱点の位置と、前記第一移動経路に復帰する復帰点の位置を決定する
    　ことを特徴とする請求項３に記載の経路生成システム。
  5. 　前記流線生成部は、前記環境情報に基づき、前記流線上の前記物体の密度を算出し、
    　前記第二移動経路取得部は、前記交差点の位置と前記物体の密度に基づき、前記合流点の位置を決定する
    　ことを特徴とする請求項３または４に記載の経路生成システム。
  6. 　前記流線生成部は、前記環境情報が示す前記物体の位置と前記物体の速度とに基づき、前記物体の移動の流れの流速を示すベクトル場を生成し、前記ベクトル場に基づき前記流線を生成する
    　ことを特徴とする請求項１から５のいずれか一項に記載の経路生成システム。
  7. 　前記流線生成部は、前記環境情報が示す前記物体の位置と前記物体の速度とに基づき、前記物体を少なくとも２つ以上含むクラスタを生成し、前記クラスタに含まれる前記物体の位置に基づいた回帰演算により前記流線を生成する
    　ことを特徴とする請求項１から５のいずれか一項に記載の経路生成システム。
  8. 　請求項１から７のいずれか１項に記載の経路生成システムと、
    　前記第二移動経路取得部が設定した前記移動経路に基づき、前記自律移動体の制御量を算出し、前記制御量を示す制御信号を出力する走行制御システムと、
    　前記走行制御システムから受信した前記制御信号に基づき前記自律移動体を駆動する駆動システムと、
    　を備える自律移動体。
  9. 　自律移動体の周囲に存在する物体の位置と前記物体の速度とを示す環境情報を取得する環境情報取得工程と、
    　前記環境情報に基づき、前記物体の移動の流れを示す流線を生成する流線生成工程と、
    　前記自律移動体の目的地までの移動経路である第一移動経路を取得する第一移動経路取得工程と、
    　前記流線と前記第一移動経路が交差するかを判定する交差判定工程と、
    　前記交差判定工程で前記流線と前記第一移動経路が交差すると判定した場合、前記第一移動経路を修正した第二移動経路を生成し、前記第二移動経路を前記自律移動体の移動経路として設定する第二移動経路取得工程と、
    　を備える経路生成方法。
  10. 　請求項９に記載の全工程をコンピュータに実行させる経路生成プログラム。
PCT/JP2020/042433 2020-11-13 2020-11-13 経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体 WO2022102078A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
PCT/JP2020/042433 WO2022102078A1 (ja) 2020-11-13 2020-11-13 経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体
JP2021532872A JP7004475B1 (ja) 2020-11-13 2020-11-13 経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2020/042433 WO2022102078A1 (ja) 2020-11-13 2020-11-13 経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体

Publications (1)

Publication Number Publication Date
WO2022102078A1 true WO2022102078A1 (ja) 2022-05-19

Family

ID=80621028

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2020/042433 WO2022102078A1 (ja) 2020-11-13 2020-11-13 経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体

Country Status (2)

Country Link
JP (1) JP7004475B1 (ja)
WO (1) WO2022102078A1 (ja)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023166561A1 (ja) * 2022-03-01 2023-09-07 日本電気株式会社 移動経路設定方法
JP2024005845A (ja) * 2022-06-30 2024-01-17 三菱重工業株式会社 経路設定方法、プログラム及び移動体

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008152600A (ja) * 2006-12-19 2008-07-03 Toyota Motor Corp 移動経路作成方法、自律移動体および自律移動体制御システム
WO2016104265A1 (ja) * 2014-12-25 2016-06-30 株式会社エクォス・リサーチ 移動体
JP2019144612A (ja) * 2018-02-15 2019-08-29 日本信号株式会社 走行装置
JP2020046773A (ja) * 2018-09-14 2020-03-26 学校法人早稲田大学 ロボット、並びに、その行動計画装置及び行動計画用プログラム
JP2020149095A (ja) * 2019-03-11 2020-09-17 本田技研工業株式会社 倒立振子型ロボット

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008152600A (ja) * 2006-12-19 2008-07-03 Toyota Motor Corp 移動経路作成方法、自律移動体および自律移動体制御システム
WO2016104265A1 (ja) * 2014-12-25 2016-06-30 株式会社エクォス・リサーチ 移動体
JP2019144612A (ja) * 2018-02-15 2019-08-29 日本信号株式会社 走行装置
JP2020046773A (ja) * 2018-09-14 2020-03-26 学校法人早稲田大学 ロボット、並びに、その行動計画装置及び行動計画用プログラム
JP2020149095A (ja) * 2019-03-11 2020-09-17 本田技研工業株式会社 倒立振子型ロボット

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
YAMAMOTO E, MASASHI OKADA: "RSJ2011AC1N1-2 Parameter identification of pedestrian group model using human walking trajectory data and application to cross-flow control", PROCEEDINGS OF THE 29TH ANNUAL MEETING OF THE ROBOTICS SOCIETY OF JAPAN DVD-ROM, SEPTEMBER 7-11, 2011, ROBOTICS SOCIETY OF JAPAN, JP, 7 September 2011 (2011-09-07) - 9 September 2011 (2011-09-09), JP, pages 1 - 4, XP055939605 *

Also Published As

Publication number Publication date
JP7004475B1 (ja) 2022-01-21
JPWO2022102078A1 (ja) 2022-05-19

Similar Documents

Publication Publication Date Title
US10802494B2 (en) Method for motion planning for autonomous moving objects
US10598505B2 (en) Travel route generation apparatus and method for generating travel route
Larson et al. Autonomous navigation and obstacle avoidance for unmanned surface vehicles
US11512975B2 (en) Method of navigating an unmanned vehicle and system thereof
US20210365038A1 (en) Local sensing based autonomous navigation, and associated systems and methods
JP7004475B1 (ja) 経路生成システム、経路生成方法、経路生成プログラム、及び自律移動体
Raja et al. PFIN: An efficient particle filter-based indoor navigation framework for UAVs
Lin et al. Autonomous quadrotor navigation with vision based obstacle avoidance and path planning
Heimes et al. Towards active machine-vision-based driver assistance for urban areas
CN112212872A (zh) 基于激光雷达和导航地图的端到端自动驾驶方法及系统
Ferguson et al. A reasoning framework for autonomous urban driving
Yu et al. A hybrid path planning method for an unmanned cruise ship in water quality sampling
CN110702112B (zh) 导航装置及路线生成方法
KR102615505B1 (ko) 자율주행 차량의 운행 경로 노드 별 경로생성 시스템
Meng et al. Improved hybrid A-star algorithm for path planning in autonomous parking system based on multi-stage dynamic optimization
CN113498498B (zh) 行动控制设备和行动控制方法、以及程序
CN115061499A (zh) 无人机控制方法及无人机控制装置
WO2021229825A1 (ja) 操船計算装置
Bender et al. Map-based drone homing using shortcuts
Yoon et al. Shape-Aware and G 2 Continuous Path Planning Based on Bidirectional Hybrid A∗ for Car-Like Vehicles
Wang et al. Real-time path planning algorithm for autonomous vehicles in unknown environments
Kurdi et al. Navigation of mobile robot with cooperation of quadcopter
Wang et al. Dynamic path planning algorithm for autonomous vehicles in cluttered environments
US20200257296A1 (en) Plan buffering for low-latency policy updates
Xie et al. Development of navigation system for autonomous vehicle to meet the DARPA urban grand challenge

Legal Events

Date Code Title Description
ENP Entry into the national phase

Ref document number: 2021532872

Country of ref document: JP

Kind code of ref document: A

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20961606

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 20961606

Country of ref document: EP

Kind code of ref document: A1