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

Abstract

This route generation device is provided with a map creation unit (142) and a route creation unit (144). The map creation unit (142): sets, as a target grid, each grid of a plurality of grids obtained by dividing an environmental map composed of three-dimensional point clouds; extracts a planar region from each target grid if it is determined that a three-dimensional point cloud corresponding to the planar region exists in the target grid; for each grid from which a planar region is extracted, sets the extracted planar region as a target plane; classifies each target plane as an obstacle plane if, in the grid containing the target plane, 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; and creates a planar map showing planar regions that are not classified as obstacle planes among the extracted planar regions. The route creation unit (144) creates a route for the target moving body from a movement start position to a movement target position on the basis of the planar map.

Description

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

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

[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 performing such tasks are equipped with a mobility mechanism appropriate for the task. For example, for robots engaged in transportation tasks, stable transport of passengers and goods is important, so wheeled robots are preferred. On the other hand, for robots engaged in inspection and security tasks, the ability to traverse steps or other obstacles accessible to humans is often required, so walking robots are also used in addition to wheeled robots. Thus, even if autonomous mobile robots with different mobility mechanisms have the same start position and target position, the intermediate paths for efficiently traveling from the start position to the target position may differ.

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

[0004] Patent No. 3994950

[0005] Conventional path generation programs determine whether a plane is navigable by taking into account constraints such as the traversal capability of the autonomous mobile robot's mobility mechanism, but do not consider the dimensions of the autonomous mobile robot, such as its overall height. Therefore, when conventional path generation programs acquire 3D data of structures, such as ceilings and bridge girders, vertically above the detected plane that the autonomous mobile robot would not normally come into contact with when passing through, they may identify the structures as obstacles that the mobility mechanism cannot traverse, resulting in the creation of an environmental map that shows a narrower area as the navigable area than the actual navigable area. Furthermore, to avoid creating such an environmental map, conventional path generation programs must impose constraints on sensor placement so as not to acquire 3D data of structures vertically above the plane.

[0006] The present disclosure aims to eliminate the need to impose restrictions 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 the autonomous mobile robot but also the dimensions of the autonomous mobile robot, such as its overall height, so that if there are no structures within a three-dimensional area that the autonomous mobile robot passes through while moving, it will not determine that an obstacle is present within the three-dimensional area.

[0007] A path 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 planar surfaces included in the target area are 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 in the target grid, the path generation device extracts a planar area from the target grid; when a planar area extracted from each grid from which a planar area has been extracted is set as a target plane, and when a three-dimensional point cloud exists in a grid including the target plane between a height of the target plane and a height obtained by adding the height of the target plane to a movable height threshold that is equal to or greater than the height of a target moving body, in a height direction relative to the target plane; a map creation unit that creates a planar map indicating planar areas that are not classified as obstacle planes among the extracted planar areas; and a path creation unit that creates a path for the 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 areas not classified as obstacle planes shown on the planar map.

[0008] According to the present disclosure, a map creation unit creates a planar map showing planar areas not classified as obstacle planes. Furthermore, a path creation unit creates a path for a target moving object based on the planar map. Here, the map creation unit determines whether to classify each planar area as an obstacle plane by taking into account the overall height of the target moving object. Furthermore, the target moving object may be an autonomous mobile robot. Therefore, according to the present disclosure, by taking into account not only the traversal capability of the autonomous mobile robot but also the dimensions of the autonomous mobile robot, such as its overall height, a three-dimensional area through which the autonomous mobile robot passes during movement is not determined to contain an obstacle if no structure is present within the area. This eliminates the need for constraints on sensor placement and enables the autonomous mobile robot to move indoors, under bridge girders, under tree branches, and the like.

[0009] 1 is a diagram showing an example of the configuration of a route generation system 90 according to the first embodiment. A diagram showing an example of the hardware configuration of a control device 1 according to the first embodiment. A flowchart showing the operation of a map creation unit 142 according to the first embodiment. A flowchart showing the operation of the map creation unit 142 according to the first embodiment. A diagram explaining a grid division method according to the first embodiment. A diagram explaining a plane extraction method according to the first embodiment. A diagram explaining a virtual plane calculation method according to the first embodiment. A diagram explaining obstacle determination processing according to the first embodiment. A diagram explaining processing by the map creation unit 142 ... a connection relationship registration unit 143 according to the first embodiment. A diagram explaining processing by the connection relationship registration unit 143 according to the first embodiment. A flowchart showing the operation of a route creation unit 144 according to the first embodiment. A flowchart showing the operation of the route creation unit 144 according to the first embodiment. A diagram explaining processing by the route creation unit 144 according to the first embodiment. 1A and 1B are diagrams illustrating the processing of the route creation unit 144 according to 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. FIG. 1C is a diagram illustrating the processing of the route creation unit 144 according to the first embodiment, where (a) is a table corresponding to the end of the third iteration and (b) is a table corresponding to the end of the fourth iteration. FIG. 1D is a diagram illustrating the processing of the route creation unit 144 according to the first embodiment, where (a) is a table corresponding to the end of the fifth iteration and (b) is a table corresponding to the end of the sixth iteration. FIG. 1E is a diagram illustrating a route list 51 according to the first embodiment. FIG. 1F is a diagram illustrating an example of the hardware configuration of a control device 1 according to a modification of the first embodiment. FIG. 1G is a diagram illustrating a route list 52 according to the second embodiment. FIG. 1G is a diagram illustrating speed setting according to the second embodiment, where (a) is a graph showing the height of a plane and (b) is a graph showing speed constraints. FIG. 1H is a diagram illustrating a route list 53 according to the third embodiment. FIG. 1H is a diagram illustrating speed setting according to the third embodiment, where (a) is a graph showing the height of a plane, (b) is a graph showing the shortest blind spot grid distance, and (c) is a graph showing speed constraints. 10A and 10B are diagrams illustrating a blind spot grid according to a third embodiment, in which FIG. 10A is a schematic diagram illustrating the positional relationship between a movement candidate plane and an obstacle plane, and FIG. 10B is a diagram illustrating a field of view reduction rate.

[0010] In the description of the embodiments and the drawings, the same elements and corresponding elements are given the same reference numerals. The description of elements given the same reference numerals will be omitted or simplified as appropriate. Arrows in the drawings mainly indicate the flow of data or the flow of processing. Furthermore, "unit" may be read as "circuit," "step," "procedure," "process," or "circuitry" as appropriate.

[0011] Embodiment 1. Specific embodiments 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 account the specifications of a robot for which a path is to be generated, creates an environmental map indicating the connection relationships between the planes based on the results of determining whether or not nearby planes are movable with respect to each extracted plane, and creates a path from the robot's movement start position to its movement target position based on the connection relationships indicated 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 the path 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 calculation results of 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 device 2 as a movement start position from the robot device 2 via the communication network, and transmits information indicating at least the path calculated by the path generation program 14 to the robot device 2. The communication unit 12 may transmit and receive information to and from other robot devices (not shown) and terminals such as smartphones.

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

[0017] The path generation program 14 is composed of a program input / output unit 141, a map creation unit 142, a connection relationship registration unit 143, and a path creation unit 144. The map creation unit 142, the connection relationship registration unit 143, and the path creation unit 144 each correspond 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 device 2 to a communication network. Thereafter, the path generation program 14 generates a movement path for the robot device 2, using the current position indicated by the received information as a movement start position. The information indicating the generated movement path is transmitted to the robot device 2 by the communication unit 12. In this specification, the movement path may also be simply referred to as a "path."

[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 results.

[0019] When a plane included in a target area is divided into multiple grids, the map creation unit 142 designates each grid of the multiple grids as a target grid. If it is determined that a 3D point cloud corresponding to the planar area exists within the target grid, the map creation unit 142 extracts a planar area from 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 also be simply referred to as a plane. The map creation unit 142 designates a planar area extracted from each grid from which a planar area has been extracted as a target plane. If a 3D point cloud exists in the grid including the target plane between the height of the target plane and a height equal to or greater than the height of the target moving object plus the height of the target plane, the map creation unit 142 classifies the target plane as an obstacle plane. If the map creation unit 142 does not classify the target plane as an obstacle plane, it may classify the target plane as a candidate moving plane. A specific example of the target moving object is the robot device 2. The target moving object may also be a robot device that moves by tires. The height direction relative to the target plane is, for example, a direction perpendicular to the target plane. The height of the target moving object is the total height of the target moving object. The map creation unit 142 creates a planar map showing planar areas that are not classified as obstacle planes among the extracted planar areas. The map creation unit 142 may include information indicating the height of each planar area shown on the planar map. The map creation unit 142 may include each planar area classified as an obstacle plane on the planar map, and may include information indicating the height of an obstacle in each planar area classified as an obstacle plane on the planar map.

[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 body can traverse between the two planar areas included in the target adjacent planar pair based on the structure of the moving mechanism of the target moving body 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 body can traverse between the two planar areas included in the target adjacent planar pair.

[0021] The path creation unit 144 creates a path 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 implement at least a portion of the functions of a path generation device.

[0023] The communication unit 21 receives information indicating at least the movement route from the control device 1 via the communication network, and transmits information indicating at least the current position calculated by the control program 27 to the control device 1. The communication unit 12 may also transmit and receive information to and from 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 that is similar to the three-dimensional global point cloud data 31 but has a reduced capacity 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 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, as a specific example. The acquired three-dimensional local point cloud data corresponding to the surrounding environment of the robot device 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). Data indicating the number of rotations is mainly used for mechanism 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 the 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). As a specific example, the robot device 2 has two pairs of actuators 25 and tire mechanisms 26. In this example, each pair of the two tire mechanisms 26 shares a tire axis, and the two pairs of actuators 25 transmit different driving forces to each tire mechanism 26, thereby enabling movement such as translation and turning.

[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 three-dimensional point cloud sensor 23 acquires three-dimensional local point cloud data, the self-position estimation unit 272 calculates the current position of the robot device 2 by matching the acquired three-dimensional local point cloud data with the three-dimensional 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 cloud, a point cloud at a location in the 3D local point cloud data used by the self-position estimation unit 272 for matching that does not match the 3D global point cloud data. Next, the surrounding environment detection unit 273 extracts, from the 3D global point cloud data, a 3D point cloud corresponding to the determined inconsistent point cloud and a certain range surrounding that range as a corresponding point cloud, and compares the average heights of the inconsistent point cloud and the corresponding point cloud. Next, if the average height of the inconsistent point cloud is higher than the average height of the corresponding point cloud, the surrounding environment detection unit 273 determines that a temporary obstacle exists in the range corresponding to the inconsistent point cloud. Otherwise, the surrounding environment detection unit 273 determines that an inconsistent point cloud exists due to an inconsistency caused by the previous presence of an obstacle, and performs plane detection on the inconsistent point cloud. If the surrounding environment detection unit 273 detects a planar area from the inconsistent point cloud, it determines that the range corresponding to the inconsistent point cloud is a temporary plane. Furthermore, when no planar area is detected from the inconsistent point cloud, the surrounding environment detection unit 273 determines that a temporary obstacle exists in the range corresponding to the inconsistent point cloud. Note that the surrounding environment detection unit 273 may classify at least a portion of the 3D local point cloud data into pedestrians, bicycles, or the like using a deep learning method or the like, and consider the point cloud data classified into pedestrians, bicycles, or the like as data corresponding to a temporary obstacle.

[0033] The path following calculation unit 274 calculates the movement vector of the robot device 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 device 2, and so that the robot device 2 does not come into contact with 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 observed value of the actuator 25 by the rotation sensor 24, taking into account 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 the hardware configuration of the control device 1 according to this embodiment. The control device 1 is realized by a general computer, for example. The robot device 2 may also include a similar computer. The control device 1 may also be realized by multiple computers.

[0036] As shown in the figure, the control device 1 is a computer including 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 connected via signal lines. The control device 1 is also connected to a display 107, a wired LAN (Local Area Network) port 108, a keyboard 109, a mouse 110, and the like.

[0037] The processor 101 is an integrated circuit (IC) that performs arithmetic processing for 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), or a graphics processing unit (GPU). The control device 1 may include multiple processors that replace the processor 101. The multiple processors share the role of the processor 101.

[0038] The memory 102 is typically a volatile storage device, and a specific example is RAM (Random Access Memory). 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 needed.

[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 needed. The memory 102 and the auxiliary storage device 103 may be configured integrally.

[0040] The display interface 104 converts the processing results of the processor 101 into a format that can be displayed on 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 can be processed by the processor 101. Specific examples of the communication interface 105 include a communication chip or a NIC (Network Interface Card).

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

[0043] A human sets a movement target position, which is the destination of the robot device 2, by inputting coordinates numerically using the keyboard 109 or by displaying a map corresponding to the area in which the robot device 2 will operate on the display 107 and specifying a point on the displayed map using the 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 the display 107, keyboard 109, and mouse 110. The functions of the communication unit 12 are realized by the wired LAN port 108.

[0044] The auxiliary storage device 103 stores a path generation program. The path generation program is a program that causes a computer to realize the functions of each unit of the control device 1. The path 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 in which various parameters such as specification information of the robot device 2 are described.

[0045] Data used when executing the route generation program and data obtained by executing the route generation program are stored in a storage device as appropriate. Each part of the control device 1 uses a storage device as appropriate. As a specific example, the storage device comprises 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 on 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 operating procedures of each device provided in the route generation system 90 are collectively referred to as a route generation method. Furthermore, the programs that realize the operations of each device provided in the route generation system 90 are collectively referred to as a route generation program. Below, the flow in which each calculation unit of the route generation program 14 generates a route from the three-dimensional global point cloud data 31 will be explained in order using figures.

[0048] 3 and 4 are flowcharts showing an example of the processing flow of the map creation unit 142. The processing flow will be explained using 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 at an angle parallel to the horizontal plane or the plane of movement of the robot device 2. That is, the environmental map is divided into a plurality of grids so that the planes included in the target area are divided. If the robot device 2 is a wheeled robot, it is desirable that the length of one side of the grid be equal to or greater than the longer of the total length or width of the robot device 2 so that the robot device 2 fits within the grid. That is, it is desirable that the length of each side of the faces of the target grid that correspond to the planes included in the target area be equal to or greater than the longer of the total length or width of the target moving object.

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

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

[0052] (Step S330) The map creation unit 142 checks whether a point cloud exists in the target lattice. If a point cloud exists in the target lattice, such as lattice 32 and lattice 33 shown in Fig. 5, the map creation unit 142 proceeds to step S340. If a point cloud does not exist in the target lattice, such as lattice 34 shown in Fig. 5, the map creation unit 142 proceeds to step S460.

[0053] (Step S340) The map creation unit 142 extracts planes that the robot device 2 can enter from the point cloud present in the target lattice. If one or more planes are extracted from the target lattice, the map creation unit 142 proceeds to step S370. Otherwise, the map creation unit 142 proceeds to step S350.

[0054] FIG. 6 is a schematic diagram illustrating an example of a plane extraction method for extracting planes that the robot device 2 can enter using a histogram. First, the map creation unit 142 creates a histogram based on the number of points contained in each rectangular parallelepiped obtained by dividing the target grid by a fixed width in the height direction. Next, the map creation unit 142 determines, in the created histogram, each bin whose number of bins is equal to or greater than a threshold as a plane candidate, and calculates the average height and standard deviation of the 3D point cloud contained in each bin determined to be a plane candidate. The map creation unit 142 then determines, among the plane candidates, those whose corresponding height standard deviation is equal to or less than a threshold as a plane, and proceeds to step S370. The bin containing the 3D point cloud determined to be a plane corresponds to the bin determined to contain a plane. If no plane is extracted from the target grid, the map creation unit 142 proceeds to step S350. It is preferable that the bin width be set 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 located above a bin from which a plane has been extracted contains 3D point cloud data, the bin located above the bin will be determined to be an obstacle. The upper limit of the step height 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 having the robot device 2 traverse steps of varying step heights. In FIG. 6 , the threshold number of bins determined to be plane candidates is set to five. Furthermore, the bins determined to be plane candidates in the 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 the grid 33 have a relatively large standard deviation because they contain both road surface reflection point clouds and tree reflection point clouds.

[0055] Here, when a grid contains a plane close to a horizontal plane, the points within the grid are characterized by a wide distribution of points and the heights of the points in the point cloud being approximately the same. Therefore, in this case, the plane can be detected using the above-described plane extraction method. However, when the grid size is expanded or when an inclined plane is desired to be extracted as a plane accessible to the robot device 2, the above-described plane extraction method may not be applicable. Therefore, in these cases, the map creation unit 142 may use an algorithm such as RANSAC (RANdom SAMPLE Consensus) to detect a plane from the points contained in each grid, and extract the detected plane as a plane accessible to the robot device 2, provided that the angle between the detected plane and the horizontal plane is equal to or less than a threshold. The detected plane is also referred to as a detected plane. The detected plane may not be a flat surface. In the example shown in Figure 6, only one plane is extracted from one grid, but in cases where one grid contains a step or an overpass, 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 each adjacent lattice to the target lattice contains a plane. Here, only the four lattices located to the front, back, left, and right of the target lattice may be considered as adjacent lattices to the target lattice, or eight lattices, including these four lattices and lattices diagonally adjacent to the target lattice, may be considered as adjacent lattices to the target lattice. If at least one adjacent lattice to the target lattice contains a plane, the map creation unit 142 proceeds to step S360. If none of the adjacent lattices to the target lattice contain a plane, the map creation unit 142 proceeds to step S460.

[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] 7 is a schematic diagram showing an example of a virtual plane calculation method for calculating a virtual plane from four adjacent grids located to the front, rear, left, and right of grid 33. In FIG. 7, the standard deviation threshold in step S340 is set between the standard deviation calculated for grid 32 and the standard deviation calculated for grid 33. Therefore, it is assumed that grid 32 is determined to contain a plane, and grid 33 is determined to contain no plane. Here, when a grid contains a plane, such as grid 32 adjacent to the right of grid 33, the average height of each plane included in that grid is calculated. When one plane exists in each of the grids adjacent to grid 33 to the front, rear, left, and right, and the average height calculated for the planes included in each grid is expressed as h ｆ and ｂ and ｌ and ｒ 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, and calculates the height h of the virtual plane of the grid 33. ｃ，ｉｍｇ Let h be the average height of one or more selected planes. In Figure 7, all four planes are assumed to be selected, i.e., h ｃ，ｉｍｇ = (h ｆ +h ｂ +h ｌ +h ｒ ) / 4. As a specific example, the predetermined threshold is a threshold set to approximately the width of the bins of the histogram used in step S340. Note that although at most one plane exists in each grid in FIG. 7, even if multiple planes exist in each grid, the map creation unit 142 sets a threshold related to the difference in height between the planes and selects planes included in grids adjacent to the target grid. Furthermore, the planes included in each grid adjacent to the target grid may be detected planes or virtual planes. When a new virtual plane is created in a grid adjacent to the target grid, a new virtual plane for the target grid may be created, or the virtual plane for the target grid may be updated.

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

[0060] (Step S370) The processes from step S370 to step S410 are repeated processes that are executed the same number of times as 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 the height obtained by adding the height of the detection plane to a movable height threshold. The movable height threshold is a threshold determined based on the minimum height required for the three-dimensional area in which the robot apparatus 2 can move.

[0062] FIG. 8 is a schematic diagram illustrating an example of a flow for calculating the height for obstacle determination using a histogram. Note that in FIG. 8 , it is assumed that in step S340, the standard deviation threshold was set equal to or greater than the standard deviation calculated for grid 33, and thus grids 32 and 33 were determined to each contain a plane. The map creation unit 142 determines the lowest-point bin from among the bins located above the bin determined to contain a plane, which is closest to the bin determined to contain a plane and has one or more bins, as the lowest-point bin for the histogram created in step S340. The map creation unit 142 then determines the lowest point in 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 minimum height value. If the calculated minimum obstacle height value, as in the example of grid 32 shown in FIG. 8 , is equal to or greater than the movable height threshold set equal to or greater than the height of the robot device 2, the map creation unit 142 determines that no obstacle is present and proceeds to step S400. The map creation unit 142 determines that an obstacle is present when the minimum obstacle height value is less than the movable height threshold, as in the example of the grid 33 shown in FIG. 8 . The map creation unit 142 then searches each bin located above the bin containing the lowest obstacle point, starting from the bottom, and determines the highest 3D point cloud data among the 3D point clouds contained in the bin immediately below the bin containing zero bins as the highest obstacle point. The map creation unit 142 then calculates the difference between the height of the detected plane and the height of the highest obstacle point as the maximum obstacle height, and proceeds to step S390. Note that the width of the bins is set taking into account the traversal capability of the robot device 2, so it is not necessary to extract only flat road surface reflection point clouds. Therefore, as in the example of the grid 33 shown in FIG. 8 , a bin determined to contain 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 to contain an obstacle in step S380 as an obstacle plane.

[0064] 9 and 10 are schematic diagrams showing an example of a method for registering candidate travel planes and obstacle planes in the plane list 41 in list format. The plane list 41 corresponds to a planar map. First, the map creation unit 142 assigns i+4 as an integer ID (Identifier) ​​that is unique to each grid 33 determined to contain an obstacle in step S380. Next, the map creation unit 142 registers (x, y) = (k+2, l+2) as "coordinate information," the "obstacle plane" as the "determination plane," and the "plane height" and "maximum obstacle height" as numerical information in the plane list 41.

[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] 9 and 10 , first, the map creation unit 142 assigns i+7 to the grid 32 determined in step S380 as having no obstacle 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," a "candidate plane for movement" as a "determination plane," and only the "plane height" as numerical information in the plane list 41. Note that, because there is no obstacle in the grid 32, the map creation unit 142 may leave the "maximum obstacle height" field blank or may register an invalid value for the "maximum obstacle height."

[0067] (Step S410) This step marks the end of each iteration in the repeating process started in step S370. The map creation unit 142 transitions to step S460 when processing has been completed for all detected planes, and transitions to step S370 when processing has not been completed for all detected planes. Note that with regard to the repeating process on the virtual plane from step S420 to step S450, which has not been described, if the detected plane is replaced with the virtual plane, step S370 corresponds to step S420, step S380 to step S430, step S390 to step S440, and step S410 to step S450, respectively.

[0068] (Step S460) This step marks the end of each iteration in the repeating process started in step S320. The map creation unit 142 ends the processing of the flowchart when processing has been completed for all grids, and otherwise proceeds to step S320. Through the above processing, a plane list 41 is created in the form of a grid map, in which movement candidate planes and obstacle planes are described.

[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, as registered by the connection relationship registration unit 143. FIG. 12 shows an extended plane list 42, which is an extension of the plane list 41 to describe the connection relationships between planes. The extended plane list 42 corresponds to a plane map and is also referred to as 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" of the plane list shown in FIG. 9 in an easily understandable manner. Here, since the robot device 2 used in this embodiment is a wheeled robot, if the length of one side of each grid is equal to or greater than the greater of the total length or width of the robot device 2, it is clear that the lengths of the sides between the target grid and adjacent grids on the front, rear, left, and right of the target grid are lengths that the robot device 2 can pass through. However, the conditions for the robot device 2 to move from one grid to a plane included in an adjacent grid are not limited to the length of one side of the grid. Therefore, it is necessary to register the connection relationships between the planes. Therefore, the connection relationship registration unit 143 registers connection relationships according to the following procedure: 1. For all movement candidate planes, the connection relationship registration unit 143 searches for movement candidate planes in each grid adjacent to the grid containing each movement candidate plane (in FIG. 11 , the four adjacent grids are in the front, back, left, and right directions). 2. For each movement candidate plane, the connection relationship registration unit 143 compares the height of each movement candidate plane with the height of movement candidate planes in grids adjacent to the grid containing each movement candidate plane, and if the difference in height between the planes in the adjacent grids is equal to or less than the traversal threshold, determines that movement is possible and registers the connection relationship.

[0070] Using the candidate plane ID = 10 shown in Figures 11 and 12 as an example, a method for registering a connection relationship when the traversable threshold is 0.25 m will be described. In this example, the candidate plane ID = 10 is the target plane, and the grid containing the target plane is the target grid. A candidate plane ID = 11 exists as an adjacent grid in the x+1 direction relative to the target grid. Here, the height difference between the target grid and the adjacent grid is 0.4 m. Because this height difference exceeds the traversable threshold, it is determined that movement from the target grid to the adjacent grid is not possible, and the connection relationship between the target grid and the adjacent grid is not registered in the extended grid list 42. Because there is no plane in the grid adjacent to the target grid in the x-1 direction, the connection relationship between both grids is not registered in the extended grid list 42. A candidate plane ID = 15 exists as an adjacent grid in the y+1 direction relative to the target grid. Here, the height difference between the target grid and the adjacent grid is 0.8 m. Because the height difference exceeds the traversable threshold, it is determined that movement from the target plane to the adjacent plane is not possible, and the connection relationship between the target plane and the adjacent plane is not registered in the extended plane list 42. The lattice adjacent to the target lattice 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 plane and the adjacent plane is 0 m. Because the height difference is less than or equal to the traversable threshold, it is determined that movement from the target plane to the adjacent plane is possible, and the connection relationship between the target plane and the adjacent plane is registered in the extended plane list 42. The connection relationship registration unit 143 creates the extended plane list 42 by performing a process of appropriately registering connection relationships for all candidate planes for movement in the plane list 41 in this way.

[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 explained using Fig. 13 and Fig. 14. Note that this flowchart was created based on an existing algorithm known as the A* (A-star) 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 movement target position may be input as local coordinate values ​​of 3D global point cloud data, or data indicating latitude, longitude, altitude, etc. The path creation unit 144 assigns an ID to each acquired position. In the description of this flowchart, the extended plane list 42 is as shown in FIG. 12, and the ID of the movement start position is SID=1 and the ID of the movement target position is 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. 11 . FIGS. 16 to 18 show the transitions of the unsearched list and the searched list from the start of processing 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, both the unsearched list and the searched list also hold coordinate information. The parent ID is the ID of the plane one step back along the route. However, since no such plane exists at the start of movement, the ID of the start of movement is set as the parent ID corresponding to the start of movement. If the cost decreases as the target position is approached, it is possible to generate a route to the target position based on the cost. While there are several methods for calculating such costs, this embodiment employs [Formula 1] as the formula for calculating the cost of a 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 of the target whose cost is to be calculated in the initial unsearched list, since the parent ID is the target ID (own ID), the cost of the parent ID, C(PID) = 0. Furthermore, 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 processing of this flowchart as "non-arrival end." Note that in the initial state, the route creation unit 144 transitions to step S540 because the unsearched list is not empty.

[0078] (Step S540) The path creation unit 144 sorts the unsearched list in ascending order of cost. At this time, the sorting algorithm may be a commonly known quick sort or bubble sort. In addition, since there may be multiple IDs with the same corresponding cost, a second priority is set in the sorting, such that a priority is given to an ID whose y component of 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 path creation unit 144 acquires the ID of the row with the smallest cost 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 path creation unit 144 may acquire the first row of the unsearched list as the row with the smallest cost 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 creation 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 generation 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. When the current search ID: NID=1, the connection ID: CID=2,5.

[0082] (Step S580) The processes from step S580 to step S630 are repeated a 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 repeated processes 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 link IDs CID=2 and CID=5 are calculated using formula (1), C(2)=C(5)=1.

[0084] (Step S600) The path creation unit 144 searches the unsearched list and the searched list to determine whether a row having the same ID as the target link ID exists.

[0085] (Step S610) Based on the search results in step S600, the path creation unit 144 confirms that neither the unsearched list nor the searched list contains an ID that is the same as the target link ID. If neither of the lists contains an ID that is the same as the target link ID, the path creation unit 144 transitions to step S620, and if either the unsearched list or the searched list contains an ID that is the same as the target link ID, the path creation unit 144 transitions to step S650. In the first iteration of the repetitive process, the unsearched list and the searched list remain in their initial states, so that an ID that is the same as the link ID: CID = 2, 5 does not exist in either list. Therefore, if the target link ID is 2 or 5, the path creation unit 144 transitions to step S620.

[0086] (Step S620) In the unsearched list, the path creating 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 processing has been completed for all link IDs, and transitions to step S580 when processing has not been completed for all link 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. Figure 16(b) shows the unsearched list and the searched list at the end of the first round of step S640. The row of the current search ID: NID = 1 has been moved to the searched list, and the rows of CID = 2 and CID = 5, which were previously combined IDs, 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: NID=5 is set according to the second priority. Also, in step S570, the bond ID: CID=1 and CID=9 is set. In the repeated process of each bond ID starting from step S580, when the bond ID: CID=1, the cost calculated in step S590 is C(1)=3, and ID=1 is also in the searched list. Therefore, a branch to step S650 occurs in step S610.

[0090] (Step S650) The path creation unit 144 checks whether the unsearched list contains an ID that is the same as the target link ID. If the unsearched list contains an ID that is the same as the target link ID, the path creation unit 144 transitions to step S660. Otherwise, that is, if the searched list contains an ID that is the same as the target link ID, the path creation unit 144 transitions to step S670. Since ID=1 exists in the searched list, the path creation unit 144 transitions to step S670 here. Note that the list updates in steps S660 and S670 are the same process except that the target lists are different. Therefore, 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 ID that is the same as the target link ID in the searched list. If the cost most recently calculated in step S590 is smaller, the path creation unit 144 updates the "parent ID" for the row with the same ID to the parent ID that corresponds 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 rows within the list are not moved.

[0092] For ID=1, the most recently calculated cost C(1)=3, and the cost C(1)=1 in the searched list. 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. Therefore, the row with CID=9 is written to the unsearched list. As a result, the unsearched list and 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 performed in order again. In step S550, since the cost is the same between IDs 2 and 9, the current search ID: NID=9 is set according to the second priority. Also, in step S570, the combined ID: CID=5, 12 is set. For CID=5, the cost C(5)=3, so the cost indicated in the searched list is smaller. Therefore, the list is not updated. For CID=12, the cost C(12)=3, so the cost C(12) is entered in the unsearched list. As a result, the unsearched list and searched list at the end of the third iteration of step S640 are as shown in FIG. 17(a).

[0094] After that, the process returns to step S530, and when each step is performed again in order, a cost difference occurs in step S550. Therefore, the current search ID: NID = 2, and the route creation unit 144 again starts a search from an ID where the parent ID: PID = 1. The remaining repeated processes are omitted, but in the seventh 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 parent IDs in the searched list from the movement target position to the movement start position, and ends the processing of this flowchart with the notation "arrival completed." Specifically, the parent ID: PID = 7 corresponding to the current search ID: NID = 10, and the parent ID: PID = 3 of the row with ID = 7 in the searched list, are traced in order, and so on, making it possible to obtain coordinate information for the route connecting the movement start position to the movement target position, with ID = 1 → 2 → 3 → 7 → 10.

[0096] Here, it is possible to control the robot device 2 along the route using only the coordinate information of the route. However, in controlling the robot device 2 along the route, it is common to use speed information at each position on the route. FIG. 19 shows a route list 51 with speed information added. This route list 51 corresponds to (b) in FIG. 18 . The route creation unit 144 may generate this route list 51. In the speed information, only the speed corresponding to the movement target position may be zero, and the speeds corresponding to all other positions on the route may be the maximum speed of the robot device 2. However, when the robot device 2 is engaged in transportation work, attention must also be paid to acceleration. Therefore, it is desirable to add speed information indicating that the robot device 2 gradually accelerates from the movement start position, decelerates before a 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 upper limit of the absolute value of the speed change for moving through one grid is 2 km / h, the upper limit of the speed at a corner is 3 km / h, and the maximum speed of the robot device 2 is 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 path generation program 14 is included in the control device 1, but the robot device 2 may execute the processing of the path generation program 14 by using three-dimensional global point cloud data included in the storage unit 22. Also, in the first embodiment, only the control device 1 and one robot device 2 are described. 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 have to be identical to each other.

[0098] ***Explanation of the Effects of Embodiment 1*** As described above, according to this embodiment, by taking into consideration not only the autonomous mobile robot's traversal ability 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 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*** <Modification 1> Fig. 20 shows an example of the hardware configuration of the control device 1 according to this modification. The control device 1 includes a processing circuit 111 instead of the processor 101, the processor 101 and memory 102, the processor 101 and auxiliary storage device 103, or the processor 101, memory 102, and auxiliary storage device 103. The processing circuit 111 is hardware that realizes at least a part of the components included in the control device 1. The processing circuit 111 may be dedicated hardware, or may be a processor that executes a program stored in the memory 102.

[0100] When the processing circuit 111 is dedicated hardware, the processing circuit 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 be provided with multiple processing circuits that replace the processing circuit 111. The multiple processing circuits share the role of the processing circuit 111.

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

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

[0103] Second Embodiment. Differences from the above-described embodiments will be mainly described below 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 slope may decrease depending on the weight of the load. Furthermore, if the robot device 2 enters an uneven road at high speed, vertical acceleration may occur, posing 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 that is vertically inclined or uneven compared to the speed on a flat road surface. Therefore, in this embodiment, a speed constraint on a non-flat detection plane is added.

[0104] ***Description of Configuration*** 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 a route list 52 that lists route coordinate information, plane heights, and speed information. Fig. 22 is a diagram corresponding to Fig. 21. Fig. 22(a) shows a graph showing plane heights in the order of via planes. Fig. 22(b) shows a graph showing speed constraints in the order of via planes. In the examples shown in Figs. 21 and 22, the step threshold between grids is set to 3 cm.

[0106] The path creation unit 144 according to the present embodiment defines a plurality of 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 object on the target continuous plane. As a specific example, the path creation unit 144 executes a slope and unevenness determination process. The slope and unevenness determination process is a process for creating a speed constraint on a surface that has at least one of a slope and an unevenness. In the slope and unevenness determination process, the path creation unit 144 determines a plurality of planar areas as an upslope area if the height between consecutively adjacent planar areas increases by more than a step threshold, determines a downslope area if the height between consecutively adjacent planar areas decreases by more than a step threshold, determines a downslope area if an increase in height by more than a step threshold and a decrease in height by more than a step threshold alternately repeat, and determines a flat area otherwise. Upslope areas and downslope areas are collectively referred to as slope areas. Note that some flat areas may correspond to both the inclined portion and the uneven portion. As a specific example, the lattice G and the lattice H correspond to both the upward inclined portion and the uneven portion.

[0107] In FIG. 22B, the "initial speed, cornering, and stopping constraints" indicate speed constraints of the robot apparatus 2 when the absolute upper limit of the speed change when moving between one grid is 1 km / h, the upper limit of the speed around corners is 2 km / h, and the maximum speed of the robot apparatus 2 is 4 km / h. On the other hand, the "speed constraints on slopes and uneven surfaces" indicate speed constraints of the robot apparatus 2 when the maximum speed of 4 km / h is permitted on flat sections, the upper limit of the speed on uneven sections is 2.5 km / h, the upper limit of the speed on upslope sections is 3.5 km / h, and the upper limit of the speed on downslope sections is 3 km / h. The "target speed setting" is a target value for the speed setting of the robot apparatus 2 in each flat area, and is obtained by selecting the lower speed from the speed constraints in each flat area. Here, for the "target speed setting" of each of grids G and H, the upper limit speed on the uneven portion is set because the upper limit speed on the uneven portion is lower than the upper limit speed on the uphill portion. Note that in the examples shown in Figs. 21 and 22, the upper limit speed on the inclined portion and the uneven portion is set to a constant value, but each upper limit speed may be changed depending on the load on the robot device 2, such as the weight of the payload. Furthermore, the upper limit speed may be changed depending on the gradient of the inclined portion (the degree of monotonically changing step height) or the size of the step on the uneven portion.

[0108] ***Explanation of Effects of Embodiment 2*** According to this embodiment, the speed of the robot device 2 is set taking into consideration uneven portions and slopes. Therefore, this embodiment is more suitable for cases where the robot device 2 is engaged in transportation work.

[0109] Third Embodiment. Differences from the above-described embodiments will be mainly described below with reference to the drawings. When the robot device 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 causes the path to be traveled in the future to be in a blind spot, it is desirable to slow down the speed of the robot device 2 near the blind spot. Therefore, in this embodiment, a speed constraint regarding the blind spot is set.

[0110] ***Description of Configuration*** 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 along a line segment connecting the first point and the second point, the route creation unit 144 determines whether 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 heights of each planar area included in the target obstacle group and the heights of the obstacles in each planar area included in the target obstacle group. The target obstacle group consists of one or more planar areas classified as obstacle planes. Here, on the route, the target moving object passes through the first point and then the second point. The distance between the first point and the second point is equal to or less than the field of view distance. Specifically, the field of view distance is set to be equal to or less than the detection distance of the 3D 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 a speed to the target moving object at the first point that is lower than the speed of the target moving object at the first point that would be assigned if it was determined that the second point is not in a blind spot relative to the first point. In addition, when a group of target obstacles exists 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 the blind spot of the second point when the target moving body is located at the first point, based on the height of each planar area included in the group of target obstacles, the height of the obstacles in each planar area included in the group of target obstacles, and the field of view of the target moving body, and may 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 listing the route, the obstacle's "coordinate information," "plane height," "maximum obstacle height," and "target speed setting." FIG. 24(a) shows a graph showing plane heights in order of via planes. FIG. 24(b) shows a graph showing the shortest blind spot grid distance in order of via planes. FIG. 24(c) shows a graph showing speed constraints in order of via planes. FIG. 25(a) shows a schematic diagram showing the positional relationship between a candidate movement plane and an 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 determined when an obstacle plane that satisfies certain conditions exists between the line segment connecting the centers of two grids, the reference grid and the grid to be determined. In this example, if a line segment on a side or corner of an obstacle plane is just in contact with the obstacle plane, the obstacle plane is not considered to be an obstacle plane between the line segments.

[0113] An obstacle plane that satisfies a predetermined condition is an obstacle plane that prevents an object present within a grid to be detected by an external sensor, such as the 3D point cloud sensor 23 of the robot device 2, from being detected. In the examples shown in FIGS. 23 to 25 , the path creation unit 144 determines whether an obstacle plane satisfies the predetermined condition based on whether the corresponding "maximum obstacle height" is equal to or greater than the blind spot determination height threshold (0.75 m). The blind spot determination height threshold is determined based on the installation height of the 3D point cloud sensor 23, the posture of the robot device 2, and other factors. By setting a threshold for the obstacle height, it is possible to exclude obstacle planes, such as curbs, that do not prevent the 3D point cloud sensor 23 from detecting the obstacle.

[0114] The shortest blind spot grid distance is the Manhattan distance from the reference grid to the grid that is first determined to be a blind spot grid when the above-mentioned determination is made in order from the grid two grids away on the route toward the movement target position. The reference grid corresponds to the first point. Each grid that exists beyond 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 away on the route, the grid one grid away on the route cannot be a blind spot grid. Therefore, there is no need to determine the grid one grid away on the route. Therefore, the minimum value of the shortest blind spot grid distance is 2.

[0115] Here, the procedure for determining the shortest blind spot grid distance corresponding to grid a will be described. Grid h is the grid two steps ahead of grid a on the route. Grid b, which exists between the line segment connecting grid a and grid h, has a maximum obstacle height that does not exceed the blind spot determination height threshold, so grid h is not determined to be a blind spot grid. The same applies to grid i, which is the grid three steps ahead of grid a on the route. On the other hand, grid c, which exists between the line segment connecting grid a and grid j, has a maximum obstacle height that exceeds the blind spot determination height threshold. Therefore, grid j is determined to be a blind spot grid. Therefore, since the Manhattan distance between grid a and grid j is 4, the shortest blind spot grid distance corresponding to grid 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. 24B, 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 speed limit among the speed constraints for each planar region.

[0117] The path creation unit 144 may determine that a grid is a blind spot grid when the field of view reduction rate exceeds a threshold. As a specific example, in the example shown in FIG. 25A, when attempting to detect an object present within grid d from grid i, it is highly likely that an obstacle present in grid c with a maximum obstacle height of 2.0 m is actually blocking part of the field of view of the 3D point cloud sensor 23. FIG. 25B is a schematic diagram showing an example of a method for calculating the field of view reduction rate by taking into account an obstacle grid present near a line segment connecting two grids. When calculating the field of view reduction rate of grid d based on grid i, first, line segments are drawn from the center of grid i, which is assumed to be the position of the 3D point cloud sensor 23, to each of the four corners of grid d. An obstacle plane (grid c) is included between the line segment to the lower left corner of grid d and the line segment to the lower right corner of grid d. Therefore, the path creation unit 144 extends each line segment to the opposite side or diagonal of grid d if it is possible to extend each line segment, and determines the area within grid d sandwiched between the two line segments as the field of view reduction area. The field of view reduction area of ​​grid d, based on grid i, is caused only by grid c, which is an obstacle plane. That is, the field of view reduction area is a triangular area with vertices at the lower left, lower right, and upper left corners of grid d, and the field of view reduction rate is 1 / 2 (50%). Furthermore, when calculating the field of view reduction rate of grid e, based on grid i, line segments are 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 within grid e sandwiched between the two line segments is determined to be the field of view reduction area. The field of view reduction area of ​​grid e, based on grid i, is caused only by grid k, which is an obstacle plane. That is, the field of view reduction area is a triangle with vertices at the upper left corner, upper right corner, and the point at the top right side of grid e. Since the slope of the line segment from the center of grid i to the point at the top of the right side of grid e is 1 / 3, the point at the top of the right side is located 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] Note that, for simplicity's sake, in FIG. 25B , the description uses a field-of-view reduction area caused by a single obstacle plane. However, if field-of-view reduction areas caused by multiple obstacle planes are included, calculations must be performed taking into account the overlap of the field-of-view reduction areas. Also, in FIG. 25B , the center of the grid is assumed to be the position of the 3D point cloud sensor 23. However, if the 3D point cloud sensor 23 is attached to the robot device 2 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 that other position to the grid where the blind spot grid determination is performed. 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, by using a high-performance computer, or by future improvements in computer performance, if each obstacle plane has a 3D model of each obstacle contained in that obstacle plane and it is possible to calculate the detection range of the 3D point cloud sensor 23, the field-of-view reduction area may be calculated using a collision detection method such as ray tracing.

[0119] ***Explanation of Effects of Third Embodiment*** As described above, according to this embodiment, it is possible to appropriately set speed constraints regarding blind spots.

[0120] Fourth Embodiment. The following mainly describes differences from the above-described embodiments. When the robot device 2 performs operations in a place frequented by many people, such as a commercial facility or an apartment complex, the location of obstacles may continually change due to vehicles parked for long periods for transportation operations or business signs that are only up 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 path that should be traversable may not be generated, or conversely, a path that is actually impassable may be generated. Therefore, it is desirable to change the grid map to suit the real environment. Therefore, in this embodiment, the grid map is changed to suit the real environment.

[0121] ***Description of Configuration*** 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 this embodiment performs a contradiction determination a predetermined number of times or more. The contradiction determination is a process for determining whether or not there is a contradiction 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 with the 3D point cloud sensor 23. The measurement target area is at least a part of the target area. If the map creation unit 142 determines that there is a contradiction consecutively for a predetermined number of times or more in the contradiction determination, it 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] ***Description of Operation*** The surrounding environment detection unit 273 determines whether the local point cloud data in the portion that does not match the global point cloud data corresponds to a temporary obstacle or a temporary plane. The robot device 2 transmits data indicating its own position, the position of the inconsistent point cloud, and the determination result to the control device 1.

[0125] The map creation unit 142 compares the created grid map with the grid corresponding to the location of the contradiction point group. If the grid corresponding to the movement candidate plane is continuously determined to be a temporary obstacle, the map creation unit 142 changes the classification of the planar area corresponding to the grid to an obstacle plane. Furthermore, if the grid corresponding to the obstacle plane is continuously determined to be a temporary plane, the map creation unit 142 changes the classification of the planar area corresponding to the grid to a movement candidate plane. Note that, since the robot device 2 continues to move, a case where a continuous determination is made means, for example, a case where the robot device 2 passes within a certain range of a position corresponding to a certain contradiction point group, and the determination results corresponding to the contradiction point group transmitted at a certain interval continuously indicate the same determination.

[0126] Even if the robot device 2 is located within a certain range of the position corresponding to the inconsistent point cloud, such as when the surrounding area is crowded, the position corresponding to the inconsistent 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 inconsistent point cloud. Furthermore, when an invalid value is transmitted, the map creation unit 142 may suspend determination as to whether or not the determination has been continued.

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

[0128] ***Other Embodiments*** The above-described embodiments can be freely combined, or any of the components of each embodiment can be modified, or any of the components can be omitted from each embodiment. Furthermore, the embodiments are not limited to those shown in embodiments 1 to 4, and various modifications are possible as needed. The procedures described using flowcharts, etc., can be modified as appropriate.

[0129] 1 Control device, 11 User input / output unit, 12 Communication unit, 13 Memory unit, 14 Route generation program, 141 Program input / output unit, 142 Map creation unit, 143 Connection relationship registration unit, 144 Route creation unit, 2 Robot device, 21 Communication unit, 22 Memory unit, 23 Three-dimensional 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 Route following calculation unit, 275 Mechanism calculation unit, 31 Three-dimensional global point cloud data, 32, 33, 34 Lattice, 41 Plane list, 42 Extended plane list, 51, 52, 53 Route list, 90 Route 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. A path generation device comprising: a map creation unit that creates a planar map indicating planar areas not classified as obstacle planes among the extracted planar areas; and a path creation unit that creates a path for the target moving object from a movement start position on a planar area not classified as an obstacle plane and a movement target position on a planar area not classified as an obstacle plane, based on the planar areas not classified as obstacle planes and shown on the planar map.

2. The route generation device of claim 1, wherein the map creation unit includes information indicating the height of each planar area shown on the planar map, and the route creation unit defines a plurality of consecutive planar areas passed through on the route as a target continuous plane, determines the road surface conditions of the target continuous plane based on the difference in height 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. The map creation unit includes each planar area classified as an obstacle plane in the planar map, includes information indicating the height of each planar area shown by the planar map in the planar map, and includes information indicating the height of obstacles in each planar area classified as an obstacle plane in the planar map; the route creation unit selects a first point and a second point on the route, and when one or more planar areas classified as the obstacle plane as a target obstacle group exist along a line segment connecting the first point and the second point, determines whether 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 the obstacle in each planar area included in the target obstacle group; when it is determined that the second point is in a blind spot relative to the first point, assigns a speed to the target moving body at the first point that is lower than the speed of the target moving body at the first point that would be assigned if it was determined that the second point is not in a blind spot relative to the first point; The route generation device according to claim 1 or 2, wherein, on the route, the target moving object passes through the first point and then the second point, and the distance between the first point and the second point is equal to or less than a visual distance.

4. The path generation device described in claim 3, 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 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 determines whether the second point is a blind spot with respect to the first point based on the size of the target blind spot.

5. The map creation unit classifies the target plane as a movement candidate plane when the target plane is not classified as an obstacle plane, performs a contradiction determination a reference number of times or more to determine whether there is a contradiction between a three-dimensional point cloud corresponding to a measurement target area that is at least a part of the target area in an environmental map consisting 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, and if it determines that there is a contradiction for the reference number of times or more in the contradiction determination, updates the classification of each planar area corresponding to the measurement target area so as to eliminate the contradiction corresponding to the contradiction determination, and the path creation unit recreates the path for at least one planar area corresponding to the measurement target area when the classification of the planar area corresponding to the obstacle plane is updated to the movement candidate plane or when the classification of the planar area corresponding to the movement candidate plane is updated to the obstacle plane.

6. A path generation device according to any one of claims 1 to 5, wherein the target moving body is a robotic device that moves by means of tires.

7. A path generation device as described in claim 6, wherein the length of each side of the faces of the target lattice that correspond 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.

8. A path generation method in which a computer, 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 flat surfaces included in the target area are divided, selects each grid of the plurality of grids as a target grid, and extracts a flat surface from the target grid when it is determined that a three-dimensional point cloud corresponding to a flat surface exists within the target grid; selects a planar surface extracted from each grid from which a planar surface has been extracted as a target plane; and classifies the target plane as an obstacle plane when, in a grid including the target plane, 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 that is equal to or greater than the height of the target moving object; creates a planar map showing, from the extracted planar surfaces, those planar surfaces that are not classified as obstacle planes; and creates a path for the target moving object from a movement start position on the planar surface not classified as an obstacle plane shown on the planar map to a movement target position on the planar surface not classified as an obstacle plane shown on the planar map, based on the planar surface not classified as an obstacle plane shown on the planar map.

9. A path generation program that causes a path generation device, which is a computer, to execute the following steps: 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 flat surfaces included in the target area are 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 flat surface exists in the target grid, extracting a flat surface from the target grid; setting a planar surface extracted from each grid from which a planar surface has been extracted as a target plane; and when, in a grid including the target plane, 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 that is equal to or greater than the height of the target moving object, classifying the target plane as an obstacle plane; a map generation process that creates a planar map showing the planar areas that are not classified as obstacle planes among the extracted planar areas; and a path generation process that creates a path 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 areas not classified as obstacle planes shown on the planar map.

Images