Route generation device, route generation method, and route generation program

Abstract

The path generating device includes a map creation unit (142) and a path creation unit (144). The map creation unit (142) takes each of a plurality of grids obtained by dividing an environmental map made up of a three-dimensional point cloud as a target grid, extracts a planar area from the target grid when it is determined that a three-dimensional point cloud corresponding to a planar area exists in the target grid, and takes the planar area extracted from each grid from which the planar area is extracted as a target plane. In the grid including the target plane, when a three-dimensional point cloud exists between the height of the target plane and a height obtained by adding a value equal to or greater than the height of a target moving body to the height of the target plane in the height direction relative to the target plane, the map creation unit classifies the target plane as an obstacle plane, and creates a planar map showing the planar areas not classified as obstacle planes among the extracted planar areas. The path creation unit (144) creates a path for the target moving body from a movement start position to a movement target position based on the planar map.

Description

[Technical field]

[0001] The present disclosure relates to a route generation device, a route generation method, and a route generation program. [Background technology]

[0002] In recent years, autonomous mobile robots are expected to perform tasks such as transporting passengers and goods, or inspecting and guarding facilities not only in limited environments such as factories and warehouses, but also in environments frequented by the general public, such as commercial facilities and urban areas. Autonomous mobile robots that perform such tasks are equipped with an appropriate movement mechanism according to the type of task. As a specific example, for a robot engaged in transportation tasks, it is important to transport passengers and goods stably, so a wheeled robot is preferable. On the other hand, for a robot engaged in inspection and security tasks, the ability to traverse steps or other steps that people can enter is often expected, so not only wheeled robots but also walking robots are used. In this way, even if the start position and the target position of movement are the same between autonomous mobile robots having different moving mechanisms, the intermediate paths for efficiently moving from the start position of movement to the target position of movement may differ.

[0003] As a specific example of a means for generating a path according to the difference in the movement mechanism of an autonomous mobile robot, an environmental recognition device is known that detects planes and obstacles from three-dimensional data to create an environmental map, and creates a movement path using the created environmental map, as disclosed in Patent Document 1. In this environmental recognition device, for robots with different movement mechanisms such as wheeled robots and mobile robots, planes such as movable floor surfaces that have as small a height difference as possible from a reference plane are extracted as other planes rather than as obstacles to create an environmental map, and a path is created based on the created environmental map, enabling each robot to traverse steps of a predetermined height. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Patent No. 3994950 Summary of the Invention [Problem to be solved by the invention]

[0005] In the conventional path generation program, whether or not a plane is navigable is determined taking into consideration constraints such as the traversability of the autonomous mobile robot's mobile mechanism, but the dimensions of the autonomous mobile robot, such as its overall height, are not considered. Therefore, in the conventional path generation program, when structures such as ceiling surfaces and bridge girders that the autonomous mobile robot would not normally come into contact with when passing vertically above the detected plane are acquired as 3D data, the conventional path generation program may determine the structures as obstacles that the mobile mechanism cannot traverse, resulting in the creation of an environmental map that shows a narrower area as a navigable area than the actual navigable area. In addition, in order to avoid creating such an environmental map, the conventional path generation program has a problem in that it is necessary to impose constraints on the sensor placement so as not to acquire 3D data of structures that exist vertically above the plane.

[0006] The present disclosure aims to eliminate the need to place constraints on sensor placement and enable an autonomous mobile robot to move indoors, under bridge girders, under tree branches, etc., by taking into account not only the traversal ability of an autonomous mobile robot but also the dimensions of the autonomous mobile robot, such as its overall height, and thereby not determining that an obstacle is present in a three-dimensional area that the autonomous mobile robot passes through when moving if there is no structure within the three-dimensional area. [Means for solving the problem]

[0007] A route generation device according to the present disclosure includes: When an environmental map consisting of a three-dimensional point cloud measured in a target area is divided into a plurality of grids such that a plane included in the target area is divided, each grid of the plurality of grids is set as a target grid, and when it is determined that a three-dimensional point cloud corresponding to a planar area exists within the target grid, a planar area is extracted from the target grid; a planar area extracted from each lattice from which a planar area is extracted is taken as a target plane, and in a lattice including the target plane, if a three-dimensional point cloud exists between a height of the target plane and a height obtained by adding the height of the target plane to a movable height threshold value that is equal to or greater than the height of a target moving body, the target plane is classified as an obstacle plane; a map creation unit that creates a planar map showing the planar areas that are not classified as obstacle planes among the extracted planar areas; a route creation unit that creates a route for the target moving object from a movement start position on the planar area not classified as an obstacle plane, which is shown on the planar map, to a movement target position on the planar area not classified as an obstacle plane, which is shown on the planar map, based on the planar area not classified as an obstacle plane, which is shown on the planar map; Equipped with. Effect of the Invention

[0008] According to the present disclosure, a map creation unit creates a planar map showing planar areas not classified as obstacle planes. A path creation unit creates a path for a target moving object based on the planar map. Here, the map creation unit determines whether or not to classify each planar area as an obstacle plane in consideration of the overall height of the target moving object. The target moving object may be an autonomous mobile robot. Therefore, according to the present disclosure, by taking into consideration not only the traversal ability of the autonomous mobile robot but also the dimensions of the autonomous mobile robot, such as its overall height, if there is no structure within a three-dimensional area that the autonomous mobile robot passes through while moving, it is possible to not determine that an obstacle is present within the three-dimensional area, thereby eliminating the need to place constraints on sensor placement and enabling the autonomous mobile robot to move indoors, under bridge girders, under tree branches, etc. [Brief description of the drawings]

[0009] [Figure 1] FIG. 2 is a diagram showing an example of the configuration of a route generation system 90 according to the first embodiment. [Diagram 2] 2 is a diagram showing an example of a hardware configuration of a control device 1 according to the first embodiment. [Diagram 3] 5 is a flowchart showing the operation of a map creation unit 142 according to the first embodiment. [Figure 4] 5 is a flowchart showing the operation of a map creation unit 142 according to the first embodiment. [Diagram 5] 3A to 3C are diagrams for explaining a grid division method according to the first embodiment. [Figure 6] 3A to 3C are diagrams for explaining a plane extraction method according to the first embodiment. [Figure 7] 3A to 3C are diagrams for explaining a virtual plane calculation method according to the first embodiment. [Figure 8] FIG. 4 is a diagram for explaining an obstacle determination process according to the first embodiment. [Figure 9] FIG. 4 is a diagram for explaining the processing of a map creation unit 142 according to the first embodiment. [Figure 10] FIG. 4 is a diagram for explaining the processing of a map creation unit 142 according to the first embodiment. [Figure 11] 5 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the first embodiment. FIG. [Figure 12] 5 is a diagram for explaining the processing of a connection relationship registration unit 143 according to the first embodiment. FIG. [Figure 13] 5 is a flowchart showing the operation of a path creation unit 144 according to the first embodiment. [Figure 14] 5 is a flowchart showing the operation of a path creation unit 144 according to the first embodiment. [Figure 15] FIG. 4 is a diagram for explaining the process of a path creation unit 144 according to the first embodiment. [Figure 16] 1A and 1B are diagrams for explaining the processing of the path creation unit 144 in the first embodiment, where (a) is a table corresponding to the initial state, (b) is a table corresponding to the end of the first iteration, and (c) is a table corresponding to the end of the second iteration. [Figure 17]5A and 5B are diagrams for explaining the processing of the path creation unit 144 according to the first embodiment, where FIG. 5A is a table corresponding to the end of the third iteration, and FIG. 5B is a table corresponding to the end of the fourth iteration. [Figure 18] 5A and 5B are diagrams for explaining the processing of the path creation unit 144 according to the first embodiment, where FIG. 5A is a table corresponding to the end of the fifth iteration, and FIG. 5B is a table corresponding to the end of the sixth iteration. [Figure 19] FIG. 4 shows a route list 51 according to the first embodiment. [Figure 20] FIG. 13 is a diagram showing an example of a hardware configuration of a control device 1 according to a modified example of the first embodiment. [Figure 21] FIG. 11 is a diagram showing a route list 52 according to the second embodiment. [Figure 22] 13A and 13B are diagrams for explaining speed settings according to the second embodiment, where FIG. 13A is a graph showing the height of a plane, and FIG. 13B is a graph showing speed constraints. [Diagram 23] FIG. 13 is a diagram showing a route list 53 according to the third embodiment. [Figure 24] 13A is a graph showing the height of a plane, FIG. 13B is a graph showing the shortest blind spot grid distance, and FIG. 13C is a graph showing a speed constraint. [Diagram 25] 13A is a schematic diagram illustrating the positional relationship between a movement candidate plane and an obstacle plane, and FIG. 13B is a diagram illustrating a visual field reduction rate according to the third embodiment. FIG. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] In the description of the embodiments and the drawings, the same elements and corresponding elements are given the same reference numerals. Descriptions of elements given the same reference numerals are omitted or simplified as appropriate. Arrows in the drawings primarily indicate data flow or processing flow. In addition, "part" may be read as "circuit," "step," "procedure," "processing," or "circuitry" as appropriate.

[0011] Embodiment 1 A specific embodiment to which the present disclosure is applied will be described in detail below with reference to the drawings. This embodiment relates to a path generation program that extracts multiple planes from three-dimensional data while taking into consideration the specifications of a robot for which a path is to be generated, creates an environmental map showing the connection relationships between the planes based on the results of determining whether or not each of the extracted planes is movable between planes that exist in the vicinity of the extracted planes, and creates a path from the movement start position of the robot to the movement target position based on the connection relationships shown in the created environmental map. The environmental map is made up of a three-dimensional point cloud measured in a target area. As an example of this embodiment, a control device equipped with a route generation program and an autonomously mobile wheeled robot device that transmits and receives information by communicating with the control device will be described.

[0012] 1 is a block diagram showing a specific example of a route generation system 90 according to embodiment 1. The route generation system 90 includes a control device 1 and a robot device 2.

[0013] The control device 1 is a device for remotely transmitting commands to the robot device 2, and includes a user input / output unit 11, a communication unit 12, a storage unit 13, and a route generation program 14. The control device 1 functions as a route generation device.

[0014] The user input / output unit 11 has a function of accepting commands input by a human to the control device 1, and a function of displaying the results of calculations by the control device 1 in a format that can be understood by a human.

[0015] The communication unit 12 receives information indicating at least the current position of the robot apparatus 2 as a movement start position from the robot apparatus 2 via the communication network, and transmits information indicating at least the path calculated by the path generation program 14 to the robot apparatus 2. The communication unit 12 may transmit and receive information to and from other robot apparatuses (not shown) and terminals such as smartphones.

[0016] The memory unit 13 stores three-dimensional global point cloud data 31, which is three-dimensional data corresponding to the area in which the robot apparatus 2 operates, specification information used in route generation such as the ability of the robot apparatus 2 to traverse steps and the dimensions of the robot apparatus 2, and program information for the route generation program 14. The environment map shows the three-dimensional global point cloud data 31 .

[0017] The route generation program 14 is made up of a program input / output unit 141, a map creation unit 142, a connection relationship registration unit 143, and a route creation unit 144. Each of the map creation unit 142, the connection relationship registration unit 143, and the route creation unit 144 corresponds to a calculation unit. When the communication unit 12 receives information indicating a movement target position from the user input / output unit 11 or a terminal such as a smartphone (not shown), the communication unit 12 receives information indicating a current position transmitted by the robot apparatus 2 to the communication network. After that, the path generation program 14 generates a movement path for the robot apparatus 2, with the current position indicated by the received information set as a movement start position. The information indicating the generated movement path is transmitted to the robot apparatus 2 by the communication unit 12. In this specification, the movement path may be simply referred to as a "route".

[0018] The program input / output unit 141 converts input from outside the program into a format that can be used for calculations, and also converts the results of calculations within the program into at least one of a format that can be output by the user input / output unit 11, a format that can be transmitted by the communication unit 12, and a format that can be held by the memory unit 13, and outputs the converted results.

[0019] When a plane included in a target area is divided into a plurality of grids, the map creation unit 142 sets each of the plurality of grids as a target grid, and extracts a planar area from the target grid when it is determined that a 3D point cloud corresponding to the planar area exists in the target grid. The planar area may be an area including an actual plane or an area including a virtual plane. Note that a planar area may be simply referred to as a plane. The map creation unit 142 regards the planar area extracted from each grid from which the planar area is extracted as a target plane, and classifies the target plane as an obstacle plane when a three-dimensional point cloud exists in the grid including the target plane between the height of the target plane and a height obtained by adding the height of the target plane to a movable height threshold value that is equal to or greater than the height of the target moving body, in the height direction relative to the target plane. When the map creation unit 142 does not classify the target plane as an obstacle plane, it may classify the target plane as a movement candidate plane. A specific example of the target moving body is the robot device 2. The target moving body may be a robot device that moves by tires. A specific example of the height direction relative to the target plane is a direction perpendicular to the target plane. The height of the target moving body is the total height of the target moving body. The map creation unit 142 creates a planar map showing those planar areas that have not been classified as obstacle planes out of the extracted planar areas. The map creation unit 142 may include, in the planar map, information indicating the height of each planar area shown by the planar map. The map creation unit 142 may include, in the planar map, each planar area classified as an obstacle planar area, and may include, in the planar map, information indicating the height of an obstacle in each planar area classified as an obstacle planar area.

[0020] The connection relationship registration unit 143 treats each two adjacent planar areas on the planar map as a target adjacent planar pair, and determines whether the target moving object can traverse between the two planar areas included in the target adjacent planar pair based on the structure of the movement mechanism of the target moving object and the height difference between the two planar areas included in the target adjacent planar pair, and registers the connection relationship of the target adjacent planar pair if the target moving object can traverse between the two planar areas included in the target adjacent planar pair.

[0021] The route creation unit 144 creates a route for the target moving object from the movement start position to the movement target position based on a planar area not classified as an obstacle plane shown on the planar map. The movement start position is a position on a planar area not classified as an obstacle plane shown on the planar map. The movement target position is a position on a planar area not classified as an obstacle plane shown on the planar map.

[0022] The robot device 2 is a wheeled robot that receives commands remotely from the control device 1 and moves through space in accordance with the received commands. The robot device 2 includes a communication unit 21, a storage unit 22, a three-dimensional point cloud sensor 23, a rotation sensor 24, an actuator 25, a tire mechanism 26, and a control program 27. The robot device 2 may realize at least a part of the functions of a path generation device.

[0023] The communication unit 21 receives at least information indicating a moving route from the control device 1 via a communication network, and transmits at least information indicating a current position calculated by the control program 27 to the control device 1. The communication unit 12 may also transmit and receive information between other robot devices (not shown) and terminals such as smartphones.

[0024] The memory unit 22 stores three-dimensional global point cloud data that is the same as the three-dimensional global point cloud data 31 stored in the control device 1, or three-dimensional global point cloud data similar to the three-dimensional global point cloud data 31 but with a reduced capacity obtained by reducing the number of points or reducing some areas from the three-dimensional global point cloud data 31, specification information used in control of the robot device 2, such as its ability to traverse steps, dimensions, and gear ratio, and program information for the control program 27.

[0025] The three-dimensional point cloud sensor 23 is, for example, a sensor capable of acquiring the surrounding environment as three-dimensional point cloud data, such as a 3D LiDAR (3-Dimensions Light Detection and Ranging) or a stereo camera. The acquired three-dimensional local point cloud data, which is three-dimensional point cloud data corresponding to the surrounding environment of the robot apparatus 2, is used in the control program 27 for self-position estimation, surrounding environment detection, and the like.

[0026] The rotation sensor 24 is, for example, a rotary encoder, and is a sensor that acquires the number of rotations of the actuator 25 (or the number of rotations of the tire mechanism 26). The data indicating the number of rotations is mainly used for mechanical control in the control program 27. The data may also be used for self-position estimation.

[0027] The actuator 25 is, for example, an electric rotary motor, and drives the tire mechanism 26 directly or via a gear (not shown) based on a command value from a control program 27 .

[0028] The tire mechanism 26 transmits the driving force received from the actuator 25 to the ground in contact with the tire mechanism 26, thereby enabling the robot device 2 to accelerate or decelerate. The tire mechanism 26 may also have a suspension mechanism (not shown) or the like. As a specific example, there are two pairs of actuators 25 and tire mechanisms 26 in the robot device 2. In this example, the tire axis is common to each pair of the two tire mechanisms 26, and the two pairs of actuators 25 transmit different driving forces to each tire mechanism 26, thereby enabling movement such as translation and rotation.

[0029] The control program 27 is composed of a program input / output unit 271, a self-position estimation unit 272, a surrounding environment detection unit 273, a path following calculation unit 274, and a mechanism calculation unit 275. Each of the self-position estimation unit 272, the surrounding environment detection unit 273, the path following calculation unit 274, and the mechanism calculation unit 275 corresponds to a calculation unit.

[0030] The program input / output unit 271 converts input from outside the program into a format that can be used for calculations, and also converts the results of calculations within the program into at least one of a format that can be transmitted by the communication unit 21, a format that can be held by the memory unit 22, and a format that can be driven by the actuator 25, and outputs the converted results.

[0031] When the 3D point cloud sensor 23 acquires 3D local point cloud data, the self-position estimation unit 272 calculates the current position of the robot device 2 by matching the acquired 3D local point cloud data with the 3D global point cloud data held in the memory unit 22 using a SLAM (Simultaneous Localization And Mapping) method.

[0032] First, the surrounding environment detection unit 273 determines, as an inconsistent point group, a point group at a location where the 3D local point cloud data used by the self-position estimation unit 272 in matching does not match the 3D global point cloud data. Next, the surrounding environment detection unit 273 extracts a 3D point cloud corresponding to the range corresponding to the determined contradictory point cloud and a certain range surrounding that range as a corresponding point cloud from the 3D global point cloud data, and compares the average height between the contradictory point cloud and the corresponding point cloud. Next, if the average height of the inconsistency points is higher than the average height of the corresponding points, the surrounding environment detection unit 273 determines that a temporary obstacle exists in the range corresponding to the inconsistency points. Otherwise, the surrounding environment detection unit 273 determines that the inconsistency points exist due to a conflict caused by the previous existence of an obstacle, and performs plane detection on the inconsistency points. If the surrounding environment detection unit 273 detects a planar area from the inconsistency points, it determines that the range corresponding to the inconsistency points is a temporary plane. Also, if no planar area is detected from the inconsistency points, the surrounding environment detection unit 273 determines that a temporary obstacle exists in the range corresponding to the inconsistency points. In addition, the surrounding environment detection unit 273 may classify at least a portion of the three-dimensional local point cloud data into pedestrians, bicycles, etc. using a deep learning method or the like, and may consider the point cloud data classified into pedestrians, bicycles, etc. as data corresponding to a temporary obstacle.

[0033] The path following calculation unit 274 calculates a movement vector of the robot apparatus 2 so that the path can be followed based on the deviation between the movement path indicated by the information received from the control device 1 and the current position of the robot apparatus 2, and so that the robot apparatus 2 does not come into contact with any temporary obstacles detected by the surrounding environment detection unit 273.

[0034] The mechanism calculation unit 275 calculates the next command value for the actuator 25 based on the observation value of the actuator 25 by the rotation sensor 24, taking into consideration the tire diameter, gear ratio, etc. stored in the memory unit 22, so that the robot device 2 moves in the direction and at the speed indicated by the movement vector calculated by the path following calculation unit 274.

[0035] 2 shows an example of a hardware configuration of the control device 1 according to the present embodiment. The control device 1 is realized by a general computer, for example. The robot device 2 may be equipped with a similar computer. The control device 1 may be realized by multiple computers.

[0036] As shown in the figure, the control device 1 is a computer equipped with hardware such as a processor 101, a memory 102, an auxiliary storage device 103, a display interface 104, a communication interface 105, and an input interface 106. These pieces of hardware are appropriately connected via signal lines. In addition, the control device 1 is connected with a display 107, a wired LAN (Local Area Network) port 108, a keyboard 109, a mouse 110, etc.

[0037] The processor 101 is an integrated circuit (IC) that performs arithmetic processing of each arithmetic unit, and controls the hardware of the computer. Specific examples of the processor 101 include a central processing unit (CPU), a digital signal processor (DSP), and a graphics processing unit (GPU). The control device 1 may include a plurality of processors that replace the processor 101. The plurality of processors share the role of the processor 101.

[0038] The memory 102 is typically a volatile storage device, and a specific example is a random access memory (RAM). The memory 102 is also called a primary storage device or a main memory. Data stored in the memory 102 is saved in the secondary storage device 103 as necessary.

[0039] The auxiliary storage device 103 is typically a non-volatile storage device, and specific examples thereof include a ROM (Read Only Memory), an HDD (Hard Disk Drive), or a flash memory. Data stored in the auxiliary storage device 103 is loaded into the memory 102 as necessary. The memory 102 and the auxiliary storage device 103 may be integrated into one unit.

[0040] The display interface 104 converts the processing results of the processor 101 into a format that can be displayed by the display 107 .

[0041] The communication interface 105 converts the processing results of the processor 101 into a format that can be transmitted to the robot device 2 or other terminals (not shown) connected to a network such as Ethernet via the wired LAN port 108. The communication interface 105 also receives information from devices such as the robot device 2, and converts the received information into information that the processor 101 can process. The communication interface 105 is, as a specific example, a communication chip or a NIC (Network Interface Card).

[0042] The input interface 106 converts information input by a human being through the operation of a keyboard 109 and a mouse 110 into information that can be processed by the processor 101 .

[0043] A human being sets a movement target position, which is the destination of the robot apparatus 2, by inputting coordinates numerically using a keyboard 109, or by displaying a map corresponding to the area in which the robot apparatus 2 is to operate on a display 107 and specifying a point on the displayed map using a mouse 110. The functions of the program input / output unit 141 are realized by each interface. The functions of the user input / output unit 11 are realized by a display 107 , a keyboard 109 and a mouse 110 . The function of the communication unit 12 is realized by a wired LAN port 108 .

[0044] The auxiliary storage device 103 stores a route generation program. The route generation program is a program that causes a computer to realize the functions of each unit of the control device 1. The route generation program is loaded into the memory 102 and executed by the processor 101. The functions of each unit of the control device 1 are realized by software. Even when the control device 1 is not running, the auxiliary storage device 103 holds the path generation program 14, the three-dimensional global point cloud data 31, and a file containing various parameters such as the specifications information of the robot device 2.

[0045] Data used when executing the route generation program and data obtained by executing the route generation program are appropriately stored in a storage device. Each part of the control device 1 uses a storage device as appropriate. As a specific example, the storage device is composed of at least one of the memory 102, the auxiliary storage device 103, a register in the processor 101, and a cache memory in the processor 101. The storage unit 13 is realized by a storage device. Note that the terms "data" and "information" may have the same meaning. The storage device may be independent of the computer. The functions of the memory 102 and the auxiliary storage device 103 may be realized by other storage devices.

[0046] The route generation program may be recorded in a computer-readable non-volatile recording medium. Specific examples of the non-volatile recording medium include an optical disk and a flash memory. The route generation program may be provided as a program product.

[0047] ***Explanation of Operation*** The operation procedures of the devices included in the route generation system 90 are collectively referred to as a route generation method. Also, the programs that realize the operations of the devices included in the route generation system 90 are collectively referred to as a route generation program. The flow of each calculation unit of the route generation program 14 generating a route from the three-dimensional global point cloud data 31 will be described below with reference to the drawings.

[0048] 3 and 4 are flowcharts showing an example of the process flow of the map creation unit 142. The process flow will be described with reference to FIGS.

[0049] (Step S310) The map creation unit 142 divides the 3D global point cloud data 31 input via the program input / output unit 141 into a grid shape at an angle parallel to the horizontal plane or the moving plane of the robot device 2. That is, the environmental map is divided into a plurality of grids so that the plane included in the target area is divided. When the robot device 2 is a wheeled robot, it is desirable that the length of one side of the lattice is equal to or greater than the greater of the total length and total width of the robot device 2 so that the robot device 2 fits within the lattice. In other words, it is desirable that the length of each side of the faces of the target lattice that correspond to the planes included in the target area is equal to or greater than the greater of the total length and total width of the target moving object.

[0050] 5 is a schematic diagram showing an example of a grid division method, in which three-dimensional global point cloud data 31 is divided into a square grid with a length of L on each side. In Fig. 5, coordinate information is assigned to each grid in order along each axis direction. Specifically, the grid located at the bottom left is assigned (k, l), the grids adjacent to the grid located at the bottom left in the right direction are assigned (k+1, l) and (k+2, l), and the grids adjacent to the grid located at the bottom left in the depth direction are assigned (k, l+1) and (k, l+2). For simplicity, it is assumed in Fig. 5 that the point cloud of the 3D global point cloud data 31 exists only in the dotted area, i.e., the road surface and the surface of the trees, although this is not shown. Also, from the total of 25 grids in Fig. 5, three types of grids 32 to 34 with different distributions of point clouds are illustrated. For the convenience of the following explanation of the map creation unit 142, the 3D point cloud data is illustrated as being divided into two types: a road surface reflection point cloud and a tree reflection point cloud. However, in the processing of the map creation unit 142, it is not necessary to classify the 3D point cloud data in advance into data corresponding to the road surface and data corresponding to trees.

[0051] (Step S320) The process from step S320 to step S460 is a repetitive process that is executed the same number of times as the number of grids generated in step S310. The repetitive process may be executed in parallel. The map creation unit 142 selects a grid that has not yet been selected in the repetitive process as a target grid. The processes from step S330 to step S360 correspond to a plane extraction process for extracting a plane from each lattice.

[0052] (Step S330) The map creation unit 142 checks whether or not a point group exists within the target grid. The map creation unit 142 transitions to step S340 if a point group exists in the target lattice, such as lattice 32 and lattice 33 shown in Figure 5, and transitions to step S460 if a point group does not exist in the target lattice, such as lattice 34 shown in Figure 5.

[0053] (Step S340) The map creation unit 142 extracts a plane into which the robot apparatus 2 can enter from the group of points present in the target grid. If one or more planes are extracted from the target lattice, the map creation unit 142 transitions to step S370. Otherwise, the map creation unit 142 transitions to step S350.

[0054] FIG. 6 is a schematic diagram showing an example of a plane extraction method for extracting a plane into which the robot apparatus 2 can enter, using a histogram. First, the map creation unit 142 creates a histogram from the number of points contained in each rectangular parallelepiped obtained by dividing the target grid by a certain width in the height direction. Next, the map creation unit 142 determines each bin in the created histogram in which the number of bins is equal to or greater than a threshold value as a plane candidate, and calculates the average height and standard deviation value of the 3D point cloud contained in each bin determined to be a plane candidate. After that, the map creation unit 142 determines the plane candidates in which the standard deviation value of the corresponding height is equal to or less than the threshold value as a plane, and transitions to step S370. The bin containing the 3D point cloud determined to be a plane corresponds to the bin determined to include a plane. The map creation unit 142 transitions to step S350 if no plane is extracted from the target grid. It is preferable that the width of the bin is set to be equal to or less than the upper limit of the height of the step that the robot device 2 can traverse. This is because, in the obstacle extraction process described below, if a bin above a bin whose plane has been extracted contains 3D point cloud data, the bin above is determined to be an obstacle. The upper limit of the height of the step that the robot device 2 can traverse may be a value obtained by multiplying the tire diameter of the tire mechanism 26 by a certain coefficient, or may be an actual measurement value obtained by changing the height of the step in stages and having the robot device 2 actually traverse the step. 6, the threshold for the number of bins determined to be plane candidates is set to 5. Also, the bins determined to be plane candidates in grid 32 have a relatively small standard deviation because they contain only road surface reflection point clouds. On the other hand, the bins determined to be plane candidates in grid 33 have a relatively large standard deviation because they contain both road surface reflection point clouds and tree reflection point clouds.

[0055] When a plane close to a horizontal plane is included in the grid, the points are widely distributed in the grid and the heights of the points are almost the same. Therefore, in this case, the plane can be detected by the above-mentioned plane extraction method. However, when the size of the grid is wider, or when it is desired to extract an inclined plane as a plane that the robot device 2 can enter, the above-mentioned plane extraction method may not be usable. In such cases, the map creation unit 142 may use an algorithm such as RANSAC (RANdom SAmple Consensus) to detect a plane from the group of points included in each grid, and extract the detected plane as a plane that the robot device 2 can enter, provided that the angle between the detected plane and the horizontal plane is equal to or smaller than a threshold. The detected plane is also called a detected plane. The detected plane may not be a flat plane. In the example shown in FIG. 6, only one plane is extracted from one grid, but in cases where a step or a three-dimensional intersection is included within one grid, the map creation unit 142 may extract multiple planes from one grid.

[0056] (Step S350) After completing step S340 for all adjacent lattices to the target lattice, the map creation unit 142 checks whether or not each adjacent lattice to the target lattice contains a plane. Here, only the four lattices existing to the front, rear, left and right of the target lattice may be set as adjacent lattices to the target lattice, or eight lattices, including the four lattices and the lattices diagonally adjacent to the target lattice, may be set as adjacent lattices to the target lattice. The map creation unit 142 transitions to step S360 if at least one adjacent lattice to the target lattice includes a plane, and transitions to step S460 if none of the adjacent lattices to the target lattice includes a plane.

[0057] (Step S360) The map creation unit 142 calculates a virtual plane based on the plane information of the adjacent lattice in which it was determined in step S350 that a plane exists.

[0058] Fig. 7 is a schematic diagram showing an example of a virtual plane calculation method for calculating a virtual plane from four adjacent lattices located to the front, rear, left and right of lattice 33. In Fig. 7, the threshold value of the standard deviation in step S340 is set between the standard deviation calculated for lattice 32 and the standard deviation calculated for lattice 33. Therefore, it is assumed that lattice 32 is determined to include a plane, and lattice 33 is determined to not include a plane. Here, when a grid contains a plane, such as grid 32 adjacent to grid 33 on the right, the average height of each plane contained in the grid is calculated. There is one plane in each grid adjacent to grid 33 on the front, back, left and right, and the average height calculated for the planes contained in each grid is expressed as h f and b and l and r Let us assume that. The map creation unit 142 selects one or more planes whose height difference from the planes included in the adjacent grids is within a predetermined threshold value, and calculates the height h of the virtual plane of the grid 33. c,img Let h be the average height of one or more selected planes. In Fig. 7, all four planes are assumed to be selected, i.e., h c,img =(h f +h b +h l +h r ) / 4. As a specific example, the predetermined threshold is a threshold set to approximately the width of the bin of the histogram used in step S340. In Fig. 7, at most one plane exists in each grid, but even if multiple planes exist in each grid, the map creation unit 142 sets a threshold value for the difference in height between the planes to select a plane included in the grid adjacent to the target grid. The plane included in each grid adjacent to the target grid may be a detected plane or a virtual plane. When a new virtual plane is created in the grid adjacent to the target grid, a new virtual plane may be created for the target grid, or the virtual plane for the target grid may be updated.

[0059] The processes from step S370 to step S450 correspond to the obstacle extraction process. 3 and 4, the obstacle extraction process is divided into two parts: a repetitive process for the detected plane from step S370 to step S410, and a repetitive process for the virtual plane from step S420 to step S450. However, the difference between these two repetitive processes is whether or not the process corresponding to "registering the plane as a candidate plane for movement" in step S400 is performed, and the rest of the process is essentially the same. Therefore, steps S370 to S410, which are the repetitive processes for the detected plane, will be described.

[0060] (Step S370) The process from step S370 to step S410 is a repetitive process that is executed the number of times equal to the number of detection planes detected in step S340.

[0061] (Step S380) The map creation unit 142 extracts obstacles that exist between the height of the detection plane and a height obtained by adding the height of the detection plane to a movable height threshold. The movable height threshold is a threshold determined according to the minimum necessary height of a three-dimensional area in which the robot apparatus 2 can move.

[0062] FIG. 8 is a schematic diagram showing an example of a flow for determining a height for obstacle determination using a histogram. In FIG. 8, it is assumed that in step S340, the standard deviation threshold is set to be equal to or greater than the standard deviation calculated for lattice 33, and thus it is determined that each of lattice 32 and lattice 33 contains a plane. For the histogram created in step S340, the map creation unit 142 determines the bin that is located above the bin determined to include a plane, that is closest to the bin determined to include a plane, and that has one or more bins as the lowest-point bin. After that, the map creation unit 142 determines the lowest point of the 3D point cloud included in the lowest-point bin as the obstacle lowest point, and calculates the difference between the height of the detected plane and the height of the obstacle lowest point as the obstacle height minimum value. If the minimum obstacle height value calculated as in the example of the grid 32 shown in FIG. 8 is equal to or greater than the movable height threshold value set to be equal to or greater than the height of the robot apparatus 2, the map creation unit 142 determines that there is no obstacle and transitions to step S400. The map creation unit 142 determines that an obstacle is present when the obstacle height minimum value is less than the movable height threshold value as in the example of grid 33 shown in Fig. 8. After that, the map creation unit 142 searches each bin located further up from the bin containing the obstacle lowest point in order from below, and determines the 3D point cloud data at the highest position among the 3D point clouds contained in the bin one bin below the bin with 0 bins as the obstacle highest point. After that, the map creation unit 142 calculates the difference between the height of the detection plane and the height of the obstacle highest point as the obstacle height maximum value, and then transitions to step S390. Note that the width of the bin is set taking into consideration the traversability of the robot device 2, so it is not necessary to extract only road surface reflection point clouds that are strictly flat. Therefore, as in the example of the grid 33 shown in Fig. 8, a bin determined to include a flat surface may contain not only road surface reflection point clouds but also tree reflection point clouds.

[0063] (Step S390) The map creation unit 142 registers the plane determined in step S380 to include an obstacle as an obstacle plane.

[0064] 9 and 10 are schematic diagrams showing an example of a method for registering travel candidate planes and obstacle planes in list format in the plane list 41. The plane list 41 corresponds to a planar map. First, the map creation unit 142 assigns i+4 to the grid 33 in which it has been determined in step S380 that an obstacle exists, as an integer ID (identifier) ​​that does not overlap with other planes. Next, the map creation unit 142 registers in the plane list 41 the "coordinate information" (x, y) = (k + 2, l + 2), the "obstacle plane" as the "judgment plane", and the "plane height" and "maximum obstacle height" as numerical information.

[0065] (Step S400) The map creation unit 142 registers the plane determined to be free of obstacles in step S380 as a candidate plane for movement.

[0066] In the example shown in FIG. 9 and FIG. 10, first, the map creation unit 142 assigns i+7 to the grid 32 determined in step S380 to be free of obstacles as an integer ID that does not overlap with other planes. Next, the map creation unit 142 registers (x, y) = (k + 3, l + 2) as "coordinate information", the "movement candidate plane" as the "judgment plane", and only the "plane height" as the numerical information in the plane list 41. Note that since there are no obstacles in the grid 32, the map creation unit 142 may leave the "maximum obstacle height" blank, or may register an invalid value for the "maximum obstacle height".

[0067] (Step S410) This step corresponds to the end point of each iteration in the repeated process started in step S370. When the map creation unit 142 has completed the process for all the detection planes, it transitions to step S460, and when the map creation unit 142 has not completed the process for all the detection planes, it transitions to step S370. Regarding the repeated processing on the virtual plane from step S420 to step S450, whose description has been omitted, if the detected plane is replaced with the virtual plane, step S370 corresponds to step S420, step S380 corresponds to step S430, step S390 corresponds to step S440, and step S410 corresponds to step S450, respectively.

[0068] (Step S460) This step is the end point of each iteration in the repeated processing started in step S320. The map creation unit 142 ends the processing of the flowchart when the processing has been completed for all the grids, and transitions to step S320 in other cases. Through the above processing, a plane list 41 is created in which travel candidate planes and obstacle planes are described in the format of a grid map.

[0069] Fig. 11 is a schematic diagram showing the information of the plane list 41 created by the map creation unit 142 in the form of a grid map by the connection relationship registration unit 143. Fig. 12 shows an extended plane list 42 that extends the plane list 41 so that connection relationships between planes can be described. The extended plane list 42 corresponds to a planar map, and is also called a connection relationship extended plane list. The schematic diagram shown in the form of a grid map in FIG. 11 visually expresses the "coordinate information," "determination plane," and "plane height" in the plane list shown in FIG. Here, since the robot device 2 used in this embodiment is a wheeled robot, if the length of one side of each lattice is set to be equal to or greater than the overall length or width of the robot device 2, it is clear that the length of the sides between the target lattice and each of the adjacent lattices on the front, rear, left and right sides of the target lattice is a length that allows the robot device 2 to pass through. However, since the condition for the robot device 2 to move from one lattice to a plane included in an adjacent lattice is not limited to the condition related to the length of one side of the lattice, it is necessary to register the connection relationship between the planes. Therefore, the connection relationship registration unit 143 registers the connection relationship according to the following procedure. 1. The connection relationship registration unit 143 searches for movement candidate planes in each lattice adjacent to the lattice containing each movement candidate plane (in FIG. 11, four adjacent lattices on the front, back, left and right). 2. The connection relationship registration unit 143 compares the height of each movement candidate plane with the height of the movement candidate plane of the lattice adjacent to the lattice containing each movement candidate plane, and if the difference in plane height between the adjacent lattices is less than or equal to the traversable threshold, it determines that movement is possible and registers the connection relationship.

[0070] A method for registering a connection relationship when the traversable threshold is 0.25 m will be described using the candidate plane with ID=10 shown in Figures 11 and 12 as an example. In the description of this example, the candidate plane with ID=10 is taken as the target plane, and the grid including the target plane is taken as the target grid. In the grid adjacent to the target grid in the x+1 direction, there is a candidate plane for movement with ID=11 as an adjacent plane. Here, the height difference between the target grid and the adjacent plane is 0.4 m. Since the height difference exceeds the traversable threshold, it is determined that it is not possible to move from the target grid to the adjacent plane, and the connection relationship between the target grid and the adjacent plane is not registered in the extended plane list 42. Since no plane exists in the lattice adjacent to the target lattice in the x-1 direction, no connection relationship is registered in the extended plane list 42 between the two lattices. The grid adjacent to the target grid in the y+1 direction has a candidate plane for movement with ID=15 as an adjacent plane. Here, the height difference between the target grid and the adjacent plane is 0.8 m. Since the height difference exceeds the traversable threshold, it is determined that it is not possible to move from the target grid to the adjacent plane, and the connection relationship between the target grid and the adjacent plane is not registered in the extended plane list 42. The grid adjacent to the target grid in the y-1 direction has a candidate plane for movement with ID=7 as an adjacent plane. Here, the height difference between the target grid and the adjacent plane is 0 m. Since the height difference is equal to or less than the traversable threshold, it is determined that the target grid can be moved to the adjacent plane, and the connection relationship between the target grid and the adjacent plane is registered in the extended plane list 42. The connection relationship registration unit 143 creates an extended plane list 42 by performing a process of appropriately registering connection relationships for all movement candidate planes in the plane list 41 in this manner.

[0071] 13 and 14 are flowcharts showing an example of the flow of a route search process by the route creation unit 144. The flow of this process will be described with reference to FIGS. This flowchart was created based on an existing algorithm called the A* (Aster) method.

[0072] (Step S510) The path creation unit 144 acquires the movement start position and movement target position input via the program input / output unit 141 , and the extended plane list 42 created by the connection relationship registration unit 143 . The movement start position and the movement target position may be input as local coordinate values ​​of three-dimensional global point cloud data, or data indicating latitude, longitude, altitude, etc. The route creation unit 144 assigns an ID to each acquired position. In the explanation of this flowchart, the extended plane list 42 is assumed to be that shown in FIG. 12, the movement start position ID: SID=1, and the movement target position ID: GID=10.

[0073] (Step S520) The route creation unit 144 initializes the unsearched list with the movement start position, and initializes the searched list with an empty list.

[0074] FIG. 15 is a schematic diagram showing the search order of the grid map shown in FIG. 16 to 18 show the transitions of the unsearched list and the searched list from the start to the end of processing. Both the unsearched list and the searched list must hold at least three pieces of information: ID, parent ID, and cost. For ease of understanding, we will assume that both the unsearched list and the searched list also hold coordinate information. The parent ID is the ID of the plane one step back on the route. However, since there is no such plane at the start position of the movement, the ID of the start position of the movement is set as the parent ID corresponding to the start position of the movement. If the cost decreases as the user approaches the target location, it is possible to generate a route to reach the target location based on the cost. There are several methods for calculating such a cost, but in this embodiment, we use [Formula 1] as the formula for calculating the cost of the target ID.

[0075] [Formula 1] C(TID)=C(PID)+1+M(TID,GID)-M(PID,GID)

[0076] Here, C(TID) indicates the cost of the target ID:TID, C(PID) indicates the cost of the parent ID:PID, M(TID,GID) indicates the Manhattan distance from the target ID to the movement target position ID, and M(PID,GID) indicates the Manhattan distance from the parent ID to the movement target position ID. The movement target position ID is the ID of the movement target position. The Manhattan distance between two IDs is the distance between the lattices corresponding to each of the two IDs. For the ID to calculate the cost in the initial unsearched list, the parent ID is the target ID (own ID), so the cost of the parent ID, C(PID) = 0. Also, since M(ID,GID) = M(PID,GID), M(ID,GID) and M(PID,GID) cancel each other out. Therefore, C(1) = 1.

[0077] (Step S530) The route creation unit 144 checks whether the unsearched list is empty. If the unsearched list is not empty, the route creation unit 144 transitions to step S540. If the unsearched list is empty, the route creation unit 144 concludes that the movement target position cannot be reached, i.e., ends the process of this flowchart as "non-reach end". In the initial state, the route creation unit 144 transitions to step S540 because the unsearched list is not empty.

[0078] (Step S540) The route creation unit 144 sorts the unsearched list in ascending order of cost. In this case, the sorting algorithm may be a commonly known quick sort or bubble sort. Since there may be a plurality of IDs with the same corresponding cost, a second priority is set in the sorting, such that the y component of the coordinate information is closer to the y component of the movement target position. Note that the second priority does not have to be set in the sorting.

[0079] (Step S550) The route creation unit 144 acquires the ID of the minimum-cost row in the unsearched list as the current search ID. At this time, since the unsearched list was sorted in ascending cost order in step S540, the route creation unit 144 may acquire the top row of the unsearched list as the minimum-cost row in the unsearched list. In the initial state shown in FIG. 16, the current search ID: NID=1.

[0080] (Step S560) The route creation unit 144 confirms that the current search ID is not equal to the movement target position ID. If the current search ID and the movement target position ID are not equal, the route generation unit 144 transitions to step S570, and otherwise transitions to step S680. In the first iteration of the repetitive process, the current search ID: NID=1 and the movement target position ID: GID=10, so the route creation unit 144 transitions to step S570.

[0081] (Step S570) The path creation unit 144 refers to the extended plane list 42 and acquires an ID that has a connection relationship with the current search ID as a connection ID. If the current search ID: NID=1, then the combined ID: CID=2,5.

[0082] (Step S580) The process from step S580 to step S630 is a repetitive process that is executed the number of times equal to the number of connection IDs acquired in step S570. The path creation unit 144 selects a connection ID that has not yet been selected in the repetitive process as a target connection ID.

[0083] (Step S590) The path creation unit 144 calculates the cost of the target link ID. When the costs C(2) and C(5) of the bond IDs CID=2, 5 are calculated using formula (1), C(2)=C(5)=1.

[0084] (Step S600) The path creation unit 144 searches each of the unsearched list and the searched list to see if there is a row having the same ID as the target link ID.

[0085] (Step S610) Based on the search result in step S600, the path creation unit 144 confirms that there is no ID that is the same as the target link ID in both the unsearched list and the searched list. The path creation unit 144 transitions to step S620 if there is no ID that is the same as the target link ID in either of the lists, and transitions to step S650 if there is an ID that is the same as the target link ID in either the unsearched list or the searched list. In the first iteration of the repetition process, since the unsearched list and the searched list remain in the initial state, the same ID as the connection ID: CID=2, 5 does not exist in either list. Therefore, when the target connection ID is 2 or 5, the path creation unit 144 transitions to step S620.

[0086] (Step S620) In the unsearched list, the path creation unit 144 registers the target link ID in "ID", the current search ID in "parent ID", and the calculated cost in "cost".

[0087] (Step S630) This step is the end point of each iteration in the repeating process started in step S580. The path creation unit 144 transitions to step S640 when the process is completed for all the connection IDs, and transitions to step S580 when the process is not completed for all the connection IDs.

[0088] (Step S640) The path creation unit 144 moves the row of the current search ID from the unsearched list to the searched list. 16B shows the unsearched list and the searched list at the end of the first round of step S640. The row with the current search ID: NID=1 has been moved to the searched list, and the rows with the combined IDs CID=2 and CID=5 have been added to the unsearched list.

[0089] After that, the process returns to step S530, and each step is performed in order again. In step S550, since the cost is the same between CID=2 and CID=5, the current search ID becomes NID=5 according to the second priority. In step S570, the combined ID becomes CID=1,9. In the repeated process for each bond ID starting from step S580, if the bond ID: CID=1, the cost C(1) calculated in step S590 is 3, and ID=1 is in the searched list. Therefore, in step S610, a branch to step S650 occurs.

[0090] (Step S650) The path creation unit 144 checks whether or not the unsearched list contains an ID that is the same as the target link ID. The path creation unit 144 transitions to step S660 when the unsearched list contains the same ID as the target link ID. Otherwise, that is, when the searched list contains the same ID as the target link ID, the path creation unit 144 transitions to step S670. Since ID=1 exists in the searched list, the route creation unit 144 transitions to step S670. Note that the process of updating the lists in steps S660 and S670 is the same except that the lists being updated are different, so only step S670 will be described and a description of step S660 will be omitted.

[0091] (Step S670) First, the path creation unit 144 compares the cost of the target link ID with the cost of the same ID as the target link ID in the searched list. If the most recently calculated cost is smaller in step S590, the path creation unit 144 updates the "parent ID" of the row of the same ID to the parent ID corresponding to the most recently calculated cost, and updates the "cost" to the most recently calculated cost. Next, the path creation unit 144 moves the row with the same ID from the searched list to the unsearched list. Note that in step S660, only the list is updated, and the rows within the list are not moved.

[0092] For ID=1, the most recently calculated cost C(1) = 3, and the cost of the searched list C(1) = 1. Therefore, the searched list is not updated, and the row with ID=1 is not moved. For CID=9, the cost C(9)=3, and CID=9 does not exist in either list, so the row with CID=9 is listed in the unsearched list. As a result, the unsearched list and the searched list at the end of the second round of step S640 are as shown in FIG. 16(c).

[0093] After that, the process returns to step S530, and each step is executed in order again. In step S550, since the cost is the same between ID=2 and ID=9, the current search ID becomes NID=9 according to the second priority. In step S570, the combined ID becomes CID=5,12. For CID=5, the cost C(5)=3, so the cost indicated by the searched list is smaller, so the list is not updated. For CID=12, the cost C(12)=3, and the cost C(12) is placed on the unsearched list. As a result, the unsearched list and the searched list at the end of the third round of step S640 are as shown in FIG. 17(a).

[0094] After that, the process returns to step S530, and each step is executed in order again. At step S550, a cost difference occurs. Therefore, the current search ID becomes NID=2, and the route creation unit 144 starts a search from the parent ID PID=1 again. The remaining repeated processes are omitted, but in the seventh iteration, at step S560, the current search ID: NID=10, that is, the current search ID becomes the movement target position ID, and the process branches to step S680.

[0095] (Step S680) The route creation unit 144 creates a route list by tracing the searched list from the movement target position to the movement start position in order of parent IDs, and ends the processing of this flowchart with the result "arrival completed." Specifically, the parent ID corresponding to the current search ID: NID=10 is PID=7, and the parent ID of the row with ID=7 in the searched list is PID=3, and so on. By tracing the parent IDs in order, it is possible to obtain coordinate information for the route connecting the start position of movement, ID=1→2→3→7→10, to the target position of movement.

[0096] Here, it is possible to control the robot device 2 with respect to the path using only the coordinate information of the path. However, in controlling the robot device 2 with respect to the path, it is common to use speed information at each position on the path. Fig. 19 shows a route list 51 to which speed information has been added. The route list 51 corresponds to (b) of Fig. 18. The route creation unit 144 may generate the route list 51. In the speed information, only the speed corresponding to the movement target position may be set to zero, and the speeds corresponding to all other positions on the path may be set to the maximum speed of the robot apparatus 2. However, when the robot apparatus 2 is engaged in transportation work, attention must also be paid to acceleration. For this reason, it is desirable to provide speed information indicating that the robot apparatus 2 gradually accelerates from the movement start position, decelerates before the corner, accelerates again to return to the rated speed after passing the corner, and gradually decelerates to zero before the movement target position. FIG. 19 shows speed settings in which the absolute upper limit of the speed change for moving one space between the lattices is set to 2 km / h, the upper speed limit at the corner is set to 3 km / h, and the maximum speed of the robot apparatus 2 is set to 3.5 km / h. In FIG. 19, the initial speed at ID=1 is 2 km / h, which is the upper limit of the absolute value of the speed change. At ID=2, adding the upper limit of the absolute value of the speed change to the initial speed exceeds the maximum speed. Therefore, the speed at ID=2 is 3.5 km / h, which is the maximum speed. Since ID=3 corresponds to a corner, the robot device 2 decelerates to 3 km / h, which is the upper limit of the speed at a corner, at ID=3. Since ID=7 is one position before the movement target position, the speed at ID=7 is 2 km / h. The speed at ID=10, which is the movement target position, is 0 km / h.

[0097] In the first embodiment, the route generation program 14 is included in the control device 1, but the robot device 2 may execute the processing of the route generation program 14 by using the three-dimensional global point cloud data included in the storage unit 22. Moreover, in the first embodiment, there is only a description of the control device 1 and one robot device 2. However, the control device 1 may generate paths for a plurality of robot devices 2, and the configurations and mechanisms of the robot devices 2 do not need to be identical to each other.

[0098] ***Explanation of the Effects of the First Embodiment*** As described above, according to this embodiment, by taking into consideration not only the traversal ability of the autonomous mobile robot but also the dimensions of the autonomous mobile robot, such as its overall height, if there is no structure within a three-dimensional area through which the autonomous mobile robot passes while moving, it is not determined that an obstacle is present within the three-dimensional area. This eliminates the need to place restrictions on sensor placement and enables the autonomous mobile robot to move indoors, under bridge girders, under tree branches, etc.

[0099] ***Other configurations*** <Variation 1> FIG. 20 shows an example of the hardware configuration of the control device 1 according to this modified example. The control device 1 includes a processing circuit 111 in place of the processor 101 , the processor 101 and a memory 102 , the processor 101 and an auxiliary storage device 103 , or the processor 101 , the memory 102 , and the auxiliary storage device 103 . The processing circuit 111 is hardware that realizes at least a part of each unit of the control device 1. The processing circuitry 111 may be dedicated hardware, or may be a processor that executes a program stored in the memory 102 .

[0100] When the processing circuitry 111 is dedicated hardware, the processing circuitry 111 may be, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or a combination thereof. The control device 1 may include a plurality of processing circuits that replace the processing circuit 111. The plurality of processing circuits share the role of the processing circuit 111.

[0101] In the control device 1, some of the functions may be realized by dedicated hardware, and the remaining functions may be realized by software or firmware.

[0102] The processing circuitry 111 may be implemented, for example, in hardware, software, firmware, or a combination thereof. The processor 101, the memory 102, the auxiliary storage device 103, and the processing circuit 111 are collectively referred to as the "processing circuitry." In other words, the functions of the functional components of the control device 1 are realized by the processing circuitry. Each device according to the other embodiments may have a similar configuration to this modified example.

[0103] Embodiment 2 The following mainly describes the differences from the above-described embodiment with reference to the drawings. When the robot device 2 is engaged in transportation work, the maximum speed that can be output on an uphill road may decrease depending on the weight of the load. In addition, when the robot device 2 enters a road with unevenness at high speed, vertical acceleration occurs, which may cause a risk of damaging the load. Therefore, in this case, attention must be paid to the posture and acceleration of the robot device 2. Therefore, it is desirable to reduce the speed of the robot device 2 on a road surface with a vertical incline or unevenness, as compared to the speed on a flat road surface. Therefore, in this embodiment, a speed constraint on a detection plane that is not flat is added.

[0104] ***Configuration Description*** The route generation system 90 according to the second embodiment is similar to the route generation system 90 according to the first embodiment.

[0105] ***Explanation of Operation*** FIG. 21 shows an example of the route list 52 in which coordinate information, plane height, and speed information of the route are described. Fig. 22 is a diagram corresponding to Fig. 21. Fig. 22(a) shows a graph showing the plane heights in the order of the via planes. Fig. 22(b) shows a graph showing the speed constraints in the order of the via planes. In the examples shown in FIGS. 21 and 22, the step threshold between the grids is set to 3 cm.

[0106] The path creation unit 144 in this embodiment treats multiple consecutive planar areas passed through on the created path as a target continuous plane, determines the road surface conditions of the target continuous plane based on the height difference between the planar areas of the target continuous plane, and assigns a speed corresponding to the determined road surface conditions of the target continuous plane as the speed of the target moving body on the target continuous plane. As a specific example, the path creation unit 144 executes a process for determining an inclined portion and an uneven portion. The process for determining an inclined portion and an uneven portion is a process for creating a speed constraint on a surface having at least one of an inclination and an uneven portion. In the process for determining an inclined portion and an uneven portion, the path creation unit 144 determines that a plurality of planar regions are upwardly inclined portions when the height between adjacent planar regions increases continuously and monotonically by a step threshold or more, determines that a plurality of planar regions are downwardly inclined portions when the height between adjacent planar regions decreases continuously and monotonically by a step threshold or more, determines that a plurality of planar regions are uneven portions when an increase in height by a step threshold or more and a decrease in height by a step threshold or more are alternately repeated, and otherwise determines that a plurality of planar regions are flat portions. The upwardly inclined portions and the downwardly inclined portions are collectively called inclined portions. Note that a plurality of planar regions may be applicable to both an inclined portion and an uneven portion. As a specific example, the lattice G and the lattice H are applicable to both an upwardly inclined portion and an uneven portion.

[0107] 22(b), "Initial speed, cornering, and stopping constraints" indicate speed constraints of the robotic device 2 when the absolute upper limit of the speed change when moving between one grid is 1 km / h, the upper speed limit around corners is 2 km / h, and the maximum speed of the robotic device 2 is 4 km / h. On the other hand, "Speed ​​constraints on slopes and unevenness" indicates speed constraints of the robotic device 2 when the maximum speed of 4 km / h is permitted on flat sections, the upper speed limit on uneven sections is 2.5 km / h, the upper speed limit on upward slope sections is 3.5 km / h, and the upper speed limit on downward slope sections is 3 km / h. The "target speed setting" is a target value for the speed setting of the robot device 2 in each planar region, and is obtained by selecting a lower speed from the speed constraints in each planar region. Here, for the "target speed setting" of each of the lattices G and H, the upper limit speed in the uneven portion is set because the upper limit speed in the uneven portion is lower than the upper limit speed in the upwardly sloping portion. 21 and 22, the upper limit speeds on the inclined portion and the uneven portion are set to constant values, but the upper limit speeds may be changed according to the load on the robot device 2, such as the weight of the load. The upper limit speed may also be changed according to the gradient of the inclined portion (the degree of the monotonically changing step height) or the size of the step on the uneven portion.

[0108] ***Explanation of the effect of the second embodiment*** According to this embodiment, the speed of the robot apparatus 2 is set taking into consideration unevenness and inclinations. Therefore, this embodiment is more suitable for the case where the robot apparatus 2 is engaged in transportation work.

[0109] Embodiment 3 The following mainly describes the differences from the above-described embodiment with reference to the drawings. When the robot apparatus 2 detects a pedestrian, a bicycle, or the like on its path, it is required to slow down or stop. However, since it may be difficult to detect a pedestrian, a bicycle, or the like because an obstacle or the like may enter a blind spot on the path to be traveled in the future, it is desirable to slow down the speed of the robot apparatus 2 near the blind spot. Therefore, in this embodiment, a speed constraint regarding the blind spot is set.

[0110] ***Configuration Description*** The route generation system 90 according to the third embodiment is similar to the route generation system 90 according to the first embodiment.

[0111] The route creation unit 144 according to this embodiment selects a first point and a second point that exist on the created route. When a target obstacle group exists on the line segment connecting the first point and the second point, the route creation unit 144 determines whether or not the second point is a blind spot with respect to the first point due to the presence of an obstacle between the first point and the second point, based on the height of each planar area included in the target obstacle group and the height of the obstacle in each planar area included in the target obstacle group. The target obstacle group is made up of one or more planar areas classified as an obstacle plane. Here, on the route, the target moving object passes the second point after passing the first point. The distance between the first point and the second point is equal to or less than the viewing distance. As a specific example, the viewing distance is set to equal to or less than the detection distance of the three-dimensional point cloud sensor 23. When the route creation unit 144 determines that the second point is in a blind spot relative to the first point, it assigns, as the speed of the target moving body at the first point, a speed that is lower than the speed of the target moving body at the first point that would be assigned when it is determined that the second point is not in a blind spot relative to the first point. In addition, when a target obstacle group is present along the line segment connecting the first point and the second point, the route creation unit 144 may calculate the size of the target blind spot, which is the blind spot of the target moving body and is the blind spot of the second point when the target moving body is present at the first point, based on the height of each planar area included in the target obstacle group, the height of the obstacles in each planar area included in the target obstacle group, and the field of view of the target moving body, and determine whether the second point is a blind spot relative to the first point based on the size of the target blind spot.

[0112] ***Explanation of Operation*** FIG. 23 shows a route list 53 which lists the route, the "coordinate information", the "plane height", the "maximum obstacle height", and the "target speed setting" of an obstacle. Fig. 24(a) shows a graph showing the plane height in the order of the via planes. Fig. 24(b) shows a graph showing the shortest blind spot grid distance in the order of the via planes. Fig. 24(c) shows a graph showing the speed constraints in the order of the via planes. FIG. 25(a) shows a schematic diagram illustrating the layout relationship between the movement candidate plane and the obstacle plane. In FIG. 24, a "blind spot speed constraint" is introduced as a new speed constraint, and the shortest blind spot grid distance is first calculated to calculate the "blind spot speed constraint". A blind spot grid is a grid to be judged when there is an obstacle plane that satisfies a predetermined condition between a line segment connecting the centers of two grids, a reference grid and a grid to be judged. In this example, when a line segment on a side or corner of an obstacle plane only touches the line segment, the obstacle plane is not regarded as an obstacle plane between the line segment.

[0113] An obstacle plane that satisfies a predetermined condition is an obstacle plane that prevents the detection of an object present in a grid to be judged by an external sensor such as the three-dimensional point cloud sensor 23 of the robot apparatus 2. In the example shown in Fig. 23 to Fig. 25, the path creation unit 144 judges whether an obstacle plane satisfies a predetermined condition based on whether the corresponding "maximum obstacle height" is equal to or greater than a blind spot judgment height threshold (0.75 m). The blind spot judgment height threshold is determined according to the installation height of the three-dimensional point cloud sensor 23 and the attitude of the robot apparatus 2. By setting a threshold for the obstacle height, it is possible to exclude obstacle planes such as curbs that do not prevent the detection by the three-dimensional point cloud sensor 23.

[0114] The shortest blind grid distance is the Manhattan distance from the reference grid to the grid that is first determined to be a blind grid when the above-mentioned determination is performed in order from the grid two grids ahead of the reference grid on the route toward the movement target position. The reference grid corresponds to the first point. Each grid that exists ahead of the first point on the route corresponds to the second point. Note that since there is no obstacle plane between the reference grid and the grid one grid ahead on the route, the grid one grid ahead on the route cannot be a blind grid. Therefore, there is no need to determine the grid one grid ahead on the route. Therefore, the minimum value of the shortest blind grid distance is 2.

[0115] Here, the procedure for finding the shortest blind spot grid distance corresponding to grid a will be explained. Grid h is the grid two steps ahead of grid a on the route. Grid h is not judged as a blind spot grid because the maximum obstacle height of grid b, which exists between the line segment connecting grid a and grid h, does not exceed the blind spot judgment height threshold. The same applies to grid i, which is the grid three steps ahead of grid a on the route. On the other hand, the maximum obstacle height of the grid c existing between the line segment connecting the grid a and the grid j exceeds the blind spot determination height threshold, so the grid j is determined to be a blind spot grid. Therefore, since the Manhattan distance between lattice a and lattice j is 4, the shortest blind lattice distance corresponding to lattice a is 4.

[0116] The "blind spot speed constraint" is a speed limit set according to the shortest blind spot grid distance. In the example shown in FIG. 24(b), the blind spot speed constraint is 1 km / h when the shortest blind spot grid distance is 2, 2 km / h when the shortest blind spot grid distance is 3, 3 km / h when the shortest blind spot grid distance is 4, and the maximum speed (4 km / h) when the shortest blind spot grid distance is 5 or more. The "target speed setting" is obtained by selecting the lowest upper speed limit of each speed constraint for each planar region.

[0117] The path generating unit 144 may determine that a grid is a blind spot grid when the visual field reduction rate exceeds a threshold. As a specific example, in the example shown in (a) of Figure 25, when trying to detect an object existing within grid i to grid d, there is a high possibility that an obstacle existing in grid c with a maximum obstacle height of 2.0 m is blocking part of the field of view of the 3D point cloud sensor 23. FIG. 25(b) is a schematic diagram showing an example of a method for calculating the reduction rate of the visual field by taking into consideration an obstacle grid that exists near a line segment connecting two grids. When calculating the visual field reduction rate of the grid d based on the grid i, first, a line segment is drawn from the center of the grid i, which is assumed to be the position of the 3D point cloud sensor 23, to each of the four corners of the grid d. An obstacle plane (grid c) is included between the line segment to the lower left corner of the grid d and the line segment to the lower right corner of the grid d. Therefore, the path creation unit 144 extends each line segment when it is possible to extend each line segment to the opposite side or diagonal of the grid d, and the area within the grid d that is sandwiched between the two line segments is set as the visual field reduction area. The visual field reduction area of ​​the grid d based on the grid i is only due to the grid c, which is the obstacle plane. In other words, the visual field reduction area is a triangular area with the lower left corner, lower right corner, and upper left corner of the grid d as vertices, and the visual field reduction rate is 1 / 2 (50%). In addition, when calculating the visual field reduction rate of grid e based on grid i, a line segment is drawn from the center of grid i to each of the four corners of grid e. An obstacle plane (grid k) is included between the line segment to the upper left corner of grid e and the line segment to the upper right corner of grid e. Therefore, the path creation unit 144 extends the line segment to the upper left corner to the opposite side of grid e, and the area in grid e that is sandwiched between the two line segments is the visual field reduction area. The visual field reduction area of ​​grid e based on grid i is only due to grid k, which is an obstacle plane. In other words, the visual field reduction area is a triangle with vertices at the upper left corner, upper right corner, and upper right point of grid e. Since the slope of the line segment from the center of grid i to the upper right point of grid e is 1 / 3, the upper right point is located at a distance of 1 / 3 of the length of the right side from the upper right corner of grid e. In other words, the visual field reduction rate is 1 / 6 (16.7%). If the visual field reduction rate threshold for blind spot grid determination is 1 / 3 (33.3%), then when grid i is used as the reference, grid d is determined to be a blind spot grid, and grid e is not determined to be a blind spot grid.

[0118] In (b) of Fig. 25, in order to simplify the explanation, the explanation is given using a field of view reduction area caused by a single obstacle plane. However, when a field of view reduction area caused by multiple obstacle planes is included, it is necessary to perform a calculation taking into account the overlap of the field of view reduction areas. 25(b), the center of the grid is assumed to be the position of the 3D point cloud sensor 23. However, if the attachment position of the 3D point cloud sensor 23 on the robot device 2 is away from the center, another position within the grid may be assumed to be the position of the 3D point cloud sensor 23, and a line segment may be extended from the other position to the grid where the blind spot grid determination is performed. In addition, in this embodiment, the field of view reduction area is calculated using a simple planar grid so that a general computer can perform the calculation. However, when a high-performance computer is used or the performance of the computer is improved in the future, each obstacle plane has a 3D model of each obstacle included in each obstacle plane, and the detection range of the three-dimensional point cloud sensor 23 can be calculated, the field of view reduction area may be calculated using a collision determination method such as ray tracing.

[0119] ***Explanation of the Effects of the Third Embodiment*** As described above, according to this embodiment, the speed constraint regarding the blind spot can be set appropriately.

[0120] Embodiment 4 The following mainly describes the differences from the above-described embodiment. When the robot device 2 performs its work in a place used by many people, such as a commercial facility or an apartment building, the arrangement of obstacles may change continuously due to automobiles parked for long periods for transportation work or business signs that are only placed during the day. Therefore, when such obstacles are added or removed, a contradiction occurs between the grid map and the real environment. If the contradiction cannot be resolved, a route that can be passed through may not be generated, or conversely, a route that cannot be passed through in reality may be generated. Therefore, it is desirable to change the grid map to match the real environment. Therefore, in this embodiment, the grid map is changed to match the real environment.

[0121] ***Configuration Description*** The route generation system 90 according to the fourth embodiment is similar to the route generation system 90 according to the first embodiment.

[0122] The map creation unit 142 according to the present embodiment performs the inconsistency determination a reference number of times or more. The inconsistency determination is a process of determining whether or not there is an inconsistency between a 3D point cloud corresponding to a measurement target area in an environmental map made up of 3D point clouds previously measured in the target area, and a 3D point cloud newly acquired by measuring the measurement target area by the 3D point cloud sensor 23. The measurement target area is at least a part of the target area. When the map creation unit 142 determines that there is a contradiction a reference number of times or more in the contradiction determination, the map creation unit 142 updates the classification of each planar area corresponding to the measurement target area so as to resolve the contradiction corresponding to the contradiction determination.

[0123] The path creation unit 144 in this embodiment recreates a path for at least one planar area corresponding to the measurement target area when the classification of a planar area whose corresponding classification is an obstacle plane is updated to a movement candidate plane, or when the classification of a planar area whose corresponding classification is a movement candidate plane is updated to an obstacle plane.

[0124] ***Explanation of Operation*** The surrounding environment detection unit 273 regards the local point cloud data in the portion that does not match the global point cloud data as a contradictory point cloud, and determines whether the contradictory point cloud corresponds to a temporary obstacle or a temporary plane. The robot device 2 transmits to the control device 1 data indicating its own position, the positions of the contradiction point group, and the determination result.

[0125] The map creation unit 142 compares the completed grid map with the grid corresponding to the position of the contradiction point group. When the map creation unit 142 continuously determines that the grid corresponding to the movement candidate plane is a temporary obstacle, the map creation unit 142 changes the classification of the planar area corresponding to the grid to an obstacle plane. Also, when the map creation unit 142 continuously determines that the grid corresponding to the obstacle plane is a temporary plane, the map creation unit 142 changes the classification of the planar area corresponding to the grid to a movement candidate plane. In addition, since the robot device 2 continues to move, continuous judgments refer to, for example, a case in which the robot device 2 passes within a certain range of a position corresponding to a certain group of contradictory points, and the judgment results corresponding to the group of contradictory points transmitted at a certain period of time continuously show the same judgment.

[0126] Even if the robot device 2 is present within a certain range of the position corresponding to the contradiction point cloud, for example, when the surrounding area is crowded, the position corresponding to the contradiction point cloud may be in a blind spot of the 3D point cloud sensor 23. Therefore, the surrounding environment detection unit 273 may transmit an invalid value when it is unable to detect the position corresponding to the contradiction point cloud. Furthermore, when an invalid value is transmitted, the map creation unit 142 may withhold the determination as to whether or not the determination has been made continuously.

[0127] ***Explanation of the effect of the fourth embodiment*** As described above, according to this embodiment, the grid map can be appropriately changed to suit the actual environment.

[0128] ***Other embodiments*** The above-described embodiments may be freely combined, or any of the components in each embodiment may be modified, or any of the components in each embodiment may be omitted. Moreover, the embodiments are not limited to those shown in the first to fourth embodiments, and various modifications are possible as necessary. The procedures explained using the flowcharts and the like may be modified as appropriate. [Explanation of symbols]

[0129] 1 Control device, 11 User input / output unit, 12 Communication unit, 13 Memory unit, 14 Path generation program, 141 Program input / output unit, 142 Map creation unit, 143 Connection relationship registration unit, 144 Path creation unit, 2 Robot device, 21 Communication unit, 22 Memory unit, 23 3D point cloud sensor, 24 Rotation sensor, 25 Actuator, 26 Tire mechanism, 27 Control program, 271 Program input / output unit, 272 Self-position estimation unit, 273 Surrounding environment detection unit, 274 Path following calculation unit, 275 Mechanism calculation unit, 31 3D global point cloud data, 32, 33, 34 Grid, 41 Plane list, 42 Extended plane list, 51, 52, 53 Path list, 90 Path generation system, 101 Processor, 102 Memory, 103 Auxiliary storage device, 104 Display interface, 105 Communication interface, 106 Input interface, 107 display, 108 wired LAN port, 109 keyboard, 110 mouse, 111 processing circuit.

Claims

1. When an environmental map consisting of a three-dimensional point cloud measured in a target area is divided into a plurality of grids so that a plane included in the target area is divided, each grid of the plurality of grids is set as a target grid, and when it is determined that a three-dimensional point cloud corresponding to a planar area exists within the target grid, a planar area is extracted from the target grid; A planar area extracted from each grid from which a planar area is extracted is taken as a target plane, and in a grid including the target plane, if a three-dimensional point cloud exists between the height of the target plane and a height obtained by adding the height of the target plane to a movable height threshold value that is equal to or greater than the height of the target moving body, the target plane is classified as an obstacle plane; a map creation unit that creates a planar map showing the planar areas that are not classified as obstacle planes among the extracted planar areas; a route creation unit that creates a route for the target moving object from a movement start position on the planar area that is not classified as an obstacle plane and that is shown on the planar map, to a movement target position on the planar area that is not classified as an obstacle plane and that is shown on the planar map, based on the planar area that is not classified as an obstacle plane and that is shown on the planar map; A route generation device comprising:

2. the map creation unit includes information indicating the height of each planar area shown on the planar map, The path generation device of claim 1, wherein the path creation unit defines a plurality of consecutive planar areas passed through on the path as a target continuous plane, determines the road surface conditions of the target continuous plane based on the height difference between the planar areas of the target continuous plane, and assigns a speed corresponding to the determined road surface conditions of the target continuous plane as the speed of the target moving body on the target continuous plane.

3. A path generation device as described in Claim 2, wherein the upper limit speed of the target moving body on the target continuous plane is a speed set according to either the magnitude of the inclination of the inclined portion of the target continuous plane or the magnitude of the step of the uneven portion of the target continuous plane.

4. A path generation device as described in Claim 2, wherein the upper limit speed of the target moving body in the target continuous plane is a speed set according to the load of the target moving body.

5. the map creation unit includes each planar area classified as an obstacle plane in the planar map, includes information indicating a height of each planar area shown by the planar map in the planar map, includes information indicating a height of an obstacle in each planar area classified as an obstacle plane in the planar map, The path creation unit selecting a first point and a second point on the route; when one or more planar areas classified as the obstacle plane as a target obstacle group are present along the line segment connecting the first point and the second point, determining whether or not the second point is in a blind spot relative to the first point due to the presence of an obstacle between the first point and the second point, based on the height of each planar area included in the target obstacle group and the height of an obstacle in each planar area included in the target obstacle group; when it is determined that the second point is in a blind spot with respect to the first point, assigning, as the speed of the target moving body at the first point, a speed that is lower than the speed of the target moving body at the first point that would be assigned when it is determined that the second point is not in a blind spot with respect to the first point; On the route, the target moving object passes through the first point and then passes through the second point, The route generation device according to claim 1 or 2, wherein the distance between the first point and the second point is equal to or less than a line-of-sight distance.

6. 6. The path generation device according to claim 5, wherein, when the target obstacle group is present along the line segment connecting the first point and the second point, the path creation unit calculates the size of a target blind spot, which is a blind spot of the target moving body and is a blind spot of the second point when the target moving body is present at the first point, based on the height of each planar area included in the target obstacle group, the height of an obstacle in each planar area included in the target obstacle group, and the field of view of the target moving body, and determines whether the second point is a blind spot relative to the first point based on the size of the target blind spot.

7. The map creation unit If the target plane is not classified as the obstacle plane, classify the target plane as a movement candidate plane; performing a contradiction determination for a predetermined number of times or more in succession to determine whether or not there is a contradiction between a three-dimensional point cloud corresponding to a measurement target area, which is at least a part of the target area, in an environmental map made up of three-dimensional point clouds previously measured in the target area, and a three-dimensional point cloud newly acquired by measuring the measurement target area with a three-dimensional point cloud sensor; When it is determined in the inconsistency determination that there is a contradiction consecutively for the reference number of times or more, the classification of each planar area corresponding to the measurement target area is updated so that the contradiction corresponding to the inconsistency determination is resolved; 3. The path generation device according to claim 1, wherein the path creation unit recreates the path when, for at least one planar area corresponding to the measurement target area, the classification of the planar area whose corresponding classification is the obstacle plane is updated to the movement candidate plane, or when the classification of the planar area whose corresponding classification is the movement candidate plane is updated to the obstacle plane.

8. The path generation device according to claim 1 or 2, wherein the target moving object is a robotic device that moves by tires.

9. The path generation device according to claim 8, wherein the length of each side of the faces of the target lattice corresponding to the planar areas included in the target area is equal to or greater than the greater of the total length and total width of the target moving body.

10. The computer When an environmental map consisting of a three-dimensional point cloud measured in a target area is divided into a plurality of grids so that a plane included in the target area is divided, each grid of the plurality of grids is set as a target grid, and when it is determined that a three-dimensional point cloud corresponding to a planar area exists within the target grid, a planar area is extracted from the target grid; A planar area extracted from each grid from which a planar area is extracted is taken as a target plane, and in a grid including the target plane, if a three-dimensional point cloud exists between the height of the target plane and a height obtained by adding the height of the target plane to a movable height threshold value that is equal to or greater than the height of the target moving body, the target plane is classified as an obstacle plane; creating a planar map showing the planar areas that are not classified as obstacle planes among the extracted planar areas; A route generation method for generating a route for a target moving body from a movement start position on a planar area not classified as an obstacle plane shown on the planar map to a movement target position on a planar area not classified as an obstacle plane shown on the planar map, based on the planar area not classified as an obstacle plane shown on the planar map.

11. When an environmental map consisting of a three-dimensional point cloud measured in a target area is divided into a plurality of grids so that a plane included in the target area is divided, each grid of the plurality of grids is set as a target grid, and when it is determined that a three-dimensional point cloud corresponding to a planar area exists within the target grid, a planar area is extracted from the target grid; A planar area extracted from each grid from which a planar area is extracted is taken as a target plane, and in a grid including the target plane, if a three-dimensional point cloud exists between the height of the target plane and a height obtained by adding the height of the target plane to a movable height threshold value that is equal to or greater than the height of the target moving body, the target plane is classified as an obstacle plane; a map creation process for creating a planar map showing the planar areas that are not classified as obstacle planes among the extracted planar areas; a route creation process for creating a route for the target moving object from a movement start position on the planar area not classified as an obstacle plane shown on the planar map to a movement target position on the planar area not classified as an obstacle plane shown on the planar map, based on the planar area not classified as an obstacle plane shown on the planar map; A route generation program that causes a route generation device, which is a computer, to execute the above.

Images